1 #ifndef __quadruped_walkgen_quadruped_augmented_hxx__
2 #define __quadruped_walkgen_quadruped_augmented_hxx__
4 #include "crocoddyl/core/utils/exception.hpp"
7 template <
typename Scalar>
9 typename Eigen::Matrix<Scalar, 3, 1> offset_CoM)
10 : crocoddyl::ActionModelAbstractTpl<
Scalar>(
11 boost::make_shared<crocoddyl::StateVectorTpl<
Scalar> >(20), 12, 32) {
13 relative_forces =
true;
20 min_fz_in_contact =
Scalar(0.0);
21 max_fz_in_contact =
Scalar(25.0);
25 g[8] =
Scalar(-9.81) * dt_;
29 A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
35 force_weights_.setConstant(
Scalar(0.2));
39 friction_weight_ =
Scalar(10);
40 heuristic_weights_.setConstant(
Scalar(1));
41 stop_weights_.setConstant(
Scalar(1));
53 for (
int i = 0; i < 4; i = i + 1) {
54 ub(6 * i + 5) = max_fz_in_contact;
70 gait_double.setZero();
73 centrifugal_term =
true;
83 sh_weight.setConstant(
Scalar(1.));
86 pheuristic_.setZero();
87 offset_com = offset_CoM;
89 shoulder_reference_position =
false;
92 template <
typename Scalar>
95 template <
typename Scalar>
97 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
98 const Eigen::Ref<const typename MathBase::VectorXs>& x,
99 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
100 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
101 throw_pretty(
"Invalid argument: "
102 <<
"x has wrong dimension (it should be " +
103 std::to_string(state_->get_nx()) +
")");
105 if (
static_cast<std::size_t
>(u.size()) != nu_) {
106 throw_pretty(
"Invalid argument: "
107 <<
"u has wrong dimension (it should be " +
108 std::to_string(nu_) +
")");
115 for (
int i = 0; i < 4; i = i + 1) {
117 if (gait(i, 0) != 0) {
118 lever_tmp.head(2) = x.block(12 + 2 * i, 0, 2, 1);
119 lever_tmp += -x.block(0, 0, 3, 1);
120 R_tmp <<
Scalar(0.0), -lever_tmp[2], lever_tmp[1], lever_tmp[2],
121 Scalar(0.0), -lever_tmp[0], -lever_tmp[1], lever_tmp[0],
Scalar(0.0);
122 B.block(9, 3 * i, 3, 3) << dt_ * R * R_tmp;
125 if (shoulder_reference_position) {
128 psh.block(0, i, 3, 1) << xref_(0, 0) - offset_com(0, 0) +
129 pshoulder_0(0, i) * cos(xref_(5, 0)) -
130 pshoulder_0(1, i) * sin(xref_(5, 0)) -
132 xref_(1, 0) - offset_com(1, 0) +
133 pshoulder_0(0, i) * sin(xref_(5, 0)) +
134 pshoulder_0(1, i) * cos(xref_(5, 0)) - x(12 + 2 * i + 1),
135 xref_(2, 0) - offset_com(2, 0);
144 psh.block(0, i, 3, 1)
145 << x(0) - offset_com(0, 0) + pshoulder_0(0, i) * cos(x(5)) -
146 pshoulder_0(1, i) * sin(x(5)) - x(12 + 2 * i),
147 x(1) - offset_com(1, 0) + pshoulder_0(0, i) * sin(x(5)) +
148 pshoulder_0(1, i) * cos(x(5)) - x(12 + 2 * i + 1),
149 x(2) - offset_com(2, 0) + pshoulder_0(1, i) * x(3) -
150 pshoulder_0(0, i) * x(4);
155 psh.block(0, i, 3, 1).setZero();
160 d->xnext.template head<12>() =
161 A.diagonal().cwiseProduct(x.block(0, 0, 12, 1)) + g;
162 d->xnext.template head<6>() =
163 d->xnext.template head<6>() +
164 A.topRightCorner(6, 6).diagonal().cwiseProduct(x.block(6, 0, 6, 1));
165 d->xnext.template segment<6>(6) =
166 d->xnext.template segment<6>(6) + B.block(6, 0, 6, 12) * u;
167 d->xnext.template tail<8>() = x.tail(8);
170 d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
171 d->r.template segment<8>(12) =
172 ((heuristic_weights_.cwiseProduct(x.tail(8) - pheuristic_)).array() *
175 d->r.template tail<12>() = force_weights_.cwiseProduct(u - uref_);
178 for (
int i = 0; i < 4; i = i + 1) {
179 Fa_x_u.segment(6 * i, 6) << u(3 * i) - mu * u(3 * i + 2),
180 -u(3 * i) - mu * u(3 * i + 2), u(3 * i + 1) - mu * u(3 * i + 2),
181 -u(3 * i + 1) - mu * u(3 * i + 2), -u(3 * i + 2), u(3 * i + 2);
183 rub_max_ = (Fa_x_u - ub).cwiseMax(
Scalar(0.));
186 sh_ub_max_ <<
Scalar(0.5) * sh_weight(0) *
187 (psh.block(0, 0, 3, 1).squaredNorm() - sh_hlim * sh_hlim),
188 Scalar(0.5) * sh_weight(1) *
189 (psh.block(0, 1, 3, 1).squaredNorm() - sh_hlim * sh_hlim),
190 Scalar(0.5) * sh_weight(2) *
191 (psh.block(0, 2, 3, 1).squaredNorm() - sh_hlim * sh_hlim),
192 Scalar(0.5) * sh_weight(3) *
193 (psh.block(0, 3, 3, 1).squaredNorm() - sh_hlim * sh_hlim);
195 sh_ub_max_ = sh_ub_max_.cwiseMax(
Scalar(0.));
205 Scalar(0.5) * d->r.transpose() * d->r +
206 friction_weight_ *
Scalar(0.5) * rub_max_.squaredNorm() +
207 Scalar(0.5) * ((stop_weights_.cwiseProduct(x.tail(8) - pstop_)).array() *
214 template <
typename Scalar>
216 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
217 const Eigen::Ref<const typename MathBase::VectorXs>& x,
218 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
219 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
220 throw_pretty(
"Invalid argument: "
221 <<
"x has wrong dimension (it should be " +
222 std::to_string(state_->get_nx()) +
")");
224 if (
static_cast<std::size_t
>(u.size()) != nu_) {
225 throw_pretty(
"Invalid argument: "
226 <<
"u has wrong dimension (it should be " +
227 std::to_string(nu_) +
")");
235 d->Lx.template head<12>() =
236 (state_weights_.array() * d->r.template head<12>().array()).matrix();
237 d->Lx.template tail<8>() =
238 (heuristic_weights_.array() * d->r.template segment<8>(12).array())
240 ((stop_weights_.cwiseProduct(x.tail(8) - pstop_)).array() *
241 gait_double.array() * stop_weights_.array())
245 for (
int i = 0; i < 4; i = i + 1) {
246 r = friction_weight_ * rub_max_.segment(6 * i, 6);
247 d->Lu.block(i * 3, 0, 3, 1) << r(0) - r(1), r(2) - r(3),
248 -mu * (r(0) + r(1) + r(2) + r(3)) - r(4) + r(5);
251 (force_weights_.array() * d->r.template tail<12>().array()).matrix();
257 d->Lxx.diagonal().head(12) =
258 (state_weights_.array() * state_weights_.array()).matrix();
259 d->Lxx.diagonal().tail(8) =
260 (gait_double.array() * heuristic_weights_.array() *
261 heuristic_weights_.array())
263 d->Lxx.diagonal().tail(8) +=
264 (gait_double.array() * stop_weights_.array() * stop_weights_.array())
268 for (
int j = 0; j < 4; j = j + 1) {
269 if (sh_ub_max_[j] >
Scalar(0.)) {
270 if (shoulder_reference_position) {
271 d->Lx(12 + 2 * j, 0) += -sh_weight(j) * psh(0, j);
272 d->Lx(12 + 2 * j + 1, 0) += -sh_weight(j) * psh(1, j);
274 d->Lxx(12 + 2 * j, 12 + 2 * j) += sh_weight(j);
275 d->Lxx(12 + 2 * j + 1, 12 + 2 * j + 1) += sh_weight(j);
327 d->Lx(0, 0) += sh_weight(j) * psh(0, j);
328 d->Lx(1, 0) += sh_weight(j) * psh(1, j);
329 d->Lx(2, 0) += sh_weight(j) * psh(2, j);
330 d->Lx(3, 0) += sh_weight(j) * pshoulder_0(1, j) * psh(2, j);
331 d->Lx(4, 0) += -sh_weight(j) * pshoulder_0(0, j) * psh(2, j);
334 ((-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j)) *
336 (cos(x(5)) * pshoulder_0(0, j) - sin(x(5)) * pshoulder_0(1, j)) *
339 d->Lx(12 + 2 * j, 0) += -sh_weight(j) * psh(0, j);
340 d->Lx(12 + 2 * j + 1, 0) += -sh_weight(j) * psh(1, j);
342 d->Lxx(0, 0) += sh_weight(j);
343 d->Lxx(1, 1) += sh_weight(j);
344 d->Lxx(2, 2) += sh_weight(j);
345 d->Lxx(3, 3) += sh_weight(j) * pshoulder_0(1, j) * pshoulder_0(1, j);
346 d->Lxx(3, 3) += sh_weight(j) * pshoulder_0(0, j) * pshoulder_0(0, j);
349 ((-cos(x(5)) * pshoulder_0(0, j) + sin(x(5)) * pshoulder_0(1, j)) *
351 (-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j)) *
352 (-sin(x(5)) * pshoulder_0(0, j) -
353 cos(x(5)) * pshoulder_0(1, j)) +
354 (-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j)) *
356 (cos(x(5)) * pshoulder_0(0, j) - sin(x(5)) * pshoulder_0(1, j)) *
357 (cos(x(5)) * pshoulder_0(0, j) -
358 sin(x(5)) * pshoulder_0(1, j)));
360 d->Lxx(12 + 2 * j, 12 + 2 * j) += sh_weight(j);
361 d->Lxx(12 + 2 * j + 1, 12 + 2 * j + 1) += sh_weight(j);
363 d->Lxx(0, 5) += sh_weight(j) * (-sin(x(5)) * pshoulder_0(0, j) -
364 cos(x(5)) * pshoulder_0(1, j));
365 d->Lxx(5, 0) += sh_weight(j) * (-sin(x(5)) * pshoulder_0(0, j) -
366 cos(x(5)) * pshoulder_0(1, j));
368 d->Lxx(1, 5) += sh_weight(j) * (cos(x(5)) * pshoulder_0(0, j) -
369 sin(x(5)) * pshoulder_0(1, j));
370 d->Lxx(5, 1) += sh_weight(j) * (cos(x(5)) * pshoulder_0(0, j) -
371 sin(x(5)) * pshoulder_0(1, j));
373 d->Lxx(2, 3) += sh_weight(j) * pshoulder_0(1, j);
374 d->Lxx(2, 4) += -sh_weight(j) * pshoulder_0(0, j);
375 d->Lxx(3, 2) += sh_weight(j) * pshoulder_0(1, j);
376 d->Lxx(4, 2) += -sh_weight(j) * pshoulder_0(0, j);
378 d->Lxx(3, 4) += -sh_weight(j) * pshoulder_0(1, j) * pshoulder_0(0, j);
379 d->Lxx(4, 3) += -sh_weight(j) * pshoulder_0(1, j) * pshoulder_0(0, j);
381 d->Lxx(0, 12 + 2 * j) += -sh_weight(j);
382 d->Lxx(12 + 2 * j, 0) += -sh_weight(j);
384 d->Lxx(5, 12 + 2 * j) +=
386 (-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j));
387 d->Lxx(12 + 2 * j, 5) +=
389 (-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j));
391 d->Lxx(1, 12 + 2 * j + 1) += -sh_weight(j);
392 d->Lxx(12 + 2 * j + 1, 1) += -sh_weight(j);
394 d->Lxx(5, 12 + 2 * j + 1) +=
396 (cos(x(5)) * pshoulder_0(0, j) - sin(x(5)) * pshoulder_0(1, j));
397 d->Lxx(12 + 2 * j + 1, 5) +=
399 (cos(x(5)) * pshoulder_0(0, j) - sin(x(5)) * pshoulder_0(1, j));
407 ((Fa_x_u - ub).array() >= 0.).matrix().template cast<Scalar>();
408 for (
int i = 0; i < 4; i = i + 1) {
409 r = friction_weight_ * Arr.diagonal().segment(6 * i, 6);
410 d->Luu.block(3 * i, 3 * i, 3, 3) << r(0) + r(1), 0.0, mu * (r(1) - r(0)),
411 0.0, r(2) + r(3), mu * (r(3) - r(2)), mu * (r(1) - r(0)),
412 mu * (r(3) - r(2)), mu * mu * (r(0) + r(1) + r(2) + r(3)) + r(4) + r(5);
416 (force_weights_.array() * force_weights_.array()).matrix();
420 d->Fx.block(0, 0, 12, 12) << A;
421 d->Fx.block(12, 12, 8, 8) << Eigen::Matrix<Scalar, 8, 8>::Identity();
423 for (
int i = 0; i < 4; i = i + 1) {
424 if (gait(i, 0) != 0) {
425 forces_3d = u.block(3 * i, 0, 3, 1);
426 d->Fx.block(9, 0, 3, 1) += -dt_ * R * (base_vector_x.cross(forces_3d));
427 d->Fx.block(9, 1, 3, 1) += -dt_ * R * (base_vector_y.cross(forces_3d));
428 d->Fx.block(9, 2, 3, 1) += -dt_ * R * (base_vector_z.cross(forces_3d));
430 d->Fx.block(9, 12 + 2 * i, 3, 1) +=
431 dt_ * R * (base_vector_x.cross(forces_3d));
432 d->Fx.block(9, 12 + 2 * i + 1, 3, 1) +=
433 dt_ * R * (base_vector_y.cross(forces_3d));
437 d->Fu.block(0, 0, 12, 12) << B;
440 template <
typename Scalar>
441 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
443 return boost::make_shared<ActionDataQuadrupedAugmentedTpl<Scalar> >(
this);
450 template <
typename Scalar>
451 const typename Eigen::Matrix<Scalar, 12, 1>&
453 return force_weights_;
455 template <
typename Scalar>
457 const typename MathBase::VectorXs& weights) {
458 if (
static_cast<std::size_t
>(weights.size()) != 12) {
459 throw_pretty(
"Invalid argument: "
460 <<
"Weights vector has wrong dimension (it should be 12)");
462 force_weights_ = weights;
465 template <
typename Scalar>
466 const typename Eigen::Matrix<Scalar, 12, 1>&
468 return state_weights_;
470 template <
typename Scalar>
472 const typename MathBase::VectorXs& weights) {
473 if (
static_cast<std::size_t
>(weights.size()) != 12) {
474 throw_pretty(
"Invalid argument: "
475 <<
"Weights vector has wrong dimension (it should be 12)");
477 state_weights_ = weights;
480 template <
typename Scalar>
481 const typename Eigen::Matrix<Scalar, 8, 1>&
483 return heuristic_weights_;
485 template <
typename Scalar>
487 const typename MathBase::VectorXs& weights) {
488 if (
static_cast<std::size_t
>(weights.size()) != 8) {
489 throw_pretty(
"Invalid argument: "
490 <<
"Weights vector has wrong dimension (it should be 8)");
492 heuristic_weights_ = weights;
495 template <
typename Scalar>
496 const typename Eigen::Matrix<Scalar, 8, 1>&
498 return stop_weights_;
500 template <
typename Scalar>
502 const typename MathBase::VectorXs& weights) {
503 if (
static_cast<std::size_t
>(weights.size()) != 8) {
504 throw_pretty(
"Invalid argument: "
505 <<
"Weights vector has wrong dimension (it should be 8)");
507 stop_weights_ = weights;
510 template <
typename Scalar>
513 return friction_weight_;
515 template <
typename Scalar>
518 friction_weight_ = weight;
521 template <
typename Scalar>
525 template <
typename Scalar>
530 template <
typename Scalar>
534 template <
typename Scalar>
540 template <
typename Scalar>
544 template <
typename Scalar>
548 g[8] =
Scalar(-9.81) * dt_;
549 A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
552 template <
typename Scalar>
553 const typename Eigen::Matrix<Scalar, 3, 3>&
557 template <
typename Scalar>
559 const typename MathBase::Matrix3s& inertia_matrix) {
561 if (
static_cast<std::size_t
>(inertia_matrix.size()) != 9) {
562 throw_pretty(
"Invalid argument: "
563 <<
"gI has wrong dimension : 3x3");
568 template <
typename Scalar>
572 return min_fz_in_contact;
574 template <
typename Scalar>
578 min_fz_in_contact = min_fz;
581 template <
typename Scalar>
585 return max_fz_in_contact;
587 template <
typename Scalar>
591 max_fz_in_contact = max_fz;
592 for (
int i = 0; i < 4; i = i + 1) {
593 ub(6 * i + 5) = max_fz_in_contact;
597 template <
typename Scalar>
600 return symmetry_term;
602 template <
typename Scalar>
604 const bool& sym_term) {
606 symmetry_term = sym_term;
609 template <
typename Scalar>
612 return centrifugal_term;
614 template <
typename Scalar>
616 const bool& cent_term) {
618 centrifugal_term = cent_term;
621 template <
typename Scalar>
623 Scalar>::get_shoulder_reference_position()
const {
624 return shoulder_reference_position;
626 template <
typename Scalar>
628 const bool& reference) {
629 shoulder_reference_position = reference;
632 template <
typename Scalar>
637 template <
typename Scalar>
644 template <
typename Scalar>
649 template <
typename Scalar>
656 template <
typename Scalar>
657 const typename Eigen::Matrix<Scalar, 4, 1>&
661 template <
typename Scalar>
663 const typename Eigen::Matrix<Scalar, 4, 1>& weight) {
671 template <
typename Scalar>
672 const typename Eigen::Matrix<Scalar, 12, 12>&
676 template <
typename Scalar>
677 const typename Eigen::Matrix<Scalar, 12, 12>&
684 template <
typename Scalar>
687 return relative_forces;
689 template <
typename Scalar>
691 const bool& rel_forces) {
692 relative_forces = rel_forces;
694 if (relative_forces) {
695 for (
int i = 0; i < 4; i = i + 1) {
697 uref_[3 * i + 2] = (
Scalar(9.81) * mass) / (gait.sum());
707 template <
typename Scalar>
709 const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
710 const Eigen::Ref<const typename MathBase::MatrixXs>& l_stop,
711 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
712 const Eigen::Ref<const typename MathBase::MatrixXs>& S) {
713 if (
static_cast<std::size_t
>(l_feet.size()) != 12) {
714 throw_pretty(
"Invalid argument: "
715 <<
"l_feet matrix has wrong dimension (it should be : 3x4)");
717 if (
static_cast<std::size_t
>(xref.size()) != 12) {
718 throw_pretty(
"Invalid argument: "
719 <<
"xref vector has wrong dimension (it should be 12 )");
721 if (
static_cast<std::size_t
>(S.size()) != 4) {
722 throw_pretty(
"Invalid argument: "
723 <<
"S vector has wrong dimension (it should be 4x1)");
730 if (relative_forces) {
731 for (
int i = 0; i < 4; i = i + 1) {
733 uref_[3 * i + 2] = (
Scalar(9.81) * mass) / (gait.sum());
738 for (
int i = 0; i < 4; i = i + 1) {
739 gait_double(2 * i, 0) = gait(i, 0);
740 gait_double(2 * i + 1, 0) = gait(i, 0);
742 pheuristic_.block(2 * i, 0, 2, 1) = l_feet.block(0, i, 2, 1);
743 pstop_.block(2 * i, 0, 2, 1) = l_stop.block(0, i, 2, 1);
746 R_tmp << cos(xref(5, 0)), -sin(xref(5, 0)),
Scalar(0), sin(xref(5, 0)),
763 R = (R_tmp.transpose() * gI * R_tmp).inverse();
765 for (
int i = 0; i < 4; i = i + 1) {
771 ub[5 * i + 4] = -min_fz_in_contact;
774 B.block(6, 3 * i, 3, 3).diagonal() << dt_ / mass, dt_ / mass, dt_ / mass;
786 ub[5 * i + 4] =
Scalar(0.0);
787 B.block(6, 3 * i, 3, 3).setZero();
788 B.block(9, 3 * i, 3, 3).setZero();
Definition: quadruped_augmented.hpp:14
const bool & get_centrifugal_term() const
Definition: quadruped_augmented.hxx:610
const Scalar & get_max_fz_contact() const
Definition: quadruped_augmented.hxx:582
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_augmented.hxx:638
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_augmented.hxx:482
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const typename MathBase::VectorXs > &x, const Eigen::Ref< const typename MathBase::VectorXs > &u)
Definition: quadruped_augmented.hxx:96
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:486
const Scalar & get_shoulder_hlim() const
Definition: quadruped_augmented.hxx:645
void set_min_fz_contact(const Scalar &min_fz)
Definition: quadruped_augmented.hxx:575
void set_stop_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:501
const Scalar & get_T_gait() const
Definition: quadruped_augmented.hxx:633
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition: quadruped_augmented.hxx:673
void set_friction_weight(const Scalar &weight)
Definition: quadruped_augmented.hxx:516
const Scalar & get_min_fz_contact() const
Definition: quadruped_augmented.hxx:569
const Eigen::Matrix< Scalar, 4, 1 > & get_shoulder_contact_weight() const
Definition: quadruped_augmented.hxx:658
~ActionModelQuadrupedAugmentedTpl()
Definition: quadruped_augmented.hxx:93
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &l_stop, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::MatrixXs > &S)
Definition: quadruped_augmented.hxx:708
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_augmented.hxx:467
const Scalar & get_friction_weight() const
Definition: quadruped_augmented.hxx:511
const Scalar & get_mu() const
Definition: quadruped_augmented.hxx:522
void set_max_fz_contact(const Scalar &max_fz)
Definition: quadruped_augmented.hxx:588
void set_relative_forces(const bool &rel_forces)
Definition: quadruped_augmented.hxx:690
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition: quadruped_augmented.hxx:558
const Scalar & get_dt() const
Definition: quadruped_augmented.hxx:541
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_augmented.hxx:442
void set_centrifugal_term(const bool ¢_term)
Definition: quadruped_augmented.hxx:615
void set_dt(const Scalar &dt)
Definition: quadruped_augmented.hxx:545
const bool & get_symmetry_term() const
Definition: quadruped_augmented.hxx:598
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition: quadruped_augmented.hxx:554
void set_shoulder_hlim(const Scalar &hlim)
Definition: quadruped_augmented.hxx:650
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:456
void set_mu(const Scalar &mu_coeff)
Definition: quadruped_augmented.hxx:526
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:471
_Scalar Scalar
Definition: quadruped_augmented.hpp:16
ActionModelQuadrupedAugmentedTpl(typename Eigen::Matrix< Scalar, 3, 1 > offset_CoM=Eigen::Matrix< Scalar, 3, 1 >::Zero())
Definition: quadruped_augmented.hxx:8
void set_mass(const Scalar &m)
Definition: quadruped_augmented.hxx:535
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_augmented.hxx:603
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const typename MathBase::VectorXs > &x, const Eigen::Ref< const typename MathBase::VectorXs > &u)
Definition: quadruped_augmented.hxx:215
void set_shoulder_contact_weight(const typename Eigen::Matrix< Scalar, 4, 1 > &weight)
Definition: quadruped_augmented.hxx:662
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition: quadruped_augmented.hxx:678
void set_shoulder_reference_position(const bool &reference)
Definition: quadruped_augmented.hxx:627
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition: quadruped_augmented.hxx:452
const bool & get_relative_forces() const
Definition: quadruped_augmented.hxx:685
const Scalar & get_mass() const
Definition: quadruped_augmented.hxx:531
const Eigen::Matrix< Scalar, 8, 1 > & get_stop_weights() const
Definition: quadruped_augmented.hxx:497
Definition: quadruped.hpp:11
Definition: quadruped_augmented.hpp:183