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