10 #ifdef CROCODDYL_WITH_MULTITHREADING 12 #endif // CROCODDYL_WITH_MULTITHREADING 14 #include "crocoddyl/core/solvers/ddp.hpp" 15 #include "crocoddyl/core/utils/exception.hpp" 16 #include "crocoddyl/core/utils/stop-watch.hpp" 31 was_feasible_(false) {
34 const std::size_t n_alphas = 10;
36 for (std::size_t n = 0; n < n_alphas; ++n) {
37 alphas_[n] = 1. / pow(2., static_cast<double>(n));
41 std::cerr <<
"Warning: th_stepinc has higher value than lowest alpha value, set to " 42 << std::to_string(
alphas_[n_alphas - 1]) << std::endl;
46 SolverDDP::~SolverDDP() {}
48 bool SolverDDP::solve(
const std::vector<Eigen::VectorXd>& init_xs,
const std::vector<Eigen::VectorXd>& init_us,
49 const std::size_t maxiter,
const bool is_feasible,
const double reginit) {
53 if (std::isnan(reginit)) {
62 bool recalcDiff =
true;
67 }
catch (std::exception& e) {
82 for (std::vector<double>::const_iterator it =
alphas_.begin(); it !=
alphas_.end(); ++it) {
87 }
catch (std::exception& e) {
114 const std::size_t n_callbacks =
callbacks_.size();
115 for (std::size_t c = 0; c < n_callbacks; ++c) {
128 START_PROFILER(
"SolverDDP::computeDirection");
133 STOP_PROFILER(
"SolverDDP::computeDirection");
143 const std::size_t T = this->
problem_->get_T();
144 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
146 for (std::size_t t = 0; t < T; ++t) {
147 const std::size_t nu = models[t]->get_nu();
149 stop_ +=
Qu_[t].head(nu).squaredNorm();
157 const std::size_t T = this->
problem_->get_T();
158 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
159 for (std::size_t t = 0; t < T; ++t) {
160 const std::size_t nu = models[t]->get_nu();
162 d_[0] +=
Qu_[t].head(nu).dot(
k_[t].head(nu));
163 d_[1] -=
k_[t].head(nu).dot(
Quuk_[t].head(nu));
170 START_PROFILER(
"SolverDDP::calcDiff");
175 const Eigen::VectorXd& x0 =
problem_->get_x0();
176 problem_->get_runningModels()[0]->get_state()->diff(
xs_[0], x0,
fs_[0]);
177 bool could_be_feasible =
true;
179 could_be_feasible =
false;
181 const std::size_t T =
problem_->get_T();
182 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
183 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
184 #ifdef CROCODDYL_WITH_MULTITHREADING 185 #pragma omp parallel for num_threads(problem_->get_nthreads()) 187 for (std::size_t t = 0; t < T; ++t) {
188 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
189 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
190 model->get_state()->diff(
xs_[t + 1], d->xnext,
fs_[t + 1]);
193 if (could_be_feasible) {
194 for (std::size_t t = 0; t < T; ++t) {
196 could_be_feasible =
false;
204 for (std::vector<Eigen::VectorXd>::iterator it =
fs_.begin(); it !=
fs_.end(); ++it) {
208 STOP_PROFILER(
"SolverDDP::calcDiff");
213 START_PROFILER(
"SolverDDP::backwardPass");
214 const boost::shared_ptr<ActionDataAbstract>& d_T =
problem_->get_terminalData();
215 Vxx_.back() = d_T->Lxx;
216 Vx_.back() = d_T->Lx;
218 if (!std::isnan(
xreg_)) {
223 Vx_.back().noalias() +=
Vxx_.back() *
fs_.back();
225 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
226 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
227 for (
int t = static_cast<int>(
problem_->get_T()) - 1; t >= 0; --t) {
228 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
229 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
230 const Eigen::MatrixXd& Vxx_p =
Vxx_[t + 1];
231 const Eigen::VectorXd& Vx_p =
Vx_[t + 1];
232 const std::size_t nu = m->get_nu();
236 START_PROFILER(
"SolverDDP::Qxx");
237 FxTVxx_p_.noalias() = d->Fx.transpose() * Vxx_p;
239 STOP_PROFILER(
"SolverDDP::Qxx");
240 Qx_[t].noalias() += d->Fx.transpose() * Vx_p;
242 Qxu_[t].leftCols(nu) = d->Lxu;
243 Quu_[t].topLeftCorner(nu, nu) = d->Luu;
244 Qu_[t].head(nu) = d->Lu;
245 START_PROFILER(
"SolverDDP::Qxu");
247 STOP_PROFILER(
"SolverDDP::Qxu");
248 START_PROFILER(
"SolverDDP::Quu");
249 FuTVxx_p_[t].topRows(nu).noalias() = d->Fu.transpose() * Vxx_p;
250 Quu_[t].topLeftCorner(nu, nu).noalias() +=
FuTVxx_p_[t].topRows(nu) * d->Fu;
251 STOP_PROFILER(
"SolverDDP::Quu");
252 Qu_[t].head(nu).noalias() += d->Fu.transpose() * Vx_p;
254 if (!std::isnan(
ureg_)) {
255 Quu_[t].diagonal().head(nu).array() +=
ureg_;
264 if (std::isnan(
ureg_)) {
265 Vx_[t].noalias() -=
K_[t].topRows(nu).transpose() *
Qu_[t].head(nu);
267 Quuk_[t].head(nu).noalias() =
Quu_[t].topLeftCorner(nu, nu) *
k_[t].head(nu);
268 Vx_[t].noalias() +=
K_[t].topRows(nu).transpose() *
Quuk_[t].head(nu);
269 Vx_[t].noalias() -= 2 * (
K_[t].topRows(nu).transpose() *
Qu_[t].head(nu));
271 START_PROFILER(
"SolverDDP::Vxx");
272 Vxx_[t].noalias() -=
Qxu_[t].leftCols(nu) *
K_[t].topRows(nu);
273 STOP_PROFILER(
"SolverDDP::Vxx");
278 if (!std::isnan(
xreg_)) {
287 if (raiseIfNaN(
Vx_[t].lpNorm<Eigen::Infinity>())) {
288 throw_pretty(
"backward_error");
290 if (raiseIfNaN(
Vxx_[t].lpNorm<Eigen::Infinity>())) {
291 throw_pretty(
"backward_error");
294 STOP_PROFILER(
"SolverDDP::backwardPass");
298 START_PROFILER(
"SolverDDP::forwardPass");
299 if (steplength > 1. || steplength < 0.) {
300 throw_pretty(
"Invalid argument: " 301 <<
"invalid step length, value is between 0. to 1.");
304 const std::size_t T =
problem_->get_T();
305 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
306 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
307 for (std::size_t t = 0; t < T; ++t) {
308 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
309 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
311 m->get_state()->diff(
xs_[t],
xs_try_[t], dx_[t]);
312 if (m->get_nu() != 0) {
313 const std::size_t nu = m->get_nu();
315 us_try_[t].head(nu).noalias() =
us_[t].head(nu);
316 us_try_[t].head(nu).noalias() -=
k_[t].head(nu) * steplength;
317 us_try_[t].head(nu).noalias() -=
K_[t].topRows(nu) * dx_[t];
326 throw_pretty(
"forward_error");
328 if (raiseIfNaN(
xs_try_[t + 1].lpNorm<Eigen::Infinity>())) {
329 throw_pretty(
"forward_error");
333 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
334 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
339 throw_pretty(
"forward_error");
341 STOP_PROFILER(
"SolverDDP::forwardPass");
345 START_PROFILER(
"SolverDDP::computeGains");
346 const std::size_t nu =
problem_->get_runningModels()[t]->get_nu();
348 START_PROFILER(
"SolverDDP::Quu_inv");
350 STOP_PROFILER(
"SolverDDP::Quu_inv");
351 const Eigen::ComputationInfo& info =
Quu_llt_[t].info();
352 if (info != Eigen::Success) {
353 throw_pretty(
"backward_error");
355 K_[t].topRows(nu) =
Qxu_[t].leftCols(nu).transpose();
357 Eigen::Block<MatrixXdRowMajor, Eigen::Dynamic, Eigen::internal::traits<MatrixXdRowMajor>::ColsAtCompileTime,
true>
358 K =
K_[t].topRows(nu);
359 START_PROFILER(
"SolverDDP::Quu_inv_Qux");
361 STOP_PROFILER(
"SolverDDP::Quu_inv_Qux");
362 k_[t].head(nu) =
Qu_[t].head(nu);
363 Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> k =
k_[t].head(nu);
366 STOP_PROFILER(
"SolverDDP::computeGains");
386 const std::size_t T =
problem_->get_T();
406 const std::size_t ndx =
problem_->get_ndx();
407 const std::size_t nu =
problem_->get_nu_max();
408 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
409 for (std::size_t t = 0; t < T; ++t) {
410 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
411 Vxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
412 Vx_[t] = Eigen::VectorXd::Zero(ndx);
413 Qxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
414 Qxu_[t] = Eigen::MatrixXd::Zero(ndx, nu);
415 Quu_[t] = Eigen::MatrixXd::Zero(nu, nu);
416 Qx_[t] = Eigen::VectorXd::Zero(ndx);
417 Qu_[t] = Eigen::VectorXd::Zero(nu);
418 K_[t] = MatrixXdRowMajor::Zero(nu, ndx);
419 k_[t] = Eigen::VectorXd::Zero(nu);
420 fs_[t] = Eigen::VectorXd::Zero(ndx);
425 xs_try_[t] = model->get_state()->zero();
427 us_try_[t] = Eigen::VectorXd::Zero(nu);
428 dx_[t] = Eigen::VectorXd::Zero(ndx);
430 FuTVxx_p_[t] = MatrixXdRowMajor::Zero(nu, ndx);
431 Quu_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(model->get_nu());
432 Quuk_[t] = Eigen::VectorXd(nu);
434 Vxx_.back() = Eigen::MatrixXd::Zero(ndx, ndx);
435 Vxx_tmp_ = Eigen::MatrixXd::Zero(ndx, ndx);
436 Vx_.back() = Eigen::VectorXd::Zero(ndx);
438 fs_.back() = Eigen::VectorXd::Zero(ndx);
440 FxTVxx_p_ = MatrixXdRowMajor::Zero(ndx, ndx);
441 fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
448 double SolverDDP::get_regfactor()
const {
return reg_incfactor_; }
450 double SolverDDP::get_reg_min()
const {
return reg_min_; }
452 double SolverDDP::get_regmin()
const {
return reg_min_; }
456 double SolverDDP::get_regmax()
const {
return reg_max_; }
482 const std::vector<typename MathBaseTpl<double>::MatrixXsRowMajor>&
SolverDDP::get_K()
const {
return K_; }
489 if (regfactor <= 1.) {
490 throw_pretty(
"Invalid argument: " 491 <<
"reg_incfactor value is higher than 1.");
497 if (regfactor <= 1.) {
498 throw_pretty(
"Invalid argument: " 499 <<
"reg_decfactor value is higher than 1.");
504 void SolverDDP::set_regfactor(
const double regfactor) {
505 if (regfactor <= 1.) {
506 throw_pretty(
"Invalid argument: " 507 <<
"regfactor value is higher than 1.");
513 void SolverDDP::set_reg_min(
const double regmin) {
515 throw_pretty(
"Invalid argument: " 516 <<
"regmin value has to be positive.");
521 void SolverDDP::set_regmin(
const double regmin) {
523 throw_pretty(
"Invalid argument: " 524 <<
"regmin value has to be positive.");
531 throw_pretty(
"Invalid argument: " 532 <<
"regmax value has to be positive.");
537 void SolverDDP::set_regmax(
const double regmax) {
539 throw_pretty(
"Invalid argument: " 540 <<
"regmax value has to be positive.");
546 double prev_alpha = alphas[0];
547 if (prev_alpha != 1.) {
548 std::cerr <<
"Warning: alpha[0] should be 1" << std::endl;
550 for (std::size_t i = 1; i < alphas.size(); ++i) {
551 double alpha = alphas[i];
553 throw_pretty(
"Invalid argument: " 554 <<
"alpha values has to be positive.");
556 if (alpha >= prev_alpha) {
557 throw_pretty(
"Invalid argument: " 558 <<
"alpha values are monotonously decreasing.");
566 if (0. >= th_stepdec || th_stepdec > 1.) {
567 throw_pretty(
"Invalid argument: " 568 <<
"th_stepdec value should between 0 and 1.");
574 if (0. >= th_stepinc || th_stepinc > 1.) {
575 throw_pretty(
"Invalid argument: " 576 <<
"th_stepinc value should between 0 and 1.");
583 throw_pretty(
"Invalid argument: " 584 <<
"th_grad value has to be positive.");
590 if (0. > th_gaptol) {
591 throw_pretty(
"Invalid argument: " 592 <<
"th_gaptol value has to be positive.");
double reg_max_
Maximum allowed regularization value.
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
bool is_feasible_
Label that indicates is the iteration is feasible.
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 ureg_
Current control regularization values.
double get_th_stepdec() const
Return the step-length threshold used to decrease regularization.
double steplength_
Current applied step-length.
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 .
std::vector< MatrixXdRowMajor > K_
Feedback gains.
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 .
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< MatrixXdRowMajor > & get_K() const
Return the feedback gains .
const std::vector< Eigen::MatrixXd > & get_Quu() const
Return the Hessian of the Hamiltonian function .
double th_acceptstep_
Threshold used for accepting step.
double reg_min_
Minimum allowed regularization value.
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.
double get_th_grad() const
Return the tolerance of the expected gradient used for testing the step.
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
double reg_incfactor_
Regularization factor used to increase the damping value.
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
std::vector< Eigen::MatrixXd > Qxx_
Hessian of the Hamiltonian.
double dVexp_
Expected cost reduction.
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 .
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.
std::vector< Eigen::VectorXd > xs_
State trajectory.
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.
double th_stop_
Tolerance for stopping the algorithm.
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.
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.
Abstract class for solver callbacks.
std::size_t iter_
Number of iteration performed by the solver.
std::vector< Eigen::MatrixXd > Qxu_
Hessian of the Hamiltonian.
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_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.