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 const std::size_t& nu = models[t]->get_nu();
140 stop_ +=
Qu_[t].head(nu).squaredNorm();
148 const std::size_t& T = this->
problem_->get_T();
149 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
150 for (std::size_t t = 0; t < T; ++t) {
151 const std::size_t& nu = models[t]->get_nu();
153 d_[0] +=
Qu_[t].head(nu).dot(
k_[t].head(nu));
154 d_[1] -=
k_[t].head(nu).dot(
Quuk_[t].head(nu));
165 const Eigen::VectorXd& x0 =
problem_->get_x0();
166 problem_->get_runningModels()[0]->get_state()->diff(
xs_[0], x0,
fs_[0]);
167 bool could_be_feasible =
true;
169 could_be_feasible =
false;
171 const std::size_t& T =
problem_->get_T();
172 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
173 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
174 for (std::size_t t = 0; t < T; ++t) {
175 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
176 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
177 model->get_state()->diff(
xs_[t + 1], d->xnext,
fs_[t + 1]);
178 if (could_be_feasible) {
180 could_be_feasible =
false;
187 for (std::vector<Eigen::VectorXd>::iterator it =
fs_.begin(); it !=
fs_.end(); ++it) {
195 const boost::shared_ptr<ActionDataAbstract>& d_T =
problem_->get_terminalData();
196 Vxx_.back() = d_T->Lxx;
197 Vx_.back() = d_T->Lx;
199 if (!std::isnan(
xreg_)) {
204 Vx_.back().noalias() +=
Vxx_.back() *
fs_.back();
206 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
207 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
208 for (
int t = static_cast<int>(
problem_->get_T()) - 1; t >= 0; --t) {
209 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
210 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
211 const Eigen::MatrixXd& Vxx_p =
Vxx_[t + 1];
212 const Eigen::VectorXd& Vx_p =
Vx_[t + 1];
213 const std::size_t& nu = m->get_nu();
217 FxTVxx_p_.noalias() = d->Fx.transpose() * Vxx_p;
219 Qx_[t].noalias() += d->Fx.transpose() * Vx_p;
221 Qxu_[t].leftCols(nu) = d->Lxu;
222 Quu_[t].topLeftCorner(nu, nu) = d->Luu;
223 Qu_[t].head(nu) = d->Lu;
224 FuTVxx_p_[t].topRows(nu).noalias() = d->Fu.transpose() * Vxx_p;
226 Quu_[t].topLeftCorner(nu, nu).noalias() +=
FuTVxx_p_[t].topRows(nu) * d->Fu;
227 Qu_[t].head(nu).noalias() += d->Fu.transpose() * Vx_p;
229 if (!std::isnan(
ureg_)) {
230 Quu_[t].diagonal().head(nu).array() +=
ureg_;
239 if (std::isnan(
ureg_)) {
240 Vx_[t].noalias() -=
K_[t].topRows(nu).transpose() *
Qu_[t].head(nu);
242 Quuk_[t].head(nu).noalias() =
Quu_[t].topLeftCorner(nu, nu) *
k_[t].head(nu);
243 Vx_[t].noalias() +=
K_[t].topRows(nu).transpose() *
Quuk_[t].head(nu);
244 Vx_[t].noalias() -= 2 * (
K_[t].topRows(nu).transpose() *
Qu_[t].head(nu));
246 Vxx_[t].noalias() -=
Qxu_[t].leftCols(nu) *
K_[t].topRows(nu);
250 if (!std::isnan(
xreg_)) {
259 if (raiseIfNaN(
Vx_[t].lpNorm<Eigen::Infinity>())) {
260 throw_pretty(
"backward_error");
262 if (raiseIfNaN(
Vxx_[t].lpNorm<Eigen::Infinity>())) {
263 throw_pretty(
"backward_error");
269 if (steplength > 1. || steplength < 0.) {
270 throw_pretty(
"Invalid argument: " 271 <<
"invalid step length, value is between 0. to 1.");
274 const std::size_t& T =
problem_->get_T();
275 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
276 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
277 for (std::size_t t = 0; t < T; ++t) {
278 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
279 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
281 m->get_state()->diff(
xs_[t],
xs_try_[t], dx_[t]);
282 if (m->get_nu() != 0) {
283 const std::size_t& nu = m->get_nu();
285 us_try_[t].head(nu).noalias() =
us_[t].head(nu);
286 us_try_[t].head(nu).noalias() -=
k_[t].head(nu) * steplength;
287 us_try_[t].head(nu).noalias() -=
K_[t].topRows(nu) * dx_[t];
296 throw_pretty(
"forward_error");
298 if (raiseIfNaN(
xs_try_[t + 1].lpNorm<Eigen::Infinity>())) {
299 throw_pretty(
"forward_error");
303 const boost::shared_ptr<ActionModelAbstract>& m =
problem_->get_terminalModel();
304 const boost::shared_ptr<ActionDataAbstract>& d =
problem_->get_terminalData();
309 throw_pretty(
"forward_error");
314 const std::size_t& nu =
problem_->get_runningModels()[t]->get_nu();
317 const Eigen::ComputationInfo& info =
Quu_llt_[t].info();
318 if (info != Eigen::Success) {
319 throw_pretty(
"backward_error");
321 K_[t].topRows(nu).noalias() =
Qxu_[t].leftCols(nu).transpose();
323 Eigen::Block<Eigen::MatrixXd> K =
K_[t].topRows(nu);
325 k_[t].head(nu) =
Qu_[t].head(nu);
326 Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> k =
k_[t].head(nu);
348 const std::size_t& T =
problem_->get_T();
368 const std::size_t& ndx =
problem_->get_ndx();
369 const std::size_t& nu =
problem_->get_nu_max();
370 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
371 for (std::size_t t = 0; t < T; ++t) {
372 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
373 Vxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
374 Vx_[t] = Eigen::VectorXd::Zero(ndx);
375 Qxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
376 Qxu_[t] = Eigen::MatrixXd::Zero(ndx, nu);
377 Quu_[t] = Eigen::MatrixXd::Zero(nu, nu);
378 Qx_[t] = Eigen::VectorXd::Zero(ndx);
379 Qu_[t] = Eigen::VectorXd::Zero(nu);
380 K_[t] = Eigen::MatrixXd::Zero(nu, ndx);
381 k_[t] = Eigen::VectorXd::Zero(nu);
382 fs_[t] = Eigen::VectorXd::Zero(ndx);
387 xs_try_[t] = model->get_state()->zero();
389 us_try_[t] = Eigen::VectorXd::Zero(nu);
390 dx_[t] = Eigen::VectorXd::Zero(ndx);
392 FuTVxx_p_[t] = Eigen::MatrixXd::Zero(nu, ndx);
393 Quu_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(model->get_nu());
394 Quuk_[t] = Eigen::VectorXd(nu);
396 Vxx_.back() = Eigen::MatrixXd::Zero(ndx, ndx);
397 Vx_.back() = Eigen::VectorXd::Zero(ndx);
399 fs_.back() = Eigen::VectorXd::Zero(ndx);
401 FxTVxx_p_ = Eigen::MatrixXd::Zero(ndx, ndx);
402 fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
442 if (regfactor <= 1.) {
443 throw_pretty(
"Invalid argument: " 444 <<
"regfactor value is higher than 1.");
451 throw_pretty(
"Invalid argument: " 452 <<
"regmin value has to be positive.");
459 throw_pretty(
"Invalid argument: " 460 <<
"regmax value has to be positive.");
466 double prev_alpha = alphas[0];
467 if (prev_alpha != 1.) {
468 std::cerr <<
"Warning: alpha[0] should be 1" << std::endl;
470 for (std::size_t i = 1; i < alphas.size(); ++i) {
471 double alpha = alphas[i];
473 throw_pretty(
"Invalid argument: " 474 <<
"alpha values has to be positive.");
476 if (alpha >= prev_alpha) {
477 throw_pretty(
"Invalid argument: " 478 <<
"alpha values are monotonously decreasing.");
486 if (0. >= th_stepdec || th_stepdec > 1.) {
487 throw_pretty(
"Invalid argument: " 488 <<
"th_stepdec value should between 0 and 1.");
494 if (0. >= th_stepinc || th_stepinc > 1.) {
495 throw_pretty(
"Invalid argument: " 496 <<
"th_stepinc value should between 0 and 1.");
503 throw_pretty(
"Invalid argument: " 504 <<
"th_grad value has to be positive.");
510 if (0. > th_gaptol) {
511 throw_pretty(
"Invalid argument: " 512 <<
"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.