1 #ifndef __quadruped_walkgen_quadruped_augmented_time_hxx__
2 #define __quadruped_walkgen_quadruped_augmented_time_hxx__
4 #include "crocoddyl/core/utils/exception.hpp"
7 template <
typename Scalar>
8 ActionModelQuadrupedAugmentedTimeTpl<
10 : crocoddyl::ActionModelAbstractTpl<
Scalar>(
11 boost::make_shared<crocoddyl::StateVectorTpl<
Scalar> >(21), 12, 33) {
15 min_fz_in_contact =
Scalar(0.0);
19 relative_forces =
false;
32 force_weights_.setConstant(
Scalar(0.2));
36 friction_weight_ =
Scalar(10);
37 heuristicWeights.setConstant(
Scalar(1));
38 last_position_weights_.setConstant(
Scalar(1));
42 pheuristic_.setZero();
46 pshoulder_tmp.setZero();
47 pcentrifugal_tmp_1.setZero();
48 pcentrifugal_tmp_2.setZero();
49 pcentrifugal_tmp.setZero();
52 for (
int i = 0; i < 4; i = i + 1) {
53 ub(6 * i + 5) = max_fz;
68 gait_double.setZero();
71 centrifugal_term =
true;
76 rub_max_dt_bool.setZero();
78 dt_min_.setConstant(
Scalar(0.005));
79 dt_max_.setConstant(
Scalar(0.1));
80 dt_bound_weight =
Scalar(0.);
96 template <
typename Scalar>
98 Scalar>::~ActionModelQuadrupedAugmentedTimeTpl() {}
100 template <
typename Scalar>
102 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
103 const Eigen::Ref<const typename MathBase::VectorXs>& x,
104 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
105 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
106 throw_pretty(
"Invalid argument: "
107 <<
"x has wrong dimension (it should be " +
108 std::to_string(state_->get_nx()) +
")");
110 if (
static_cast<std::size_t
>(u.size()) != nu_) {
111 throw_pretty(
"Invalid argument: "
112 <<
"u has wrong dimension (it should be " +
113 std::to_string(nu_) +
")");
119 A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() *
121 g[8] =
Scalar(-9.81) * x.tail(1)[0];
124 for (
int i = 0; i < 4; i = i + 1) {
126 if (gait(i, 0) != 0) {
127 lever_tmp.head(2) = x.block(12 + 2 * i, 0, 2, 1);
128 lever_tmp += -x.block(0, 0, 3, 1);
129 R_tmp <<
Scalar(0.0), -lever_tmp[2], lever_tmp[1], lever_tmp[2],
130 Scalar(0.0), -lever_tmp[0], -lever_tmp[1], lever_tmp[0],
Scalar(0.0);
132 B.block(9, 3 * i, 3, 3) << x.tail(1)[0] * R * R_tmp;
133 B.block(6, 3 * i, 3, 3).diagonal() << x.tail(1)[0] / mass,
134 x.tail(1)[0] / mass, x.tail(1)[0] / mass;
137 psh.block(0, i, 3, 1) << x(0) + pshoulder_0(0, i) -
138 pshoulder_0(1, i) * x(5) - x(12 + 2 * i),
139 x(1) + pshoulder_0(1, i) + pshoulder_0(0, i) * x(5) -
141 x(2) + pshoulder_0(1, i) * x(3) - pshoulder_0(0, i) * x(4);
144 psh.block(0, i, 3, 1).setZero();
155 d->xnext.template head<12>() =
156 A.diagonal().cwiseProduct(x.block(0, 0, 12, 1)) + g;
157 d->xnext.template head<6>() =
158 d->xnext.template head<6>() +
159 A.topRightCorner(6, 6).diagonal().cwiseProduct(x.block(6, 0, 6, 1));
160 d->xnext.template segment<6>(6) =
161 d->xnext.template segment<6>(6) + B.block(6, 0, 6, 12) * u;
162 d->xnext.template segment<8>(12) = x.segment(12, 8);
163 d->xnext.template tail<1>() = x.tail(1);
166 d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
167 d->r.template segment<8>(12) =
168 ((heuristicWeights.cwiseProduct(x.segment(12, 8) - pheuristic_)).array() *
172 d->r.template tail<12>() = force_weights_.cwiseProduct(u - uref_);
175 for (
int i = 0; i < 4; i = i + 1) {
176 Fa_x_u.segment(6 * i, 6) << u(3 * i) - mu * u(3 * i + 2),
177 -u(3 * i) - mu * u(3 * i + 2), u(3 * i + 1) - mu * u(3 * i + 2),
178 -u(3 * i + 1) - mu * u(3 * i + 2), -u(3 * i + 2), u(3 * i + 2);
180 rub_max_ = (Fa_x_u - ub).cwiseMax(
Scalar(0.));
182 rub_max_dt << dt_min_ - x.tail(1), x.tail(1) - dt_max_;
184 (rub_max_dt.array() >=
Scalar(0.)).matrix().template cast<Scalar>();
185 rub_max_dt = rub_max_dt.cwiseMax(
Scalar(0.));
188 sh_ub_max_ << psh.block(0, 0, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
189 psh.block(0, 1, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
190 psh.block(0, 2, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
191 psh.block(0, 3, 3, 1).squaredNorm() - sh_hlim * sh_hlim;
193 sh_ub_max_ = sh_ub_max_.cwiseMax(
Scalar(0.));
197 Scalar(0.5) * d->r.segment(12, 8).transpose() * d->r.segment(12, 8) +
198 friction_weight_ *
Scalar(0.5) * rub_max_.squaredNorm() +
200 ((last_position_weights_.cwiseProduct(x.segment(12, 8) - pref_))
205 dt_bound_weight *
Scalar(0.5) * rub_max_dt.squaredNorm() +
206 x(20) *
Scalar(0.5) * d->r.head(12).transpose() * d->r.head(12) +
207 x(20) *
Scalar(0.5) * d->r.tail(12).transpose() * d->r.tail(12) +
208 sh_weight *
Scalar(0.5) * sh_ub_max_.sum();
211 cost_[0] = x(20) *
Scalar(0.5) * d->r.head(12).transpose() *
213 cost_[1] =
Scalar(0.5) * d->r.segment(12, 8).transpose() *
215 cost_[2] = x(20) *
Scalar(0.5) * d->r.tail(12).transpose() *
217 cost_[3] = dt_bound_weight *
Scalar(0.5) *
218 rub_max_dt.squaredNorm();
220 ((last_position_weights_.cwiseProduct(x.segment(12, 8) - pref_))
225 cost_[5] = friction_weight_ *
Scalar(0.5) *
226 rub_max_.squaredNorm();
230 template <
typename Scalar>
232 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
233 const Eigen::Ref<const typename MathBase::VectorXs>& x,
234 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
235 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
236 throw_pretty(
"Invalid argument: "
237 <<
"x has wrong dimension (it should be " +
238 std::to_string(state_->get_nx()) +
")");
240 if (
static_cast<std::size_t
>(u.size()) != nu_) {
241 throw_pretty(
"Invalid argument: "
242 <<
"u has wrong dimension (it should be " +
243 std::to_string(nu_) +
")");
251 d->Lx.template head<12>() =
253 (state_weights_.array() * d->r.template head<12>().array()).matrix();
254 d->Lx.template segment<8>(12) =
255 (heuristicWeights.array() * d->r.template segment<8>(12).array())
257 ((last_position_weights_.cwiseProduct(x.segment(12, 8) - pref_)).array() *
258 gait_double.array() * last_position_weights_.array())
260 d->Lx.template tail<1>() << dt_bound_weight *
261 (-rub_max_dt[0] + rub_max_dt[1]);
264 d->Lx.template tail<1>() +=
265 Scalar(0.5) * d->r.head(12).transpose() * d->r.head(12) +
266 Scalar(0.5) * d->r.tail(12).transpose() * d->r.tail(12);
269 for (
int i = 0; i < 4; i = i + 1) {
270 r = friction_weight_ * rub_max_.segment(6 * i, 6);
271 d->Lu.block(i * 3, 0, 3, 1) << r(0) - r(1), r(2) - r(3),
272 -mu * (r(0) + r(1) + r(2) + r(3)) - r(4) + r(5);
277 (force_weights_.array() * d->r.template tail<12>().array()).matrix();
281 d->Lxx.diagonal().head(12) =
282 x(20) * (state_weights_.array() * state_weights_.array()).matrix();
283 d->Lxx.diagonal().segment(12, 8) =
284 (gait_double.array() * heuristicWeights.array() *
285 heuristicWeights.array())
287 d->Lxx.diagonal().segment(12, 8) +=
288 (gait_double.array() * last_position_weights_.array() *
289 last_position_weights_.array())
292 d->Lxx.diagonal().tail(1) << dt_bound_weight * rub_max_dt_bool[0] +
293 dt_bound_weight * rub_max_dt_bool[1];
296 d->Lxx.col(20).head(12) =
297 (state_weights_.array() * d->r.template head<12>().array()).matrix();
298 d->Lxx.row(20).head(12) = d->Lxx.col(20).head(12);
300 for (
int j = 0; j < 4; j = j + 1) {
301 if (sh_ub_max_[j] >
Scalar(0.)) {
302 d->Lx(0, 0) += sh_weight * psh(0, j);
303 d->Lx(1, 0) += sh_weight * psh(1, j);
304 d->Lx(2, 0) += sh_weight * psh(2, j);
305 d->Lx(3, 0) += sh_weight * pshoulder_0(1, j) * psh(2, j);
306 d->Lx(4, 0) += -sh_weight * pshoulder_0(0, j) * psh(2, j);
307 d->Lx(5, 0) += sh_weight * (-pshoulder_0(1, j) * psh(0, j) +
308 pshoulder_0(0, j) * psh(1, j));
310 d->Lx(12 + 2 * j, 0) += -sh_weight * psh(0, j);
311 d->Lx(12 + 2 * j + 1, 0) += -sh_weight * psh(1, j);
313 d->Lxx(0, 0) += sh_weight;
314 d->Lxx(1, 1) += sh_weight;
315 d->Lxx(2, 2) += sh_weight;
316 d->Lxx(3, 3) += sh_weight * pshoulder_0(1, j) * pshoulder_0(1, j);
317 d->Lxx(3, 3) += sh_weight * pshoulder_0(0, j) * pshoulder_0(0, j);
318 d->Lxx(5, 5) += sh_weight * (pshoulder_0(1, j) * pshoulder_0(1, j) +
319 pshoulder_0(0, j) * pshoulder_0(0, j));
321 d->Lxx(12 + 2 * j, 12 + 2 * j) += sh_weight;
322 d->Lxx(12 + 2 * j + 1, 12 + 2 * j + 1) += sh_weight;
324 d->Lxx(0, 5) += -sh_weight * pshoulder_0(1, j);
325 d->Lxx(5, 0) += -sh_weight * pshoulder_0(1, j);
327 d->Lxx(1, 5) += sh_weight * pshoulder_0(0, j);
328 d->Lxx(5, 1) += sh_weight * pshoulder_0(0, j);
330 d->Lxx(2, 3) += sh_weight * pshoulder_0(1, j);
331 d->Lxx(2, 4) += -sh_weight * pshoulder_0(0, j);
332 d->Lxx(3, 2) += sh_weight * pshoulder_0(1, j);
333 d->Lxx(4, 2) += -sh_weight * pshoulder_0(0, j);
335 d->Lxx(3, 4) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
336 d->Lxx(4, 3) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
338 d->Lxx(0, 12 + 2 * j) += -sh_weight;
339 d->Lxx(12 + 2 * j, 0) += -sh_weight;
341 d->Lxx(5, 12 + 2 * j) += sh_weight * pshoulder_0(1, j);
342 d->Lxx(12 + 2 * j, 5) += sh_weight * pshoulder_0(1, j);
344 d->Lxx(1, 12 + 2 * j + 1) += -sh_weight;
345 d->Lxx(12 + 2 * j + 1, 1) += -sh_weight;
347 d->Lxx(5, 12 + 2 * j + 1) += -sh_weight * pshoulder_0(0, j);
348 d->Lxx(12 + 2 * j + 1, 5) += -sh_weight * pshoulder_0(0, j);
355 ((Fa_x_u - ub).array() >=
Scalar(0.)).matrix().template cast<Scalar>();
356 for (
int i = 0; i < 4; i = i + 1) {
357 r = friction_weight_ * Arr.diagonal().segment(6 * i, 6);
358 d->Luu.block(3 * i, 3 * i, 3, 3) << r(0) + r(1), 0.0, mu * (r(1) - r(0)),
359 0.0, r(2) + r(3), mu * (r(3) - r(2)), mu * (r(1) - r(0)),
360 mu * (r(3) - r(2)), mu * mu * (r(0) + r(1) + r(2) + r(3)) + r(4) + r(5);
364 x(20) * (force_weights_.array() * force_weights_.array()).matrix();
367 (force_weights_.array() * d->r.template tail<12>().array()).matrix();
371 d->Fx.block(0, 0, 12, 12) << A;
372 d->Fx.block(12, 12, 8, 8) << Eigen::Matrix<Scalar, 8, 8>::Identity();
373 d->Fx.block(20, 20, 1, 1) <<
Scalar(1);
374 d->Fx.block(8, 20, 1, 1) << -
Scalar(9.81);
375 d->Fx.block(0, 20, 8, 1) << x.segment(6, 6);
377 for (
int i = 0; i < 4; i = i + 1) {
378 if (gait(i, 0) != 0) {
379 forces_3d = u.block(3 * i, 0, 3, 1);
380 d->Fx.block(9, 0, 3, 1) +=
381 -x.tail(1)[0] * R * (base_vector_x.cross(forces_3d));
382 d->Fx.block(9, 1, 3, 1) +=
383 -x.tail(1)[0] * R * (base_vector_y.cross(forces_3d));
384 d->Fx.block(9, 2, 3, 1) +=
385 -x.tail(1)[0] * R * (base_vector_z.cross(forces_3d));
387 d->Fx.block(9, 12 + 2 * i, 3, 1) +=
388 x.tail(1)[0] * R * (base_vector_x.cross(forces_3d));
389 d->Fx.block(9, 12 + 2 * i + 1, 3, 1) +=
390 x.tail(1)[0] * R * (base_vector_y.cross(forces_3d));
392 d->Fx.block(6, 20, 3, 1) += (1 / mass) * forces_3d;
395 lever_tmp.head(2) = x.block(12 + 2 * i, 0, 2, 1);
396 lever_tmp += -x.block(0, 0, 3, 1);
397 R_tmp <<
Scalar(0.0), -lever_tmp[2], lever_tmp[1], lever_tmp[2],
398 Scalar(0.0), -lever_tmp[0], -lever_tmp[1], lever_tmp[0],
Scalar(0.0);
399 d->Fx.block(9, 20, 3, 1) += R * R_tmp * forces_3d;
403 d->Fu.block(0, 0, 12, 12) << B;
406 template <
typename Scalar>
407 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
409 return boost::make_shared<ActionDataQuadrupedAugmentedTimeTpl<Scalar> >(
this);
416 template <
typename Scalar>
417 const typename Eigen::Matrix<Scalar, 12, 1>&
419 return force_weights_;
421 template <
typename Scalar>
423 const typename MathBase::VectorXs& weights) {
424 if (
static_cast<std::size_t
>(weights.size()) != 12) {
425 throw_pretty(
"Invalid argument: "
426 <<
"Weights vector has wrong dimension (it should be 12)");
428 force_weights_ = weights;
431 template <
typename Scalar>
432 const typename Eigen::Matrix<Scalar, 12, 1>&
434 return state_weights_;
436 template <
typename Scalar>
438 const typename MathBase::VectorXs& weights) {
439 if (
static_cast<std::size_t
>(weights.size()) != 12) {
440 throw_pretty(
"Invalid argument: "
441 <<
"Weights vector has wrong dimension (it should be 12)");
443 state_weights_ = weights;
446 template <
typename Scalar>
447 const typename Eigen::Matrix<Scalar, 8, 1>&
449 return heuristicWeights;
451 template <
typename Scalar>
453 const typename MathBase::VectorXs& weights) {
454 if (
static_cast<std::size_t
>(weights.size()) != 8) {
455 throw_pretty(
"Invalid argument: "
456 <<
"Weights vector has wrong dimension (it should be 8)");
458 heuristicWeights = weights;
461 template <
typename Scalar>
462 const typename Eigen::Matrix<Scalar, 8, 1>&
466 template <
typename Scalar>
468 const typename MathBase::VectorXs& pos) {
469 if (
static_cast<std::size_t
>(pos.size()) != 8) {
470 throw_pretty(
"Invalid argument: "
471 <<
"Weights vector has wrong dimension (it should be 8)");
476 template <
typename Scalar>
477 const typename Eigen::Matrix<Scalar, 8, 1>&
479 return last_position_weights_;
481 template <
typename Scalar>
483 const typename MathBase::VectorXs& weights) {
484 if (
static_cast<std::size_t
>(weights.size()) != 8) {
485 throw_pretty(
"Invalid argument: "
486 <<
"Weights vector has wrong dimension (it should be 8)");
488 last_position_weights_ = weights;
491 template <
typename Scalar>
494 return friction_weight_;
496 template <
typename Scalar>
499 friction_weight_ = weight;
502 template <
typename Scalar>
506 template <
typename Scalar>
512 template <
typename Scalar>
516 template <
typename Scalar>
522 template <
typename Scalar>
526 template <
typename Scalar>
533 template <
typename Scalar>
537 template <
typename Scalar>
544 template <
typename Scalar>
545 const typename Eigen::Matrix<Scalar, 3, 3>&
549 template <
typename Scalar>
551 const typename MathBase::Matrix3s& inertia_matrix) {
553 if (
static_cast<std::size_t
>(inertia_matrix.size()) != 9) {
554 throw_pretty(
"Invalid argument: "
555 <<
"gI has wrong dimension : 3x3");
560 template <
typename Scalar>
564 return min_fz_in_contact;
566 template <
typename Scalar>
570 min_fz_in_contact = min_fz;
573 template <
typename Scalar>
579 template <
typename Scalar>
584 for (
int i = 0; i < 4; i = i + 1) {
585 ub(6 * i + 5) = max_fz;
589 template <
typename Scalar>
594 template <
typename Scalar>
601 template <
typename Scalar>
603 Scalar>::get_shoulder_contact_weight()
const {
606 template <
typename Scalar>
616 template <
typename Scalar>
619 return symmetry_term;
621 template <
typename Scalar>
623 const bool& sym_term) {
625 symmetry_term = sym_term;
628 template <
typename Scalar>
631 return centrifugal_term;
633 template <
typename Scalar>
635 const bool& cent_term) {
637 centrifugal_term = cent_term;
640 template <
typename Scalar>
645 template <
typename Scalar>
655 template <
typename Scalar>
659 return dt_bound_weight;
661 template <
typename Scalar>
665 dt_bound_weight = weight_;
669 template <
typename Scalar>
670 const typename Eigen::Matrix<Scalar, 7, 1>&
678 template <
typename Scalar>
679 const typename Eigen::Matrix<Scalar, 12, 12>&
683 template <
typename Scalar>
684 const typename Eigen::Matrix<Scalar, 12, 12>&
691 template <
typename Scalar>
694 return relative_forces;
696 template <
typename Scalar>
698 const bool& rel_forces) {
699 relative_forces = rel_forces;
701 if (relative_forces) {
702 for (
int i = 0; i < 4; i = i + 1) {
704 uref_[3 * i + 2] = (
Scalar(9.81) * mass) / (gait.sum());
714 template <
typename Scalar>
716 const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
717 const Eigen::Ref<const typename MathBase::MatrixXs>& l_stop,
718 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
719 const Eigen::Ref<const typename MathBase::MatrixXs>& S) {
720 if (
static_cast<std::size_t
>(l_feet.size()) != 12) {
721 throw_pretty(
"Invalid argument: "
722 <<
"l_feet matrix has wrong dimension (it should be : 3x4)");
724 if (
static_cast<std::size_t
>(xref.size()) != 12) {
725 throw_pretty(
"Invalid argument: "
726 <<
"xref vector has wrong dimension (it should be 12 )");
728 if (
static_cast<std::size_t
>(S.size()) != 4) {
729 throw_pretty(
"Invalid argument: "
730 <<
"S vector has wrong dimension (it should be 4x1)");
736 if (relative_forces) {
737 for (
int i = 0; i < 4; i = i + 1) {
739 uref_[3 * i + 2] = (
Scalar(9.81) * mass) / (gait.sum());
744 for (
int i = 0; i < 4; i = i + 1) {
745 gait_double(2 * i, 0) = gait(i, 0);
746 gait_double(2 * i + 1, 0) = gait(i, 0);
748 pref_.block(2 * i, 0, 2, 1) = l_stop.block(0, i, 2, 1);
749 pheuristic_.block(2 * i, 0, 2, 1) = l_feet.block(0, i, 2, 1);
752 R_tmp << cos(xref(5, 0)), -sin(xref(5, 0)),
Scalar(0), sin(xref(5, 0)),
768 R = (R_tmp * gI).inverse();
770 for (
int i = 0; i < 4; i = i + 1) {
776 ub(6 * i + 4) = -min_fz_in_contact;
779 ub(6 * i + 4) =
Scalar(0.0);
780 B.block(6, 3 * i, 3, 3).setZero();
781 B.block(9, 3 * i, 3, 3).setZero();
Definition: quadruped_augmented_time.hpp:14
const Eigen::Matrix< Scalar, 8, 1 > & get_shoulder_position() const
Definition: quadruped_augmented_time.hxx:463
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_augmented_time.hxx:622
const Eigen::Matrix< Scalar, 7, 1 > & get_cost() const
Definition: quadruped_augmented_time.hxx:671
void set_dt_min(const Scalar &dt)
Definition: quadruped_augmented_time.hxx:527
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition: quadruped_augmented_time.hxx:680
const Scalar & get_T_gait() const
Definition: quadruped_augmented_time.hxx:641
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:437
void set_shoulder_contact_weight(const Scalar &weight)
Definition: quadruped_augmented_time.hxx:607
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:452
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_augmented_time.hxx:433
const bool & get_relative_forces() const
Definition: quadruped_augmented_time.hxx:692
const Scalar & get_min_fz_contact() const
Definition: quadruped_augmented_time.hxx:561
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_time.hxx:231
const bool & get_symmetry_term() const
Definition: quadruped_augmented_time.hxx:617
void set_stop_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:482
void set_max_fz_contact(const Scalar &max_fz_)
Definition: quadruped_augmented_time.hxx:580
const Scalar & get_shoulder_hlim() const
Definition: quadruped_augmented_time.hxx:590
void set_centrifugal_term(const bool ¢_term)
Definition: quadruped_augmented_time.hxx:634
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_augmented_time.hxx:646
void set_shoulder_hlim(const Scalar &hlim)
Definition: quadruped_augmented_time.hxx:595
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition: quadruped_augmented_time.hxx:685
void set_mass(const Scalar &m)
Definition: quadruped_augmented_time.hxx:517
void set_relative_forces(const bool &rel_forces)
Definition: quadruped_augmented_time.hxx:697
const Scalar & get_dt_max() const
Definition: quadruped_augmented_time.hxx:534
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_time.hxx:101
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_time.hxx:715
const Eigen::Matrix< Scalar, 8, 1 > & get_stop_weights() const
Definition: quadruped_augmented_time.hxx:478
void set_dt_max(const Scalar &dt)
Definition: quadruped_augmented_time.hxx:538
const Scalar & get_mu() const
Definition: quadruped_augmented_time.hxx:503
const bool & get_centrifugal_term() const
Definition: quadruped_augmented_time.hxx:629
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_augmented_time.hxx:448
void set_dt_bound_weight(const Scalar &weight_)
Definition: quadruped_augmented_time.hxx:662
const Scalar & get_dt_min() const
Definition: quadruped_augmented_time.hxx:523
const Scalar & get_friction_weight() const
Definition: quadruped_augmented_time.hxx:493
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_augmented_time.hxx:408
const Scalar & get_dt_bound_weight() const
Definition: quadruped_augmented_time.hxx:657
void set_friction_weight(const Scalar &weight)
Definition: quadruped_augmented_time.hxx:497
const Scalar & get_mass() const
Definition: quadruped_augmented_time.hxx:513
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition: quadruped_augmented_time.hxx:546
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:422
_Scalar Scalar
Definition: quadruped_augmented_time.hpp:16
void set_mu(const Scalar &mu_coeff)
Definition: quadruped_augmented_time.hxx:507
void set_shoulder_position(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented_time.hxx:467
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition: quadruped_augmented_time.hxx:550
void set_min_fz_contact(const Scalar &min_fz)
Definition: quadruped_augmented_time.hxx:567
const Scalar & get_max_fz_contact() const
Definition: quadruped_augmented_time.hxx:574
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition: quadruped_augmented_time.hxx:418
Definition: quadruped.hpp:11
Definition: quadruped_augmented_time.hpp:199