10 #include "crocoddyl/core/utils/exception.hpp" 11 #include "crocoddyl/core/solvers/ddp.hpp" 24 was_feasible_(false) {
27 const std::size_t& n_alphas = 10;
29 for (std::size_t n = 0; n < n_alphas; ++n) {
30 alphas_[n] = 1. / pow(2., static_cast<double>(n));
34 std::cerr <<
"Warning: th_stepinc has higher value than lowest alpha value, set to " 35 << std::to_string(
alphas_[n_alphas - 1]) << std::endl;
39 SolverDDP::~SolverDDP() {}
41 bool SolverDDP::solve(
const std::vector<Eigen::VectorXd>& init_xs,
const std::vector<Eigen::VectorXd>& init_us,
42 const std::size_t& maxiter,
const bool& is_feasible,
const double& reginit) {
46 if (std::isnan(reginit)) {
55 bool recalcDiff =
true;
60 }
catch (std::exception& e) {
75 for (std::vector<double>::const_iterator it =
alphas_.begin(); it !=
alphas_.end(); ++it) {
80 }
catch (std::exception& e) {
107 const std::size_t& n_callbacks =
callbacks_.size();
108 for (std::size_t c = 0; c < n_callbacks; ++c) {
134 const std::size_t& T = this->
problem_->get_T();
135 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
136 for (std::size_t t = 0; t < T; ++t) {
137 if (models[t]->get_nu() != 0) {
146 const std::size_t& T = this->
problem_->get_T();
147 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
148 for (std::size_t t = 0; t < T; ++t) {
149 if (models[t]->get_nu() != 0) {
161 const Eigen::VectorXd& x0 =
problem_->get_x0();
162 problem_->get_runningModels()[0]->get_state()->diff(
xs_[0], x0,
fs_[0]);
164 const std::size_t& T =
problem_->get_T();
165 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
166 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
167 for (std::size_t t = 0; t < T; ++t) {
168 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
169 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
170 model->get_state()->diff(
xs_[t + 1], d->xnext,
fs_[t + 1]);
173 for (std::vector<Eigen::VectorXd>::iterator it =
fs_.begin(); it !=
fs_.end(); ++it) {
181 const boost::shared_ptr<ActionDataAbstract>& d_T =
problem_->get_terminalData();
182 Vxx_.back() = d_T->Lxx;
183 Vx_.back() = d_T->Lx;
185 if (!std::isnan(
xreg_)) {
190 Vx_.back().noalias() +=
Vxx_.back() *
fs_.back();
193 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
194 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
195 for (
int t = static_cast<int>(
problem_->get_T()) - 1; t >= 0; --t) {
196 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
197 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
198 const Eigen::MatrixXd& Vxx_p =
Vxx_[t + 1];
199 const Eigen::VectorXd& Vx_p =
Vx_[t + 1];
200 const std::size_t& nu = m->get_nu();
204 FxTVxx_p_.noalias() = d->Fx.transpose() * Vxx_p;
206 Qx_[t].noalias() += d->Fx.transpose() * Vx_p;
211 FuTVxx_p_[t].noalias() = d->Fu.transpose() * Vxx_p;
214 Qu_[t].noalias() += d->Fu.transpose() * Vx_p;
216 if (!std::isnan(
ureg_)) {
226 if (std::isnan(
ureg_)) {
227 Vx_[t].noalias() -=
K_[t].transpose() *
Qu_[t];
230 Vx_[t].noalias() +=
K_[t].transpose() *
Quuk_[t];
231 Vx_[t].noalias() -= 2 * (
K_[t].transpose() *
Qu_[t]);
237 if (!std::isnan(
xreg_)) {
246 if (raiseIfNaN(
Vx_[t].lpNorm<Eigen::Infinity>())) {
247 throw_pretty(
"backward_error");
249 if (raiseIfNaN(
Vxx_[t].lpNorm<Eigen::Infinity>())) {
250 throw_pretty(
"backward_error");
256 if (steplength > 1. || steplength < 0.) {
257 throw_pretty(
"Invalid argument: " 258 <<
"invalid step length, value is between 0. to 1.");
261 const std::size_t& T =
problem_->get_T();
262 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
263 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
264 for (std::size_t t = 0; t < T; ++t) {
265 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
266 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
268 m->get_state()->diff(
xs_[t],
xs_try_[t], dx_[t]);
269 if (m->get_nu() != 0) {
271 us_try_[t].noalias() -=
k_[t] * steplength;
281 throw_pretty(
"forward_error");
283 if (raiseIfNaN(
xs_try_[t + 1].lpNorm<Eigen::Infinity>())) {
284 throw_pretty(
"forward_error");
288 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
289 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
294 throw_pretty(
"forward_error");
299 if (
problem_->get_runningModels()[t]->get_nu() > 0) {
301 const Eigen::ComputationInfo& info =
Quu_llt_[t].info();
302 if (info != Eigen::Success) {
303 throw_pretty(
"backward_error");
305 K_[t] =
Qxu_[t].transpose();
329 const std::size_t& T =
problem_->get_T();
349 const std::size_t& ndx =
problem_->get_ndx();
350 const std::size_t& nu =
problem_->get_nu_max();
351 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
352 for (std::size_t t = 0; t < T; ++t) {
353 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
354 Vxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
355 Vx_[t] = Eigen::VectorXd::Zero(ndx);
356 Qxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
357 Qxu_[t] = Eigen::MatrixXd::Zero(ndx, nu);
358 Quu_[t] = Eigen::MatrixXd::Zero(nu, nu);
359 Qx_[t] = Eigen::VectorXd::Zero(ndx);
360 Qu_[t] = Eigen::VectorXd::Zero(nu);
361 K_[t] = Eigen::MatrixXd::Zero(nu, ndx);
362 k_[t] = Eigen::VectorXd::Zero(nu);
363 fs_[t] = Eigen::VectorXd::Zero(ndx);
368 xs_try_[t] = model->get_state()->zero();
370 us_try_[t] = Eigen::VectorXd::Zero(nu);
371 dx_[t] = Eigen::VectorXd::Zero(ndx);
373 FuTVxx_p_[t] = Eigen::MatrixXd::Zero(nu, ndx);
374 Quu_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(nu);
375 Quuk_[t] = Eigen::VectorXd(nu);
377 Vxx_.back() = Eigen::MatrixXd::Zero(ndx, ndx);
378 Vx_.back() = Eigen::VectorXd::Zero(ndx);
380 fs_.back() = Eigen::VectorXd::Zero(ndx);
382 FxTVxx_p_ = Eigen::MatrixXd::Zero(ndx, ndx);
383 fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
421 if (regfactor <= 1.) {
422 throw_pretty(
"Invalid argument: " 423 <<
"regfactor value is higher than 1.");
430 throw_pretty(
"Invalid argument: " 431 <<
"regmin value has to be positive.");
438 throw_pretty(
"Invalid argument: " 439 <<
"regmax value has to be positive.");
445 double prev_alpha = alphas[0];
446 if (prev_alpha != 1.) {
447 std::cerr <<
"Warning: alpha[0] should be 1" << std::endl;
449 for (std::size_t i = 1; i < alphas.size(); ++i) {
450 double alpha = alphas[i];
452 throw_pretty(
"Invalid argument: " 453 <<
"alpha values has to be positive.");
455 if (alpha >= prev_alpha) {
456 throw_pretty(
"Invalid argument: " 457 <<
"alpha values are monotonously decreasing.");
465 if (0. >= th_stepdec || th_stepdec > 1.) {
466 throw_pretty(
"Invalid argument: " 467 <<
"th_stepdec value should between 0 and 1.");
473 if (0. >= th_stepinc || th_stepinc > 1.) {
474 throw_pretty(
"Invalid argument: " 475 <<
"th_stepinc value should between 0 and 1.");
482 throw_pretty(
"Invalid argument: " 483 <<
"th_grad value has to be positive.");
const double & get_regfactor() const
Return the regularization factor used to decrease / increase it.
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
bool is_feasible_
Label that indicates is the iteration is feasible.
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.
double ureg_
Current control regularization values.
double steplength_
Current applied step-length.
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 .
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 .
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.
double xreg_
Current state regularization value.
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.
const std::vector< Eigen::MatrixXd > & get_Quu() const
Return the Hessian of the Hamiltonian function .
double th_acceptstep_
Threshold used for accepting step.
std::vector< Eigen::VectorXd > us_
Control trajectory.
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.
double dV_
Cost reduction obtained by tryStep()
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::Vector2d d_
LQ approximation of the expected improvement.
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.
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
std::vector< Eigen::MatrixXd > Qxx_
Hessian of the Hamiltonian.
double dVexp_
Expected cost reduction.
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 .
std::vector< Eigen::VectorXd > xs_
State trajectory.
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.
double th_stop_
Tolerance for stopping the algorithm.
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.
std::vector< Eigen::VectorXd > Quuk_
Quuk term.
virtual double calcDiff()
Update the Jacobian and Hessian of the optimal control problem.
Abstract class for solver callbacks.
virtual double tryStep(const double &steplength=1)
Try a predefined step length and compute its cost improvement.
std::size_t iter_
Number of iteration performed by the solver.
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.
double stop_
Value computed by stoppingCriteria()
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.