18#include <boost/unordered_map.hpp>
49 using CallbackMap = boost::unordered_map<std::string, CallbackPtr>;
56 struct LinesearchVariant {
57 using VariantType = std::variant<std::monostate, ArmijoLinesearch<Scalar>,
64 return std::numeric_limits<Scalar>::quiet_NaN();
67 return method.run(fun, phi0, dphi0, alpha_try);
73 std::visit(
overloads{[](std::monostate &) {},
74 [&](
auto &&method) { method.reset(); }},
94 "provided StepAcceptanceStrategy is invalid.");
98 friend SolverProxDDPTpl;
201 std::size_t num_threads_ = 1;
221 target_dual_tol_ = tol;
222 sync_dual_tol =
false;
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 = {});
287 std::vector<std::string> keys;
288 for (
const auto &item : callbacks_) {
289 keys.push_back(item.first);
295 auto cb = callbacks_.find(name);
296 if (cb != end(callbacks_)) {
304 for (
const auto &cb : callbacks_) {
305 cb.second->call(workspace, results);
315 const std::vector<VectorXs> &lams,
316 const std::vector<VectorXs> &vs);
330 const Scalar arg = std::min(mu_penal_, 0.99);
336 const Scalar arg = std::min(mu_penal_, 0.99);
343 mu_penal_ = std::max(new_mu,
bcl_params.mu_lower_bound);
368#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
369#include "solver-proxddp.txx"
Basic backtracking Armijo line-search strategy.
#define ALIGATOR_WARNING(loc,...)
Implements a basic Armijo back-tracking strategy.
VerboseLevel
Verbosity level.
StepAcceptanceStrategy
Whether to use linesearch or filter during step acceptance phase.
@ NONLINEAR
Nonlinear rollout, using the full dynamics.
@ GAUSS_NEWTON
Use the Gauss-Newton approximation.
LinesearchMode
Whether to use merit functions in primal or primal-dual mode.
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.
A table logging utility to log the trace of the numerical solvers.
Nonmonotone Linesearch algorithm. Modifies the Armijo condition with a moving average of function val...
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
MultiplierUpdateMode multiplier_update_mode
void updateGains()
Update primal-dual feedback gains (control, costate, path multiplier)
void computeCriterion()
Compute stationarity criterion (dual infeasibility).
void removeCallback(const std::string &name)
Scalar refinement_threshold_
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
ResultsTpl< Scalar > Results
size_t max_refinement_steps_
auto getCallbackNames() const
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
bool force_initial_condition_
LinesearchVariant linesearch_
LQSolverChoice linear_solver_choice
Scalar forwardPass(const Problem &problem, const Scalar alpha)
StageDataTpl< Scalar > StageData
ConstraintSetTpl< Scalar > CstrSet
void cycleProblem(const Problem &problem, shared_ptr< StageDataTpl< Scalar > > data)
RolloutType rollout_type_
StageModelTpl< Scalar > StageModel
LinesearchOptions ls_params
ConstraintStackTpl< Scalar > ConstraintStack
void setup(const Problem &problem)
Allocate new workspace and results instances according to the specifications of problem.
HessianApprox hess_approx_
const CallbackMap & getCallbacks() const
void setNumThreads(const std::size_t num_threads)
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
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)
Scalar getDualTolerance() const
ALIGATOR_INLINE Scalar mu_inv() const
LinearSolverPtr linear_solver_
ALIGATOR_INLINE void setAlmPenalty(Scalar new_mu) noexcept
Set dual proximal/ALM penalty parameter.
StepAcceptanceStrategy sa_strategy_
FilterTpl< Scalar > filter_
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
WorkspaceTpl< Scalar > Workspace
typename Linesearch< Scalar >::Options LinesearchOptions
void updateLQSubproblem()
Data struct for stage models StageModelTpl.
Base struct for function data.
A stage in the control problem.
Trajectory optimization problem.
Workspace for solver SolverProxDDP.
Utility helper struct for creating visitors from lambdas.