71 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 typedef _Scalar Scalar;
81 typedef typename MathBase::VectorXs VectorXs;
82 typedef typename MathBase::MatrixXs MatrixXs;
84 typedef std::map<std::string, std::shared_ptr<ConstraintItem> >
85 ConstraintModelContainer;
86 typedef std::map<std::string, std::shared_ptr<ConstraintDataAbstract> >
87 ConstraintDataContainer;
96 const std::size_t nu);
117 std::shared_ptr<ConstraintModelAbstract> constraint,
118 const bool active =
true);
143 void calc(
const std::shared_ptr<ConstraintDataManager>& data,
144 const Eigen::Ref<const VectorXs>& x,
145 const Eigen::Ref<const VectorXs>& u);
157 void calc(
const std::shared_ptr<ConstraintDataManager>& data,
158 const Eigen::Ref<const VectorXs>& x);
167 void calcDiff(
const std::shared_ptr<ConstraintDataManager>& data,
168 const Eigen::Ref<const VectorXs>& x,
169 const Eigen::Ref<const VectorXs>& u);
182 void calcDiff(
const std::shared_ptr<ConstraintDataManager>& data,
183 const Eigen::Ref<const VectorXs>& x);
208 template <
typename NewScalar>
214 const std::shared_ptr<StateAbstract>&
get_state()
const;
276 template <
class Scalar>
281 std::shared_ptr<StateAbstract> state_;
282 ConstraintModelContainer constraints_;
290 std::set<std::string> active_set_;
291 std::set<std::string>
297 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
299 typedef _Scalar Scalar;
303 typedef typename MathBase::VectorXs VectorXs;
304 typedef typename MathBase::MatrixXs MatrixXs;
306 template <
template <
typename Scalar>
class Model>
309 : g_internal(model->get_ng()),
310 Gx_internal(model->get_ng(), model->get_state()->get_ndx()),
311 Gu_internal(model->get_ng(), model->get_nu()),
312 h_internal(model->get_nh()),
313 Hx_internal(model->get_nh(), model->get_state()->get_ndx()),
314 Hu_internal(model->get_nh(), model->get_nu()),
316 g(g_internal.data(), model->get_ng()),
317 Gx(Gx_internal.data(), model->get_ng(), model->get_state()->get_ndx()),
318 Gu(Gu_internal.data(), model->get_ng(), model->get_nu()),
319 h(h_internal.data(), model->get_nh()),
320 Hx(Hx_internal.data(), model->get_nh(), model->get_state()->get_ndx()),
321 Hu(Hu_internal.data(), model->get_nh(), model->get_nu()) {
329 Scalar>::ConstraintModelContainer::const_iterator it =
331 it != model->get_constraints().end(); ++it) {
332 const std::shared_ptr<ConstraintItem>& item = it->second;
334 std::make_pair(item->name, item->constraint->createData(data)));
338 template <
class ActionData>
339 void shareMemory(ActionData*
const data) {
341 g_internal.resize(0);
342 Gx_internal.resize(0, 0);
343 Gu_internal.resize(0, 0);
344 h_internal.resize(0);
345 Hx_internal.resize(0, 0);
346 Hu_internal.resize(0, 0);
348 new (&g) Eigen::Map<VectorXs>(data->g.data(), data->g.size());
350 Eigen::Map<MatrixXs>(data->Gx.data(), data->Gx.rows(), data->Gx.cols());
352 Eigen::Map<MatrixXs>(data->Gu.data(), data->Gu.rows(), data->Gu.cols());
353 new (&h) Eigen::Map<VectorXs>(data->h.data(), data->h.size());
355 Eigen::Map<MatrixXs>(data->Hx.data(), data->Hx.rows(), data->Hx.cols());
357 Eigen::Map<MatrixXs>(data->Hu.data(), data->Hu.rows(), data->Hu.cols());
360 template <
class Model>
361 void resize(Model*
const model,
const bool running_node =
true) {
362 const std::size_t ndx = model->get_state()->get_ndx();
363 const std::size_t nu = model->get_nu();
364 const std::size_t ng = running_node ? model->get_ng() : model->get_ng_T();
365 const std::size_t nh = running_node ? model->get_nh() : model->get_nh_T();
366 new (&g) Eigen::Map<VectorXs>(g_internal.data(), ng);
367 new (&Gx) Eigen::Map<MatrixXs>(Gx_internal.data(), ng, ndx);
368 new (&Gu) Eigen::Map<MatrixXs>(Gu_internal.data(), ng, nu);
369 new (&h) Eigen::Map<VectorXs>(h_internal.data(), nh);
370 new (&Hx) Eigen::Map<MatrixXs>(Hx_internal.data(), nh, ndx);
371 new (&Hu) Eigen::Map<MatrixXs>(Hu_internal.data(), nh, nu);
374 template <
class ActionModel,
class ActionData>
375 void resize(ActionModel*
const model, ActionData*
const data,
376 const bool running_node =
true) {
377 const std::size_t ndx = model->get_state()->get_ndx();
378 const std::size_t nu = model->get_nu();
379 const std::size_t ng = running_node ? model->get_ng() : model->get_ng_T();
380 const std::size_t nh = running_node ? model->get_nh() : model->get_nh_T();
381 data->g.conservativeResize(ng);
382 data->Gx.conservativeResize(ng, ndx);
383 data->Gu.conservativeResize(ng, nu);
384 data->h.conservativeResize(nh);
385 data->Hx.conservativeResize(nh, ndx);
386 data->Hu.conservativeResize(nh, nu);
387 new (&g) Eigen::Map<VectorXs>(data->g.data(), ng);
388 new (&Gx) Eigen::Map<MatrixXs>(data->Gx.data(), ng, ndx);
389 new (&Gu) Eigen::Map<MatrixXs>(data->Gu.data(), ng, nu);
390 new (&h) Eigen::Map<VectorXs>(data->h.data(), nh);
391 new (&Hx) Eigen::Map<MatrixXs>(data->Hx.data(), nh, ndx);
392 new (&Hu) Eigen::Map<MatrixXs>(data->Hu.data(), nh, nu);
395 VectorXs get_g()
const {
return g; }
396 MatrixXs get_Gx()
const {
return Gx; }
397 MatrixXs get_Gu()
const {
return Gu; }
398 VectorXs get_h()
const {
return h; }
399 MatrixXs get_Hx()
const {
return Hx; }
400 MatrixXs get_Hu()
const {
return Hu; }
402 void set_g(
const VectorXs& _g) {
403 if (g.size() != _g.size()) {
405 "Invalid argument: " <<
"g has wrong dimension (it should be " +
406 std::to_string(g.size()) +
")");
410 void set_Gx(
const MatrixXs& _Gx) {
411 if (Gx.rows() != _Gx.rows() || Gx.cols() != _Gx.cols()) {
413 "Invalid argument: " <<
"Gx has wrong dimension (it should be " +
414 std::to_string(Gx.rows()) +
", " +
415 std::to_string(Gx.cols()) +
")");
419 void set_Gu(
const MatrixXs& _Gu) {
420 if (Gu.rows() != _Gu.rows() || Gu.cols() != _Gu.cols()) {
422 "Invalid argument: " <<
"Gu has wrong dimension (it should be " +
423 std::to_string(Gu.rows()) +
", " +
424 std::to_string(Gu.cols()) +
")");
428 void set_h(
const VectorXs& _h) {
429 if (h.size() != _h.size()) {
431 "Invalid argument: " <<
"h has wrong dimension (it should be " +
432 std::to_string(h.size()) +
")");
436 void set_Hx(
const MatrixXs& _Hx) {
437 if (Hx.rows() != _Hx.rows() || Hx.cols() != _Hx.cols()) {
439 "Invalid argument: " <<
"Hx has wrong dimension (it should be " +
440 std::to_string(Hx.rows()) +
", " +
441 std::to_string(Hx.cols()) +
")");
445 void set_Hu(
const MatrixXs& _Hu) {
446 if (Hu.rows() != _Hu.rows() || Hu.cols() != _Hu.cols()) {
448 "Invalid argument: " <<
"Hu has wrong dimension (it should be " +
449 std::to_string(Hu.rows()) +
", " +
450 std::to_string(Hu.cols()) +
")");
457 MatrixXs Gx_internal;
458 MatrixXs Gu_internal;
460 MatrixXs Hx_internal;
461 MatrixXs Hu_internal;
463 typename ConstraintModelManagerTpl<Scalar>::ConstraintDataContainer
466 Eigen::Map<VectorXs> g;
467 Eigen::Map<MatrixXs> Gx;
468 Eigen::Map<MatrixXs> Gu;
469 Eigen::Map<VectorXs> h;
470 Eigen::Map<MatrixXs> Hx;
471 Eigen::Map<MatrixXs> Hu;