9 : crocoddyl::ActionModelAbstractTpl<
Scalar>(
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(
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()) {
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_) {
356 <<
"u has wrong dimension (it should be " +
357 std::to_string(
nu_) +
")");
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_) {
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);
438 acc_weight_ *
pow(oRh_(0, 0), 2) *
pow(alpha_(
i), 2);
440 acc_weight_ *
pow(oRh_(0, 1), 2) *
pow(alpha_(
i), 2);
443 acc_weight_ * oRh_(0, 0) * oRh_(0, 1) *
pow(alpha_(
i), 2);
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);
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);
463 acc_weight_ *
pow(alpha_(
i), 2) *
pow(oRh_(0, 0), 2);
465 acc_weight_ *
pow(alpha_(
i), 2) * oRh_(0, 0) * oRh_(0, 1);
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);
478 acc_weight_ *
pow(oRh_(0, 0), 2) *
pow(alpha_(
i), 2);
480 acc_weight_ *
pow(oRh_(0, 1), 2) *
pow(alpha_(
i), 2);
483 acc_weight_ * oRh_(0, 0) * oRh_(0, 1) *
pow(alpha_(
i), 2);
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);
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);
503 acc_weight_ *
pow(alpha_(
i), 2) *
pow(oRh_(0, 0), 2);
505 acc_weight_ *
pow(alpha_(
i), 2) * oRh_(0, 0) * oRh_(0, 1);
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);
518 acc_weight_ *
pow(oRh_(1, 0), 2) *
pow(alpha_(
i), 2);
520 acc_weight_ *
pow(oRh_(1, 1), 2) *
pow(alpha_(
i), 2);
523 acc_weight_ * oRh_(1, 0) * oRh_(1, 1) *
pow(alpha_(
i), 2);
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);
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);
543 acc_weight_ *
pow(alpha_(
i), 2) *
pow(oRh_(1, 0), 2);
545 acc_weight_ *
pow(alpha_(
i), 2) * oRh_(1, 0) * oRh_(1, 1);
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);
558 acc_weight_ *
pow(oRh_(1, 0), 2) *
pow(alpha_(
i), 2);
560 acc_weight_ *
pow(oRh_(1, 1), 2) *
pow(alpha_(
i), 2);
563 acc_weight_ * oRh_(1, 0) * oRh_(1, 1) *
pow(alpha_(
i), 2);
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);
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);
583 acc_weight_ *
pow(alpha_(
i), 2) *
pow(oRh_(1, 0), 2);
585 acc_weight_ *
pow(alpha_(
i), 2) * oRh_(1, 0) * oRh_(1, 1);
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_) {
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);
651 vel_weight_ *
pow(oRh_(0, 0), 2) *
pow(alpha_v(
i), 2);
653 vel_weight_ *
pow(oRh_(0, 1), 2) *
pow(alpha_v(
i), 2);
656 vel_weight_ * oRh_(0, 0) * oRh_(0, 1) *
pow(alpha_v(
i), 2);
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);
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);
676 vel_weight_ *
pow(alpha_v(
i), 2) *
pow(oRh_(0, 0), 2);
678 vel_weight_ *
pow(alpha_v(
i), 2) * oRh_(0, 0) * oRh_(0, 1);
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);
691 vel_weight_ *
pow(oRh_(0, 0), 2) *
pow(alpha_v(
i), 2);
693 vel_weight_ *
pow(oRh_(0, 1), 2) *
pow(alpha_v(
i), 2);
696 vel_weight_ * oRh_(0, 0) * oRh_(0, 1) *
pow(alpha_v(
i), 2);
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);
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);
716 vel_weight_ *
pow(alpha_v(
i), 2) *
pow(oRh_(0, 0), 2);
718 vel_weight_ *
pow(alpha_v(
i), 2) * oRh_(0, 0) * oRh_(0, 1);
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);
731 vel_weight_ *
pow(oRh_(1, 0), 2) *
pow(alpha_v(
i), 2);
733 vel_weight_ *
pow(oRh_(1, 1), 2) *
pow(alpha_v(
i), 2);
736 vel_weight_ * oRh_(1, 0) * oRh_(1, 1) *
pow(alpha_v(
i), 2);
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);
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);
756 vel_weight_ *
pow(alpha_v(
i), 2) *
pow(oRh_(1, 0), 2);
758 vel_weight_ *
pow(alpha_v(
i), 2) * oRh_(1, 0) * oRh_(1, 1);
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);
771 vel_weight_ *
pow(oRh_(1, 0), 2) *
pow(alpha_v(
i), 2);
773 vel_weight_ *
pow(oRh_(1, 1), 2) *
pow(alpha_v(
i), 2);
776 vel_weight_ * oRh_(1, 0) * oRh_(1, 1) *
pow(alpha_v(
i), 2);
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);
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);
796 vel_weight_ *
pow(alpha_v(
i), 2) *
pow(oRh_(1, 0), 2);
798 vel_weight_ *
pow(alpha_v(
i), 2) * oRh_(1, 0) * oRh_(1, 1);
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_) {
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);
819 jerk_weight_ *
pow(oRh_(0, 0), 2) *
pow(alpha_j, 2);
821 jerk_weight_ *
pow(oRh_(0, 1), 2) *
pow(alpha_j, 2);
824 jerk_weight_ * oRh_(0, 0) * oRh_(0, 1) *
pow(alpha_j, 2);
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);
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);
844 jerk_weight_ *
pow(alpha_j, 2) *
pow(oRh_(0, 0), 2);
846 jerk_weight_ *
pow(alpha_j, 2) * oRh_(0, 0) * oRh_(0, 1);
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);
859 jerk_weight_ *
pow(oRh_(1, 0), 2) *
pow(alpha_j, 2);
861 jerk_weight_ *
pow(oRh_(1, 1), 2) *
pow(alpha_j, 2);
864 jerk_weight_ * oRh_(1, 0) * oRh_(1, 1) *
pow(alpha_j, 2);
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);
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);
884 jerk_weight_ *
pow(alpha_j, 2) *
pow(oRh_(1, 0), 2);
886 jerk_weight_ *
pow(alpha_j, 2) * oRh_(1, 0) * oRh_(1, 1);
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;
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,