10 #include "crocoddyl/core/utils/exception.hpp" 11 #include "crocoddyl/core/solvers/ddp.hpp" 25 was_feasible_(false) {
28 const std::size_t& n_alphas = 10;
30 for (std::size_t n = 0; n < n_alphas; ++n) {
31 alphas_[n] = 1. / pow(2., static_cast<double>(n));
35 std::cerr <<
"Warning: th_stepinc has higher value than lowest alpha value, set to " 36 << std::to_string(
alphas_[n_alphas - 1]) << std::endl;
40 SolverDDP::~SolverDDP() {}
42 bool SolverDDP::solve(
const std::vector<Eigen::VectorXd>& init_xs,
const std::vector<Eigen::VectorXd>& init_us,
43 const std::size_t& maxiter,
const bool& is_feasible,
const double& reginit) {
47 if (std::isnan(reginit)) {
56 bool recalcDiff =
true;
61 }
catch (std::exception& e) {
76 for (std::vector<double>::const_iterator it =
alphas_.begin(); it !=
alphas_.end(); ++it) {
81 }
catch (std::exception& e) {
108 const std::size_t& n_callbacks =
callbacks_.size();
109 for (std::size_t c = 0; c < n_callbacks; ++c) {
135 const std::size_t& T = this->
problem_->get_T();
136 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
137 for (std::size_t t = 0; t < T; ++t) {
138 if (models[t]->get_nu() != 0) {
147 const std::size_t& T = this->
problem_->get_T();
148 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
149 for (std::size_t t = 0; t < T; ++t) {
150 if (models[t]->get_nu() != 0) {
163 const Eigen::VectorXd& x0 =
problem_->get_x0();
164 problem_->get_runningModels()[0]->get_state()->diff(
xs_[0], x0,
fs_[0]);
165 bool could_be_feasible =
true;
167 could_be_feasible =
false;
169 const std::size_t& T =
problem_->get_T();
170 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
171 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
172 for (std::size_t t = 0; t < T; ++t) {
173 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
174 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
175 model->get_state()->diff(
xs_[t + 1], d->xnext,
fs_[t + 1]);
176 if(could_be_feasible) {
178 could_be_feasible =
false;
185 for (std::vector<Eigen::VectorXd>::iterator it =
fs_.begin(); it !=
fs_.end(); ++it) {
193 const boost::shared_ptr<ActionDataAbstract>& d_T =
problem_->get_terminalData();
194 Vxx_.back() = d_T->Lxx;
195 Vx_.back() = d_T->Lx;
197 if (!std::isnan(
xreg_)) {
202 Vx_.back().noalias() +=
Vxx_.back() *
fs_.back();
204 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
205 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
206 for (
int t = static_cast<int>(
problem_->get_T()) - 1; t >= 0; --t) {
207 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
208 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
209 const Eigen::MatrixXd& Vxx_p =
Vxx_[t + 1];
210 const Eigen::VectorXd& Vx_p =
Vx_[t + 1];
211 const std::size_t& nu = m->get_nu();
215 FxTVxx_p_.noalias() = d->Fx.transpose() * Vxx_p;
217 Qx_[t].noalias() += d->Fx.transpose() * Vx_p;
222 FuTVxx_p_[t].noalias() = d->Fu.transpose() * Vxx_p;
225 Qu_[t].noalias() += d->Fu.transpose() * Vx_p;
227 if (!std::isnan(
ureg_)) {
237 if (std::isnan(
ureg_)) {
238 Vx_[t].noalias() -=
K_[t].transpose() *
Qu_[t];
241 Vx_[t].noalias() +=
K_[t].transpose() *
Quuk_[t];
242 Vx_[t].noalias() -= 2 * (
K_[t].transpose() *
Qu_[t]);
248 if (!std::isnan(
xreg_)) {
257 if (raiseIfNaN(
Vx_[t].lpNorm<Eigen::Infinity>())) {
258 throw_pretty(
"backward_error");
260 if (raiseIfNaN(
Vxx_[t].lpNorm<Eigen::Infinity>())) {
261 throw_pretty(
"backward_error");
267 if (steplength > 1. || steplength < 0.) {
268 throw_pretty(
"Invalid argument: " 269 <<
"invalid step length, value is between 0. to 1.");
272 const std::size_t& T =
problem_->get_T();
273 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
274 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
275 for (std::size_t t = 0; t < T; ++t) {
276 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
277 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
279 m->get_state()->diff(
xs_[t],
xs_try_[t], dx_[t]);
280 if (m->get_nu() != 0) {
282 us_try_[t].noalias() -=
k_[t] * steplength;
292 throw_pretty(
"forward_error");
294 if (raiseIfNaN(
xs_try_[t + 1].lpNorm<Eigen::Infinity>())) {
295 throw_pretty(
"forward_error");
299 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
300 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
305 throw_pretty(
"forward_error");
310 if (
problem_->get_runningModels()[t]->get_nu() > 0) {
312 const Eigen::ComputationInfo& info =
Quu_llt_[t].info();
313 if (info != Eigen::Success) {
314 throw_pretty(
"backward_error");
316 K_[t] =
Qxu_[t].transpose();
340 const std::size_t& T =
problem_->get_T();
360 const std::size_t& ndx =
problem_->get_ndx();
361 const std::size_t& nu =
problem_->get_nu_max();
362 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
363 for (std::size_t t = 0; t < T; ++t) {
364 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
365 Vxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
366 Vx_[t] = Eigen::VectorXd::Zero(ndx);
367 Qxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
368 Qxu_[t] = Eigen::MatrixXd::Zero(ndx, nu);
369 Quu_[t] = Eigen::MatrixXd::Zero(nu, nu);
370 Qx_[t] = Eigen::VectorXd::Zero(ndx);
371 Qu_[t] = Eigen::VectorXd::Zero(nu);
372 K_[t] = Eigen::MatrixXd::Zero(nu, ndx);
373 k_[t] = Eigen::VectorXd::Zero(nu);
374 fs_[t] = Eigen::VectorXd::Zero(ndx);
379 xs_try_[t] = model->get_state()->zero();
381 us_try_[t] = Eigen::VectorXd::Zero(nu);
382 dx_[t] = Eigen::VectorXd::Zero(ndx);
384 FuTVxx_p_[t] = Eigen::MatrixXd::Zero(nu, ndx);
385 Quu_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(nu);
386 Quuk_[t] = Eigen::VectorXd(nu);
388 Vxx_.back() = Eigen::MatrixXd::Zero(ndx, ndx);
389 Vx_.back() = Eigen::VectorXd::Zero(ndx);
391 fs_.back() = Eigen::VectorXd::Zero(ndx);
393 FxTVxx_p_ = Eigen::MatrixXd::Zero(ndx, ndx);
394 fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
434 if (regfactor <= 1.) {
435 throw_pretty(
"Invalid argument: " 436 <<
"regfactor value is higher than 1.");
443 throw_pretty(
"Invalid argument: " 444 <<
"regmin value has to be positive.");
451 throw_pretty(
"Invalid argument: " 452 <<
"regmax value has to be positive.");
458 double prev_alpha = alphas[0];
459 if (prev_alpha != 1.) {
460 std::cerr <<
"Warning: alpha[0] should be 1" << std::endl;
462 for (std::size_t i = 1; i < alphas.size(); ++i) {
463 double alpha = alphas[i];
465 throw_pretty(
"Invalid argument: " 466 <<
"alpha values has to be positive.");
468 if (alpha >= prev_alpha) {
469 throw_pretty(
"Invalid argument: " 470 <<
"alpha values are monotonously decreasing.");
478 if (0. >= th_stepdec || th_stepdec > 1.) {
479 throw_pretty(
"Invalid argument: " 480 <<
"th_stepdec value should between 0 and 1.");
486 if (0. >= th_stepinc || th_stepinc > 1.) {
487 throw_pretty(
"Invalid argument: " 488 <<
"th_stepinc value should between 0 and 1.");
495 throw_pretty(
"Invalid argument: " 496 <<
"th_grad value has to be positive.");
502 if (0. > th_gaptol) {
503 throw_pretty(
"Invalid argument: " 504 <<
"th_gaptol 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 .
double th_gaptol_
Threshold limit to check non-zero gaps.
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.
const double & get_th_gaptol() const
Return the threshold for accepting a gap as non-zero.
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 .
void set_th_gaptol(const double &th_gaptol)
Modify the threshold for accepting a gap as non-zero.
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.