aligator  0.16.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 {
22
24
33template <typename _Scalar> struct SolverProxDDPTpl {
34public:
35 using Scalar = _Scalar;
44 using CallbackPtr = shared_ptr<CallbackBaseTpl<Scalar>>;
45 using CallbackMap = boost::unordered_map<std::string, CallbackPtr>;
49 using LinearSolverPtr = std::unique_ptr<gar::RiccatiSolverBase<Scalar>>;
50
51 struct LinesearchVariant {
52 using VariantType = std::variant<std::monostate, ArmijoLinesearch<Scalar>,
54
55 Scalar run(const std::function<Scalar(Scalar)> &fun, const Scalar phi0,
56 const Scalar dphi0, Scalar &alpha_try) {
57 return std::visit(
58 overloads{[](std::monostate &) {
59 return std::numeric_limits<Scalar>::quiet_NaN();
60 },
61 [&](auto &method) {
62 return method.run(fun, phi0, dphi0, alpha_try);
63 }},
64 impl_);
65 }
66
67 void reset() {
68 std::visit(overloads{[](std::monostate &) {},
69 [&](auto &method) { method.reset(); }},
70 impl_);
71 }
72
73 bool isValid() const { return impl_.index() > 0ul; }
74
75 operator const VariantType &() const { return impl_; }
76
77 private:
78 explicit LinesearchVariant() {}
79 void init(StepAcceptanceStrategy strat,
80 const LinesearchOptions<Scalar> &options) {
81 switch (strat) {
83 impl_ = ArmijoLinesearch(options);
84 break;
86 impl_ = NonmonotoneLinesearch(options);
87 break;
88 default:
90 "Provided StepAcceptanceStrategy value is invalid.");
91 break;
92 }
93 }
94 friend SolverProxDDPTpl;
95 VariantType impl_;
96 };
97
112
121 Scalar mu_init_; //< Initial AL parameter
122
125
126 Scalar reg_min = 1e-10; //< Minimal nonzero regularization
127 Scalar reg_max = 1e9; //< Maximum regularization value
128 Scalar reg_init = 1e-9; //< Initial regularization value (can be zero)
129 Scalar reg_inc_k_ = 10.; //< Regularization increase factor
130 Scalar reg_inc_first_k_ = 100.; //< Regularization increase (critical)
131 Scalar reg_dec_k_ = 1. / 3.; //< Regularization decrease factor
132 Scalar preg_ = reg_init; //< Primal regularization value
133 Scalar preg_last_ = 0.; //< Last "good" regularization value
134
136
137 Scalar inner_tol0 = 1.; //< Initial BCL inner subproblem tolerance
138 Scalar prim_tol0 = 1.; //< Initial BCL constraint infeasibility tolerance
139
142
160 AlmParams bcl_params;
163
164 bool force_initial_condition_ = true; //< Force initial condition @f$ x_0 @f$.
165 size_t max_refinement_steps_ = 0; //< Max KKT system refinement iters.
166 Scalar refinement_threshold_ = 1e-13; //< Target tol. for the KKT system.
167 size_t max_iters; //< Max number of Newton iterations.
168 size_t max_al_iters = 100; //< Maximum number of ALM iterations.
169
171 Workspace workspace_; //< Solver workspace
172 Results results_; //< Solver results
173 LinearSolverPtr linear_solver_; //< LQR subproblem solver
174 FilterTpl<Scalar> filter_; //< Filter linesearch
175 LinesearchVariant linesearch_; //< Linesearch routine
176
177protected:
187 size_t num_threads_ = 1;
192
193public:
194 SolverProxDDPTpl(const Scalar tol = 1e-6, const Scalar mu_init = 0.01,
195 const size_t max_iters = 1000,
197 StepAcceptanceStrategy sa_strategy =
200
201 inline size_t getNumThreads() const { return num_threads_; }
202 void setNumThreads(const size_t num_threads);
203
206 void setDualTolerance(const Scalar tol) {
207 target_dual_tol_ = tol;
208 sync_dual_tol_ = false;
209 }
210
216 Scalar tryLinearStep(const Problem &problem, const Scalar alpha);
217
222 Scalar tryNonlinearRollout(const Problem &problem, const Scalar alpha);
223
224 Scalar forwardPass(const Problem &problem, const Scalar alpha);
225
227
232 void setup(const Problem &problem);
233 void cycleProblem(const Problem &problem, const shared_ptr<StageData> &data);
234
243 bool run(const Problem &problem, const std::vector<VectorXs> &xs_init = {},
244 const std::vector<VectorXs> &us_init = {},
245 const std::vector<VectorXs> &vs_init = {},
246 const std::vector<VectorXs> &lams_init = {});
247
250 bool innerLoop(const Problem &problem);
251
254
257
259 void registerCallback(const std::string &name, CallbackPtr cb) {
260 callbacks_.insert_or_assign(name, cb);
261 }
262
264 void clearCallbacks() noexcept { callbacks_.clear(); }
265
266 const CallbackMap &getCallbacks() const { return callbacks_; }
267
268 [[nodiscard]] bool removeCallback(const std::string &name) {
269 return callbacks_.erase(name);
270 }
271
272 [[nodiscard]] auto getCallbackNames() const {
273 std::vector<std::string> keys;
274 for (const auto &item : callbacks_) {
275 keys.push_back(item.first);
276 }
277 return keys;
278 }
279
280 [[nodiscard]] CallbackPtr getCallback(const std::string &name) const {
281 auto cb = callbacks_.find(name);
282 if (cb != end(callbacks_)) {
283 return cb->second;
284 }
285 return nullptr;
286 }
287
290 for (const auto &cb : callbacks_) {
291 cb.second->call(workspace_, results_);
292 }
293 }
294
295
301 bool computeMultipliers(const Problem &problem,
302 const std::vector<VectorXs> &xs,
303 const std::vector<VectorXs> &lams,
304 const std::vector<VectorXs> &vs);
305
306 ALIGATOR_INLINE Scalar mu() const { return mu_penal_; }
307 ALIGATOR_INLINE Scalar mu_inv() const { return 1. / mu_penal_; }
308 ALIGATOR_INLINE Scalar mu_dyn() const { return 0.1 * mu_penal_; }
309
310protected:
311 void updateTolsOnFailure() noexcept {
312 const Scalar arg = std::min(mu_penal_, 0.99);
313 prim_tol_ = prim_tol0 * std::pow(arg, bcl_params.prim_alpha);
314 inner_tol_ = inner_tol0 * std::pow(arg, bcl_params.dual_alpha);
315 }
316
317 void updateTolsOnSuccess() noexcept {
318 const Scalar arg = std::min(mu_penal_, 0.99);
319 prim_tol_ = prim_tol_ * std::pow(arg, bcl_params.prim_beta);
320 inner_tol_ = inner_tol_ * std::pow(arg, bcl_params.dual_beta);
321 }
322
324 ALIGATOR_INLINE void setAlmPenalty(Scalar new_mu) noexcept {
325 mu_penal_ = std::max(new_mu, bcl_params.mu_lower_bound);
326 }
327
328 // See sec. 3.1 of the IPOPT paper [Wächter, Biegler 2006]
329 // called before first bwd pass attempt
330 inline void initializeRegularization() noexcept {
331 if (preg_last_ == 0.) {
332 // this is the 1st iteration
333 preg_ = std::max(reg_init, reg_min);
334 } else {
335 // attempt decrease from last "good" value
336 preg_ = std::max(reg_min, preg_last_ * reg_dec_k_);
337 }
338 }
339
340 inline void increaseRegularization() noexcept {
341 if (preg_last_ == 0.)
343 else
344 preg_ *= reg_inc_k_;
345 }
346};
347
348#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
349extern template struct SolverProxDDPTpl<context::Scalar>;
350#endif
351} // namespace aligator
A convenience subclass of std::pmr::polymorphic_allocator for bytes.
Definition allocator.hpp:16
#define ALIGATOR_RUNTIME_ERROR(...)
Definition exceptions.hpp:7
#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
@ LINEAR
Linear rollout.
Definition enums.hpp:7
NonmonotoneLinesearch(const LinesearchOptions< Scalar > &) -> NonmonotoneLinesearch< Scalar >
ArmijoLinesearch(const LinesearchOptions< Scalar > &) -> ArmijoLinesearch< Scalar >
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:30
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 ALM parameter.
Scalar dual_beta
for dual tolerance (success)
Scalar dual_alpha
for dual tolerance (failure)
Scalar prim_beta
for primal tolerance (success)
Scalar prim_alpha
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 computeCriterion()
Compute stationarity criterion (dual infeasibility).
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.
ALIGATOR_INLINE Scalar mu() const
shared_ptr< CallbackBaseTpl< Scalar > > CallbackPtr
TrajOptDataTpl< Scalar > TrajOptData
void invokeCallbacks()
Invoke callbacks.
LinesearchOptions< Scalar > ls_params
void clearCallbacks() noexcept
Remove all callbacks from the instance.
boost::unordered_map< std::string, CallbackPtr > CallbackMap
std::unique_ptr< gar::RiccatiSolverBase< Scalar > > LinearSolverPtr
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)
Scalar forwardPass(const Problem &problem, const Scalar alpha)
ConstraintSetTpl< Scalar > CstrSet
bool removeCallback(const std::string &name)
ConstraintStackTpl< Scalar > ConstraintStack
bool computeMultipliers(const Problem &problem, const std::vector< VectorXs > &xs, const std::vector< VectorXs > &lams, const std::vector< VectorXs > &vs)
void setup(const Problem &problem)
Allocate new workspace and results instances according to the specifications of problem.
const CallbackMap & getCallbacks() const
void cycleProblem(const Problem &problem, const shared_ptr< StageData > &data)
void initializeRegularization() noexcept
void updateTolsOnSuccess() noexcept
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.
CostDataAbstractTpl< Scalar > CostData
void setNumThreads(const size_t num_threads)
ALIGATOR_INLINE Scalar mu_dyn() const
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