crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
solver-base.cpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh, University of Oxford
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifdef CROCODDYL_WITH_MULTITHREADING
10#include <omp.h>
11#endif // CROCODDYL_WITH_MULTITHREADING
12
13#include "crocoddyl/core/utils/exception.hpp"
14#include "crocoddyl/core/solver-base.hpp"
15
16namespace crocoddyl {
17
18SolverAbstract::SolverAbstract(boost::shared_ptr<ShootingProblem> problem)
19 : problem_(problem),
20 is_feasible_(false),
21 was_feasible_(false),
22 cost_(0.),
23 stop_(0.),
24 xreg_(NAN),
25 ureg_(NAN),
26 steplength_(1.),
27 dV_(0.),
28 dVexp_(0.),
29 th_acceptstep_(0.1),
30 th_stop_(1e-9),
31 iter_(0),
32 th_gaptol_(1e-16),
33 ffeas_(NAN),
34 inffeas_(true),
35 tmp_feas_(0.) {
36 // Allocate common data
37 const std::size_t ndx = problem_->get_ndx();
38 const std::size_t T = problem_->get_T();
39 xs_.resize(T + 1);
40 us_.resize(T);
41 fs_.resize(T + 1);
42 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = 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 xs_[t] = model->get_state()->zero();
47 us_[t] = Eigen::VectorXd::Zero(nu);
48 fs_[t] = Eigen::VectorXd::Zero(ndx);
49 }
50 xs_.back() = problem_->get_terminalModel()->get_state()->zero();
51 fs_.back() = Eigen::VectorXd::Zero(ndx);
52}
53
54SolverAbstract::~SolverAbstract() {}
55
57 const std::size_t T = problem_->get_T();
58 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
59 for (std::size_t t = 0; t < T; ++t) {
60 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
61 const std::size_t nu = model->get_nu();
62 us_[t].conservativeResize(nu);
63 }
64}
65
67 tmp_feas_ = 0.;
68 if (!is_feasible_) {
69 const std::size_t T = problem_->get_T();
70 const Eigen::VectorXd& x0 = problem_->get_x0();
71 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
72 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
73
74 models[0]->get_state()->diff(xs_[0], x0, fs_[0]);
75#ifdef CROCODDYL_WITH_MULTITHREADING
76#pragma omp parallel for num_threads(problem_->get_nthreads())
77#endif
78 for (std::size_t t = 0; t < T; ++t) {
79 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
80 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
81 m->get_state()->diff(xs_[t + 1], d->xnext, fs_[t + 1]);
82 }
83
84 if (inffeas_) {
85 tmp_feas_ = std::max(tmp_feas_, fs_[0].lpNorm<Eigen::Infinity>());
86 for (std::size_t t = 0; t < T; ++t) {
87 tmp_feas_ = std::max(tmp_feas_, fs_[t + 1].lpNorm<Eigen::Infinity>());
88 }
89 } else {
90 tmp_feas_ = fs_[0].lpNorm<1>();
91 for (std::size_t t = 0; t < T; ++t) {
92 tmp_feas_ += fs_[t + 1].lpNorm<1>();
93 }
94 }
95 } else if (!was_feasible_) { // closing the gaps
96 for (std::vector<Eigen::VectorXd>::iterator it = fs_.begin(); it != fs_.end(); ++it) {
97 it->setZero();
98 }
99 }
100 return tmp_feas_;
101}
102
103void SolverAbstract::setCandidate(const std::vector<Eigen::VectorXd>& xs_warm,
104 const std::vector<Eigen::VectorXd>& us_warm, bool is_feasible) {
105 const std::size_t T = problem_->get_T();
106
107 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
108 if (xs_warm.size() == 0) {
109 for (std::size_t t = 0; t < T; ++t) {
110 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
111 xs_[t] = model->get_state()->zero();
112 }
113 xs_.back() = problem_->get_terminalModel()->get_state()->zero();
114 } else {
115 if (xs_warm.size() != T + 1) {
116 throw_pretty("Warm start state vector has wrong dimension, got " << xs_warm.size() << " expecting " << (T + 1));
117 }
118 for (std::size_t t = 0; t < T; ++t) {
119 const std::size_t nx = models[t]->get_state()->get_nx();
120 if (static_cast<std::size_t>(xs_warm[t].size()) != nx) {
121 throw_pretty("Invalid argument: "
122 << "xs_init[" + std::to_string(t) + "] has wrong dimension (" << xs_warm[t].size()
123 << " provided - it should be equal to " + std::to_string(nx) + "). ActionModel: " << *models[t]);
124 }
125 }
126 const std::size_t nx = problem_->get_terminalModel()->get_state()->get_nx();
127 if (static_cast<std::size_t>(xs_warm[T].size()) != nx) {
128 throw_pretty("Invalid argument: "
129 << "xs_init[" + std::to_string(T) + "] (terminal state) has wrong dimension (" << xs_warm[T].size()
130 << " provided - it should be equal to " + std::to_string(nx) + "). ActionModel: "
131 << *problem_->get_terminalModel());
132 }
133 std::copy(xs_warm.begin(), xs_warm.end(), xs_.begin());
134 }
135
136 if (us_warm.size() == 0) {
137 for (std::size_t t = 0; t < T; ++t) {
138 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
139 const std::size_t nu = model->get_nu();
140 us_[t] = Eigen::VectorXd::Zero(nu);
141 }
142 } else {
143 if (us_warm.size() != T) {
144 throw_pretty("Warm start control has wrong dimension, got " << us_warm.size() << " expecting " << T);
145 }
146 for (std::size_t t = 0; t < T; ++t) {
147 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
148 const std::size_t nu = model->get_nu();
149 if (static_cast<std::size_t>(us_warm[t].size()) != nu) {
150 throw_pretty("Invalid argument: "
151 << "us_init[" + std::to_string(t) + "] has wrong dimension (" << us_warm[t].size()
152 << " provided - it should be equal to " + std::to_string(nu) + "). ActionModel: " << *model);
153 }
154 }
155 std::copy(us_warm.begin(), us_warm.end(), us_.begin());
156 }
157 is_feasible_ = is_feasible;
158}
159
160void SolverAbstract::setCallbacks(const std::vector<boost::shared_ptr<CallbackAbstract> >& callbacks) {
161 callbacks_ = callbacks;
162}
163
164const std::vector<boost::shared_ptr<CallbackAbstract> >& SolverAbstract::getCallbacks() const { return callbacks_; }
165
166const boost::shared_ptr<ShootingProblem>& SolverAbstract::get_problem() const { return problem_; }
167
168const std::vector<Eigen::VectorXd>& SolverAbstract::get_xs() const { return xs_; }
169
170const std::vector<Eigen::VectorXd>& SolverAbstract::get_us() const { return us_; }
171
172const std::vector<Eigen::VectorXd>& SolverAbstract::get_fs() const { return fs_; }
173
175
176double SolverAbstract::get_cost() const { return cost_; }
177
178double SolverAbstract::get_stop() const { return stop_; }
179
180const Eigen::Vector2d& SolverAbstract::get_d() const { return d_; }
181
182double SolverAbstract::get_xreg() const { return xreg_; }
183
184double SolverAbstract::get_ureg() const { return ureg_; }
185
187
188double SolverAbstract::get_dV() const { return dV_; }
189
190double SolverAbstract::get_dVexp() const { return dVexp_; }
191
193
194double SolverAbstract::get_th_stop() const { return th_stop_; }
195
196std::size_t SolverAbstract::get_iter() const { return iter_; }
197
199
200double SolverAbstract::get_ffeas() const { return ffeas_; }
201
202bool SolverAbstract::get_inffeas() const { return inffeas_; }
203
204void SolverAbstract::set_xs(const std::vector<Eigen::VectorXd>& xs) {
205 const std::size_t T = problem_->get_T();
206 if (xs.size() != T + 1) {
207 throw_pretty("Invalid argument: "
208 << "xs list has to be of length " + std::to_string(T + 1));
209 }
210
211 const std::size_t nx = problem_->get_nx();
212 for (std::size_t t = 0; t < T; ++t) {
213 if (static_cast<std::size_t>(xs[t].size()) != nx) {
214 throw_pretty("Invalid argument: "
215 << "xs[" + std::to_string(t) + "] has wrong dimension (" << xs[t].size()
216 << " provided - it should be " + std::to_string(nx) + ")")
217 }
218 }
219 if (static_cast<std::size_t>(xs[T].size()) != nx) {
220 throw_pretty("Invalid argument: "
221 << "xs[" + std::to_string(T) + "] (terminal state) has wrong dimension (" << xs[T].size()
222 << " provided - it should be " + std::to_string(nx) + ")")
223 }
224 xs_ = xs;
225}
226
227void SolverAbstract::set_us(const std::vector<Eigen::VectorXd>& us) {
228 const std::size_t T = problem_->get_T();
229 if (us.size() != T) {
230 throw_pretty("Invalid argument: "
231 << "us list has to be of length " + std::to_string(T));
232 }
233
234 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
235 for (std::size_t t = 0; t < T; ++t) {
236 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
237 const std::size_t nu = model->get_nu();
238 if (static_cast<std::size_t>(us[t].size()) != nu) {
239 throw_pretty("Invalid argument: "
240 << "us[" + std::to_string(t) + "] has wrong dimension (" << us[t].size()
241 << " provided - it should be " + std::to_string(nu) + ")")
242 }
243 }
244 us_ = us;
245}
246
247void SolverAbstract::set_xreg(const double xreg) {
248 if (xreg < 0.) {
249 throw_pretty("Invalid argument: "
250 << "xreg value has to be positive.");
251 }
252 xreg_ = xreg;
253}
254
255void SolverAbstract::set_ureg(const double ureg) {
256 if (ureg < 0.) {
257 throw_pretty("Invalid argument: "
258 << "ureg value has to be positive.");
259 }
260 ureg_ = ureg;
261}
262
263void SolverAbstract::set_th_acceptstep(const double th_acceptstep) {
264 if (0. >= th_acceptstep || th_acceptstep > 1) {
265 throw_pretty("Invalid argument: "
266 << "th_acceptstep value should between 0 and 1.");
267 }
268 th_acceptstep_ = th_acceptstep;
269}
270
271void SolverAbstract::set_th_stop(const double th_stop) {
272 if (th_stop <= 0.) {
273 throw_pretty("Invalid argument: "
274 << "th_stop value has to higher than 0.");
275 }
276 th_stop_ = th_stop;
277}
278
279void SolverAbstract::set_th_gaptol(const double th_gaptol) {
280 if (0. > th_gaptol) {
281 throw_pretty("Invalid argument: "
282 << "th_gaptol value has to be positive.");
283 }
284 th_gaptol_ = th_gaptol;
285}
286
287void SolverAbstract::set_inffeas(const bool inffeas) { inffeas_ = inffeas; }
288
289bool raiseIfNaN(const double value) {
290 if (std::isnan(value) || std::isinf(value) || value >= 1e30) {
291 return true;
292 } else {
293 return false;
294 }
295}
296
297} // namespace crocoddyl
double get_cost() const
Return the total cost.
double get_th_gaptol() const
Return the threshold for accepting a gap as non-zero.
double get_xreg() const
Return the state regularization value.
bool was_feasible_
Label that indicates in the previous iterate was feasible.
double dVexp_
Expected cost reduction.
std::vector< Eigen::VectorXd > xs_
State trajectory.
void set_inffeas(const bool inffeas)
Modify the current norm used for computed the feasibility.
std::size_t get_iter() const
Return the number of iterations performed by the solver.
void set_th_stop(const double th_stop)
Modify the tolerance for stopping the algorithm.
double stop_
Value computed by stoppingCriteria()
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
void set_ureg(const double ureg)
Modify the control regularization value.
const std::vector< boost::shared_ptr< CallbackAbstract > > & getCallbacks() const
Return the list of callback functions using for diagnostic.
void set_xreg(const double xreg)
Modify the state regularization value.
void set_xs(const std::vector< Eigen::VectorXd > &xs)
Modify the state trajectory .
double get_dVexp() const
Return the expected cost reduction .
bool is_feasible_
Label that indicates is the iteration is feasible.
std::vector< Eigen::VectorXd > us_
Control trajectory.
double th_acceptstep_
Threshold used for accepting step.
double get_steplength() const
Return the step length .
void set_th_gaptol(const double th_gaptol)
Modify the threshold for accepting a gap as non-zero.
double th_stop_
Tolerance for stopping the algorithm.
double computeDynamicFeasibility()
Compute the dynamic feasibility for the current guess .
Definition: solver-base.cpp:66
const Eigen::Vector2d & get_d() const
Return the LQ approximation of the expected improvement.
void setCandidate(const std::vector< Eigen::VectorXd > &xs_warm=DEFAULT_VECTOR, const std::vector< Eigen::VectorXd > &us_warm=DEFAULT_VECTOR, const bool is_feasible=false)
Set the solver candidate trajectories .
double get_th_stop() const
Return the tolerance for stopping the algorithm.
double get_ffeas() const
Return the feasibility of the dynamic constraints of the current guess.
double cost_
Total cost.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverAbstract(boost::shared_ptr< ShootingProblem > problem)
Initialize the solver.
Definition: solver-base.cpp:18
double xreg_
Current state regularization value.
void setCallbacks(const std::vector< boost::shared_ptr< CallbackAbstract > > &callbacks)
Set a list of callback functions using for the solver diagnostic.
double ureg_
Current control regularization values.
void set_us(const std::vector< Eigen::VectorXd > &us)
Modify the control trajectory .
double steplength_
Current applied step-length.
double th_gaptol_
Threshold limit to check non-zero gaps.
std::size_t iter_
Number of iteration performed by the solver.
double get_ureg() const
Return the control regularization value.
const boost::shared_ptr< ShootingProblem > & get_problem() const
Return the shooting problem.
double dV_
Cost reduction obtained by tryStep()
double get_stop() const
Return the value computed by stoppingCriteria()
Eigen::Vector2d d_
LQ approximation of the expected improvement.
double get_dV() const
Return the cost reduction .
const std::vector< Eigen::VectorXd > & get_fs() const
Return the gaps .
const std::vector< Eigen::VectorXd > & get_xs() const
Return the state trajectory .
bool get_inffeas() const
Return the norm used for the computing the feasibility (true for , false for )
double ffeas_
Feasibility of the dynamic constraints.
bool get_is_feasible() const
Return the feasibility status of the trajectory.
const std::vector< Eigen::VectorXd > & get_us() const
Return the control trajectory .
virtual void resizeData()
Resizing the solver data.
Definition: solver-base.cpp:56
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
void set_th_acceptstep(const double th_acceptstep)
Modify the threshold used for accepting step.
double tmp_feas_
Temporal variables used for computed the feasibility.
double get_th_acceptstep() const
Return the threshold used for accepting a step.
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.