|
crocoddyl
1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
|
|
|
crocoddyl
1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
|
|
Feasibility-driven Differential Dynamic Programming (FDDP) solver. More...
#include <crocoddyl/core/solvers/fddp.hpp>
Public Member Functions | |
| EIGEN_MAKE_ALIGNED_OPERATOR_NEW | SolverFDDP (boost::shared_ptr< ShootingProblem > problem) |
| Initialize the FDDP solver. More... | |
| virtual const Eigen::Vector2d & | expectedImprovement () |
| Return the expected improvement \(dV_{exp}\) from a given current search direction \((\delta\mathbf{x}^k,\delta\mathbf{u}^k)\). More... | |
| 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 | |
| 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, Hessian and feasibility of the optimal control problem. More... | |
| virtual void | computeDirection (const bool recalc=true) |
| Compute the search direction \((\delta\mathbf{x}^k,\delta\mathbf{u}^k)\) for the current guess \((\mathbf{x}^k_s,\mathbf{u}^k_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. | |
| DEPRECATED ("Use get_reg_incfactor() or get_reg_decfactor()", double get_regfactor() const ;) double get_reg_min() const | |
| Return the regularization factor used to decrease / increase it. More... | |
| DEPRECATED ("Use get_reg_max()", double get_regmax() const) | |
| DEPRECATED ("Use get_reg_min()", double get_regmin() const) | |
| DEPRECATED ("Use set_reg_incfactor() or set_reg_decfactor()", void set_regfactor(const double reg_factor);) void set_reg_min(const double regmin) | |
| Modify the regularization factor used to decrease / increase it. More... | |
| DEPRECATED ("Use set_reg_max()", void set_regmax(const double regmax)) | |
| DEPRECATED ("Use set_reg_min()", void set_regmin(const double regmin)) | |
| const std::vector< double > & | get_alphas () const |
| Return the set of step lengths using by the line-search procedure. | |
| const std::vector< MatrixXdRowMajor > & | 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}\). | |
| double | get_reg_decfactor () const |
| Return the regularization factor used to decrease the damping value. | |
| double | get_reg_incfactor () const |
| Return the regularization factor used to increase the damping value. | |
| double | get_reg_max () const |
| Return the maximum regularization value. | |
| double | get_th_grad () const |
| Return the tolerance of the expected gradient used for testing the step. | |
| double | get_th_stepdec () const |
| Return the step-length threshold used to decrease regularization. | |
| 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. | |
| virtual void | resizeData () |
| Resizing the solver data. More... | |
| void | set_alphas (const std::vector< double > &alphas) |
| Modify the set of step lengths using by the line-search procedure. | |
| void | set_reg_decfactor (const double reg_factor) |
| Modify the regularization factor used to decrease the damping value. | |
| void | set_reg_incfactor (const double reg_factor) |
| Modify the regularization factor used to increase the damping value. | |
| void | set_reg_max (const double regmax) |
| Modify the maximum regularization value. | |
| 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 \(\alpha\) and compute its cost improvement \(dV\). More... | |
| Public Member Functions inherited from SolverAbstract | |
| EIGEN_MAKE_ALIGNED_OPERATOR_NEW | SolverAbstract (boost::shared_ptr< ShootingProblem > problem) |
| Initialize the solver. More... | |
| double | computeDynamicFeasibility () |
| Compute the dynamic feasibility \(\|\mathbf{f}_{\mathbf{s}}\|_{\infty,1}\) for the current guess \((\mathbf{x}^k,\mathbf{u}^k)\). More... | |
| double | get_cost () const |
| Return the total cost. | |
| const Eigen::Vector2d & | get_d () const |
| Return the LQ approximation of the expected improvement. | |
| double | get_dV () const |
| Return the cost reduction \(dV\). | |
| double | get_dVexp () const |
| Return the expected cost reduction \(dV_{exp}\). | |
| double | get_ffeas () const |
| Return the feasibility of the dynamic constraints \(\|\mathbf{f}_{\mathbf{s}}\|_{\infty,1}\) of the current guess. | |
| const std::vector< Eigen::VectorXd > & | get_fs () const |
| Return the gaps \(\mathbf{f}_{s}\). | |
| bool | get_inffeas () const |
| Return the norm used for the computing the feasibility (true for \(\ell_\infty\), false for \(\ell_1\)) | |
| bool | get_is_feasible () const |
| Return the feasibility status of the \((\mathbf{x}_s,\mathbf{u}_s)\) trajectory. | |
| 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. | |
| double | get_steplength () const |
| Return the step length \(\alpha\). | |
| double | get_stop () const |
Return the value computed by stoppingCriteria() | |
| double | get_th_acceptstep () const |
| Return the threshold used for accepting a step. | |
| double | get_th_gaptol () const |
| Return the threshold for accepting a gap as non-zero. | |
| double | get_th_stop () const |
| Return the tolerance for stopping the algorithm. | |
| double | get_ureg () const |
| Return the control regularization value. | |
| const std::vector< Eigen::VectorXd > & | get_us () const |
| Return the control trajectory \(\mathbf{u}_s\). | |
| 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_inffeas (const bool inffeas) |
| Modify the current norm used for computed the feasibility. | |
| void | set_th_acceptstep (const double th_acceptstep) |
| Modify the threshold used for accepting step. | |
| void | set_th_gaptol (const double th_gaptol) |
| Modify the threshold for accepting a gap as non-zero. | |
| 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 the solver 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 trajectories \((\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_ |
| State error during the roll-out/forward-pass (size T) | |
| Eigen::VectorXd | fTVxx_p_ |
| Store the value of \(\mathbf{\bar{f}}^T\mathbf{V_{xx}}^{'}\). | |
| std::vector< MatrixXdRowMajor > | FuTVxx_p_ |
| Store the values of \(\mathbf{f_u}^T\mathbf{V_{xx}}^{'}\) per each running node. | |
| MatrixXdRowMajor | FxTVxx_p_ |
| Store the value of \(\mathbf{f_x}^T\mathbf{V_{xx}}^{'}\). | |
| std::vector< MatrixXdRowMajor > | K_ |
| Feedback gains \(\mathbf{K}\). | |
| std::vector< Eigen::VectorXd > | k_ |
| Feed-forward terms \(\mathbf{l}\). | |
| std::vector< Eigen::VectorXd > | Qu_ |
| Gradient of the Hamiltonian \(\mathbf{Q_u}\). | |
| std::vector< Eigen::MatrixXd > | Quu_ |
| Hessian of the Hamiltonian \(\mathbf{Q_{uu}}\). | |
| std::vector< Eigen::LLT< Eigen::MatrixXd > > | Quu_llt_ |
| Cholesky LLT solver. | |
| std::vector< Eigen::VectorXd > | Quuk_ |
| Store the values of. | |
| std::vector< Eigen::VectorXd > | Qx_ |
| Gradient of the Hamiltonian \(\mathbf{Q_x}\). | |
| std::vector< Eigen::MatrixXd > | Qxu_ |
| Hessian of the Hamiltonian \(\mathbf{Q_{xu}}\). | |
| std::vector< Eigen::MatrixXd > | Qxx_ |
| Hessian of the Hamiltonian \(\mathbf{Q_{xx}}\). | |
| double | reg_decfactor_ |
| Regularization factor used to decrease the damping value. | |
| double | reg_incfactor_ |
| Regularization factor used to increase the damping value. | |
| double | reg_max_ |
| Maximum allowed regularization value. | |
| double | reg_min_ |
| Minimum allowed regularization value. | |
| 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 \(\mathbf{V_x}\). | |
| std::vector< Eigen::MatrixXd > | Vxx_ |
| Hessian of the Value function \(\mathbf{V_{xx}}\). | |
| Eigen::MatrixXd | Vxx_tmp_ |
| Temporary variable for ensuring symmetry of Vxx. | |
| Eigen::VectorXd | xnext_ |
| Next state \(\mathbf{x}^{'}\). | |
| 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. | |
| double | ffeas_ |
| Feasibility of the dynamic constraints. | |
| std::vector< Eigen::VectorXd > | fs_ |
| Gaps/defects between shooting nodes. | |
| bool | inffeas_ |
| 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< ShootingProblem > | problem_ |
| 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_gaptol_ |
| Threshold limit to check non-zero gaps. | |
| double | th_stop_ |
| Tolerance for stopping the algorithm. | |
| double | tmp_feas_ |
| Temporal variables used for computed the feasibility. | |
| double | ureg_ |
| Current control regularization values. | |
| std::vector< Eigen::VectorXd > | us_ |
| Control trajectory. | |
| bool | was_feasible_ |
| Label that indicates in the previous iterate was feasible. | |
| double | xreg_ |
| Current state regularization value. | |
| std::vector< Eigen::VectorXd > | xs_ |
| State trajectory. | |
Additional Inherited Members | |
| Public Attributes inherited from SolverDDP | |
| EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef MathBaseTpl< double >::MatrixXsRowMajor | MatrixXdRowMajor |
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:
|
explicit |
|
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.
| [in] | init_xs | initial guess for state trajectory with \(T+1\) elements (default []) |
| [in] | init_us | initial guess for control trajectory with \(T\) elements (default []) |
| [in] | maxiter | maximum allowed number of iterations (default 100) |
| [in] | isFeasible | true if the init_xs are obtained from integrating the init_us (rollout) (default false) |
| [in] | regInit | initial guess for the regularization value. Very low values are typical used with very good guess points (init_xs, init_us) |
Reimplemented from SolverDDP.
|
virtual |
Return the expected improvement \(dV_{exp}\) from a given current search direction \((\delta\mathbf{x}^k,\delta\mathbf{u}^k)\).
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.
|
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\).
| stepLength | applied step length ( \(0\leq\alpha\leq1\)) |
Reimplemented from SolverDDP.
Reimplemented in SolverBoxFDDP.
1.8.17