aligator
0.6.1
A primal-dual augmented Lagrangian-type solver for nonlinear trajectory optimization.
|
A proximal, augmented Lagrangian-type solver for trajectory optimization. More...
#include <aligator/solvers/proxddp/solver-proxddp.hpp>
Public Types | |
using | Scalar = _Scalar |
using | Problem = TrajOptProblemTpl<Scalar> |
using | Workspace = WorkspaceTpl<Scalar> |
using | Results = ResultsTpl<Scalar> |
using | StageFunctionData = StageFunctionDataTpl<Scalar> |
using | DynamicsData = DynamicsDataTpl<Scalar> |
using | CostData = CostDataAbstractTpl<Scalar> |
using | StageModel = StageModelTpl<Scalar> |
using | ConstraintType = StageConstraintTpl<Scalar> |
using | StageData = StageDataTpl<Scalar> |
using | CallbackPtr = shared_ptr<CallbackBaseTpl<Scalar>> |
using | CallbackMap = std::unordered_map<std::string, CallbackPtr> |
using | ConstraintStack = ConstraintStackTpl<Scalar> |
using | CstrSet = ConstraintSetBase<Scalar> |
using | TrajOptData = TrajOptDataTpl<Scalar> |
using | LinesearchOptions = typename Linesearch<Scalar>::Options |
using | CstrProximalScaler = ConstraintProximalScalerTpl<Scalar> |
using | LinesearchType = proxsuite::nlp::ArmijoLinesearch<Scalar> |
using | LQProblem = gar::LQRProblemTpl<Scalar> |
using | Filter = FilterTpl<Scalar> |
Public Member Functions | |
ALIGATOR_DYNAMIC_TYPEDEFS (Scalar) | |
SolverProxDDPTpl (const Scalar tol=1e-6, const Scalar mu_init=0.01, const Scalar rho_init=0., const std::size_t max_iters=1000, VerboseLevel verbose=VerboseLevel::QUIET, HessianApprox hess_approx=HessianApprox::GAUSS_NEWTON) | |
void | setNumThreads (const std::size_t num_threads) |
std::size_t | getNumThreads () const |
ALIGATOR_DEPRECATED const Results & | getResults () |
ALIGATOR_DEPRECATED const Workspace & | getWorkspace () |
Scalar | tryNonlinearRollout (const Problem &problem, const Scalar alpha) |
Policy rollout using the full nonlinear dynamics. The feedback gains need to be computed first. This will evaluate all the terms in the problem into the problem data, similar to TrajOptProblemTpl::evaluate(). | |
Scalar | forwardPass (const Problem &problem, const Scalar alpha) |
void | updateLQSubproblem () |
void | setup (const Problem &problem) |
Allocate new workspace and results instances according to the specifications of problem . | |
bool | run (const Problem &problem, const std::vector< VectorXs > &xs_init={}, const std::vector< VectorXs > &us_init={}, const std::vector< VectorXs > &lams_init={}) |
Run the numerical solver. | |
bool | innerLoop (const Problem &problem) |
Perform the inner loop of the algorithm (augmented Lagrangian minimization). | |
void | computeInfeasibilities (const Problem &problem) |
Compute the primal infeasibility measures. | |
void | computeCriterion () |
Compute stationarity criterion (dual infeasibility). | |
void | computeMultipliers (const Problem &problem, const std::vector< VectorXs > &lams, const std::vector< VectorXs > &vs) |
ALIGATOR_INLINE Scalar | mu () const |
ALIGATOR_INLINE Scalar | mu_inv () const |
ALIGATOR_INLINE Scalar | rho () const |
Primal proximal parameter \(\rho > 0\). | |
void | updateGains () |
Update primal-dual feedback gains (control, costate, path multiplier) | |
callbacks | |
void | registerCallback (const std::string &name, CallbackPtr cb) |
Add a callback to the solver instance. | |
void | clearCallbacks () noexcept |
Remove all callbacks from the instance. | |
const CallbackMap & | getCallbacks () const |
void | removeCallback (const std::string &name) |
auto | getCallback (const std::string &name) -> CallbackPtr |
void | invokeCallbacks (Workspace &workspace, Results &results) |
Invoke callbacks. | |
Static Public Member Functions | |
static Scalar | tryLinearStep (const Problem &problem, Workspace &workspace, const Results &results, const Scalar alpha) |
Try a step of size \(\alpha\). | |
Public Attributes | |
Scalar | inner_tol_ |
Subproblem tolerance. | |
Scalar | prim_tol_ |
Desired primal feasibility. | |
Scalar | target_tol_ = 1e-6 |
Solver tolerance \(\epsilon > 0\). | |
Scalar | mu_init = 0.01 |
Scalar | rho_init = 0. |
Scalar | reg_min = 1e-10 |
Scalar | reg_max = 1e9 |
Scalar | reg_init = 1e-9 |
Scalar | reg_inc_k_ = 10. |
Scalar | reg_inc_first_k_ = 100. |
Scalar | reg_dec_k_ = 1. / 3. |
Scalar | preg_ = reg_init |
Scalar | preg_last_ = 0. |
Scalar | inner_tol0 = 1. |
Scalar | prim_tol0 = 1. |
Logger | logger {} |
Logger. | |
VerboseLevel | verbose_ |
Solver verbosity level. | |
LQSolverChoice | linear_solver_choice = LQSolverChoice::SERIAL |
Choice of linear solver. | |
bool | lq_print_detailed = false |
HessianApprox | hess_approx_ = HessianApprox::GAUSS_NEWTON |
Type of Hessian approximation. Default is Gauss-Newton. | |
LinesearchOptions | ls_params |
Linesearch options, as in proxsuite-nlp. | |
LinesearchStrategy | ls_strat = LinesearchStrategy::ARMIJO |
Type of linesearch strategy. Default is Armijo. | |
MultiplierUpdateMode | multiplier_update_mode = MultiplierUpdateMode::NEWTON |
Type of Lagrange multiplier update. | |
LinesearchMode | ls_mode = LinesearchMode::PRIMAL |
Linesearch mode. | |
Scalar | dual_weight = 1.0 |
Weight of the dual variables in the primal-dual linesearch. | |
RolloutType | rollout_type_ = RolloutType::NONLINEAR |
Type of rollout for the forward pass. | |
BCLParamsTpl< Scalar > | bcl_params |
Parameters for the BCL outer loop of the augmented Lagrangian algorithm. | |
StepAcceptanceStrategy | sa_strategy = StepAcceptanceStrategy::LINESEARCH |
Step acceptance mode. | |
bool | force_initial_condition_ = true |
std::size_t | maxRefinementSteps_ |
Scalar | refinementThreshold_ = 1e-13 |
std::size_t | max_iters |
std::size_t | max_al_iters = 100 |
Scalar | mu_lower_bound = 1e-8 |
uint | rollout_max_iters |
CallbackMap | callbacks_ |
Callbacks. | |
Workspace | workspace_ |
Results | results_ |
unique_ptr< gar::RiccatiSolverBase< Scalar > > | linearSolver_ |
LQR subproblem solver. | |
Filter | filter_ |
Protected Member Functions | |
void | updateTolsOnFailure () noexcept |
void | updateTolsOnSuccess () noexcept |
ALIGATOR_INLINE void | setAlmPenalty (Scalar new_mu) noexcept |
Set dual proximal/ALM penalty parameter. | |
ALIGATOR_INLINE void | setRho (Scalar new_rho) noexcept |
void | initializeRegularization () noexcept |
void | increaseRegularization () noexcept |
A proximal, augmented Lagrangian-type solver for trajectory optimization.
This class implements the Proximal Differential Dynamic Programming algorithm, a variant of the augmented Lagrangian method for trajectory optimization. The paper "PROXDDP: Proximal Constrained Trajectory Optimization" by Jallet et al (2023) is the reference [1] for this implementation.
Definition at line 45 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::Scalar = _Scalar |
Definition at line 49 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::Problem = TrajOptProblemTpl<Scalar> |
Definition at line 51 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::Workspace = WorkspaceTpl<Scalar> |
Definition at line 52 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::Results = ResultsTpl<Scalar> |
Definition at line 53 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::StageFunctionData = StageFunctionDataTpl<Scalar> |
Definition at line 54 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::DynamicsData = DynamicsDataTpl<Scalar> |
Definition at line 55 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::CostData = CostDataAbstractTpl<Scalar> |
Definition at line 56 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::StageModel = StageModelTpl<Scalar> |
Definition at line 57 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::ConstraintType = StageConstraintTpl<Scalar> |
Definition at line 58 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::StageData = StageDataTpl<Scalar> |
Definition at line 59 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::CallbackPtr = shared_ptr<CallbackBaseTpl<Scalar>> |
Definition at line 60 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::CallbackMap = std::unordered_map<std::string, CallbackPtr> |
Definition at line 61 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::ConstraintStack = ConstraintStackTpl<Scalar> |
Definition at line 62 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::CstrSet = ConstraintSetBase<Scalar> |
Definition at line 63 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::TrajOptData = TrajOptDataTpl<Scalar> |
Definition at line 64 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::LinesearchOptions = typename Linesearch<Scalar>::Options |
Definition at line 65 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::CstrProximalScaler = ConstraintProximalScalerTpl<Scalar> |
Definition at line 66 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::LinesearchType = proxsuite::nlp::ArmijoLinesearch<Scalar> |
Definition at line 67 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::LQProblem = gar::LQRProblemTpl<Scalar> |
Definition at line 68 of file solver-proxddp.hpp.
using aligator::SolverProxDDPTpl< _Scalar >::Filter = FilterTpl<Scalar> |
Definition at line 69 of file solver-proxddp.hpp.
aligator::SolverProxDDPTpl< _Scalar >::SolverProxDDPTpl | ( | const Scalar | tol = 1e-6, |
const Scalar | mu_init = 0.01, | ||
const Scalar | rho_init = 0., | ||
const std::size_t | max_iters = 1000, | ||
VerboseLevel | verbose = VerboseLevel::QUIET, | ||
HessianApprox | hess_approx = HessianApprox::GAUSS_NEWTON ) |
aligator::SolverProxDDPTpl< _Scalar >::ALIGATOR_DYNAMIC_TYPEDEFS | ( | Scalar | ) |
|
inline |
Definition at line 163 of file solver-proxddp.hpp.
|
inline |
Definition at line 173 of file solver-proxddp.hpp.
|
inline |
Definition at line 175 of file solver-proxddp.hpp.
|
inline |
Definition at line 176 of file solver-proxddp.hpp.
|
static |
Try a step of size \(\alpha\).
Scalar aligator::SolverProxDDPTpl< _Scalar >::tryNonlinearRollout | ( | const Problem & | problem, |
const Scalar | alpha ) |
Policy rollout using the full nonlinear dynamics. The feedback gains need to be computed first. This will evaluate all the terms in the problem into the problem data, similar to TrajOptProblemTpl::evaluate().
Scalar aligator::SolverProxDDPTpl< _Scalar >::forwardPass | ( | const Problem & | problem, |
const Scalar | alpha ) |
void aligator::SolverProxDDPTpl< _Scalar >::updateLQSubproblem | ( | ) |
void aligator::SolverProxDDPTpl< _Scalar >::setup | ( | const Problem & | problem | ) |
Allocate new workspace and results instances according to the specifications of problem
.
problem | The problem instance with respect to which memory will be allocated. |
bool aligator::SolverProxDDPTpl< _Scalar >::run | ( | const Problem & | problem, |
const std::vector< VectorXs > & | xs_init = {}, | ||
const std::vector< VectorXs > & | us_init = {}, | ||
const std::vector< VectorXs > & | lams_init = {} ) |
Run the numerical solver.
problem | The trajectory optimization problem to solve. |
xs_init | Initial trajectory guess. |
us_init | Initial control sequence guess. |
lams_init | Initial multiplier guess. |
bool aligator::SolverProxDDPTpl< _Scalar >::innerLoop | ( | const Problem & | problem | ) |
Perform the inner loop of the algorithm (augmented Lagrangian minimization).
void aligator::SolverProxDDPTpl< _Scalar >::computeInfeasibilities | ( | const Problem & | problem | ) |
Compute the primal infeasibility measures.
void aligator::SolverProxDDPTpl< _Scalar >::computeCriterion | ( | ) |
Compute stationarity criterion (dual infeasibility).
|
inline |
Add a callback to the solver instance.
Definition at line 229 of file solver-proxddp.hpp.
|
inlinenoexcept |
Remove all callbacks from the instance.
Definition at line 234 of file solver-proxddp.hpp.
|
inline |
Definition at line 236 of file solver-proxddp.hpp.
|
inline |
Definition at line 237 of file solver-proxddp.hpp.
|
inline |
Definition at line 238 of file solver-proxddp.hpp.
|
inline |
Invoke callbacks.
Definition at line 247 of file solver-proxddp.hpp.
void aligator::SolverProxDDPTpl< _Scalar >::computeMultipliers | ( | const Problem & | problem, |
const std::vector< VectorXs > & | lams, | ||
const std::vector< VectorXs > & | vs ) |
Compute the merit function and stopping criterion dual terms: first-order Lagrange multiplier estimates, shifted and projected constraints.
|
inline |
Dual proximal/ALM penalty parameter \(\mu\) This is the global parameter: scales may be applied for stagewise constraints, dynamicals...
Definition at line 262 of file solver-proxddp.hpp.
|
inline |
Definition at line 265 of file solver-proxddp.hpp.
|
inline |
Primal proximal parameter \(\rho > 0\).
Definition at line 268 of file solver-proxddp.hpp.
|
inline |
Update primal-dual feedback gains (control, costate, path multiplier)
|
inlineprotectednoexcept |
Definition at line 275 of file solver-proxddp.hpp.
|
inlineprotectednoexcept |
Definition at line 280 of file solver-proxddp.hpp.
|
inlineprotectednoexcept |
Set dual proximal/ALM penalty parameter.
Definition at line 286 of file solver-proxddp.hpp.
|
inlineprotectednoexcept |
Definition at line 290 of file solver-proxddp.hpp.
|
inlineprotectednoexcept |
Definition at line 294 of file solver-proxddp.hpp.
|
inlineprotectednoexcept |
Definition at line 304 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::inner_tol_ |
Subproblem tolerance.
Definition at line 72 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::prim_tol_ |
Desired primal feasibility.
Definition at line 74 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::target_tol_ = 1e-6 |
Solver tolerance \(\epsilon > 0\).
Definition at line 76 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::mu_init = 0.01 |
Definition at line 78 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::rho_init = 0. |
Definition at line 79 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::reg_min = 1e-10 |
Definition at line 83 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::reg_max = 1e9 |
Definition at line 84 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::reg_init = 1e-9 |
Definition at line 85 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::reg_inc_k_ = 10. |
Definition at line 86 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::reg_inc_first_k_ = 100. |
Definition at line 87 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::reg_dec_k_ = 1. / 3. |
Definition at line 88 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::preg_ = reg_init |
Definition at line 89 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::preg_last_ = 0. |
Definition at line 90 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::inner_tol0 = 1. |
Definition at line 94 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::prim_tol0 = 1. |
Definition at line 95 of file solver-proxddp.hpp.
Logger aligator::SolverProxDDPTpl< _Scalar >::logger {} |
Definition at line 98 of file solver-proxddp.hpp.
VerboseLevel aligator::SolverProxDDPTpl< _Scalar >::verbose_ |
Solver verbosity level.
Definition at line 101 of file solver-proxddp.hpp.
LQSolverChoice aligator::SolverProxDDPTpl< _Scalar >::linear_solver_choice = LQSolverChoice::SERIAL |
Choice of linear solver.
Definition at line 103 of file solver-proxddp.hpp.
bool aligator::SolverProxDDPTpl< _Scalar >::lq_print_detailed = false |
Definition at line 104 of file solver-proxddp.hpp.
HessianApprox aligator::SolverProxDDPTpl< _Scalar >::hess_approx_ = HessianApprox::GAUSS_NEWTON |
Type of Hessian approximation. Default is Gauss-Newton.
Definition at line 106 of file solver-proxddp.hpp.
LinesearchOptions aligator::SolverProxDDPTpl< _Scalar >::ls_params |
Linesearch options, as in proxsuite-nlp.
Definition at line 108 of file solver-proxddp.hpp.
LinesearchStrategy aligator::SolverProxDDPTpl< _Scalar >::ls_strat = LinesearchStrategy::ARMIJO |
Type of linesearch strategy. Default is Armijo.
Definition at line 110 of file solver-proxddp.hpp.
MultiplierUpdateMode aligator::SolverProxDDPTpl< _Scalar >::multiplier_update_mode = MultiplierUpdateMode::NEWTON |
Type of Lagrange multiplier update.
Definition at line 112 of file solver-proxddp.hpp.
LinesearchMode aligator::SolverProxDDPTpl< _Scalar >::ls_mode = LinesearchMode::PRIMAL |
Linesearch mode.
Definition at line 114 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::dual_weight = 1.0 |
Weight of the dual variables in the primal-dual linesearch.
Definition at line 116 of file solver-proxddp.hpp.
RolloutType aligator::SolverProxDDPTpl< _Scalar >::rollout_type_ = RolloutType::NONLINEAR |
Type of rollout for the forward pass.
Definition at line 118 of file solver-proxddp.hpp.
BCLParamsTpl<Scalar> aligator::SolverProxDDPTpl< _Scalar >::bcl_params |
Parameters for the BCL outer loop of the augmented Lagrangian algorithm.
Definition at line 120 of file solver-proxddp.hpp.
StepAcceptanceStrategy aligator::SolverProxDDPTpl< _Scalar >::sa_strategy = StepAcceptanceStrategy::LINESEARCH |
Step acceptance mode.
Definition at line 122 of file solver-proxddp.hpp.
bool aligator::SolverProxDDPTpl< _Scalar >::force_initial_condition_ = true |
Force the initial state \( x_0 \) to be fixed to the problem initial condition.
Definition at line 126 of file solver-proxddp.hpp.
std::size_t aligator::SolverProxDDPTpl< _Scalar >::maxRefinementSteps_ |
Definition at line 128 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::refinementThreshold_ = 1e-13 |
Definition at line 130 of file solver-proxddp.hpp.
std::size_t aligator::SolverProxDDPTpl< _Scalar >::max_iters |
Definition at line 131 of file solver-proxddp.hpp.
std::size_t aligator::SolverProxDDPTpl< _Scalar >::max_al_iters = 100 |
Definition at line 132 of file solver-proxddp.hpp.
Scalar aligator::SolverProxDDPTpl< _Scalar >::mu_lower_bound = 1e-8 |
Definition at line 133 of file solver-proxddp.hpp.
uint aligator::SolverProxDDPTpl< _Scalar >::rollout_max_iters |
Definition at line 134 of file solver-proxddp.hpp.
CallbackMap aligator::SolverProxDDPTpl< _Scalar >::callbacks_ |
Callbacks.
Definition at line 137 of file solver-proxddp.hpp.
Workspace aligator::SolverProxDDPTpl< _Scalar >::workspace_ |
Definition at line 138 of file solver-proxddp.hpp.
Results aligator::SolverProxDDPTpl< _Scalar >::results_ |
Definition at line 139 of file solver-proxddp.hpp.
unique_ptr<gar::RiccatiSolverBase<Scalar> > aligator::SolverProxDDPTpl< _Scalar >::linearSolver_ |
LQR subproblem solver.
Definition at line 141 of file solver-proxddp.hpp.
Filter aligator::SolverProxDDPTpl< _Scalar >::filter_ |
Definition at line 142 of file solver-proxddp.hpp.