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;