crocoddyl
1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
|
|
11 #include "crocoddyl/core/solvers/ddp.hpp"
12 #include "crocoddyl/core/utils/exception.hpp"
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) {
44 START_PROFILER(
"SolverDDP::solve");
51 if (std::isnan(reginit)) {
60 bool recalcDiff =
true;
65 }
catch (std::exception& e) {
80 for (std::vector<double>::const_iterator it =
alphas_.begin(); it !=
alphas_.end(); ++it) {
85 }
catch (std::exception& e) {
107 STOP_PROFILER(
"SolverDDP::solve");
113 const std::size_t n_callbacks =
callbacks_.size();
114 for (std::size_t c = 0; c < n_callbacks; ++c) {
120 STOP_PROFILER(
"SolverDDP::solve");
124 STOP_PROFILER(
"SolverDDP::solve");
129 START_PROFILER(
"SolverDDP::computeDirection");
134 STOP_PROFILER(
"SolverDDP::computeDirection");
138 START_PROFILER(
"SolverDDP::tryStep");
140 STOP_PROFILER(
"SolverDDP::tryStep");
146 const std::size_t T = this->
problem_->get_T();
147 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
149 for (std::size_t t = 0; t < T; ++t) {
150 const std::size_t nu = models[t]->get_nu();
160 const std::size_t T = this->
problem_->get_T();
161 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
162 for (std::size_t t = 0; t < T; ++t) {
163 const std::size_t nu = models[t]->get_nu();
173 START_PROFILER(
"SolverDDP::resizeData");
176 const std::size_t T =
problem_->get_T();
177 const std::size_t ndx =
problem_->get_ndx();
178 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
179 for (std::size_t t = 0; t < T; ++t) {
180 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
181 const std::size_t nu = model->get_nu();
182 Qxu_[t].conservativeResize(ndx, nu);
183 Quu_[t].conservativeResize(nu, nu);
184 Qu_[t].conservativeResize(nu);
185 K_[t].conservativeResize(nu, ndx);
186 k_[t].conservativeResize(nu);
187 us_try_[t].conservativeResize(nu);
188 FuTVxx_p_[t].conservativeResize(nu, ndx);
189 Quuk_[t].conservativeResize(nu);
191 STOP_PROFILER(
"SolverDDP::resizeData");
195 START_PROFILER(
"SolverDDP::calcDiff");
202 STOP_PROFILER(
"SolverDDP::calcDiff");
207 START_PROFILER(
"SolverDDP::backwardPass");
208 const boost::shared_ptr<ActionDataAbstract>& d_T =
problem_->get_terminalData();
209 Vxx_.back() = d_T->Lxx;
210 Vx_.back() = d_T->Lx;
212 if (!std::isnan(
xreg_)) {
217 Vx_.back().noalias() +=
Vxx_.back() *
fs_.back();
219 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
220 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
221 for (
int t =
static_cast<int>(
problem_->get_T()) - 1; t >= 0; --t) {
222 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
223 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
224 const Eigen::MatrixXd& Vxx_p =
Vxx_[t + 1];
225 const Eigen::VectorXd& Vx_p =
Vx_[t + 1];
226 const std::size_t nu = m->get_nu();
228 FxTVxx_p_.noalias() = d->Fx.transpose() * Vxx_p;
229 START_PROFILER(
"SolverDDP::Qx");
231 Qx_[t].noalias() += d->Fx.transpose() * Vx_p;
232 STOP_PROFILER(
"SolverDDP::Qx");
233 START_PROFILER(
"SolverDDP::Qxx");
236 STOP_PROFILER(
"SolverDDP::Qxx");
238 FuTVxx_p_[t].noalias() = d->Fu.transpose() * Vxx_p;
239 START_PROFILER(
"SolverDDP::Qu");
241 Qu_[t].noalias() += d->Fu.transpose() * Vx_p;
242 STOP_PROFILER(
"SolverDDP::Qu");
243 START_PROFILER(
"SolverDDP::Quu");
246 STOP_PROFILER(
"SolverDDP::Quu");
247 START_PROFILER(
"SolverDDP::Qxu");
250 STOP_PROFILER(
"SolverDDP::Qxu");
251 if (!std::isnan(
ureg_)) {
262 Vx_[t].noalias() -=
K_[t].transpose() *
Qu_[t];
263 START_PROFILER(
"SolverDDP::Vxx");
265 STOP_PROFILER(
"SolverDDP::Vxx");
270 if (!std::isnan(
xreg_)) {
279 if (raiseIfNaN(
Vx_[t].lpNorm<Eigen::Infinity>())) {
280 throw_pretty(
"backward_error");
282 if (raiseIfNaN(
Vxx_[t].lpNorm<Eigen::Infinity>())) {
283 throw_pretty(
"backward_error");
286 STOP_PROFILER(
"SolverDDP::backwardPass");
290 if (steplength > 1. || steplength < 0.) {
291 throw_pretty(
"Invalid argument: "
292 <<
"invalid step length, value is between 0. to 1.");
294 START_PROFILER(
"SolverDDP::forwardPass");
296 const std::size_t T =
problem_->get_T();
297 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
298 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
299 for (std::size_t t = 0; t < T; ++t) {
300 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
301 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
304 if (m->get_nu() != 0) {
306 us_try_[t].noalias() -=
k_[t] * steplength;
316 STOP_PROFILER(
"SolverDDP::forwardPass");
317 throw_pretty(
"forward_error");
319 if (raiseIfNaN(
xs_try_[t + 1].lpNorm<Eigen::Infinity>())) {
320 STOP_PROFILER(
"SolverDDP::forwardPass");
321 throw_pretty(
"forward_error");
325 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
326 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
331 STOP_PROFILER(
"SolverDDP::forwardPass");
332 throw_pretty(
"forward_error");
334 STOP_PROFILER(
"SolverDDP::forwardPass");
338 START_PROFILER(
"SolverDDP::computeGains");
339 const std::size_t nu =
problem_->get_runningModels()[t]->get_nu();
341 START_PROFILER(
"SolverDDP::Quu_inv");
343 STOP_PROFILER(
"SolverDDP::Quu_inv");
344 const Eigen::ComputationInfo& info =
Quu_llt_[t].info();
345 if (info != Eigen::Success) {
346 STOP_PROFILER(
"SolverDDP::computeGains");
347 throw_pretty(
"backward_error");
349 K_[t] =
Qxu_[t].transpose();
351 START_PROFILER(
"SolverDDP::Quu_inv_Qux");
353 STOP_PROFILER(
"SolverDDP::Quu_inv_Qux");
357 STOP_PROFILER(
"SolverDDP::computeGains");
377 const std::size_t T =
problem_->get_T();
396 const std::size_t ndx =
problem_->get_ndx();
397 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
398 for (std::size_t t = 0; t < T; ++t) {
399 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
400 const std::size_t nu = model->get_nu();
401 Vxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
402 Vx_[t] = Eigen::VectorXd::Zero(ndx);
403 Qxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
404 Qxu_[t] = Eigen::MatrixXd::Zero(ndx, nu);
405 Quu_[t] = Eigen::MatrixXd::Zero(nu, nu);
406 Qx_[t] = Eigen::VectorXd::Zero(ndx);
407 Qu_[t] = Eigen::VectorXd::Zero(nu);
408 K_[t] = MatrixXdRowMajor::Zero(nu, ndx);
409 k_[t] = Eigen::VectorXd::Zero(nu);
414 xs_try_[t] = model->get_state()->zero();
416 us_try_[t] = Eigen::VectorXd::Zero(nu);
417 dx_[t] = Eigen::VectorXd::Zero(ndx);
419 FuTVxx_p_[t] = MatrixXdRowMajor::Zero(nu, ndx);
420 Quu_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(nu);
421 Quuk_[t] = Eigen::VectorXd(nu);
423 Vxx_.back() = Eigen::MatrixXd::Zero(ndx, ndx);
424 Vxx_tmp_ = Eigen::MatrixXd::Zero(ndx, ndx);
425 Vx_.back() = Eigen::VectorXd::Zero(ndx);
428 FxTVxx_p_ = MatrixXdRowMajor::Zero(ndx, ndx);
429 fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
436 double SolverDDP::get_regfactor()
const {
return reg_incfactor_; }
438 double SolverDDP::get_reg_min()
const {
return reg_min_; }
440 double SolverDDP::get_regmin()
const {
return reg_min_; }
444 double SolverDDP::get_regmax()
const {
return reg_max_; }
468 const std::vector<typename MathBaseTpl<double>::MatrixXsRowMajor>&
SolverDDP::get_K()
const {
return K_; }
473 if (regfactor <= 1.) {
474 throw_pretty(
"Invalid argument: "
475 <<
"reg_incfactor value is higher than 1.");
481 if (regfactor <= 1.) {
482 throw_pretty(
"Invalid argument: "
483 <<
"reg_decfactor value is higher than 1.");
488 void SolverDDP::set_regfactor(
const double regfactor) {
489 if (regfactor <= 1.) {
490 throw_pretty(
"Invalid argument: "
491 <<
"regfactor value is higher than 1.");
497 void SolverDDP::set_reg_min(
const double regmin) {
499 throw_pretty(
"Invalid argument: "
500 <<
"regmin value has to be positive.");
505 void SolverDDP::set_regmin(
const double regmin) {
507 throw_pretty(
"Invalid argument: "
508 <<
"regmin value has to be positive.");
515 throw_pretty(
"Invalid argument: "
516 <<
"regmax value has to be positive.");
521 void SolverDDP::set_regmax(
const double regmax) {
523 throw_pretty(
"Invalid argument: "
524 <<
"regmax value has to be positive.");
530 double prev_alpha = alphas[0];
531 if (prev_alpha != 1.) {
532 std::cerr <<
"Warning: alpha[0] should be 1" << std::endl;
534 for (std::size_t i = 1; i < alphas.size(); ++i) {
535 double alpha = alphas[i];
537 throw_pretty(
"Invalid argument: "
538 <<
"alpha values has to be positive.");
540 if (alpha >= prev_alpha) {
541 throw_pretty(
"Invalid argument: "
542 <<
"alpha values are monotonously decreasing.");
550 if (0. >= th_stepdec || th_stepdec > 1.) {
551 throw_pretty(
"Invalid argument: "
552 <<
"th_stepdec value should between 0 and 1.");
558 if (0. >= th_stepinc || th_stepinc > 1.) {
559 throw_pretty(
"Invalid argument: "
560 <<
"th_stepinc value should between 0 and 1.");
567 throw_pretty(
"Invalid argument: "
568 <<
"th_grad value has to be positive.");
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.
Eigen::VectorXd fTVxx_p_
Store the value of .
double th_acceptstep_
Threshold used for accepting step.
void set_th_stepdec(const double th_step)
Modify the step-length threshold used to decrease regularization.
void set_reg_incfactor(const double reg_factor)
Modify the regularization factor used to increase the damping value.
const std::vector< Eigen::VectorXd > & get_Qu() const
Return the Jacobian of the Hamiltonian function .
double th_stepinc_
Step-length threshold used to increase regularization.
Eigen::MatrixXd Vxx_tmp_
Temporary variable for ensuring symmetry of Vxx.
void set_reg_decfactor(const double reg_factor)
Modify the regularization factor used to decrease the damping value.
void set_alphas(const std::vector< double > &alphas)
Modify the set of step lengths using by the line-search procedure.
Abstract class for solver callbacks.
double get_reg_incfactor() const
Return the regularization factor used to increase the damping value.
Abstract class for optimal control solvers.
const std::vector< Eigen::MatrixXd > & get_Vxx() const
Return the Hessian of the Value function .
double computeDynamicFeasibility()
Compute the dynamic feasibility for the current guess .
virtual void forwardPass(const double stepLength)
Run the forward pass or rollout.
virtual double calcDiff()
Update the Jacobian, Hessian and feasibility of the optimal control problem.
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
MatrixXdRowMajor FxTVxx_p_
Store the value of .
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.
bool is_feasible_
Label that indicates is the iteration is feasible.
double get_th_stepinc() const
Return the step-length threshold used to increase regularization.
Eigen::Vector2d d_
LQ approximation of the expected improvement.
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.
std::vector< Eigen::VectorXd > dx_
State error during the roll-out/forward-pass (size T)
std::vector< Eigen::VectorXd > Quuk_
Store the values of.
virtual void resizeData()
Resizing the solver data.
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.
double ureg_
Current control regularization values.
double stop_
Value computed by stoppingCriteria()
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian .
std::vector< Eigen::MatrixXd > Qxu_
Hessian of the Hamiltonian .
std::vector< Eigen::VectorXd > Qx_
Gradient of the Hamiltonian .
std::vector< Eigen::MatrixXd > Quu_
Hessian of the Hamiltonian .
double steplength_
Current applied step-length.
double cost_try_
Total cost computed by line-search procedure.
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 trajectories .
double th_stop_
Tolerance for stopping the algorithm.
virtual void backwardPass()
Run the backward pass (Riccati sweep)
double reg_max_
Maximum allowed regularization value.
virtual void computeDirection(const bool recalc=true)
Compute the search direction for the current guess .
void set_th_stepinc(const double th_step)
Modify the step-length threshold used to increase regularization.
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 .
double ffeas_
Feasibility of the dynamic constraints.
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
SolverDDP(boost::shared_ptr< ShootingProblem > problem)
Initialize the DDP solver.
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.
double get_reg_max() const
Return the maximum regularization value.
void set_reg_max(const double regmax)
Modify the maximum regularization value.
std::vector< MatrixXdRowMajor > K_
Feedback gains .
void set_th_grad(const double th_grad)
Modify the tolerance of the expected gradient used for testing the step.
double xreg_
Current state regularization value.
std::vector< Eigen::LLT< Eigen::MatrixXd > > Quu_llt_
Cholesky LLT solver.
double dVexp_
Expected cost reduction.
const std::vector< Eigen::VectorXd > & get_Qx() const
Return the Jacobian of the Hamiltonian function .
double get_th_stepdec() const
Return the step-length threshold used to decrease regularization.
std::vector< Eigen::VectorXd > us_
Control trajectory.
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
virtual void resizeData()
Resizing the solver data.
std::vector< Eigen::VectorXd > xs_
State trajectory.
const std::vector< double > & get_alphas() const
Return the set of step lengths using by the line-search procedure.
double reg_min_
Minimum allowed regularization value.
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
virtual void allocateData()
Allocate all the internal data needed for the solver.
std::vector< MatrixXdRowMajor > FuTVxx_p_
Store the values of per each running node.
std::vector< Eigen::VectorXd > Vx_
Gradient of the Value function .
void increaseRegularization()
Increase the state and control regularization values by a regfactor_ factor.
const std::vector< Eigen::VectorXd > & get_k() const
Return the feedforward gains .
const std::vector< Eigen::VectorXd > & get_Vx() const
Return the Hessian of the Value function .
double dV_
Cost reduction obtained by tryStep()
const std::vector< Eigen::MatrixXd > & get_Quu() const
Return the Hessian of the Hamiltonian function .
void decreaseRegularization()
Decrease the state and control regularization values by a regfactor_ factor.
double get_th_grad() const
Return the tolerance of the expected gradient used for testing the step.
const std::vector< Eigen::MatrixXd > & get_Qxu() const
Return the Hessian of the Hamiltonian function .
std::vector< Eigen::MatrixXd > Qxx_
Hessian of the Hamiltonian .
double th_stepdec_
Step-length threshold used to decrease regularization.
const std::vector< Eigen::MatrixXd > & get_Qxx() const
Return the Hessian of the Hamiltonian function .
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function .
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
const std::vector< MatrixXdRowMajor > & get_K() const
Return the feedback gains .
bool was_feasible_
Label that indicates in the previous iterate was feasible.
double reg_incfactor_
Regularization factor used to increase the damping value.
std::vector< Eigen::VectorXd > k_
Feed-forward terms .