aligator  0.15.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
solver-proxddp.hpp
Go to the documentation of this file.
1
4#pragma once
5
14
15#include "workspace.hpp"
16#include "results.hpp"
17
18#include <boost/unordered_map.hpp>
19#include <variant>
20
21namespace aligator {
22namespace gar {
23template <typename Scalar> class RiccatiSolverBase;
24} // namespace gar
25
27
36template <typename _Scalar> struct SolverProxDDPTpl {
37public:
38 using Scalar = _Scalar;
48 using CallbackPtr = shared_ptr<CallbackBaseTpl<Scalar>>;
49 using CallbackMap = boost::unordered_map<std::string, CallbackPtr>;
54 using LinearSolverPtr = std::unique_ptr<gar::RiccatiSolverBase<Scalar>>;
55
56 struct LinesearchVariant {
57 using VariantType = std::variant<std::monostate, ArmijoLinesearch<Scalar>,
59
60 Scalar run(const std::function<Scalar(Scalar)> &fun, const Scalar phi0,
61 const Scalar dphi0, Scalar &alpha_try) {
62 return std::visit(
63 overloads{[](std::monostate &) {
64 return std::numeric_limits<Scalar>::quiet_NaN();
65 },
66 [&](auto &&method) {
67 return method.run(fun, phi0, dphi0, alpha_try);
68 }},
69 impl_);
70 }
71
72 void reset() {
73 std::visit(overloads{[](std::monostate &) {},
74 [&](auto &&method) { method.reset(); }},
75 impl_);
76 }
77
78 Scalar isValid() const { return impl_.index() > 0ul; }
79
80 operator const VariantType &() const { return impl_; }
81
82 private:
83 explicit LinesearchVariant() {}
84 void init(StepAcceptanceStrategy strat, const LinesearchOptions &options) {
85 switch (strat) {
87 impl_ = ArmijoLinesearch<Scalar>(options);
88 break;
90 impl_ = NonmonotoneLinesearch<Scalar>(options);
91 break;
92 default:
93 ALIGATOR_WARNING("LinesearchVariant::",
94 "provided StepAcceptanceStrategy is invalid.");
95 break;
96 }
97 }
98 friend SolverProxDDPTpl;
99 VariantType impl_;
100 };
101
102 struct AlmParams {
116 Scalar mu_lower_bound = 1e-8; //< Minimum possible penalty parameter.
117 };
118
127
128private:
131 Scalar target_dual_tol_;
134 bool sync_dual_tol;
135
136public:
137 Scalar mu_init = 0.01; //< Initial AL parameter
138
141
142 Scalar reg_min = 1e-10; //< Minimal nonzero regularization
143 Scalar reg_max = 1e9; //< Maximum regularization value
144 Scalar reg_init = 1e-9; //< Initial regularization value (can be zero)
145 Scalar reg_inc_k_ = 10.; //< Regularization increase factor
146 Scalar reg_inc_first_k_ = 100.; //< Regularization increase (critical)
147 Scalar reg_dec_k_ = 1. / 3.; //< Regularization decrease factor
148 Scalar preg_ = reg_init; //< Primal regularization value
149 Scalar preg_last_ = 0.; //< Last "good" regularization value
150
152
153 Scalar inner_tol0 = 1.; //< Initial BCL inner subproblem tolerance
154 Scalar prim_tol0 = 1.; //< Initial BCL constraint infeasibility tolerance
155
158
176 AlmParams bcl_params;
179
183
184 size_t max_refinement_steps_ = 0; //< Max KKT system refinement iters.
185 Scalar refinement_threshold_ = 1e-13; //< Target tol. for the KKT system.
186 size_t max_iters; //< Max number of Newton iterations.
187
195 LinesearchVariant linesearch_;
196
197private:
199 CallbackMap callbacks_;
201 std::size_t num_threads_ = 1;
205 Scalar mu_penal_ = mu_init;
206
207public:
208 SolverProxDDPTpl(const Scalar tol = 1e-6, const Scalar mu_init = 0.01,
209 const std::size_t max_iters = 1000,
211 StepAcceptanceStrategy sa_strategy =
214
215 inline std::size_t getNumThreads() const { return num_threads_; }
216 void setNumThreads(const std::size_t num_threads);
217
218 Scalar getDualTolerance() const { return target_dual_tol_; }
220 void setDualTolerance(const Scalar tol) {
221 target_dual_tol_ = tol;
222 sync_dual_tol = false;
223 }
224
230 Scalar tryLinearStep(const Problem &problem, const Scalar alpha);
231
236 Scalar tryNonlinearRollout(const Problem &problem, const Scalar alpha);
237
238 Scalar forwardPass(const Problem &problem, const Scalar alpha);
239
241
246 void setup(const Problem &problem);
247 void cycleProblem(const Problem &problem,
248 shared_ptr<StageDataTpl<Scalar>> data);
249
258 bool run(const Problem &problem, const std::vector<VectorXs> &xs_init = {},
259 const std::vector<VectorXs> &us_init = {},
260 const std::vector<VectorXs> &vs_init = {},
261 const std::vector<VectorXs> &lams_init = {});
262
265 bool innerLoop(const Problem &problem);
266
271 void computeInfeasibilities(const Problem &problem);
274
277
279 void registerCallback(const std::string &name, CallbackPtr cb);
280
282 void clearCallbacks() noexcept { callbacks_.clear(); }
283
284 const CallbackMap &getCallbacks() const { return callbacks_; }
285 void removeCallback(const std::string &name) { callbacks_.erase(name); }
286 auto getCallbackNames() const {
287 std::vector<std::string> keys;
288 for (const auto &item : callbacks_) {
289 keys.push_back(item.first);
290 }
291 return keys;
292 }
293
294 CallbackPtr getCallback(const std::string &name) const {
295 auto cb = callbacks_.find(name);
296 if (cb != end(callbacks_)) {
297 return cb->second;
298 }
299 return nullptr;
300 }
301
303 void invokeCallbacks(Workspace &workspace, Results &results) {
304 for (const auto &cb : callbacks_) {
305 cb.second->call(workspace, results);
306 }
307 }
308
309
314 bool computeMultipliers(const Problem &problem,
315 const std::vector<VectorXs> &lams,
316 const std::vector<VectorXs> &vs);
317
319 return bcl_params.dyn_al_scale * mu_penal_;
320 }
321 ALIGATOR_INLINE Scalar mu() const { return mu_penal_; }
322 ALIGATOR_INLINE Scalar mu_inv() const { return 1. / mu_penal_; }
323
326 inline void updateGains();
327
328protected:
329 void updateTolsOnFailure() noexcept {
330 const Scalar arg = std::min(mu_penal_, 0.99);
331 prim_tol_ = prim_tol0 * std::pow(arg, bcl_params.prim_alpha);
332 inner_tol_ = inner_tol0 * std::pow(arg, bcl_params.dual_alpha);
333 }
334
335 void updateTolsOnSuccess() noexcept {
336 const Scalar arg = std::min(mu_penal_, 0.99);
337 prim_tol_ = prim_tol_ * std::pow(arg, bcl_params.prim_beta);
338 inner_tol_ = inner_tol_ * std::pow(arg, bcl_params.dual_beta);
339 }
340
342 ALIGATOR_INLINE void setAlmPenalty(Scalar new_mu) noexcept {
343 mu_penal_ = std::max(new_mu, bcl_params.mu_lower_bound);
344 }
345
346 // See sec. 3.1 of the IPOPT paper [Wächter, Biegler 2006]
347 // called before first bwd pass attempt
348 inline void initializeRegularization() noexcept {
349 if (preg_last_ == 0.) {
350 // this is the 1st iteration
351 preg_ = std::max(reg_init, reg_min);
352 } else {
353 // attempt decrease from last "good" value
354 preg_ = std::max(reg_min, preg_last_ * reg_dec_k_);
355 }
356 }
357
358 inline void increaseRegularization() noexcept {
359 if (preg_last_ == 0.)
361 else
362 preg_ *= reg_inc_k_;
363 }
364};
365
366} // namespace aligator
367
368#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
369#include "solver-proxddp.txx"
370#endif
Basic backtracking Armijo line-search strategy.
#define ALIGATOR_WARNING(loc,...)
#define ALIGATOR_INLINE
Definition fwd.hpp:27
Implements a basic Armijo back-tracking strategy.
Main package namespace.
MultiplierUpdateMode
Definition enums.hpp:23
VerboseLevel
Verbosity level.
Definition fwd.hpp:82
@ QUIET
Definition fwd.hpp:82
StepAcceptanceStrategy
Whether to use linesearch or filter during step acceptance phase.
Definition enums.hpp:29
RolloutType
Definition enums.hpp:5
@ NONLINEAR
Nonlinear rollout, using the full dynamics.
Definition enums.hpp:9
HessianApprox
Definition enums.hpp:14
@ GAUSS_NEWTON
Use the Gauss-Newton approximation.
Definition enums.hpp:18
LinesearchMode
Whether to use merit functions in primal or primal-dual mode.
Definition enums.hpp:26
Define workspace for the ProxDDP solver.
Base constraint set type.
Convenience class to manage a stack of constraints.
Data struct for CostAbstractTpl.
A basic filter line-search strategy.
Definition filter.hpp:12
A table logging utility to log the trace of the numerical solvers.
Definition logger.hpp:31
Nonmonotone Linesearch algorithm. Modifies the Armijo condition with a moving average of function val...
Results holder struct.
Definition results.hpp:11
Scalar mu_update_factor
Scale factor for the dual proximal penalty.
Scalar dual_beta
Log-factor for dual tolerance (success)
Scalar dual_alpha
Log-factor for dual tolerance (failure)
Scalar dyn_al_scale
Constraints AL scaling.
Scalar prim_beta
Log-factor for primal tolerance (success)
Scalar prim_alpha
Log-factor for primal tolerance (failure)
Scalar mu_lower_bound
Lower bound on AL parameter.
Scalar run(const std::function< Scalar(Scalar)> &fun, const Scalar phi0, const Scalar dphi0, Scalar &alpha_try)
std::variant< std::monostate, ArmijoLinesearch< Scalar >, NonmonotoneLinesearch< Scalar > > VariantType
Scalar tryLinearStep(const Problem &problem, const Scalar alpha)
Try a step of size .
CallbackPtr getCallback(const std::string &name) const
void updateGains()
Update primal-dual feedback gains (control, costate, path multiplier)
void computeCriterion()
Compute stationarity criterion (dual infeasibility).
void removeCallback(const std::string &name)
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.
std::size_t getNumThreads() const
bool computeMultipliers(const Problem &problem, const std::vector< VectorXs > &lams, const std::vector< VectorXs > &vs)
ALIGATOR_INLINE Scalar mu() const
shared_ptr< CallbackBaseTpl< Scalar > > CallbackPtr
void invokeCallbacks(Workspace &workspace, Results &results)
Invoke callbacks.
TrajOptDataTpl< Scalar > TrajOptData
ALIGATOR_INLINE Scalar mudyn() const
void clearCallbacks() noexcept
Remove all callbacks from the instance.
boost::unordered_map< std::string, CallbackPtr > CallbackMap
std::unique_ptr< gar::RiccatiSolverBase< Scalar > > LinearSolverPtr
Scalar forwardPass(const Problem &problem, const Scalar alpha)
ConstraintSetTpl< Scalar > CstrSet
void cycleProblem(const Problem &problem, shared_ptr< StageDataTpl< Scalar > > data)
ConstraintStackTpl< Scalar > ConstraintStack
void setup(const Problem &problem)
Allocate new workspace and results instances according to the specifications of problem.
const CallbackMap & getCallbacks() const
void setNumThreads(const std::size_t num_threads)
void initializeRegularization() noexcept
void updateTolsOnSuccess() noexcept
SolverProxDDPTpl(const Scalar tol=1e-6, const Scalar mu_init=0.01, const std::size_t max_iters=1000, VerboseLevel verbose=VerboseLevel::QUIET, StepAcceptanceStrategy sa_strategy=StepAcceptanceStrategy::LINESEARCH_NONMONOTONE, HessianApprox hess_approx=HessianApprox::GAUSS_NEWTON)
ALIGATOR_INLINE Scalar mu_inv() const
ALIGATOR_INLINE void setAlmPenalty(Scalar new_mu) noexcept
Set dual proximal/ALM penalty parameter.
TrajOptProblemTpl< Scalar > Problem
bool innerLoop(const Problem &problem)
Perform the inner loop of the algorithm (augmented Lagrangian minimization).
void increaseRegularization() noexcept
void registerCallback(const std::string &name, CallbackPtr cb)
Add a callback to the solver instance.
StageFunctionDataTpl< Scalar > StageFunctionData
Scalar tryNonlinearRollout(const Problem &problem, const Scalar alpha)
Policy rollout using the full nonlinear dynamics. The feedback gains need to be computed first....
void updateTolsOnFailure() noexcept
void setDualTolerance(const Scalar tol)
Manually set desired dual feasibility tolerance.
DynamicsDataTpl< Scalar > DynamicsData
void computeInfeasibilities(const Problem &problem)
Compute the primal infeasibility measures.
CostDataAbstractTpl< Scalar > CostData
typename Linesearch< Scalar >::Options LinesearchOptions
Data struct for stage models StageModelTpl.
Base struct for function data.
A stage in the control problem.
Problem data struct.
Trajectory optimization problem.
Workspace for solver SolverProxDDP.
Definition workspace.hpp:27
Utility helper struct for creating visitors from lambdas.
Definition overloads.hpp:6