crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
box-fddp.cpp
1
2// 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
14namespace crocoddyl {
15
16SolverBoxFDDP::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
32SolverBoxFDDP::~SolverBoxFDDP() {}
33
34void SolverBoxFDDP::resizeData() {
35 START_PROFILER("SolverBoxFDDP::resizeData");
36 SolverFDDP::resizeData();
37
38 const std::size_t T = problem_->get_T();
39 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
40 for (std::size_t t = 0; t < T; ++t) {
41 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
42 const std::size_t nu = model->get_nu();
43 Quu_inv_[t].conservativeResize(nu, nu);
44 du_lb_[t].conservativeResize(nu);
45 du_ub_[t].conservativeResize(nu);
46 }
47 STOP_PROFILER("SolverBoxFDDP::resizeData");
48}
49
50void SolverBoxFDDP::allocateData() {
51 SolverFDDP::allocateData();
52
53 const std::size_t T = problem_->get_T();
54 Quu_inv_.resize(T);
55 du_lb_.resize(T);
56 du_ub_.resize(T);
57 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
58 for (std::size_t t = 0; t < T; ++t) {
59 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
60 const std::size_t nu = model->get_nu();
61 Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
62 du_lb_[t] = Eigen::VectorXd::Zero(nu);
63 du_ub_[t] = Eigen::VectorXd::Zero(nu);
64 }
65}
66
67void SolverBoxFDDP::computeGains(const std::size_t t) {
68 const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
69 if (nu > 0) {
70 if (!problem_->get_runningModels()[t]->get_has_control_limits() || !is_feasible_) {
71 // No control limits on this model: Use vanilla DDP
72 SolverFDDP::computeGains(t);
73 return;
74 }
75
76 du_lb_[t] = problem_->get_runningModels()[t]->get_u_lb() - us_[t];
77 du_ub_[t] = problem_->get_runningModels()[t]->get_u_ub() - us_[t];
78
79 const BoxQPSolution& boxqp_sol = qp_.solve(Quu_[t], Qu_[t], du_lb_[t], du_ub_[t], k_[t]);
80
81 // Compute controls
82 Quu_inv_[t].setZero();
83 for (std::size_t i = 0; i < boxqp_sol.free_idx.size(); ++i) {
84 for (std::size_t j = 0; j < boxqp_sol.free_idx.size(); ++j) {
85 Quu_inv_[t](boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) = boxqp_sol.Hff_inv(i, j);
86 }
87 }
88 K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose();
89 k_[t] = -boxqp_sol.x;
90
91 // The box-QP clamped the gradient direction; this is important for accounting
92 // the algorithm advancement (i.e. stopping criteria)
93 for (std::size_t i = 0; i < boxqp_sol.clamped_idx.size(); ++i) {
94 Qu_[t](boxqp_sol.clamped_idx[i]) = 0.;
95 }
96 }
97}
98
99void SolverBoxFDDP::forwardPass(const double steplength) {
100 if (steplength > 1. || steplength < 0.) {
101 throw_pretty("Invalid argument: "
102 << "invalid step length, value is between 0. to 1.");
103 }
104 cost_try_ = 0.;
105 xnext_ = problem_->get_x0();
106 const std::size_t T = problem_->get_T();
107 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
108 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
109 if ((is_feasible_) || (steplength == 1)) {
110 for (std::size_t t = 0; t < T; ++t) {
111 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
112 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
113 const std::size_t nu = m->get_nu();
114
115 xs_try_[t] = xnext_;
116 m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
117 if (nu != 0) {
118 us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
119 if (m->get_has_control_limits()) { // clamp control
120 us_try_[t] = us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
121 }
122 m->calc(d, xs_try_[t], us_try_[t]);
123 } else {
124 m->calc(d, xs_try_[t]);
125 }
126 xnext_ = d->xnext;
127 cost_try_ += d->cost;
128
129 if (raiseIfNaN(cost_try_)) {
130 throw_pretty("forward_error");
131 }
132 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
133 throw_pretty("forward_error");
134 }
135 }
136
137 const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
138 const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
139 xs_try_.back() = xnext_;
140 m->calc(d, xs_try_.back());
141 cost_try_ += d->cost;
142
143 if (raiseIfNaN(cost_try_)) {
144 throw_pretty("forward_error");
145 }
146 } else {
147 for (std::size_t t = 0; t < T; ++t) {
148 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
149 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
150 const std::size_t nu = m->get_nu();
151 m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]);
152 m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
153 if (nu != 0) {
154 us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
155 if (m->get_has_control_limits()) { // clamp control
156 us_try_[t] = us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
157 }
158 m->calc(d, xs_try_[t], us_try_[t]);
159 } else {
160 m->calc(d, xs_try_[t]);
161 }
162 xnext_ = d->xnext;
163 cost_try_ += d->cost;
164
165 if (raiseIfNaN(cost_try_)) {
166 throw_pretty("forward_error");
167 }
168 if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
169 throw_pretty("forward_error");
170 }
171 }
172
173 const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
174 const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
175 m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1), xs_try_.back());
176 m->calc(d, xs_try_.back());
177 cost_try_ += d->cost;
178
179 if (raiseIfNaN(cost_try_)) {
180 throw_pretty("forward_error");
181 }
182 }
183}
184
185const std::vector<Eigen::MatrixXd>& SolverBoxFDDP::get_Quu_inv() const { return Quu_inv_; }
186
187} // namespace crocoddyl
Box QP solution.
Definition: box-qp.hpp:28
std::vector< size_t > free_idx
Free space indexes.
Definition: box-qp.hpp:50
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
Definition: box-qp.hpp:48
Eigen::VectorXd x
Decision vector.
Definition: box-qp.hpp:49
std::vector< size_t > clamped_idx
Clamped space indexes.
Definition: box-qp.hpp:51