9 #ifndef CROCODDYL_CORE_SOLVERS_DDP_HPP_ 10 #define CROCODDYL_CORE_SOLVERS_DDP_HPP_ 13 #include <Eigen/Cholesky> 14 #include "crocoddyl/core/solver-base.hpp" 15 #include "crocoddyl/core/mathbase.hpp" 16 #include "crocoddyl/core/utils/deprecate.hpp" 51 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
60 explicit SolverDDP(boost::shared_ptr<ShootingProblem> problem);
63 virtual bool solve(
const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
64 const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR,
const std::size_t maxiter = 100,
65 const bool is_feasible =
false,
const double regInit = 1e-9);
67 virtual double tryStep(
const double steplength = 1);
169 DEPRECATED(
"Use get_reg_incfactor() or get_reg_decfactor()",
double get_regfactor()
const;)
174 double get_reg_min()
const;
175 DEPRECATED(
"Use get_reg_min()",
double get_regmin()
const);
181 DEPRECATED(
"Use get_reg_max()",
double get_regmax()
const);
186 const std::vector<double>&
get_alphas()
const;
211 const std::vector<Eigen::MatrixXd>&
get_Vxx()
const;
216 const std::vector<Eigen::VectorXd>&
get_Vx()
const;
221 const std::vector<Eigen::MatrixXd>&
get_Qxx()
const;
226 const std::vector<Eigen::MatrixXd>&
get_Qxu()
const;
231 const std::vector<Eigen::MatrixXd>&
get_Quu()
const;
236 const std::vector<Eigen::VectorXd>&
get_Qx()
const;
241 const std::vector<Eigen::VectorXd>&
get_Qu()
const;
246 const std::vector<MatrixXdRowMajor>&
get_K()
const;
251 const std::vector<Eigen::VectorXd>&
get_k()
const;
256 const std::vector<Eigen::VectorXd>&
get_fs()
const;
271 DEPRECATED(
"Use set_reg_incfactor() or set_reg_decfactor()",
void set_regfactor(
const double reg_factor);)
276 void set_reg_min(
const double regmin);
277 DEPRECATED(
"Use set_reg_min()",
void set_regmin(
const double regmin));
283 DEPRECATED(
"Use set_reg_max()",
void set_regmax(
const double regmax));
288 void set_alphas(
const std::vector<double>& alphas);
319 std::vector<Eigen::VectorXd> dx_;
322 std::vector<Eigen::MatrixXd>
Vxx_;
324 std::vector<Eigen::VectorXd>
Vx_;
325 std::vector<Eigen::MatrixXd>
Qxx_;
326 std::vector<Eigen::MatrixXd>
Qxu_;
327 std::vector<Eigen::MatrixXd>
Quu_;
328 std::vector<Eigen::VectorXd>
Qx_;
329 std::vector<Eigen::VectorXd>
Qu_;
330 std::vector<MatrixXdRowMajor>
K_;
331 std::vector<Eigen::VectorXd>
k_;
332 std::vector<Eigen::VectorXd>
fs_;
338 std::vector<Eigen::LLT<Eigen::MatrixXd> >
Quu_llt_;
350 #endif // CROCODDYL_CORE_SOLVERS_DDP_HPP_ double reg_max_
Maximum allowed regularization value.
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
double get_reg_max() const
Return the maximum regularization value.
void set_reg_max(const double regmax)
Modify the maximum regularization value.
SolverDDP(boost::shared_ptr< ShootingProblem > problem)
Initialize the DDP solver.
virtual void backwardPass()
Run the backward pass (Riccati sweep)
double get_th_stepdec() const
Return 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.
void set_th_gaptol(const double th_gaptol)
Modify the threshold for accepting a gap as non-zero.
std::vector< MatrixXdRowMajor > FuTVxx_p_
fuTVxx_p_
const std::vector< Eigen::VectorXd > & get_k() const
Return the feedforward gains .
const std::vector< Eigen::VectorXd > & get_Qx() const
Return the Jacobian of the Hamiltonian function .
const std::vector< Eigen::VectorXd > & get_Vx() const
Return the Hessian of the Value function .
double th_gaptol_
Threshold limit to check non-zero gaps.
std::vector< MatrixXdRowMajor > K_
Feedback gains.
const std::vector< Eigen::MatrixXd > & get_Qxu() const
Return the Hessian of the Hamiltonian function .
virtual void allocateData()
Allocate all the internal data needed for the solver.
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
double th_stepdec_
Step-length threshold used to decrease regularization.
const std::vector< MatrixXdRowMajor > & get_K() const
Return the feedback gains .
const std::vector< Eigen::MatrixXd > & get_Quu() const
Return the Hessian of the Hamiltonian function .
double reg_min_
Minimum allowed regularization value.
const std::vector< double > & get_alphas() const
Return the set of step lengths using by the line-search procedure.
double get_th_grad() const
Return the tolerance of the expected gradient used for testing the step.
Abstract class for optimal control solvers.
std::vector< Eigen::VectorXd > k_
Feed-forward terms.
const std::vector< Eigen::MatrixXd > & get_Qxx() const
Return the Hessian of the Hamiltonian function .
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.
std::vector< Eigen::VectorXd > Vx_
Gradient of the Value function.
void increaseRegularization()
Increase the state and control regularization values by a regfactor_ factor.
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
Eigen::VectorXd fTVxx_p_
fTVxx_p term
double reg_incfactor_
Regularization factor used to increase the damping value.
Eigen::VectorXd xnext_
Next state.
std::vector< Eigen::MatrixXd > Qxx_
Hessian of the Hamiltonian.
void set_reg_decfactor(const double reg_factor)
Modify the regularization factor used to decrease the damping value.
void decreaseRegularization()
Decrease 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.
double get_reg_incfactor() const
Return the regularization factor used to increase the damping value.
const std::vector< Eigen::VectorXd > & get_fs() const
Return the gaps .
Eigen::MatrixXd Vxx_tmp_
Temporary variable for ensuring symmetry of Vxx.
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function.
MatrixXdRowMajor FxTVxx_p_
fxTVxx_p_
void set_th_stepdec(const double th_step)
Modify the step-length threshold used to decrease regularization.
double th_stepinc_
Step-length threshold used to increase regularization.
double get_th_stepinc() const
Return the step-length threshold used to increase regularization.
bool was_feasible_
Label that indicates in the previous iterate was feasible.
double th_grad_
Tolerance of the expected gradient used for testing the step.
virtual void computeGains(const std::size_t t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
virtual double tryStep(const double steplength=1)
Try a predefined step length and compute its cost improvement.
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
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 as lists of and terms.
const std::vector< Eigen::VectorXd > & get_Qu() const
Return the Jacobian of the Hamiltonian function .
void set_reg_incfactor(const double reg_factor)
Modify the regularization factor used to increase the damping value.
double reg_decfactor_
Regularization factor used to decrease the damping value.
double get_reg_decfactor() const
Return the regularization factor used to decrease the damping value.
const std::vector< Eigen::MatrixXd > & get_Vxx() const
Return the Hessian of the Value function .
std::vector< Eigen::VectorXd > Qx_
Gradient of the Hamiltonian.
virtual void forwardPass(const double stepLength)
Run the forward pass or rollout.
double get_th_gaptol() const
Return the threshold for accepting a gap as non-zero.
Differential Dynamic Programming (DDP) solver.
std::vector< Eigen::VectorXd > Quuk_
Quuk term.
virtual void computeDirection(const bool recalc=true)
Compute the search direction for the current guess .
virtual double calcDiff()
Update the Jacobian and Hessian of the optimal control problem.
std::vector< Eigen::MatrixXd > Qxu_
Hessian of the Hamiltonian.
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
virtual const Eigen::Vector2d & expectedImprovement()
Return the expected improvement from a given current search direction.
void set_th_grad(const double th_grad)
Modify the tolerance of the expected gradient used for testing the step.
std::vector< Eigen::MatrixXd > Quu_
Hessian of the Hamiltonian.
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian.
std::vector< Eigen::LLT< Eigen::MatrixXd > > Quu_llt_
Cholesky LLT solver.
double cost_try_
Total cost computed by line-search procedure.