crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
SolverFDDP Class Reference

Feasibility-driven Differential Dynamic Programming (FDDP) solver. More...

#include <crocoddyl/core/solvers/fddp.hpp>

Inheritance diagram for SolverFDDP:
Collaboration diagram for SolverFDDP:

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverFDDP (boost::shared_ptr< ShootingProblem > problem)
 Initialize the FDDP solver.
 
virtual const Eigen::Vector2d & expectedImprovement ()
 
virtual void forwardPass (const double &stepLength)
 Run the forward pass or rollout. More...
 
double get_th_acceptnegstep () const
 Return the threshold used for accepting step along ascent direction.
 
void set_th_acceptnegstep (const double &th_acceptnegstep)
 Modify the threshold used for accepting step along ascent direction.
 
virtual bool solve (const std::vector< Eigen::VectorXd > &init_xs=DEFAULT_VECTOR, const std::vector< Eigen::VectorXd > &init_us=DEFAULT_VECTOR, const std::size_t &maxiter=100, const bool &is_feasible=false, const double &regInit=1e-9)
 Compute the optimal trajectory \(\mathbf{x}^*_s,\mathbf{u}^*_s\) as lists of \(T+1\) and \(T\) terms. More...
 
void updateExpectedImprovement ()
 Update internal values for computing the expected improvement.
 
- Public Member Functions inherited from SolverDDP
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverDDP (boost::shared_ptr< ShootingProblem > problem)
 Initialize the DDP solver. More...
 
virtual void allocateData ()
 Allocate all the internal data needed for the solver.
 
virtual void backwardPass ()
 Run the backward pass (Riccati sweep) More...
 
virtual double calcDiff ()
 Update the Jacobian and Hessian of the optimal control problem. More...
 
virtual void computeDirection (const bool &recalc=true)
 Compute the search direction \((\delta\mathbf{x},\delta\mathbf{u})\) for the current guess \((\mathbf{x}_s,\mathbf{u}_s)\). More...
 
virtual void computeGains (const std::size_t &t)
 Compute the feedforward and feedback terms using a Cholesky decomposition. More...
 
void decreaseRegularization ()
 Decrease the state and control regularization values by a regfactor_ factor.
 
const std::vector< double > & get_alphas () const
 Return the set of step lengths using by the line-search procedure.
 
const std::vector< Eigen::VectorXd > & get_fs () const
 Return the gaps \(\mathbf{\bar{f}}_{s}\).
 
const std::vector< Eigen::MatrixXd > & get_K () const
 Return the feedback gains \(\mathbf{K}_{s}\).
 
const std::vector< Eigen::VectorXd > & get_k () const
 Return the feedforward gains \(\mathbf{k}_{s}\).
 
const std::vector< Eigen::VectorXd > & get_Qu () const
 Return the Jacobian of the Hamiltonian function \(\mathbf{Q}_{\mathbf{u}_s}\).
 
const std::vector< Eigen::MatrixXd > & get_Quu () const
 Return the Hessian of the Hamiltonian function \(\mathbf{Q}_{\mathbf{uu}_s}\).
 
const std::vector< Eigen::VectorXd > & get_Qx () const
 Return the Jacobian of the Hamiltonian function \(\mathbf{Q}_{\mathbf{x}_s}\).
 
const std::vector< Eigen::MatrixXd > & get_Qxu () const
 Return the Hessian of the Hamiltonian function \(\mathbf{Q}_{\mathbf{xu}_s}\).
 
const std::vector< Eigen::MatrixXd > & get_Qxx () const
 Return the Hessian of the Hamiltonian function \(\mathbf{Q}_{\mathbf{xx}_s}\).
 
const double & get_regfactor () const
 Return the regularization factor used to decrease / increase it.
 
const double & get_regmax () const
 Return the maximum regularization value.
 
const double & get_regmin () const
 Return the minimum regularization value.
 
const double & get_th_gaptol () const
 Return the threshold for accepting a gap as non-zero.
 
const double & get_th_grad () const
 Return the tolerance of the expected gradient used for testing the step.
 
const double & get_th_stepdec () const
 Return the step-length threshold used to decrease regularization.
 
const double & get_th_stepinc () const
 Return the step-length threshold used to increase regularization.
 
const std::vector< Eigen::VectorXd > & get_Vx () const
 Return the Hessian of the Value function \(V_{\mathbf{x}_s}\).
 
const std::vector< Eigen::MatrixXd > & get_Vxx () const
 Return the Hessian of the Value function \(V_{\mathbf{xx}_s}\).
 
void increaseRegularization ()
 Increase the state and control regularization values by a regfactor_ factor.
 
void set_alphas (const std::vector< double > &alphas)
 Modify the set of step lengths using by the line-search procedure.
 
void set_regfactor (const double &reg_factor)
 Modify the regularization factor used to decrease / increase it.
 
void set_regmax (const double &regmax)
 Modify the maximum regularization value.
 
void set_regmin (const double &regmin)
 Modify the minimum regularization value.
 
void set_th_gaptol (const double &th_gaptol)
 Modify the threshold for accepting a gap as non-zero.
 
void set_th_grad (const double &th_grad)
 Modify the tolerance of the expected gradient used for testing the step.
 
void set_th_stepdec (const double &th_step)
 Modify the step-length threshold used to decrease regularization.
 
void set_th_stepinc (const double &th_step)
 Modify the step-length threshold used to increase regularization.
 
virtual double stoppingCriteria ()
 Return a positive value that quantifies the algorithm termination. More...
 
virtual double tryStep (const double &steplength=1)
 Try a predefined step length and compute its cost improvement. More...
 
- Public Member Functions inherited from SolverAbstract
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverAbstract (boost::shared_ptr< ShootingProblem > problem)
 Initialize the solver. More...
 
const double & get_cost () const
 Return the total cost.
 
const Eigen::Vector2d & get_d () const
 Return the LQ approximation of the expected improvement.
 
const double & get_dV () const
 Return the cost reduction.
 
const double & get_dVexp () const
 Return the expected cost reduction.
 
const bool & get_is_feasible () const
 Return the feasibility status of the \((\mathbf{x}_s,\mathbf{u}_s)\) trajectory.
 
const std::size_t & get_iter () const
 Return the number of iterations performed by the solver.
 
const boost::shared_ptr< ShootingProblem > & get_problem () const
 Return the shooting problem.
 
const double & get_steplength () const
 Return the step length.
 
const double & get_stop () const
 Return the value computed by stoppingCriteria()
 
const double & get_th_acceptstep () const
 Return the threshold used for accepting a step.
 
const double & get_th_stop () const
 Return the tolerance for stopping the algorithm.
 
const double & get_ureg () const
 Return the control regularization value.
 
const std::vector< Eigen::VectorXd > & get_us () const
 Return the control trajectory \(\mathbf{u}_s\).
 
const double & get_xreg () const
 Return the state regularization value.
 
const std::vector< Eigen::VectorXd > & get_xs () const
 Return the state trajectory \(\mathbf{x}_s\).
 
const std::vector< boost::shared_ptr< CallbackAbstract > > & getCallbacks () const
 "Return the list of callback functions using for diagnostic
 
void set_th_acceptstep (const double &th_acceptstep)
 Modify the threshold used for accepting step.
 
void set_th_stop (const double &th_stop)
 Modify the tolerance for stopping the algorithm.
 
void set_ureg (const double &ureg)
 Modify the control regularization value.
 
void set_us (const std::vector< Eigen::VectorXd > &us)
 Modify the control trajectory \(\mathbf{u}_s\).
 
void set_xreg (const double &xreg)
 Modify the state regularization value.
 
void set_xs (const std::vector< Eigen::VectorXd > &xs)
 Modify the state trajectory \(\mathbf{x}_s\).
 
void setCallbacks (const std::vector< boost::shared_ptr< CallbackAbstract > > &callbacks)
 Set a list of callback functions using for diagnostic. More...
 
void setCandidate (const std::vector< Eigen::VectorXd > &xs_warm=DEFAULT_VECTOR, const std::vector< Eigen::VectorXd > &us_warm=DEFAULT_VECTOR, const bool &is_feasible=false)
 Set the solver candidate warm-point values \((\mathbf{x}_s,\mathbf{u}_s)\). More...
 

Protected Attributes

double dg_
 Internal data for computing the expected improvement.
 
double dq_
 Internal data for computing the expected improvement.
 
double dv_
 Internal data for computing the expected improvement.
 
- Protected Attributes inherited from SolverDDP
std::vector< double > alphas_
 Set of step lengths using by the line-search procedure.
 
double cost_try_
 Total cost computed by line-search procedure.
 
std::vector< Eigen::VectorXd > dx_
 
std::vector< Eigen::VectorXd > fs_
 Gaps/defects between shooting nodes.
 
Eigen::VectorXd fTVxx_p_
 fTVxx_p term
 
std::vector< Eigen::MatrixXd > FuTVxx_p_
 fuTVxx_p_
 
Eigen::MatrixXd FxTVxx_p_
 fxTVxx_p_
 
std::vector< Eigen::MatrixXd > K_
 Feedback gains.
 
std::vector< Eigen::VectorXd > k_
 Feed-forward terms.
 
std::vector< Eigen::VectorXd > Qu_
 Gradient of the Hamiltonian.
 
std::vector< Eigen::MatrixXd > Quu_
 Hessian of the Hamiltonian.
 
std::vector< Eigen::LLT< Eigen::MatrixXd > > Quu_llt_
 Cholesky LLT solver.
 
std::vector< Eigen::VectorXd > Quuk_
 Quuk term.
 
std::vector< Eigen::VectorXd > Qx_
 Gradient of the Hamiltonian.
 
std::vector< Eigen::MatrixXd > Qxu_
 Hessian of the Hamiltonian.
 
std::vector< Eigen::MatrixXd > Qxx_
 Hessian of the Hamiltonian.
 
double regfactor_
 Regularization factor used to decrease / increase it.
 
double regmax_
 Maximum allowed regularization value.
 
double regmin_
 Minimum allowed regularization value.
 
double th_gaptol_
 Threshold limit to check non-zero gaps.
 
double th_grad_
 Tolerance of the expected gradient used for testing the step.
 
double th_stepdec_
 Step-length threshold used to decrease regularization.
 
double th_stepinc_
 Step-length threshold used to increase regularization.
 
std::vector< Eigen::VectorXd > us_try_
 Control trajectory computed by line-search procedure.
 
std::vector< Eigen::VectorXd > Vx_
 Gradient of the Value function.
 
std::vector< Eigen::MatrixXd > Vxx_
 Hessian of the Value function.
 
bool was_feasible_
 Label that indicates in the previous iterate was feasible.
 
Eigen::VectorXd xnext_
 Next state.
 
std::vector< Eigen::VectorXd > xs_try_
 State trajectory computed by line-search procedure.
 
- Protected Attributes inherited from SolverAbstract
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
 Callback functions.
 
double cost_
 Total cost.
 
Eigen::Vector2d d_
 LQ approximation of the expected improvement.
 
double dV_
 Cost reduction obtained by tryStep()
 
double dVexp_
 Expected cost reduction.
 
bool is_feasible_
 Label that indicates is the iteration is feasible.
 
std::size_t iter_
 Number of iteration performed by the solver.
 
boost::shared_ptr< ShootingProblemproblem_
 optimal control problem
 
double steplength_
 Current applied step-length.
 
double stop_
 Value computed by stoppingCriteria()
 
double th_acceptstep_
 Threshold used for accepting step.
 
double th_stop_
 Tolerance for stopping the algorithm.
 
double ureg_
 Current control regularization values.
 
std::vector< Eigen::VectorXd > us_
 Control trajectory.
 
double xreg_
 Current state regularization value.
 
std::vector< Eigen::VectorXd > xs_
 State trajectory.
 

Detailed Description

Feasibility-driven Differential Dynamic Programming (FDDP) solver.

The FDDP solver computes an optimal trajectory and control commands by iterates running backwardPass() and forwardPass(). The backward pass accepts infeasible guess as described in the SolverDDP::backwardPass(). Additionally, the forward pass handles infeasibility simulations that resembles the numerical behaviour of a multiple-shooting formulation, i.e.:

\begin{eqnarray} \mathbf{\hat{x}}_0 &=& \mathbf{\tilde{x}}_0 - (1 - \alpha)\mathbf{\bar{f}}_0,\\ \mathbf{\hat{u}}_k &=& \mathbf{u}_k + \alpha\mathbf{k}_k + \mathbf{K}_k(\mathbf{\hat{x}}_k-\mathbf{x}_k),\\ \mathbf{\hat{x}}_{k+1} &=& \mathbf{f}_k(\mathbf{\hat{x}}_k,\mathbf{\hat{u}}_k) - (1 - \alpha)\mathbf{\bar{f}}_{k+1}. \end{eqnarray}

Note that the forward pass keeps the gaps \(\mathbf{\bar{f}}_s\) open according to the step length \(\alpha\) that has been accepted. This solver has shown empirically greater globalization strategy. Additionally, the expected improvement computation considers the gaps in the dynamics:

\begin{equation} \Delta J(\alpha) = \Delta_1\alpha + \frac{1}{2}\Delta_2\alpha^2, \end{equation}

with

\begin{eqnarray} \Delta_1 = \sum_{k=0}^{N-1} \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{u}_k} +\mathbf{\bar{f}}_k^\top(V_{\mathbf{x}_k} - V_{\mathbf{xx}_k}\mathbf{x}_k),\nonumber\\ \Delta_2 = \sum_{k=0}^{N-1} \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{uu}_k}\mathbf{k}_k + \mathbf{\bar{f}}_k^\top(2 V_{\mathbf{xx}_k}\mathbf{x}_k - V_{\mathbf{xx}_k}\mathbf{\bar{f}}_k). \end{eqnarray}

For more details about the feasibility-driven differential dynamic programming algorithm see:

@inproceedings{mastalli-icra20,
author = {Mastalli, C. and Budhiraja, R. and Merkt, W. and Saurel, G. and Hammoud, B. and Naveau, M. and Carpentier, J. and Righetti, L. and Vijayakumar, S. and Mansard, N.},
title = {{Crocoddyl: An Efficient and Versatile Framework for Multi-Contact Optimal Control}},
booktitle = {IEEE International Conference on Robotics and Automation (ICRA)},
year = {2020}
}
See also
backwardPass(), forwardPass(), expectedImprovement() and updateExpectedImprovement()

Definition at line 50 of file fddp.hpp.

Member Function Documentation

◆ solve()

bool solve ( const std::vector< Eigen::VectorXd > &  init_xs = DEFAULT_VECTOR,
const std::vector< Eigen::VectorXd > &  init_us = DEFAULT_VECTOR,
const std::size_t &  maxiter = 100,
const bool &  is_feasible = false,
const double &  reg_init = 1e-9 
)
virtual

Compute the optimal trajectory \(\mathbf{x}^*_s,\mathbf{u}^*_s\) as lists of \(T+1\) and \(T\) terms.

From an initial guess init_xs, init_us (feasible or not), iterate over computeDirection() and tryStep() until stoppingCriteria() is below threshold. It also describes the globalization strategy used during the numerical optimization.

Parameters
[in]init_xsinitial guess for state trajectory with \(T+1\) elements (default [])
[in]init_usinitial guess for control trajectory with \(T\) elements (default [])
[in]maxitermaximum allowed number of iterations (default 100)
[in]isFeasibletrue if the init_xs are obtained from integrating the init_us (rollout) (default false)
[in]regInitinitial guess for the regularization value. Very low values are typical used with very good guess points (init_xs, init_us)
Returns
A boolean that describes if convergence was reached.

Reimplemented from SolverDDP.

Definition at line 19 of file fddp.cpp.

◆ expectedImprovement()

const Eigen::Vector2d & expectedImprovement ( )
virtual

Return the expected improvement from a given current search direction.

This function requires to first run updateExpectedImprovement(). The expected improvement computation considers the gaps in the dynamics:

\begin{equation} \Delta J(\alpha) = \Delta_1\alpha + \frac{1}{2}\Delta_2\alpha^2, \end{equation}

with

\begin{eqnarray} \Delta_1 = \sum_{k=0}^{N-1} \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{u}_k} +\mathbf{\bar{f}}_k^\top(V_{\mathbf{x}_k} - V_{\mathbf{xx}_k}\mathbf{x}_k),\nonumber\\ \Delta_2 = \sum_{k=0}^{N-1} \mathbf{k}_k^\top\mathbf{Q}_{\mathbf{uu}_k}\mathbf{k}_k + \mathbf{\bar{f}}_k^\top(2 V_{\mathbf{xx}_k}\mathbf{x}_k - V_{\mathbf{xx}_k}\mathbf{\bar{f}}_k). \end{eqnarray}

Reimplemented from SolverDDP.

Definition at line 107 of file fddp.cpp.

◆ forwardPass()

void forwardPass ( const double &  stepLength)
virtual

Run the forward pass or rollout.

It rollouts the action model given the computed policy (feedforward terns and feedback gains) by the backwardPass():

\begin{eqnarray} \mathbf{\hat{x}}_0 &=& \mathbf{\tilde{x}}_0,\\ \mathbf{\hat{u}}_k &=& \mathbf{u}_k + \alpha\mathbf{k}_k + \mathbf{K}_k(\mathbf{\hat{x}}_k-\mathbf{x}_k),\\ \mathbf{\hat{x}}_{k+1} &=& \mathbf{f}_k(\mathbf{\hat{x}}_k,\mathbf{\hat{u}}_k). \end{eqnarray}

We can define different step lengths \(\alpha\).

Parameters
stepLengthapplied step length ( \(0\leq\alpha\leq1\))

Reimplemented from SolverDDP.

Reimplemented in SolverBoxFDDP.

Definition at line 149 of file fddp.cpp.


The documentation for this class was generated from the following files: