Crocoddyl
box-ddp.cpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, CNRS-LAAS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "crocoddyl/core/solvers/box-ddp.hpp"
10 
11 #include <iostream>
12 
13 #include "crocoddyl/core/utils/exception.hpp"
14 
15 namespace crocoddyl {
16 
17 SolverBoxDDP::SolverBoxDDP(boost::shared_ptr<ShootingProblem> problem)
18  : SolverDDP(problem),
19  qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) {
20  allocateData();
21 
22  const std::size_t n_alphas = 10;
23  alphas_.resize(n_alphas);
24  for (std::size_t n = 0; n < n_alphas; ++n) {
25  alphas_[n] = 1. / pow(2., static_cast<double>(n));
26  }
27  // Change the default convergence tolerance since the gradient of the
28  // Lagrangian is smaller than an unconstrained OC problem (i.e. gradient = Qu
29  // - mu^T * C where mu > 0 and C defines the inequality matrix that bounds the
30  // control); and we don't have access to mu from the box QP.
31  th_stop_ = 5e-5;
32 }
33 
34 SolverBoxDDP::~SolverBoxDDP() {}
35 
36 void SolverBoxDDP::resizeData() {
37  START_PROFILER("SolverBoxDDP::resizeData");
38  SolverDDP::resizeData();
39 
40  const std::size_t T = problem_->get_T();
41  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
42  problem_->get_runningModels();
43  for (std::size_t t = 0; t < T; ++t) {
44  const boost::shared_ptr<ActionModelAbstract>& model = models[t];
45  const std::size_t nu = model->get_nu();
46  Quu_inv_[t].conservativeResize(nu, nu);
47  du_lb_[t].conservativeResize(nu);
48  du_ub_[t].conservativeResize(nu);
49  }
50  STOP_PROFILER("SolverBoxDDP::resizeData");
51 }
52 
53 void SolverBoxDDP::allocateData() {
54  SolverDDP::allocateData();
55 
56  const std::size_t T = problem_->get_T();
57  Quu_inv_.resize(T);
58  du_lb_.resize(T);
59  du_ub_.resize(T);
60  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
61  problem_->get_runningModels();
62  for (std::size_t t = 0; t < T; ++t) {
63  const boost::shared_ptr<ActionModelAbstract>& model = models[t];
64  const std::size_t nu = model->get_nu();
65  Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
66  du_lb_[t] = Eigen::VectorXd::Zero(nu);
67  du_ub_[t] = Eigen::VectorXd::Zero(nu);
68  }
69 }
70 
71 void SolverBoxDDP::computeGains(const std::size_t t) {
72  START_PROFILER("SolverBoxDDP::computeGains");
73  const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
74  if (nu > 0) {
75  if (!problem_->get_runningModels()[t]->get_has_control_limits() ||
76  !is_feasible_) {
77  // No control limits on this model: Use vanilla DDP
78  SolverDDP::computeGains(t);
79  return;
80  }
81 
82  du_lb_[t] = problem_->get_runningModels()[t]->get_u_lb() - us_[t];
83  du_ub_[t] = problem_->get_runningModels()[t]->get_u_ub() - us_[t];
84 
85  START_PROFILER("SolverBoxDDP::boxQP");
86  const BoxQPSolution& boxqp_sol =
87  qp_.solve(Quu_[t], Qu_[t], du_lb_[t], du_ub_[t], k_[t]);
88  START_PROFILER("SolverBoxDDP::boxQP");
89 
90  // Compute controls
91  START_PROFILER("SolverBoxDDP::Quu_invproj");
92  Quu_inv_[t].setZero();
93  for (std::size_t i = 0; i < boxqp_sol.free_idx.size(); ++i) {
94  for (std::size_t j = 0; j < boxqp_sol.free_idx.size(); ++j) {
95  Quu_inv_[t](boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) =
96  boxqp_sol.Hff_inv(i, j);
97  }
98  }
99  STOP_PROFILER("SolverBoxDDP::Quu_invproj");
100  START_PROFILER("SolverBoxDDP::Quu_invproj_Qxu");
101  K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose();
102  STOP_PROFILER("SolverBoxDDP::Quu_invproj_Qxu");
103  k_[t] = -boxqp_sol.x;
104 
105  // The box-QP clamped the gradient direction; this is important for
106  // accounting the algorithm advancement (i.e. stopping criteria)
107  START_PROFILER("SolverBoxDDP::Qu_proj");
108  for (std::size_t i = 0; i < boxqp_sol.clamped_idx.size(); ++i) {
109  Qu_[t](boxqp_sol.clamped_idx[i]) = 0.;
110  }
111  STOP_PROFILER("SolverBoxDDP::Qu_proj");
112  }
113  STOP_PROFILER("SolverBoxDDP::computeGains");
114 }
115 
116 void SolverBoxDDP::forwardPass(double steplength) {
117  if (steplength > 1. || steplength < 0.) {
118  throw_pretty("Invalid argument: "
119  << "invalid step length, value is between 0. to 1.");
120  }
121  START_PROFILER("SolverBoxDDP::forwardPass");
122  cost_try_ = 0.;
123  xnext_ = problem_->get_x0();
124  const std::size_t T = problem_->get_T();
125  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
126  problem_->get_runningModels();
127  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
128  problem_->get_runningDatas();
129  for (std::size_t t = 0; t < T; ++t) {
130  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
131  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
132  const std::size_t nu = m->get_nu();
133 
134  xs_try_[t] = xnext_;
135  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
136  if (nu != 0) {
137  us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
138  if (m->get_has_control_limits()) { // clamp control
139  us_try_[t] = us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
140  }
141  m->calc(d, xs_try_[t], us_try_[t]);
142  } else {
143  m->calc(d, xs_try_[t]);
144  }
145  xnext_ = d->xnext;
146  cost_try_ += d->cost;
147 
148  if (raiseIfNaN(cost_try_)) {
149  STOP_PROFILER("SolverBoxDDP::forwardPass");
150  throw_pretty("forward_error");
151  }
152  if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
153  STOP_PROFILER("SolverBoxDDP::forwardPass");
154  throw_pretty("forward_error");
155  }
156  }
157 
158  const boost::shared_ptr<ActionModelAbstract>& m =
159  problem_->get_terminalModel();
160  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
161  if ((is_feasible_) || (steplength == 1)) {
162  xs_try_.back() = xnext_;
163  } else {
164  m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1),
165  xs_try_.back());
166  }
167  m->calc(d, xs_try_.back());
168  cost_try_ += d->cost;
169 
170  if (raiseIfNaN(cost_try_)) {
171  STOP_PROFILER("SolverBoxDDP::forwardPass");
172  throw_pretty("forward_error");
173  }
174 }
175 
176 const std::vector<Eigen::MatrixXd>& SolverBoxDDP::get_Quu_inv() const {
177  return Quu_inv_;
178 }
179 
180 } // namespace crocoddyl