|
aligator
0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
|
A proximal, augmented Lagrangian-type solver for trajectory optimization. More...
#include <aligator/solvers/proxddp/solver-proxddp.hpp>
Classes | |
| struct | AlmParams |
| struct | LinesearchVariant |
Public Types | |
| using | Scalar = _Scalar |
| using | Problem = TrajOptProblemTpl<Scalar> |
| using | Workspace = WorkspaceTpl<Scalar> |
| using | Results = ResultsTpl<Scalar> |
| using | StageFunctionData = StageFunctionDataTpl<Scalar> |
| using | CostData = CostDataAbstractTpl<Scalar> |
| using | StageModel = StageModelTpl<Scalar> |
| using | StageData = StageDataTpl<Scalar> |
| using | CallbackPtr = shared_ptr<CallbackBaseTpl<Scalar>> |
| using | CallbackMap |
| using | ConstraintStack = ConstraintStackTpl<Scalar> |
| using | CstrSet = ConstraintSetTpl<Scalar> |
| using | TrajOptData = TrajOptDataTpl<Scalar> |
| using | LinearSolverPtr = std::unique_ptr<gar::RiccatiSolverBase<Scalar>> |
Public Member Functions | |
| ALIGATOR_DYNAMIC_TYPEDEFS (Scalar) | |
| SolverProxDDPTpl (const Scalar tol=1e-6, const Scalar mu_init=0.01, const size_t max_iters=1000, VerboseLevel verbose=VerboseLevel::QUIET, StepAcceptanceStrategy sa_strategy=StepAcceptanceStrategy::LINESEARCH_NONMONOTONE, HessianApprox hess_approx=HessianApprox::GAUSS_NEWTON) | |
| size_t | getNumThreads () const |
| void | setNumThreads (const size_t num_threads) |
| Scalar | getDualTolerance () const |
| void | setDualTolerance (const Scalar tol) |
| Manually set desired dual feasibility tolerance. | |
| Scalar | tryLinearStep (const Problem &problem, const Scalar alpha) |
| Try a step of size \(\alpha\). | |
| 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. | |
| void | cycleProblem (const Problem &problem, const shared_ptr< StageData > &data) |
| bool | run (const Problem &problem, const std::vector< VectorXs > &xs_init={}, const std::vector< VectorXs > &us_init={}, const std::vector< VectorXs > &vs_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 | computeCriterion () |
| Compute stationarity criterion (dual infeasibility). | |
| bool | computeMultipliers (const Problem &problem, const std::vector< VectorXs > &xs, const std::vector< VectorXs > &lams, const std::vector< VectorXs > &vs) |
| Scalar | mu () const |
| Scalar | mu_inv () const |
| Scalar | mu_dyn () const |
| Used in linesearch. | |
callbacks | |
| void | registerCallback (std::string_view name, CallbackPtr cb) |
| Add a callback to the solver instance. | |
| void | clearCallbacks () noexcept |
| Remove all callbacks from the instance. | |
| const CallbackMap & | getCallbacks () const |
| bool | removeCallback (std::string_view name) |
| auto | getCallbackNames () const |
| CallbackPtr | getCallback (std::string_view name) const |
| void | invokeCallbacks () |
| Invoke callbacks. | |
Protected Member Functions | |
| void | updateTolsOnFailure () noexcept |
| void | updateTolsOnSuccess () noexcept |
| void | setAlmPenalty (Scalar new_mu) noexcept |
| Set dual proximal/ALM penalty parameter. | |
| void | initializeRegularization () noexcept |
| void | increaseRegularization () noexcept |
Protected Attributes | |
| Scalar | target_dual_tol_ |
| bool | sync_dual_tol_ = true |
| CallbackMap | callbacks_ |
| size_t | num_threads_ = 1 |
| Scalar | mu_penal_ = mu_init_ |
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 35 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::Scalar = _Scalar |
Definition at line 37 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::Problem = TrajOptProblemTpl<Scalar> |
Definition at line 39 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::Workspace = WorkspaceTpl<Scalar> |
Definition at line 40 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::Results = ResultsTpl<Scalar> |
Definition at line 41 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::StageFunctionData = StageFunctionDataTpl<Scalar> |
Definition at line 42 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::CostData = CostDataAbstractTpl<Scalar> |
Definition at line 43 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::StageModel = StageModelTpl<Scalar> |
Definition at line 44 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::StageData = StageDataTpl<Scalar> |
Definition at line 45 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::CallbackPtr = shared_ptr<CallbackBaseTpl<Scalar>> |
Definition at line 46 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::CallbackMap |
Definition at line 50 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::ConstraintStack = ConstraintStackTpl<Scalar> |
Definition at line 52 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::CstrSet = ConstraintSetTpl<Scalar> |
Definition at line 53 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::TrajOptData = TrajOptDataTpl<Scalar> |
Definition at line 54 of file solver-proxddp.hpp.
| using aligator::SolverProxDDPTpl< _Scalar >::LinearSolverPtr = std::unique_ptr<gar::RiccatiSolverBase<Scalar>> |
Definition at line 55 of file solver-proxddp.hpp.
| aligator::SolverProxDDPTpl< _Scalar >::SolverProxDDPTpl | ( | const Scalar | tol = 1e-6, |
| const Scalar | mu_init = 0.01, | ||
| const size_t | max_iters = 1000, | ||
| VerboseLevel | verbose = VerboseLevel::QUIET, | ||
| StepAcceptanceStrategy | sa_strategy = StepAcceptanceStrategy::LINESEARCH_NONMONOTONE, | ||
| HessianApprox | hess_approx = HessianApprox::GAUSS_NEWTON ) |
| aligator::SolverProxDDPTpl< _Scalar >::ALIGATOR_DYNAMIC_TYPEDEFS | ( | Scalar | ) |
|
inlinenodiscard |
Definition at line 192 of file solver-proxddp.hpp.
| void aligator::SolverProxDDPTpl< _Scalar >::setNumThreads | ( | const size_t | num_threads | ) |
|
inlinenodiscard |
Definition at line 195 of file solver-proxddp.hpp.
|
inline |
Manually set desired dual feasibility tolerance.
Definition at line 197 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::tryLinearStep | ( | const Problem & | problem, |
| const Scalar | alpha ) |
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. |
| void aligator::SolverProxDDPTpl< _Scalar >::cycleProblem | ( | const Problem & | problem, |
| const shared_ptr< StageData > & | data ) |
| bool aligator::SolverProxDDPTpl< _Scalar >::run | ( | const Problem & | problem, |
| const std::vector< VectorXs > & | xs_init = {}, | ||
| const std::vector< VectorXs > & | us_init = {}, | ||
| const std::vector< VectorXs > & | vs_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. |
| vs_init | Initial path multiplier guess. |
| lams_init | Initial co-state guess. |
| bool aligator::SolverProxDDPTpl< _Scalar >::innerLoop | ( | const Problem & | problem | ) |
Perform the inner loop of the algorithm (augmented Lagrangian minimization).
| void aligator::SolverProxDDPTpl< _Scalar >::computeCriterion | ( | ) |
Compute stationarity criterion (dual infeasibility).
|
inline |
Add a callback to the solver instance.
Definition at line 250 of file solver-proxddp.hpp.
|
inlinenoexcept |
Remove all callbacks from the instance.
Definition at line 255 of file solver-proxddp.hpp.
|
inline |
Definition at line 257 of file solver-proxddp.hpp.
|
inlinenodiscard |
Definition at line 259 of file solver-proxddp.hpp.
|
inlinenodiscard |
Definition at line 263 of file solver-proxddp.hpp.
|
inlinenodiscard |
Definition at line 271 of file solver-proxddp.hpp.
|
inline |
Invoke callbacks.
Definition at line 280 of file solver-proxddp.hpp.
| bool aligator::SolverProxDDPTpl< _Scalar >::computeMultipliers | ( | const Problem & | problem, |
| const std::vector< VectorXs > & | xs, | ||
| const std::vector< VectorXs > & | lams, | ||
| const std::vector< VectorXs > & | vs ) |
Compute the multiplier estimates, along with the shifted, projected constraint values.
|
inline |
Definition at line 297 of file solver-proxddp.hpp.
|
inline |
Definition at line 298 of file solver-proxddp.hpp.
|
inline |
Used in linesearch.
Definition at line 300 of file solver-proxddp.hpp.
|
inlineprotectednoexcept |
Definition at line 313 of file solver-proxddp.hpp.
|
inlineprotectednoexcept |
Definition at line 319 of file solver-proxddp.hpp.
|
inlineprotectednoexcept |
Set dual proximal/ALM penalty parameter.
Definition at line 326 of file solver-proxddp.hpp.
|
inlineprotectednoexcept |
Initialize primal regularization for the inner loop. See sec. 3.1 of the IPOPT paper [Wächter, Biegler 2006] This called before first bwd pass attempt
Definition at line 333 of file solver-proxddp.hpp.
|
inlineprotectednoexcept |
Definition at line 343 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::inner_tol_ |
Subproblem tolerance.
Definition at line 120 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::prim_tol_ |
Desired primal feasibility (for each outer loop)
Definition at line 122 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::target_tol_ |
Solver tolerance \(\epsilon > 0\). When sync_dual_tol is false, this will be the desired primal feasibility, where the dual feasibility tolerance is controlled by SolverProxDDPTpl::target_tol_dual.
Definition at line 126 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::mu_init_ |
Definition at line 127 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::reg_min = 1e-10 |
Definition at line 132 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::reg_max = 1e9 |
Definition at line 133 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::reg_init = 1e-9 |
Definition at line 134 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::reg_inc_k_ = 10. |
Definition at line 135 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::reg_inc_first_k_ = 100. |
Definition at line 136 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::reg_dec_k_ = 1. / 3. |
Definition at line 137 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::preg_ = reg_init |
Definition at line 138 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::preg_last_ = 0. |
Definition at line 139 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::inner_tol0 = 1. |
Definition at line 143 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::prim_tol0 = 1. |
Definition at line 144 of file solver-proxddp.hpp.
| Logger aligator::SolverProxDDPTpl< _Scalar >::logger {} |
Definition at line 147 of file solver-proxddp.hpp.
| VerboseLevel aligator::SolverProxDDPTpl< _Scalar >::verbose_ |
Solver verbosity level.
Definition at line 150 of file solver-proxddp.hpp.
| LQSolverChoice aligator::SolverProxDDPTpl< _Scalar >::linear_solver_choice = LQSolverChoice::SERIAL |
Choice of linear solver.
Definition at line 152 of file solver-proxddp.hpp.
| HessianApprox aligator::SolverProxDDPTpl< _Scalar >::hess_approx_ |
Type of Hessian approximation. Default is Gauss-Newton.
Definition at line 154 of file solver-proxddp.hpp.
| LinesearchOptions<Scalar> aligator::SolverProxDDPTpl< _Scalar >::ls_params |
Linesearch options.
Definition at line 156 of file solver-proxddp.hpp.
| MultiplierUpdateMode aligator::SolverProxDDPTpl< _Scalar >::multiplier_update_mode = MultiplierUpdateMode::NEWTON |
Type of Lagrange multiplier update.
Definition at line 158 of file solver-proxddp.hpp.
| LinesearchMode aligator::SolverProxDDPTpl< _Scalar >::ls_mode = LinesearchMode::PRIMAL |
Linesearch mode.
Definition at line 160 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 162 of file solver-proxddp.hpp.
| RolloutType aligator::SolverProxDDPTpl< _Scalar >::rollout_type_ = RolloutType::LINEAR |
Type of rollout for the forward pass.
Definition at line 164 of file solver-proxddp.hpp.
| AlmParams aligator::SolverProxDDPTpl< _Scalar >::bcl_params |
Parameters for the BCL outer loop of the augmented Lagrangian algorithm.
Definition at line 166 of file solver-proxddp.hpp.
| StepAcceptanceStrategy aligator::SolverProxDDPTpl< _Scalar >::sa_strategy_ |
Step acceptance mode.
Definition at line 168 of file solver-proxddp.hpp.
| bool aligator::SolverProxDDPTpl< _Scalar >::force_initial_condition_ = true |
Definition at line 170 of file solver-proxddp.hpp.
| size_t aligator::SolverProxDDPTpl< _Scalar >::max_refinement_steps_ = 0 |
Definition at line 171 of file solver-proxddp.hpp.
| Scalar aligator::SolverProxDDPTpl< _Scalar >::refinement_threshold_ = 1e-13 |
Definition at line 172 of file solver-proxddp.hpp.
| size_t aligator::SolverProxDDPTpl< _Scalar >::max_iters |
Definition at line 173 of file solver-proxddp.hpp.
| size_t aligator::SolverProxDDPTpl< _Scalar >::max_al_iters = 100 |
Definition at line 174 of file solver-proxddp.hpp.
| mimalloc_resource aligator::SolverProxDDPTpl< _Scalar >::memory_resource_ |
Definition at line 176 of file solver-proxddp.hpp.
| polymorphic_allocator aligator::SolverProxDDPTpl< _Scalar >::allocator_ |
Definition at line 177 of file solver-proxddp.hpp.
| Workspace aligator::SolverProxDDPTpl< _Scalar >::workspace_ |
Definition at line 178 of file solver-proxddp.hpp.
| Results aligator::SolverProxDDPTpl< _Scalar >::results_ |
Definition at line 179 of file solver-proxddp.hpp.
| LinearSolverPtr aligator::SolverProxDDPTpl< _Scalar >::linear_solver_ |
Definition at line 180 of file solver-proxddp.hpp.
| FilterTpl<Scalar> aligator::SolverProxDDPTpl< _Scalar >::filter_ |
Definition at line 181 of file solver-proxddp.hpp.
| LinesearchVariant aligator::SolverProxDDPTpl< _Scalar >::linesearch_ |
Definition at line 182 of file solver-proxddp.hpp.
|
protected |
Definition at line 303 of file solver-proxddp.hpp.
|
protected |
Definition at line 305 of file solver-proxddp.hpp.
|
protected |
Definition at line 307 of file solver-proxddp.hpp.
|
protected |
Definition at line 308 of file solver-proxddp.hpp.
|
protected |
Dual proximal/ALM penalty parameter \(\mu\). This is the global parameter. There might be individual scaling for stagewise constraints.
Definition at line 311 of file solver-proxddp.hpp.