9 #ifndef CROCODDYL_CORE_SOLVERS_DDP_HPP_ 10 #define CROCODDYL_CORE_SOLVERS_DDP_HPP_ 12 #include <Eigen/Cholesky> 15 #include "crocoddyl/core/solver-base.hpp" 50 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57 explicit SolverDDP(boost::shared_ptr<ShootingProblem> problem);
60 virtual bool solve(
const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
61 const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR,
const std::size_t& maxiter = 100,
62 const bool& is_feasible =
false,
const double& regInit = 1e-9);
64 virtual double tryStep(
const double& steplength = 1);
121 virtual void forwardPass(
const double& stepLength);
171 const std::vector<double>&
get_alphas()
const;
191 const std::vector<Eigen::MatrixXd>&
get_Vxx()
const;
196 const std::vector<Eigen::VectorXd>&
get_Vx()
const;
201 const std::vector<Eigen::MatrixXd>&
get_Qxx()
const;
206 const std::vector<Eigen::MatrixXd>&
get_Qxu()
const;
211 const std::vector<Eigen::MatrixXd>&
get_Quu()
const;
216 const std::vector<Eigen::VectorXd>&
get_Qx()
const;
221 const std::vector<Eigen::VectorXd>&
get_Qu()
const;
226 const std::vector<Eigen::MatrixXd>&
get_K()
const;
231 const std::vector<Eigen::VectorXd>&
get_k()
const;
236 const std::vector<Eigen::VectorXd>&
get_fs()
const;
256 void set_alphas(
const std::vector<double>& alphas);
281 std::vector<Eigen::VectorXd> dx_;
284 std::vector<Eigen::MatrixXd>
Vxx_;
285 std::vector<Eigen::VectorXd>
Vx_;
286 std::vector<Eigen::MatrixXd>
Qxx_;
287 std::vector<Eigen::MatrixXd>
Qxu_;
288 std::vector<Eigen::MatrixXd>
Quu_;
289 std::vector<Eigen::VectorXd>
Qx_;
290 std::vector<Eigen::VectorXd>
Qu_;
291 std::vector<Eigen::MatrixXd>
K_;
292 std::vector<Eigen::VectorXd>
k_;
293 std::vector<Eigen::VectorXd>
fs_;
299 std::vector<Eigen::LLT<Eigen::MatrixXd> >
Quu_llt_;
310 #endif // CROCODDYL_CORE_SOLVERS_DDP_HPP_ const double & get_regfactor() const
Return the regularization factor used to decrease / increase it.
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
void set_th_stepdec(const double &th_step)
Modify the step-length threshold used to decrease regularization.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverDDP(boost::shared_ptr< ShootingProblem > problem)
Initialize the DDP solver.
virtual void backwardPass()
Run the backward pass (Riccati sweep)
const double & get_regmax() const
Return the maximum regularization value.
const double & get_th_grad() const
Return the tolerance of the expected gradient used for testing the step.
std::vector< Eigen::MatrixXd > FuTVxx_p_
fuTVxx_p_
const std::vector< Eigen::VectorXd > & get_k() const
Return the feedforward gains .
Eigen::MatrixXd FxTVxx_p_
fxTVxx_p_
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 .
const std::vector< Eigen::MatrixXd > & get_Qxu() const
Return the Hessian of the Hamiltonian function .
const double & get_th_stepinc() const
Return the step-length threshold used to increase regularization.
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< Eigen::MatrixXd > & get_Quu() const
Return the Hessian of the Hamiltonian function .
const std::vector< double > & get_alphas() const
Return the set of step lengths using by the line-search procedure.
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 .
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
void set_th_grad(const double &th_grad)
Modify the tolerance of the expected gradient used for testing the step.
Eigen::VectorXd xnext_
Next state.
std::vector< Eigen::MatrixXd > Qxx_
Hessian of the Hamiltonian.
virtual void forwardPass(const double &stepLength)
Run the forward pass or rollout.
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.
std::vector< Eigen::MatrixXd > K_
Feedback gains.
virtual void computeDirection(const bool &recalc=true)
Compute the search direction for the current guess .
const std::vector< Eigen::VectorXd > & get_fs() const
Return the gaps .
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function.
double th_stepinc_
Step-length threshold used to increase regularization.
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 ®Init=1e-9)
Compute the optimal trajectory as lists of and terms.
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.
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
const std::vector< Eigen::VectorXd > & get_Qu() const
Return the Jacobian of the Hamiltonian function .
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 computeGains(const std::size_t &t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
const std::vector< Eigen::MatrixXd > & get_K() const
Return the feedback gains .
double regmin_
Minimum allowed regularization value.
double regmax_
Maximum allowed regularization value.
double regfactor_
Regularization factor used to decrease / increase it.
Differential Dynamic Programming (DDP) solver.
std::vector< Eigen::VectorXd > Quuk_
Quuk term.
virtual double calcDiff()
Update the Jacobian and Hessian of the optimal control problem.
virtual double tryStep(const double &steplength=1)
Try a predefined step length and compute its cost improvement.
std::vector< Eigen::MatrixXd > Qxu_
Hessian of the Hamiltonian.
const 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_regfactor(const double ®_factor)
Modify the regularization factor used to decrease / increase it.
void set_regmin(const double ®min)
Modify the minimum regularization value.
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_regmax(const double ®max)
Modify the maximum regularization value.
const double & get_regmin() const
Return the minimum regularization value.
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.