1 #ifndef __quadruped_walkgen_quadruped_step_hxx__
2 #define __quadruped_walkgen_quadruped_step_hxx__
4 #include "crocoddyl/core/utils/exception.hpp"
7 template <
typename Scalar>
9 : crocoddyl::ActionModelAbstractTpl<
Scalar>(
10 boost::make_shared<crocoddyl::StateVectorTpl<
Scalar> >(20), 8, 28) {
19 centrifugal_term =
true;
23 step_weights_.setConstant(
Scalar(1));
24 heuristic_weights_.setConstant(
Scalar(1));
43 is_acc_activated_ =
true;
45 acc_lim_.setConstant(
Scalar(50.));
47 delta_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
49 gamma_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
51 for (
int k = 1; k < N_sampling; k++) {
53 (float)k / (
float)N_sampling;
55 delta_.col(1) << delta_.col(0).pow(2);
56 delta_.col(2) << delta_.col(0).pow(3);
57 delta_.col(3) << delta_.col(0).pow(4);
59 60 * delta_.col(0) - 180 * delta_.col(1) + 120 * delta_.col(2);
60 gamma_.col(1) = -36 * delta_.col(0) + 96 * delta_.col(1) - 60 * delta_.col(2);
61 gamma_.col(2) = -9 * delta_.col(0) + 18 * delta_.col(1) - 10 * delta_.col(2);
63 alpha_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
65 beta_x_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
67 beta_y_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
69 tmp_ones_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Ones(
72 rb_accx_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
74 rb_accy_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
77 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
80 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
84 is_vel_activated_ =
true;
86 vel_lim_.setConstant(
Scalar(3.));
88 gamma_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
90 gamma_v.col(0) = 30 * delta_.col(1) - 60 * delta_.col(2) + 30 * delta_.col(3);
91 gamma_v.col(1) = delta_.col(0);
93 -18 * delta_.col(1) + 32 * delta_.col(2) - 15 * delta_.col(3);
95 -4.5 * delta_.col(1) + 6 * delta_.col(2) - 2.5 * delta_.col(3);
97 alpha_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
99 beta_x_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
101 beta_y_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
104 rb_velx_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
106 rb_vely_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
109 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
112 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
116 is_jerk_activated_ =
true;
117 jerk_weight_ =
Scalar(1.);
119 beta_j = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
124 template <
typename Scalar>
127 template <
typename Scalar>
129 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
130 const Eigen::Ref<const typename MathBase::VectorXs>& x,
131 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
132 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
133 throw_pretty(
"Invalid argument: "
134 <<
"x has wrong dimension (it should be " +
135 std::to_string(state_->get_nx()) +
")");
137 if (
static_cast<std::size_t
>(u.size()) != nu_) {
138 throw_pretty(
"Invalid argument: "
139 <<
"u has wrong dimension (it should be " +
140 std::to_string(nu_) +
")");
146 d->xnext.template head<12>() = x.head(12);
147 d->xnext.template tail<8>() = x.tail(8) + B * u;
150 d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
151 d->r.template segment<8>(12) =
152 heuristic_weights_.cwiseProduct(x.tail(8) - pheuristic_);
153 d->r.template tail<8>() = step_weights_.cwiseProduct(u);
155 d->cost =
Scalar(0.5) * d->r.transpose() * d->r;
158 if (is_acc_activated_) {
159 for (
int i = 0; i < 4; i++) {
160 if (S_(i) ==
Scalar(1.)) {
173 rb_accx_max_.col(2 * i) =
174 (oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
175 oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(0) -
178 beta_x_.col(i) - acc_lim_(0) * tmp_ones_;
179 rb_accx_max_.col(2 * i + 1) =
180 -(oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
181 oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(0) -
184 beta_x_.col(i) - acc_lim_(0) * tmp_ones_;
187 rb_accy_max_.col(2 * i) =
188 (oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
189 oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(1) -
192 beta_y_.col(i) - acc_lim_(1) * tmp_ones_;
193 rb_accy_max_.col(2 * i + 1) =
194 -(oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
195 oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(1) -
198 beta_y_.col(i) - acc_lim_(1) * tmp_ones_;
200 rb_accx_max_.col(2 * i).setZero();
201 rb_accx_max_.col(2 * i + 1).setZero();
202 rb_accy_max_.col(2 * i).setZero();
203 rb_accy_max_.col(2 * i + 1).setZero();
207 (rb_accx_max_ >
Scalar(0.))
208 .
template cast<Scalar>();
210 (rb_accy_max_ >
Scalar(0.))
211 .
template cast<Scalar>();
213 rb_accx_max_ = rb_accx_max_.cwiseMax(
Scalar(0.));
214 rb_accy_max_ = rb_accy_max_.cwiseMax(
Scalar(0.));
216 for (
int foot = 0; foot < 4; foot++) {
217 if (S_(foot) ==
Scalar(1.)) {
218 for (
int i = 0; i < (N_sampling - 1); i++) {
219 if (rb_accx_max_bool_(i, 2 * foot)) {
221 Scalar(0.5) * acc_weight_ * pow(rb_accx_max_(i, 2 * foot), 2);
223 if (rb_accx_max_bool_(i, 2 * foot + 1)) {
224 d->cost +=
Scalar(0.5) * acc_weight_ *
225 pow(rb_accx_max_(i, 2 * foot + 1), 2);
227 if (rb_accy_max_bool_(i, 2 * foot)) {
229 Scalar(0.5) * acc_weight_ * pow(rb_accy_max_(i, 2 * foot), 2);
231 if (rb_accy_max_bool_(i, 2 * foot + 1)) {
232 d->cost +=
Scalar(0.5) * acc_weight_ *
233 pow(rb_accy_max_(i, 2 * foot + 1), 2);
241 if (is_vel_activated_) {
242 for (
int i = 0; i < 4; i++) {
243 if (S_(i) ==
Scalar(1.)) {
256 rb_velx_max_.col(2 * i) =
257 (oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
258 oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(0) -
261 beta_x_v.col(i) - vel_lim_(0) * tmp_ones_;
262 rb_velx_max_.col(2 * i + 1) =
263 -(oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
264 oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(0) -
267 beta_x_v.col(i) - vel_lim_(0) * tmp_ones_;
269 rb_vely_max_.col(2 * i) =
270 (oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
271 oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(1) -
274 beta_y_v.col(i) - vel_lim_(1) * tmp_ones_;
275 rb_vely_max_.col(2 * i + 1) =
276 -(oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
277 oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(1) -
280 beta_y_v.col(i) - vel_lim_(1) * tmp_ones_;
282 rb_velx_max_.col(2 * i).setZero();
283 rb_velx_max_.col(2 * i + 1).setZero();
284 rb_vely_max_.col(2 * i).setZero();
285 rb_vely_max_.col(2 * i + 1).setZero();
289 (rb_velx_max_ >
Scalar(0.))
290 .
template cast<Scalar>();
292 (rb_vely_max_ >
Scalar(0.))
293 .
template cast<Scalar>();
295 rb_velx_max_ = rb_velx_max_.cwiseMax(
Scalar(0.));
296 rb_vely_max_ = rb_vely_max_.cwiseMax(
Scalar(0.));
298 for (
int foot = 0; foot < 4; foot++) {
299 if (S_(foot) ==
Scalar(1.)) {
300 for (
int i = 0; i < (N_sampling - 1); i++) {
301 if (rb_velx_max_bool_(i, 2 * foot)) {
303 Scalar(0.5) * vel_weight_ * pow(rb_velx_max_(i, 2 * foot), 2);
305 if (rb_velx_max_bool_(i, 2 * foot + 1)) {
306 d->cost +=
Scalar(0.5) * vel_weight_ *
307 pow(rb_velx_max_(i, 2 * foot + 1), 2);
309 if (rb_vely_max_bool_(i, 2 * foot)) {
311 Scalar(0.5) * vel_weight_ * pow(rb_vely_max_(i, 2 * foot), 2);
313 if (rb_vely_max_bool_(i, 2 * foot + 1)) {
314 d->cost +=
Scalar(0.5) * vel_weight_ *
315 pow(rb_vely_max_(i, 2 * foot + 1), 2);
323 if (is_jerk_activated_) {
324 for (
int i = 0; i < 4; i++) {
325 if (S_(i) ==
Scalar(1.)) {
326 rb_jerk_(0, i) = (oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
327 oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) +
328 oTh_(0) - position_(0, i)) *
330 beta_j(0, i) - jerk_(0, i);
331 rb_jerk_(1, i) = (oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
332 oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) +
333 oTh_(1) - position_(1, i)) *
335 beta_j(1, i) - jerk_(1, i);
336 d->cost +=
Scalar(0.5) * jerk_weight_ * rb_jerk_.col(i).squaredNorm();
338 rb_jerk_.col(i).setZero();
344 template <
typename Scalar>
346 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
347 const Eigen::Ref<const typename MathBase::VectorXs>& x,
348 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
349 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
350 throw_pretty(
"Invalid argument: "
351 <<
"x has wrong dimension (it should be " +
352 std::to_string(state_->get_nx()) +
")");
354 if (
static_cast<std::size_t
>(u.size()) != nu_) {
355 throw_pretty(
"Invalid argument: "
356 <<
"u has wrong dimension (it should be " +
357 std::to_string(nu_) +
")");
367 d->Lx.template head<12>() =
368 (state_weights_.array() * d->r.template head<12>().array()).matrix();
369 d->Lx.template tail<8>() =
370 (heuristic_weights_.array() * d->r.template segment<8>(12).array())
373 d->Lu = (step_weights_.array() * d->r.template tail<8>().array()).matrix();
376 d->Lxx.diagonal().head(12) =
377 (state_weights_.array() * state_weights_.array()).matrix();
378 d->Lxx.diagonal().tail(8) =
379 (heuristic_weights_.array() * heuristic_weights_.array()).matrix();
381 d->Luu.diagonal() = (step_weights_.array() * step_weights_.array()).matrix();
383 if (is_acc_activated_) {
384 for (
int foot = 0; foot < 4; foot++) {
385 if (S_[foot] ==
Scalar(1)) {
386 for (
int i = 0; i < (N_sampling - 1); i++) {
431 if (rb_accx_max_bool_(i, 2 * foot)) {
432 d->Lu(2 * foot) += acc_weight_ * oRh_(0, 0) * alpha_(i) *
433 rb_accx_max_(i, 2 * foot);
434 d->Lu(2 * foot + 1) += acc_weight_ * oRh_(0, 1) * alpha_(i) *
435 rb_accx_max_(i, 2 * foot);
437 d->Luu(2 * foot, 2 * foot) +=
438 acc_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_(i), 2);
439 d->Luu(2 * foot + 1, 2 * foot + 1) +=
440 acc_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_(i), 2);
442 d->Luu(2 * foot, 2 * foot + 1) +=
443 acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
444 d->Luu(2 * foot + 1, 2 * foot) +=
445 acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
447 d->Lx(12 + 2 * foot) += acc_weight_ * oRh_(0, 0) * alpha_(i) *
448 rb_accx_max_(i, 2 * foot);
449 d->Lx(12 + 2 * foot + 1) += acc_weight_ * oRh_(0, 1) * alpha_(i) *
450 rb_accx_max_(i, 2 * foot);
452 d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
453 acc_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_(i), 2);
454 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
455 acc_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_(i), 2);
457 d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
458 acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
459 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
460 acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
462 d->Lxu(12 + 2 * foot, 2 * foot) +=
463 acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(0, 0), 2);
464 d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
465 acc_weight_ * pow(alpha_(i), 2) * oRh_(0, 0) * oRh_(0, 1);
466 d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
467 acc_weight_ * pow(alpha_(i), 2) * oRh_(0, 0) * oRh_(0, 1);
468 d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
469 acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(0, 1), 2);
471 if (rb_accx_max_bool_(i, 2 * foot + 1)) {
472 d->Lu(2 * foot) += -acc_weight_ * oRh_(0, 0) * alpha_(i) *
473 rb_accx_max_(i, 2 * foot + 1);
474 d->Lu(2 * foot + 1) += -acc_weight_ * oRh_(0, 1) * alpha_(i) *
475 rb_accx_max_(i, 2 * foot + 1);
477 d->Luu(2 * foot, 2 * foot) +=
478 acc_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_(i), 2);
479 d->Luu(2 * foot + 1, 2 * foot + 1) +=
480 acc_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_(i), 2);
482 d->Luu(2 * foot, 2 * foot + 1) +=
483 acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
484 d->Luu(2 * foot + 1, 2 * foot) +=
485 acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
487 d->Lx(12 + 2 * foot) += -acc_weight_ * oRh_(0, 0) * alpha_(i) *
488 rb_accx_max_(i, 2 * foot + 1);
489 d->Lx(12 + 2 * foot + 1) += -acc_weight_ * oRh_(0, 1) * alpha_(i) *
490 rb_accx_max_(i, 2 * foot + 1);
492 d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
493 acc_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_(i), 2);
494 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
495 acc_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_(i), 2);
497 d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
498 acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
499 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
500 acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
502 d->Lxu(12 + 2 * foot, 2 * foot) +=
503 acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(0, 0), 2);
504 d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
505 acc_weight_ * pow(alpha_(i), 2) * oRh_(0, 0) * oRh_(0, 1);
506 d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
507 acc_weight_ * pow(alpha_(i), 2) * oRh_(0, 0) * oRh_(0, 1);
508 d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
509 acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(0, 1), 2);
511 if (rb_accy_max_bool_(i, 2 * foot)) {
512 d->Lu(2 * foot) += acc_weight_ * oRh_(1, 0) * alpha_(i) *
513 rb_accy_max_(i, 2 * foot);
514 d->Lu(2 * foot + 1) += acc_weight_ * oRh_(1, 1) * alpha_(i) *
515 rb_accy_max_(i, 2 * foot);
517 d->Luu(2 * foot, 2 * foot) +=
518 acc_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_(i), 2);
519 d->Luu(2 * foot + 1, 2 * foot + 1) +=
520 acc_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_(i), 2);
522 d->Luu(2 * foot, 2 * foot + 1) +=
523 acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
524 d->Luu(2 * foot + 1, 2 * foot) +=
525 acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
527 d->Lx(12 + 2 * foot) += acc_weight_ * oRh_(1, 0) * alpha_(i) *
528 rb_accy_max_(i, 2 * foot);
529 d->Lx(12 + 2 * foot + 1) += acc_weight_ * oRh_(1, 1) * alpha_(i) *
530 rb_accy_max_(i, 2 * foot);
532 d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
533 acc_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_(i), 2);
534 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
535 acc_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_(i), 2);
537 d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
538 acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
539 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
540 acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
542 d->Lxu(12 + 2 * foot, 2 * foot) +=
543 acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(1, 0), 2);
544 d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
545 acc_weight_ * pow(alpha_(i), 2) * oRh_(1, 0) * oRh_(1, 1);
546 d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
547 acc_weight_ * pow(alpha_(i), 2) * oRh_(1, 0) * oRh_(1, 1);
548 d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
549 acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(1, 1), 2);
551 if (rb_accy_max_bool_(i, 2 * foot + 1)) {
552 d->Lu(2 * foot) += -acc_weight_ * oRh_(1, 0) * alpha_(i) *
553 rb_accy_max_(i, 2 * foot + 1);
554 d->Lu(2 * foot + 1) += -acc_weight_ * oRh_(1, 1) * alpha_(i) *
555 rb_accy_max_(i, 2 * foot + 1);
557 d->Luu(2 * foot, 2 * foot) +=
558 acc_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_(i), 2);
559 d->Luu(2 * foot + 1, 2 * foot + 1) +=
560 acc_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_(i), 2);
562 d->Luu(2 * foot, 2 * foot + 1) +=
563 acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
564 d->Luu(2 * foot + 1, 2 * foot) +=
565 acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
567 d->Lx(12 + 2 * foot) += -acc_weight_ * oRh_(1, 0) * alpha_(i) *
568 rb_accy_max_(i, 2 * foot + 1);
569 d->Lx(12 + 2 * foot + 1) += -acc_weight_ * oRh_(1, 1) * alpha_(i) *
570 rb_accy_max_(i, 2 * foot + 1);
572 d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
573 acc_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_(i), 2);
574 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
575 acc_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_(i), 2);
577 d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
578 acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
579 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
580 acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
582 d->Lxu(12 + 2 * foot, 2 * foot) +=
583 acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(1, 0), 2);
584 d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
585 acc_weight_ * pow(alpha_(i), 2) * oRh_(1, 0) * oRh_(1, 1);
586 d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
587 acc_weight_ * pow(alpha_(i), 2) * oRh_(1, 0) * oRh_(1, 1);
588 d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
589 acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(1, 1), 2);
596 if (is_vel_activated_) {
597 for (
int foot = 0; foot < 4; foot++) {
598 if (S_[foot] ==
Scalar(1)) {
599 for (
int i = 0; i < (N_sampling - 1); i++) {
644 if (rb_velx_max_bool_(i, 2 * foot)) {
645 d->Lu(2 * foot) += vel_weight_ * oRh_(0, 0) * alpha_v(i) *
646 rb_velx_max_(i, 2 * foot);
647 d->Lu(2 * foot + 1) += vel_weight_ * oRh_(0, 1) * alpha_v(i) *
648 rb_velx_max_(i, 2 * foot);
650 d->Luu(2 * foot, 2 * foot) +=
651 vel_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_v(i), 2);
652 d->Luu(2 * foot + 1, 2 * foot + 1) +=
653 vel_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_v(i), 2);
655 d->Luu(2 * foot, 2 * foot + 1) +=
656 vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
657 d->Luu(2 * foot + 1, 2 * foot) +=
658 vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
660 d->Lx(12 + 2 * foot) += vel_weight_ * oRh_(0, 0) * alpha_v(i) *
661 rb_velx_max_(i, 2 * foot);
662 d->Lx(12 + 2 * foot + 1) += vel_weight_ * oRh_(0, 1) * alpha_v(i) *
663 rb_velx_max_(i, 2 * foot);
665 d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
666 vel_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_v(i), 2);
667 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
668 vel_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_v(i), 2);
670 d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
671 vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
672 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
673 vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
675 d->Lxu(12 + 2 * foot, 2 * foot) +=
676 vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(0, 0), 2);
677 d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
678 vel_weight_ * pow(alpha_v(i), 2) * oRh_(0, 0) * oRh_(0, 1);
679 d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
680 vel_weight_ * pow(alpha_v(i), 2) * oRh_(0, 0) * oRh_(0, 1);
681 d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
682 vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(0, 1), 2);
684 if (rb_velx_max_bool_(i, 2 * foot + 1)) {
685 d->Lu(2 * foot) += -vel_weight_ * oRh_(0, 0) * alpha_v(i) *
686 rb_velx_max_(i, 2 * foot + 1);
687 d->Lu(2 * foot + 1) += -vel_weight_ * oRh_(0, 1) * alpha_v(i) *
688 rb_velx_max_(i, 2 * foot + 1);
690 d->Luu(2 * foot, 2 * foot) +=
691 vel_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_v(i), 2);
692 d->Luu(2 * foot + 1, 2 * foot + 1) +=
693 vel_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_v(i), 2);
695 d->Luu(2 * foot, 2 * foot + 1) +=
696 vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
697 d->Luu(2 * foot + 1, 2 * foot) +=
698 vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
700 d->Lx(12 + 2 * foot) += -vel_weight_ * oRh_(0, 0) * alpha_v(i) *
701 rb_velx_max_(i, 2 * foot + 1);
702 d->Lx(12 + 2 * foot + 1) += -vel_weight_ * oRh_(0, 1) * alpha_v(i) *
703 rb_velx_max_(i, 2 * foot + 1);
705 d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
706 vel_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_v(i), 2);
707 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
708 vel_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_v(i), 2);
710 d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
711 vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
712 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
713 vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
715 d->Lxu(12 + 2 * foot, 2 * foot) +=
716 vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(0, 0), 2);
717 d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
718 vel_weight_ * pow(alpha_v(i), 2) * oRh_(0, 0) * oRh_(0, 1);
719 d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
720 vel_weight_ * pow(alpha_v(i), 2) * oRh_(0, 0) * oRh_(0, 1);
721 d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
722 vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(0, 1), 2);
724 if (rb_vely_max_bool_(i, 2 * foot)) {
725 d->Lu(2 * foot) += vel_weight_ * oRh_(1, 0) * alpha_v(i) *
726 rb_vely_max_(i, 2 * foot);
727 d->Lu(2 * foot + 1) += vel_weight_ * oRh_(1, 1) * alpha_v(i) *
728 rb_vely_max_(i, 2 * foot);
730 d->Luu(2 * foot, 2 * foot) +=
731 vel_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_v(i), 2);
732 d->Luu(2 * foot + 1, 2 * foot + 1) +=
733 vel_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_v(i), 2);
735 d->Luu(2 * foot, 2 * foot + 1) +=
736 vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
737 d->Luu(2 * foot + 1, 2 * foot) +=
738 vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
740 d->Lx(12 + 2 * foot) += vel_weight_ * oRh_(1, 0) * alpha_v(i) *
741 rb_vely_max_(i, 2 * foot);
742 d->Lx(12 + 2 * foot + 1) += vel_weight_ * oRh_(1, 1) * alpha_v(i) *
743 rb_vely_max_(i, 2 * foot);
745 d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
746 vel_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_v(i), 2);
747 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
748 vel_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_v(i), 2);
750 d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
751 vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
752 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
753 vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
755 d->Lxu(12 + 2 * foot, 2 * foot) +=
756 vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(1, 0), 2);
757 d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
758 vel_weight_ * pow(alpha_v(i), 2) * oRh_(1, 0) * oRh_(1, 1);
759 d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
760 vel_weight_ * pow(alpha_v(i), 2) * oRh_(1, 0) * oRh_(1, 1);
761 d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
762 vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(1, 1), 2);
764 if (rb_vely_max_bool_(i, 2 * foot + 1)) {
765 d->Lu(2 * foot) += -vel_weight_ * oRh_(1, 0) * alpha_v(i) *
766 rb_vely_max_(i, 2 * foot + 1);
767 d->Lu(2 * foot + 1) += -vel_weight_ * oRh_(1, 1) * alpha_v(i) *
768 rb_vely_max_(i, 2 * foot + 1);
770 d->Luu(2 * foot, 2 * foot) +=
771 vel_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_v(i), 2);
772 d->Luu(2 * foot + 1, 2 * foot + 1) +=
773 vel_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_v(i), 2);
775 d->Luu(2 * foot, 2 * foot + 1) +=
776 vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
777 d->Luu(2 * foot + 1, 2 * foot) +=
778 vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
780 d->Lx(12 + 2 * foot) += -vel_weight_ * oRh_(1, 0) * alpha_v(i) *
781 rb_vely_max_(i, 2 * foot + 1);
782 d->Lx(12 + 2 * foot + 1) += -vel_weight_ * oRh_(1, 1) * alpha_v(i) *
783 rb_vely_max_(i, 2 * foot + 1);
785 d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
786 vel_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_v(i), 2);
787 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
788 vel_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_v(i), 2);
790 d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
791 vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
792 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
793 vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
795 d->Lxu(12 + 2 * foot, 2 * foot) +=
796 vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(1, 0), 2);
797 d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
798 vel_weight_ * pow(alpha_v(i), 2) * oRh_(1, 0) * oRh_(1, 1);
799 d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
800 vel_weight_ * pow(alpha_v(i), 2) * oRh_(1, 0) * oRh_(1, 1);
801 d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
802 vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(1, 1), 2);
809 if (is_jerk_activated_) {
810 for (
int foot = 0; foot < 4; foot++) {
811 if (S_[foot] ==
Scalar(1)) {
814 jerk_weight_ * oRh_(0, 0) * alpha_j * rb_jerk_(0, foot);
815 d->Lu(2 * foot + 1) +=
816 jerk_weight_ * oRh_(0, 1) * alpha_j * rb_jerk_(0, foot);
818 d->Luu(2 * foot, 2 * foot) +=
819 jerk_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_j, 2);
820 d->Luu(2 * foot + 1, 2 * foot + 1) +=
821 jerk_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_j, 2);
823 d->Luu(2 * foot, 2 * foot + 1) +=
824 jerk_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_j, 2);
825 d->Luu(2 * foot + 1, 2 * foot) +=
826 jerk_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_j, 2);
828 d->Lx(12 + 2 * foot) +=
829 jerk_weight_ * oRh_(0, 0) * alpha_j * rb_jerk_(0, foot);
830 d->Lx(12 + 2 * foot + 1) +=
831 jerk_weight_ * oRh_(0, 1) * alpha_j * rb_jerk_(0, foot);
833 d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
834 jerk_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_j, 2);
835 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
836 jerk_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_j, 2);
838 d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
839 jerk_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_j, 2);
840 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
841 jerk_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_j, 2);
843 d->Lxu(12 + 2 * foot, 2 * foot) +=
844 jerk_weight_ * pow(alpha_j, 2) * pow(oRh_(0, 0), 2);
845 d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
846 jerk_weight_ * pow(alpha_j, 2) * oRh_(0, 0) * oRh_(0, 1);
847 d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
848 jerk_weight_ * pow(alpha_j, 2) * oRh_(0, 0) * oRh_(0, 1);
849 d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
850 jerk_weight_ * pow(alpha_j, 2) * pow(oRh_(0, 1), 2);
854 jerk_weight_ * oRh_(1, 0) * alpha_j * rb_jerk_(1, foot);
855 d->Lu(2 * foot + 1) +=
856 jerk_weight_ * oRh_(1, 1) * alpha_j * rb_jerk_(1, foot);
858 d->Luu(2 * foot, 2 * foot) +=
859 jerk_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_j, 2);
860 d->Luu(2 * foot + 1, 2 * foot + 1) +=
861 jerk_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_j, 2);
863 d->Luu(2 * foot, 2 * foot + 1) +=
864 jerk_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_j, 2);
865 d->Luu(2 * foot + 1, 2 * foot) +=
866 jerk_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_j, 2);
868 d->Lx(12 + 2 * foot) +=
869 jerk_weight_ * oRh_(1, 0) * alpha_j * rb_jerk_(1, foot);
870 d->Lx(12 + 2 * foot + 1) +=
871 jerk_weight_ * oRh_(1, 1) * alpha_j * rb_jerk_(1, foot);
873 d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
874 jerk_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_j, 2);
875 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
876 jerk_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_j, 2);
878 d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
879 jerk_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_j, 2);
880 d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
881 jerk_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_j, 2);
883 d->Lxu(12 + 2 * foot, 2 * foot) +=
884 jerk_weight_ * pow(alpha_j, 2) * pow(oRh_(1, 0), 2);
885 d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
886 jerk_weight_ * pow(alpha_j, 2) * oRh_(1, 0) * oRh_(1, 1);
887 d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
888 jerk_weight_ * pow(alpha_j, 2) * oRh_(1, 0) * oRh_(1, 1);
889 d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
890 jerk_weight_ * pow(alpha_j, 2) * pow(oRh_(1, 1), 2);
897 d->Fu.block(12, 0, 8, 8) = B;
900 template <
typename Scalar>
901 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
903 return boost::make_shared<ActionDataQuadrupedStepTpl<Scalar> >(
this);
910 template <
typename Scalar>
911 const typename Eigen::Matrix<Scalar, 12, 1>&
913 return state_weights_;
915 template <
typename Scalar>
917 const typename MathBase::VectorXs& weights) {
918 if (
static_cast<std::size_t
>(weights.size()) != 12) {
919 throw_pretty(
"Invalid argument: "
920 <<
"Weights vector has wrong dimension (it should be 12)");
922 state_weights_ = weights;
925 template <
typename Scalar>
926 const typename Eigen::Matrix<Scalar, 8, 1>&
928 return step_weights_;
930 template <
typename Scalar>
932 const typename MathBase::VectorXs& weights) {
933 if (
static_cast<std::size_t
>(weights.size()) != 8) {
934 throw_pretty(
"Invalid argument: "
935 <<
"Weights vector has wrong dimension (it should be 4)");
937 step_weights_ = weights;
940 template <
typename Scalar>
941 const typename Eigen::Matrix<Scalar, 8, 1>&
943 return heuristic_weights_;
945 template <
typename Scalar>
947 const typename MathBase::VectorXs& weights) {
948 if (
static_cast<std::size_t
>(weights.size()) != 8) {
949 throw_pretty(
"Invalid argument: "
950 <<
"Weights vector has wrong dimension (it should be 8)");
952 heuristic_weights_ = weights;
955 template <
typename Scalar>
957 return symmetry_term;
959 template <
typename Scalar>
961 const bool& sym_term) {
963 symmetry_term = sym_term;
966 template <
typename Scalar>
968 return centrifugal_term;
970 template <
typename Scalar>
972 const bool& cent_term) {
974 centrifugal_term = cent_term;
977 template <
typename Scalar>
982 template <
typename Scalar>
988 template <
typename Scalar>
990 return is_acc_activated_;
992 template <
typename Scalar>
994 const bool& is_activated) {
995 is_acc_activated_ = is_activated;
998 template <
typename Scalar>
999 const typename Eigen::Matrix<Scalar, 2, 1>&
1003 template <
typename Scalar>
1005 const typename MathBase::VectorXs& acceleration_lim_) {
1006 if (
static_cast<std::size_t
>(acceleration_lim_.size()) != 2) {
1007 throw_pretty(
"Invalid argument: "
1008 <<
"Acceleration limit vector [ax_max, ay_max] has wrong "
1009 "dimension (it should be 2)");
1011 acc_lim_ = acceleration_lim_;
1014 template <
typename Scalar>
1018 template <
typename Scalar>
1021 acc_weight_ = weight_;
1024 template <
typename Scalar>
1026 return is_vel_activated_;
1028 template <
typename Scalar>
1030 const bool& is_activated) {
1031 is_vel_activated_ = is_activated;
1034 template <
typename Scalar>
1035 const typename Eigen::Matrix<Scalar, 2, 1>&
1039 template <
typename Scalar>
1041 const typename MathBase::VectorXs& velocity_lim_) {
1042 if (
static_cast<std::size_t
>(velocity_lim_.size()) != 2) {
1043 throw_pretty(
"Invalid argument: "
1044 <<
"Velocity limit vector [vx_max, vy_max] has wrong "
1045 "dimension (it should be 2)");
1047 vel_lim_ = velocity_lim_;
1050 template <
typename Scalar>
1054 template <
typename Scalar>
1057 vel_weight_ = weight_;
1060 template <
typename Scalar>
1062 return jerk_weight_;
1064 template <
typename Scalar>
1067 jerk_weight_ = weight_;
1070 template <
typename Scalar>
1072 return is_jerk_activated_;
1074 template <
typename Scalar>
1076 const bool& is_activated) {
1077 is_jerk_activated_ = is_activated;
1080 template <
typename Scalar>
1082 const int& n_sample) {
1083 N_sampling = n_sample;
1086 delta_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1088 gamma_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1090 for (
int k = 1; k < N_sampling; k++) {
1092 (float)k / (
float)N_sampling;
1094 delta_.col(1) << delta_.col(0).pow(2);
1095 delta_.col(2) << delta_.col(0).pow(3);
1096 delta_.col(3) << delta_.col(0).pow(4);
1099 60 * delta_.col(0) - 180 * delta_.col(1) + 120 * delta_.col(2);
1100 gamma_.col(1) = -36 * delta_.col(0) + 96 * delta_.col(1) - 60 * delta_.col(2);
1101 gamma_.col(2) = -9 * delta_.col(0) + 18 * delta_.col(1) - 10 * delta_.col(2);
1103 alpha_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1105 beta_x_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1107 beta_y_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1109 tmp_ones_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Ones(
1112 rb_accx_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1114 rb_accy_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1117 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
1120 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
1124 gamma_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1126 gamma_v.col(0) = 30 * delta_.col(1) - 60 * delta_.col(2) + 30 * delta_.col(3);
1127 gamma_v.col(1) = delta_.col(0);
1129 -18 * delta_.col(1) + 32 * delta_.col(2) - 15 * delta_.col(3);
1131 -4.5 * delta_.col(1) + 6 * delta_.col(2) - 2.5 * delta_.col(3);
1133 alpha_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1135 beta_x_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1137 beta_y_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1140 rb_velx_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1142 rb_vely_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1145 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
1148 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
1156 template <
typename Scalar>
1158 const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
1159 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
1160 const Eigen::Ref<const typename MathBase::VectorXs>& S,
1161 const Eigen::Ref<const typename MathBase::MatrixXs>& position,
1162 const Eigen::Ref<const typename MathBase::MatrixXs>& velocity,
1163 const Eigen::Ref<const typename MathBase::MatrixXs>& acceleration,
1164 const Eigen::Ref<const typename MathBase::MatrixXs>& jerk,
1165 const Eigen::Ref<const typename MathBase::MatrixXs>& oRh,
1166 const Eigen::Ref<const typename MathBase::MatrixXs>& oTh,
1168 if (
static_cast<std::size_t
>(l_feet.size()) != 12) {
1169 throw_pretty(
"Invalid argument: "
1170 <<
"l_feet matrix has wrong dimension (it should be : 3x4)");
1172 if (
static_cast<std::size_t
>(xref.size()) != 12) {
1173 throw_pretty(
"Invalid argument: "
1174 <<
"Weights vector has wrong dimension (it should be " +
1175 std::to_string(state_->get_nx()) +
")");
1177 if (
static_cast<std::size_t
>(S.size()) != 4) {
1178 throw_pretty(
"Invalid argument: "
1179 <<
"S vector has wrong dimension (it should be 4x1)");
1188 position_ = position;
1211 for (
int i = 0; i < 4; i = i + 1) {
1212 pheuristic_[2 * i] = l_feet(0, i);
1213 pheuristic_[2 * i + 1] = l_feet(1, i);
1221 B.block(0, 0, 2, 2).setIdentity();
1224 B.block(2, 2, 2, 2).setIdentity();
1227 B.block(4, 4, 2, 2).setIdentity();
1230 B.block(6, 6, 2, 2).setIdentity();
1233 alpha_ = (1 / pow(delta_T, 2)) * gamma_.col(0);
1234 alpha_j = (60 / pow(delta_T, 3));
1235 alpha_v = (1 / delta_T) * gamma_v.col(0);
1237 for (
int i = 0; i < 4; i++) {
1238 if (S[i] ==
Scalar(1) && is_acc_activated_) {
1239 beta_x_.col(i) = acceleration(0, i) * tmp_ones_ +
1240 (velocity(0, i) / delta_T) * gamma_.col(1) +
1241 acceleration(0, i) * gamma_.col(2);
1242 beta_y_.col(i) = acceleration(1, i) * tmp_ones_ +
1243 (velocity(1, i) / delta_T) * gamma_.col(1) +
1244 acceleration(1, i) * gamma_.col(2);
1246 beta_x_.col(i).setZero();
1247 beta_y_.col(i).setZero();
1250 if (S[i] ==
Scalar(1) && is_vel_activated_) {
1251 beta_x_v.col(i) = velocity(0, i) * tmp_ones_ +
1252 (acceleration(0, i) * delta_T) * gamma_v.col(1) +
1253 velocity(0, i) * gamma_.col(2) +
1254 (acceleration(0, i) * delta_T) * gamma_v.col(3);
1255 beta_y_v.col(i) = velocity(1, i) * tmp_ones_ +
1256 (acceleration(1, i) * delta_T) * gamma_v.col(1) +
1257 velocity(1, i) * gamma_.col(2) +
1258 (acceleration(1, i) * delta_T) * gamma_v.col(3);
1260 beta_x_v.col(i).setZero();
1261 beta_y_v.col(i).setZero();
1264 if (S[i] ==
Scalar(1) && is_jerk_activated_) {
1265 beta_j(0, i) = -(36 * velocity(0, i)) / pow(delta_T, 2) -
1266 (9 * acceleration(0, i)) / delta_T;
1267 beta_j(1, i) = -(36 * velocity(1, i)) / pow(delta_T, 2) -
1268 (9 * acceleration(1, i)) / delta_T;
1270 beta_j.col(i).setZero();