crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
box-fddp.cpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, 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-fddp.hpp"
12 
13 namespace crocoddyl {
14 
15 SolverBoxFDDP::SolverBoxFDDP(boost::shared_ptr<ShootingProblem> problem)
16  : SolverFDDP(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 SolverBoxFDDP::~SolverBoxFDDP() {}
32 
35 
36  const std::size_t& T = problem_->get_T();
37  Quu_inv_.resize(T);
38  const std::size_t& nu = problem_->get_nu_max();
39  for (std::size_t t = 0; t < T; ++t) {
40  Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
41  }
42  du_lb_.resize(nu);
43  du_ub_.resize(nu);
44 }
45 
46 void SolverBoxFDDP::computeGains(const std::size_t& t) {
47  if (problem_->get_runningModels()[t]->get_nu() > 0) {
48  if (!problem_->get_runningModels()[t]->get_has_control_limits() || !is_feasible_) {
49  // No control limits on this model: Use vanilla DDP
51  return;
52  }
53 
54  du_lb_ = problem_->get_runningModels()[t]->get_u_lb() - us_[t];
55  du_ub_ = problem_->get_runningModels()[t]->get_u_ub() - us_[t];
56 
57  const BoxQPSolution& boxqp_sol = qp_.solve(Quu_[t], Qu_[t], du_lb_, du_ub_, k_[t]);
58 
59  // Compute controls
60  Quu_inv_[t].setZero();
61  for (std::size_t i = 0; i < boxqp_sol.free_idx.size(); ++i) {
62  for (std::size_t j = 0; j < boxqp_sol.free_idx.size(); ++j) {
63  Quu_inv_[t](boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) = boxqp_sol.Hff_inv(i, j);
64  }
65  }
66  K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose();
67  k_[t].noalias() = -boxqp_sol.x;
68 
69  // The box-QP clamped the gradient direction; this is important for accounting
70  // the algorithm advancement (i.e. stopping criteria)
71  for (std::size_t i = 0; i < boxqp_sol.clamped_idx.size(); ++i) {
72  Qu_[t](boxqp_sol.clamped_idx[i]) = 0.;
73  }
74  }
75 }
76 
77 void SolverBoxFDDP::forwardPass(const double& steplength) {
78  if (steplength > 1. || steplength < 0.) {
79  throw_pretty("Invalid argument: "
80  << "invalid step length, value is between 0. to 1.");
81  }
82  cost_try_ = 0.;
83  xnext_ = problem_->get_x0();
84  const std::size_t& T = problem_->get_T();
85  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
86  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
87  if ((is_feasible_) || (steplength == 1)) {
88  for (std::size_t t = 0; t < T; ++t) {
89  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
90  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
91 
92  xs_try_[t] = xnext_;
93  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
94  if (m->get_nu() != 0) {
95  us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
96  if (m->get_has_control_limits()) { // clamp control
97  us_try_[t] = us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
98  }
99  m->calc(d, xs_try_[t], us_try_[t]);
100  } else {
101  m->calc(d, xs_try_[t]);
102  }
103  xnext_ = d->xnext;
104  cost_try_ += d->cost;
105 
106  if (raiseIfNaN(cost_try_)) {
107  throw_pretty("forward_error");
108  }
109  if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
110  throw_pretty("forward_error");
111  }
112  }
113 
114  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
115  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
116  xs_try_.back() = xnext_;
117  m->calc(d, xs_try_.back());
118  cost_try_ += d->cost;
119 
120  if (raiseIfNaN(cost_try_)) {
121  throw_pretty("forward_error");
122  }
123  } else {
124  for (std::size_t t = 0; t < T; ++t) {
125  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
126  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
127 
128  m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]);
129  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
130  if (m->get_nu() != 0) {
131  us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
132  if (m->get_has_control_limits()) { // clamp control
133  us_try_[t] = us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
134  }
135  m->calc(d, xs_try_[t], us_try_[t]);
136  } else {
137  m->calc(d, xs_try_[t]);
138  }
139  xnext_ = d->xnext;
140  cost_try_ += d->cost;
141 
142  if (raiseIfNaN(cost_try_)) {
143  throw_pretty("forward_error");
144  }
145  if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
146  throw_pretty("forward_error");
147  }
148  }
149 
150  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
151  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
152  m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1), xs_try_.back());
153  m->calc(d, xs_try_.back());
154  cost_try_ += d->cost;
155 
156  if (raiseIfNaN(cost_try_)) {
157  throw_pretty("forward_error");
158  }
159  }
160 }
161 
162 const std::vector<Eigen::MatrixXd>& SolverBoxFDDP::get_Quu_inv() const { return Quu_inv_; }
163 
164 } // namespace crocoddyl
virtual void allocateData()
Allocate all the internal data needed for the solver.
Definition: box-ddp.cpp:33
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
Definition: ddp.hpp:293
bool is_feasible_
Label that indicates is the iteration is feasible.
Box QP solution.
Definition: box-qp.hpp:29
virtual void allocateData()
Allocate all the internal data needed for the solver.
Definition: ddp.cpp:328
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
Definition: ddp.hpp:279
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:51
std::vector< Eigen::VectorXd > us_
Control trajectory.
virtual void allocateData()
Allocate all the internal data needed for the solver.
Definition: box-fddp.cpp:33
std::vector< Eigen::VectorXd > k_
Feed-forward terms.
Definition: ddp.hpp:292
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
Definition: box-qp.hpp:49
Eigen::VectorXd xnext_
Next state.
Definition: ddp.hpp:295
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
Eigen::VectorXd x
Decision vector.
Definition: box-qp.hpp:50
std::vector< Eigen::MatrixXd > K_
Feedback gains.
Definition: ddp.hpp:291
std::vector< Eigen::VectorXd > xs_
State trajectory.
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
Definition: ddp.hpp:301
double th_stop_
Tolerance for stopping the algorithm.
virtual void computeGains(const std::size_t &t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
Definition: ddp.cpp:298
std::vector< size_t > clamped_idx
Clamped space indexes.
Definition: box-qp.hpp:52
std::vector< Eigen::MatrixXd > Qxu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:287
virtual void forwardPass(const double &steplength)
Run the forward pass or rollout.
Definition: box-fddp.cpp:77
std::vector< size_t > free_idx
Free space indexes.
Definition: box-qp.hpp:51
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
Definition: ddp.hpp:280
std::vector< Eigen::MatrixXd > Quu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:288
virtual void computeGains(const std::size_t &t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
Definition: box-fddp.cpp:46
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian.
Definition: ddp.hpp:290
double cost_try_
Total cost computed by line-search procedure.
Definition: ddp.hpp:278