crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
box-fddp.cpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include <iostream>
10 
11 #include "crocoddyl/core/solvers/box-fddp.hpp"
12 #include "crocoddyl/core/utils/exception.hpp"
13 
14 namespace crocoddyl {
15 
16 SolverBoxFDDP::SolverBoxFDDP(boost::shared_ptr<ShootingProblem> problem)
17  : SolverFDDP(problem), qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) {
18  allocateData();
19 
20  const std::size_t n_alphas = 10;
21  alphas_.resize(n_alphas);
22  for (std::size_t n = 0; n < n_alphas; ++n) {
23  alphas_[n] = 1. / pow(2., static_cast<double>(n));
24  }
25  // Change the default convergence tolerance since the gradient of the Lagrangian is smaller
26  // than an unconstrained OC problem (i.e. gradient = Qu - mu^T * C where mu > 0 and C defines
27  // the inequality matrix that bounds the control); and we don't have access to mu from the
28  // box QP.
29  th_stop_ = 5e-5;
30 }
31 
32 SolverBoxFDDP::~SolverBoxFDDP() {}
33 
36 
37  const std::size_t T = problem_->get_T();
38  Quu_inv_.resize(T);
39  const std::size_t nu = problem_->get_nu_max();
40  for (std::size_t t = 0; t < T; ++t) {
41  Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
42  }
43  du_lb_.resize(nu);
44  du_ub_.resize(nu);
45 }
46 
47 void SolverBoxFDDP::computeGains(const std::size_t t) {
48  const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
49  if (nu > 0) {
50  if (!problem_->get_runningModels()[t]->get_has_control_limits() || !is_feasible_) {
51  // No control limits on this model: Use vanilla DDP
53  return;
54  }
55 
56  du_lb_.head(nu) = problem_->get_runningModels()[t]->get_u_lb() - us_[t].head(nu);
57  du_ub_.head(nu) = problem_->get_runningModels()[t]->get_u_ub() - us_[t].head(nu);
58 
59  const BoxQPSolution& boxqp_sol =
60  qp_.solve(Quu_[t].topLeftCorner(nu, nu), Qu_[t].head(nu), du_lb_.head(nu), du_ub_.head(nu), k_[t].head(nu));
61 
62  // Compute controls
63  Quu_inv_[t].topLeftCorner(nu, nu).setZero();
64  for (std::size_t i = 0; i < boxqp_sol.free_idx.size(); ++i) {
65  for (std::size_t j = 0; j < boxqp_sol.free_idx.size(); ++j) {
66  Quu_inv_[t](boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) = boxqp_sol.Hff_inv(i, j);
67  }
68  }
69  K_[t].topRows(nu).noalias() = Quu_inv_[t].topLeftCorner(nu, nu) * Qxu_[t].leftCols(nu).transpose();
70  k_[t].topRows(nu) = -boxqp_sol.x;
71 
72  // The box-QP clamped the gradient direction; this is important for accounting
73  // the algorithm advancement (i.e. stopping criteria)
74  for (std::size_t i = 0; i < boxqp_sol.clamped_idx.size(); ++i) {
75  Qu_[t].head(nu)(boxqp_sol.clamped_idx[i]) = 0.;
76  }
77  }
78 }
79 
80 void SolverBoxFDDP::forwardPass(const double steplength) {
81  if (steplength > 1. || steplength < 0.) {
82  throw_pretty("Invalid argument: "
83  << "invalid step length, value is between 0. to 1.");
84  }
85  cost_try_ = 0.;
86  xnext_ = problem_->get_x0();
87  const std::size_t T = problem_->get_T();
88  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
89  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
90  if ((is_feasible_) || (steplength == 1)) {
91  for (std::size_t t = 0; t < T; ++t) {
92  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
93  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
94  const std::size_t nu = m->get_nu();
95 
96  xs_try_[t] = xnext_;
97  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
98  if (nu != 0) {
99  us_try_[t].head(nu).noalias() = us_[t].head(nu) - k_[t].head(nu) * steplength - K_[t].topRows(nu) * dx_[t];
100  if (m->get_has_control_limits()) { // clamp control
101  us_try_[t].head(nu) = us_try_[t].head(nu).cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
102  }
103  m->calc(d, xs_try_[t], us_try_[t].head(nu));
104  } else {
105  m->calc(d, xs_try_[t]);
106  }
107  xnext_ = d->xnext;
108  cost_try_ += d->cost;
109 
110  if (raiseIfNaN(cost_try_)) {
111  throw_pretty("forward_error");
112  }
113  if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
114  throw_pretty("forward_error");
115  }
116  }
117 
118  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
119  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
120  xs_try_.back() = xnext_;
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  } else {
128  for (std::size_t t = 0; t < T; ++t) {
129  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
130  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
131  const std::size_t nu = m->get_nu();
132  m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]);
133  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
134  if (nu != 0) {
135  us_try_[t].head(nu).noalias() = us_[t].head(nu) - k_[t].head(nu) * steplength - K_[t].topRows(nu) * dx_[t];
136  if (m->get_has_control_limits()) { // clamp control
137  us_try_[t].head(nu) = us_try_[t].head(nu).cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
138  }
139  m->calc(d, xs_try_[t], us_try_[t].head(nu));
140  } else {
141  m->calc(d, xs_try_[t]);
142  }
143  xnext_ = d->xnext;
144  cost_try_ += d->cost;
145 
146  if (raiseIfNaN(cost_try_)) {
147  throw_pretty("forward_error");
148  }
149  if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
150  throw_pretty("forward_error");
151  }
152  }
153 
154  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
155  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
156  m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1), xs_try_.back());
157  m->calc(d, xs_try_.back());
158  cost_try_ += d->cost;
159 
160  if (raiseIfNaN(cost_try_)) {
161  throw_pretty("forward_error");
162  }
163  }
164 }
165 
166 const std::vector<Eigen::MatrixXd>& SolverBoxFDDP::get_Quu_inv() const { return Quu_inv_; }
167 
168 } // namespace crocoddyl
virtual void allocateData()
Allocate all the internal data needed for the solver.
Definition: box-ddp.cpp:34
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
Definition: ddp.hpp:332
bool is_feasible_
Label that indicates is the iteration is feasible.
virtual void forwardPass(const double steplength)
Run the forward pass or rollout.
Definition: box-fddp.cpp:80
virtual void computeGains(const std::size_t t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
Definition: box-fddp.cpp:47
Box QP solution.
Definition: box-qp.hpp:28
std::vector< MatrixXdRowMajor > K_
Feedback gains.
Definition: ddp.hpp:330
virtual void allocateData()
Allocate all the internal data needed for the solver.
Definition: ddp.cpp:385
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
Definition: ddp.hpp:317
const BoxQPSolution & solve(const Eigen::MatrixXd &H, const Eigen::VectorXd &q, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const Eigen::VectorXd &xinit)
Compute the solution of bound-constrained QP based on Newton projection.
Definition: box-qp.cpp:58
std::vector< Eigen::VectorXd > us_
Control trajectory.
virtual void allocateData()
Allocate all the internal data needed for the solver.
Definition: box-fddp.cpp:34
std::vector< Eigen::VectorXd > k_
Feed-forward terms.
Definition: ddp.hpp:331
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
Definition: box-qp.hpp:48
Eigen::VectorXd xnext_
Next state.
Definition: ddp.hpp:334
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
Eigen::VectorXd x
Decision vector.
Definition: box-qp.hpp:49
std::vector< Eigen::VectorXd > xs_
State trajectory.
virtual void computeGains(const std::size_t t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
Definition: ddp.cpp:344
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
Definition: ddp.hpp:340
double th_stop_
Tolerance for stopping the algorithm.
std::vector< size_t > clamped_idx
Clamped space indexes.
Definition: box-qp.hpp:51
std::vector< Eigen::MatrixXd > Qxu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:326
std::vector< size_t > free_idx
Free space indexes.
Definition: box-qp.hpp:50
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
Definition: ddp.hpp:318
std::vector< Eigen::MatrixXd > Quu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:327
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian.
Definition: ddp.hpp:329
double cost_try_
Total cost computed by line-search procedure.
Definition: ddp.hpp:316