9 #ifdef CROCODDYL_WITH_MULTITHREADING
11 #endif // CROCODDYL_WITH_MULTITHREADING
13 #include "crocoddyl/core/utils/exception.hpp"
14 #include "crocoddyl/core/solver-base.hpp"
37 const std::size_t ndx =
problem_->get_ndx();
38 const std::size_t T =
problem_->get_T();
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);
50 xs_.back() =
problem_->get_terminalModel()->get_state()->zero();
51 fs_.back() = Eigen::VectorXd::Zero(ndx);
54 SolverAbstract::~SolverAbstract() {}
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);
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();
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())
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]);
86 for (std::size_t t = 0; t < T; ++t) {
91 for (std::size_t t = 0; t < T; ++t) {
96 for (std::vector<Eigen::VectorXd>::iterator it =
fs_.begin(); it !=
fs_.end(); ++it) {
104 const std::vector<Eigen::VectorXd>& us_warm,
bool is_feasible) {
105 const std::size_t T =
problem_->get_T();
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();
113 xs_.back() =
problem_->get_terminalModel()->get_state()->zero();
115 if (xs_warm.size() != T + 1) {
116 throw_pretty(
"Warm start state vector has wrong dimension, got " << xs_warm.size() <<
" expecting " << (T + 1));
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]);
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: "
133 std::copy(xs_warm.begin(), xs_warm.end(),
xs_.begin());
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);
143 if (us_warm.size() != T) {
144 throw_pretty(
"Warm start control has wrong dimension, got " << us_warm.size() <<
" expecting " << T);
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);
155 std::copy(us_warm.begin(), us_warm.end(),
us_.begin());
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));
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) +
")")
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) +
")")
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));
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) +
")")
249 throw_pretty(
"Invalid argument: "
250 <<
"xreg value has to be positive.");
257 throw_pretty(
"Invalid argument: "
258 <<
"ureg value has to be positive.");
264 if (0. >= th_acceptstep || th_acceptstep > 1) {
265 throw_pretty(
"Invalid argument: "
266 <<
"th_acceptstep value should between 0 and 1.");
273 throw_pretty(
"Invalid argument: "
274 <<
"th_stop value has to higher than 0.");
280 if (0. > th_gaptol) {
281 throw_pretty(
"Invalid argument: "
282 <<
"th_gaptol value has to be positive.");
289 bool raiseIfNaN(
const double value) {
290 if (std::isnan(value) || std::isinf(value) || value >= 1e30) {