18#include <boost/unordered_map.hpp>
45 using CallbackMap = boost::unordered_map<std::string, CallbackPtr>;
51 struct LinesearchVariant {
52 using VariantType = std::variant<std::monostate, ArmijoLinesearch<Scalar>,
59 return std::numeric_limits<Scalar>::quiet_NaN();
62 return method.run(fun, phi0, dphi0, alpha_try);
68 std::visit(
overloads{[](std::monostate &) {},
69 [&](
auto &method) { method.reset(); }},
73 bool isValid()
const {
return impl_.index() > 0ul; }
90 "Provided StepAcceptanceStrategy value is invalid.");
94 friend SolverProxDDPTpl;
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 = {});
273 std::vector<std::string> keys;
275 keys.push_back(item.first);
302 const std::vector<VectorXs> &xs,
303 const std::vector<VectorXs> &lams,
304 const std::vector<VectorXs> &vs);
348#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
349extern template struct SolverProxDDPTpl<context::Scalar>;
A convenience subclass of std::pmr::polymorphic_allocator for bytes.
#define ALIGATOR_RUNTIME_ERROR(...)
Implements a basic Armijo back-tracking strategy.
VerboseLevel
Verbosity level.
StepAcceptanceStrategy
Whether to use linesearch or filter during step acceptance phase.
NonmonotoneLinesearch(const LinesearchOptions< Scalar > &) -> NonmonotoneLinesearch< Scalar >
ArmijoLinesearch(const LinesearchOptions< Scalar > &) -> ArmijoLinesearch< Scalar >
@ 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 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
MultiplierUpdateMode multiplier_update_mode
polymorphic_allocator allocator_
void computeCriterion()
Compute stationarity criterion (dual infeasibility).
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.
ALIGATOR_INLINE Scalar mu() const
shared_ptr< CallbackBaseTpl< Scalar > > CallbackPtr
TrajOptDataTpl< Scalar > TrajOptData
ResultsTpl< Scalar > Results
void invokeCallbacks()
Invoke callbacks.
size_t max_refinement_steps_
auto getCallbackNames() const
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)
bool force_initial_condition_
LinesearchVariant linesearch_
LQSolverChoice linear_solver_choice
Scalar forwardPass(const Problem &problem, const Scalar alpha)
StageDataTpl< Scalar > StageData
ConstraintSetTpl< Scalar > CstrSet
RolloutType rollout_type_
StageModelTpl< Scalar > StageModel
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.
HessianApprox hess_approx_
const CallbackMap & getCallbacks() const
void cycleProblem(const Problem &problem, const shared_ptr< StageData > &data)
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
void initializeRegularization() noexcept
void updateTolsOnSuccess() noexcept
size_t getNumThreads() const
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.
CostDataAbstractTpl< Scalar > CostData
WorkspaceTpl< Scalar > Workspace
void setNumThreads(const size_t num_threads)
ALIGATOR_INLINE Scalar mu_dyn() const
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.