10 #ifdef CROCODDYL_WITH_MULTITHREADING
12 #endif // CROCODDYL_WITH_MULTITHREADING
14 #include "crocoddyl/core/solver-base.hpp"
15 #include "crocoddyl/core/utils/exception.hpp"
48 const std::size_t ndx =
problem_->get_ndx();
49 const std::size_t T =
problem_->get_T();
53 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
55 for (std::size_t t = 0; t < T; ++t) {
56 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
57 const std::size_t nu = model->get_nu();
58 xs_[t] = model->get_state()->zero();
59 us_[t] = Eigen::VectorXd::Zero(nu);
60 fs_[t] = Eigen::VectorXd::Zero(ndx);
62 xs_.back() =
problem_->get_terminalModel()->get_state()->zero();
63 fs_.back() = Eigen::VectorXd::Zero(ndx);
66 SolverAbstract::~SolverAbstract() {}
69 const std::size_t T =
problem_->get_T();
70 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
72 for (std::size_t t = 0; t < T; ++t) {
73 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
74 const std::size_t nu = model->get_nu();
75 us_[t].conservativeResize(nu);
82 const std::size_t T =
problem_->get_T();
83 const Eigen::VectorXd& x0 =
problem_->get_x0();
84 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
86 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
89 models[0]->get_state()->diff(
xs_[0], x0,
fs_[0]);
90 #ifdef CROCODDYL_WITH_MULTITHREADING
91 #pragma omp parallel for num_threads(problem_->get_nthreads())
93 for (std::size_t t = 0; t < T; ++t) {
94 const boost::shared_ptr<ActionModelAbstract>& m = models[t];
95 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
96 m->get_state()->diff(
xs_[t + 1], d->xnext,
fs_[t + 1]);
101 for (std::size_t t = 0; t < T; ++t) {
107 for (std::size_t t = 0; t < T; ++t) {
113 for (std::vector<Eigen::VectorXd>::iterator it =
fs_.begin();
114 it !=
fs_.end(); ++it) {
123 const std::size_t T =
problem_->get_T();
124 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
126 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
130 for (std::size_t t = 0; t < T; ++t) {
131 if (models[t]->get_ng() > 0) {
133 std::max(
tmp_feas_, datas[t]->g.lpNorm<Eigen::Infinity>());
136 if (
problem_->get_terminalModel()->get_ng() > 0) {
139 problem_->get_terminalData()->g.lpNorm<Eigen::Infinity>());
143 for (std::size_t t = 0; t < T; ++t) {
144 if (models[t]->get_ng() > 0) {
148 if (
problem_->get_terminalModel()->get_ng() > 0) {
158 const std::size_t T =
problem_->get_T();
159 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
161 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
165 for (std::size_t t = 0; t < T; ++t) {
166 if (models[t]->get_nh() > 0) {
168 std::max(
tmp_feas_, datas[t]->h.lpNorm<Eigen::Infinity>());
171 if (
problem_->get_terminalModel()->get_nh() > 0) {
174 problem_->get_terminalData()->h.lpNorm<Eigen::Infinity>());
178 for (std::size_t t = 0; t < T; ++t) {
179 if (models[t]->get_nh() > 0) {
183 if (
problem_->get_terminalModel()->get_nh() > 0) {
192 const std::vector<Eigen::VectorXd>& us_warm,
194 const std::size_t T =
problem_->get_T();
196 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
198 if (xs_warm.size() == 0) {
199 for (std::size_t t = 0; t < T; ++t) {
200 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
201 xs_[t] = model->get_state()->zero();
203 xs_.back() =
problem_->get_terminalModel()->get_state()->zero();
205 if (xs_warm.size() != T + 1) {
206 throw_pretty(
"Warm start state vector has wrong dimension, got "
207 << xs_warm.size() <<
" expecting " << (T + 1));
209 for (std::size_t t = 0; t < T; ++t) {
210 const std::size_t nx = models[t]->get_state()->get_nx();
211 if (
static_cast<std::size_t
>(xs_warm[t].size()) != nx) {
212 throw_pretty(
"Invalid argument: "
213 <<
"xs_init[" + std::to_string(t) +
214 "] has wrong dimension ("
216 <<
" provided - it should be equal to " +
217 std::to_string(nx) +
"). ActionModel: "
221 const std::size_t nx =
problem_->get_terminalModel()->get_state()->get_nx();
222 if (
static_cast<std::size_t
>(xs_warm[T].size()) != nx) {
223 throw_pretty(
"Invalid argument: "
224 <<
"xs_init[" + std::to_string(T) +
225 "] (terminal state) has wrong dimension ("
227 <<
" provided - it should be equal to " +
228 std::to_string(nx) +
"). ActionModel: "
231 std::copy(xs_warm.begin(), xs_warm.end(),
xs_.begin());
234 if (us_warm.size() == 0) {
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 us_[t] = Eigen::VectorXd::Zero(nu);
241 if (us_warm.size() != T) {
242 throw_pretty(
"Warm start control has wrong dimension, got "
243 << us_warm.size() <<
" expecting " << T);
245 for (std::size_t t = 0; t < T; ++t) {
246 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
247 const std::size_t nu = model->get_nu();
248 if (
static_cast<std::size_t
>(us_warm[t].size()) != nu) {
249 throw_pretty(
"Invalid argument: "
250 <<
"us_init[" + std::to_string(t) +
251 "] has wrong dimension ("
253 <<
" provided - it should be equal to " +
254 std::to_string(nu) +
"). ActionModel: "
258 std::copy(us_warm.begin(), us_warm.end(),
us_.begin());
264 const std::vector<boost::shared_ptr<CallbackAbstract> >& callbacks) {
268 const std::vector<boost::shared_ptr<CallbackAbstract> >&
328 "Use get_preg for gettting the primal-dual regularization",
329 double SolverAbstract::get_xreg()
const {
return preg_; })
332 "Use get_preg for gettting the primal-dual regularization",
333 double SolverAbstract::get_ureg()
const {
return preg_; })
348 const std::size_t T =
problem_->get_T();
349 if (xs.size() != T + 1) {
350 throw_pretty(
"Invalid argument: "
351 <<
"xs list has to be of length " + std::to_string(T + 1));
354 const std::size_t nx =
problem_->get_nx();
355 for (std::size_t t = 0; t < T; ++t) {
356 if (
static_cast<std::size_t
>(xs[t].size()) != nx) {
357 throw_pretty(
"Invalid argument: "
358 <<
"xs[" + std::to_string(t) +
"] has wrong dimension ("
360 <<
" provided - it should be " + std::to_string(nx) +
")")
363 if (
static_cast<std::size_t
>(xs[T].size()) != nx) {
364 throw_pretty(
"Invalid argument: "
365 <<
"xs[" + std::to_string(T) +
366 "] (terminal state) has wrong dimension ("
368 <<
" provided - it should be " + std::to_string(nx) +
")")
374 const std::size_t T =
problem_->get_T();
375 if (us.size() != T) {
376 throw_pretty(
"Invalid argument: "
377 <<
"us list has to be of length " + std::to_string(T));
380 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
382 for (std::size_t t = 0; t < T; ++t) {
383 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
384 const std::size_t nu = model->get_nu();
385 if (
static_cast<std::size_t
>(us[t].size()) != nu) {
386 throw_pretty(
"Invalid argument: "
387 <<
"us[" + std::to_string(t) +
"] has wrong dimension ("
389 <<
" provided - it should be " + std::to_string(nu) +
")")
397 throw_pretty(
"Invalid argument: "
398 <<
"preg value has to be positive.");
405 throw_pretty(
"Invalid argument: "
406 <<
"dreg value has to be positive.");
412 "Use set_preg for gettting the primal-variable regularization",
413 void SolverAbstract::set_xreg(
const double xreg) {
415 throw_pretty(
"Invalid argument: "
416 <<
"xreg value has to be positive.");
423 "Use set_preg for gettting the primal-variable regularization",
424 void SolverAbstract::set_ureg(
const double ureg) {
426 throw_pretty(
"Invalid argument: "
427 <<
"ureg value has to be positive.");
434 if (0. >= th_acceptstep || th_acceptstep > 1) {
435 throw_pretty(
"Invalid argument: "
436 <<
"th_acceptstep value should between 0 and 1.");
438 th_acceptstep_ = th_acceptstep;
443 throw_pretty(
"Invalid argument: "
444 <<
"th_stop value has to higher than 0.");
450 if (0. > th_gaptol) {
451 throw_pretty(
"Invalid argument: "
452 <<
"th_gaptol value has to be positive.");
461 bool raiseIfNaN(
const double value) {
462 if (std::isnan(value) || std::isinf(value) || value >= 1e30) {