1#ifndef __quadruped_walkgen_quadruped_step_period_hxx__
2#define __quadruped_walkgen_quadruped_step_period_hxx__
4#include "crocoddyl/core/utils/exception.hpp"
7template <
typename Scalar>
9 : crocoddyl::ActionModelAbstractTpl<
Scalar>(
12 dt_ref_.setConstant(
Scalar(0.02));
14 rub_max_bool.setZero();
15 dt_min_.setConstant(
Scalar(0.005));
16 dt_max_.setConstant(
Scalar(0.1));
18 dt_bound_weight =
Scalar(10);
22 shoulder_weights_.setConstant(
Scalar(1));
29 pshoulder_tmp.setZero();
30 pcentrifugal_tmp_1.setZero();
31 pcentrifugal_tmp_2.setZero();
32 pcentrifugal_tmp.setZero();
33 centrifugal_term =
true;
37 step_weights_.setConstant(
Scalar(1));
42 beta_lim =
Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) /
44 speed_weight =
Scalar(10.);
47template <
typename Scalar>
49 Scalar>::~ActionModelQuadrupedStepPeriodTpl() {}
51template <
typename Scalar>
53 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >&
data,
54 const Eigen::Ref<const typename MathBase::VectorXs>&
x,
55 const Eigen::Ref<const typename MathBase::VectorXs>&
u) {
56 if (
static_cast<std::size_t
>(
x.size()) !=
state_->get_nx()) {
58 <<
"x has wrong dimension (it should be " +
59 std::to_string(
state_->get_nx()) +
")");
61 if (
static_cast<std::size_t
>(
u.size()) !=
nu_) {
63 <<
"u has wrong dimension (it should be " +
64 std::to_string(
nu_) +
")");
71 d->xnext.template
segment<8>(12) =
x.segment(12, 8) + B *
u.head(4);
75 d->r.template
head<12>() = state_weights_.cwiseProduct(
x.head(12) - xref_);
77 shoulder_weights_.cwiseProduct(
x.segment(12, 8) - pshoulder_);
78 d->r.template
segment<1>(20) = dt_weight_ * (
x.tail(1) - dt_ref_);
79 d->r.template
segment<4>(21) = step_weights_.cwiseProduct(
u.head(4));
80 d->r.template
tail<1>() = dt_weight_ * (
u.tail(1) - dt_ref_);
82 rub_max_ << dt_min_ -
x.tail(1),
x.tail(1) - dt_max_,
83 u[0] *
u[0] +
u[1] *
u[1] - beta_lim *
x[20] *
x[20],
84 u[2] *
u[2] +
u[3] *
u[3] - beta_lim *
x[20] *
x[20];
88 rub_max_ = rub_max_.cwiseMax(
Scalar(0.));
93 d->cost =
Scalar(0.5) *
d->r.transpose() *
d->r +
94 dt_bound_weight *
Scalar(0.5) * rub_max_.head(2).squaredNorm() +
95 speed_weight *
Scalar(0.5) * rub_max_.tail(2).sum();
98template <
typename Scalar>
100 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >&
data,
101 const Eigen::Ref<const typename MathBase::VectorXs>&
x,
102 const Eigen::Ref<const typename MathBase::VectorXs>&
u) {
103 if (
static_cast<std::size_t
>(
x.size()) !=
state_->get_nx()) {
105 <<
"x has wrong dimension (it should be " +
106 std::to_string(
state_->get_nx()) +
")");
108 if (
static_cast<std::size_t
>(
u.size()) !=
nu_) {
110 <<
"u has wrong dimension (it should be " +
111 std::to_string(
nu_) +
")");
121 (shoulder_weights_.array() *
d->r.template
segment<8>(12).array())
123 d->Lx.template
tail<1>() << dt_bound_weight * (-rub_max_[0] + rub_max_[1]) -
124 beta_lim * speed_weight *
x(20) *
126 beta_lim * speed_weight *
x(20) *
137 d->Lu << speed_weight *
u[0] * rub_max_bool[2],
138 speed_weight *
u[1] * rub_max_bool[2],
139 speed_weight *
u[2] * rub_max_bool[3],
140 speed_weight *
u[3] * rub_max_bool[3],
Scalar(0.);
147 d->Lxx.diagonal().head(12) =
148 (state_weights_.array() * state_weights_.array()).
matrix();
149 d->Lxx.diagonal().segment(12, 8) =
150 (shoulder_weights_.array() * shoulder_weights_.array()).
matrix();
151 d->Lxx.diagonal().tail(1) << dt_weight_ * dt_weight_ +
152 dt_bound_weight * rub_max_bool[0] +
153 dt_bound_weight * rub_max_bool[1];
155 d->Lxx(20, 20) += -beta_lim * speed_weight * rub_max_bool[2] -
156 beta_lim * speed_weight * rub_max_bool[3];
158 d->Luu.diagonal() << speed_weight * rub_max_bool[2],
159 speed_weight * rub_max_bool[2], speed_weight * rub_max_bool[3],
160 speed_weight * rub_max_bool[3],
Scalar(0.);
162 d->Luu.diagonal().head(4) +=
163 (step_weights_.array() * step_weights_.array()).
matrix();
201 d->Fx.block(20, 20, 1, 1) << 0;
202 d->Fu.block(12, 0, 8, 4) = B;
203 d->Fu.block(20, 4, 1, 1) << 1;
206template <
typename Scalar>
207boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
209 return boost::make_shared<ActionDataQuadrupedStepPeriodTpl<Scalar> >(
this);
216template <
typename Scalar>
217const typename Eigen::Matrix<Scalar, 12, 1>&
219 return state_weights_;
221template <
typename Scalar>
223 const typename MathBase::VectorXs&
weights) {
224 if (
static_cast<std::size_t
>(
weights.size()) != 12) {
226 <<
"Weights vector has wrong dimension (it should be 12)");
231template <
typename Scalar>
232const typename Eigen::Matrix<Scalar, 4, 1>&
234 return step_weights_;
236template <
typename Scalar>
238 const typename MathBase::VectorXs&
weights) {
239 if (
static_cast<std::size_t
>(
weights.size()) != 4) {
241 <<
"Weights vector has wrong dimension (it should be 4)");
246template <
typename Scalar>
247const typename Eigen::Matrix<Scalar, 8, 1>&
249 return shoulder_weights_;
251template <
typename Scalar>
253 const typename MathBase::VectorXs&
weights) {
254 if (
static_cast<std::size_t
>(
weights.size()) != 8) {
256 <<
"Weights vector has wrong dimension (it should be 8)");
261template <
typename Scalar>
262const typename Eigen::Matrix<Scalar, 8, 1>&
266template <
typename Scalar>
268 const typename MathBase::VectorXs&
pos) {
269 if (
static_cast<std::size_t
>(
pos.size()) != 8) {
271 <<
"Weights vector has wrong dimension (it should be 8)");
276template <
typename Scalar>
279 return symmetry_term;
281template <
typename Scalar>
288template <
typename Scalar>
291 return centrifugal_term;
293template <
typename Scalar>
300template <
typename Scalar>
305template <
typename Scalar>
312template <
typename Scalar>
317template <
typename Scalar>
324template <
typename Scalar>
330template <
typename Scalar>
337template <
typename Scalar>
341 return dt_bound_weight;
343template <
typename Scalar>
350template <
typename Scalar>
355template <
typename Scalar>
360 beta_lim =
Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) / 225);
364template <
typename Scalar>
369template <
typename Scalar>
373 beta_lim =
Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) / 225);
377template <
typename Scalar>
381template <
typename Scalar>
387template <
typename Scalar>
391template <
typename Scalar>
397template <
typename Scalar>
401template <
typename Scalar>
411template <
typename Scalar>
413 const Eigen::Ref<const typename MathBase::MatrixXs>&
l_feet,
414 const Eigen::Ref<const typename MathBase::MatrixXs>&
xref,
415 const Eigen::Ref<const typename MathBase::MatrixXs>&
S) {
416 if (
static_cast<std::size_t
>(
l_feet.size()) != 12) {
418 <<
"l_feet matrix has wrong dimension (it should be : 3x4)");
420 if (
static_cast<std::size_t
>(
xref.size()) != 12) {
422 <<
"Weights vector has wrong dimension (it should be " +
423 std::to_string(
state_->get_nx()) +
")");
425 if (
static_cast<std::size_t
>(
S.size()) != 4) {
427 <<
"S vector has wrong dimension (it should be 4x1)");
436 pcentrifugal_tmp_1 =
xref.block(6, 0, 3, 1);
437 pcentrifugal_tmp_2 =
xref.block(9, 0, 3, 1);
438 pcentrifugal_tmp = 0.5 * std::sqrt(
xref(2, 0) / 9.81) *
439 pcentrifugal_tmp_1.cross(pcentrifugal_tmp_2);
441 for (
int i = 0;
i < 4;
i =
i + 1) {
442 pshoulder_tmp.block(0,
i, 2, 1) =
443 R_tmp.block(0, 0, 2, 2) *
444 (pshoulder_0.block(0,
i, 2, 1) +
445 symmetry_term * 0.25 * T_gait *
xref.block(6, 0, 2, 1) +
446 centrifugal_term * pcentrifugal_tmp.block(0, 0, 2, 1));
447 pshoulder_[2 *
i] = pshoulder_tmp(0,
i) +
xref(0, 0);
448 pshoulder_[2 *
i + 1] = pshoulder_tmp(1,
i) +
xref(1, 0);
454 B.block(0, 0, 2, 2).setIdentity();
455 B.block(6, 2, 2, 2).setIdentity();
457 B.block(2, 0, 2, 2).setIdentity();
458 B.block(4, 2, 2, 2).setIdentity();
Definition quadruped_augmented_time.hpp:14
ActionModelQuadrupedAugmentedTimeTpl()
Definition quadruped_augmented_time.hxx:9
_Scalar Scalar
Definition quadruped_augmented_time.hpp:16
Definition quadruped_step_period.hpp:14
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::MatrixXs > &S)
Definition quadruped_step_period.hxx:412
const bool & get_centrifugal_term() const
Definition quadruped_step_period.hxx:289
const Scalar & get_dt_max() const
Definition quadruped_step_period.hxx:398
const Scalar & get_dt_weight() const
Definition quadruped_step_period.hxx:313
const Scalar & get_speed_weight() const
Definition quadruped_step_period.hxx:325
const Scalar & get_T_gait() const
Definition quadruped_step_period.hxx:301
_Scalar Scalar
Definition quadruped_step_period.hpp:16
const Eigen::Matrix< Scalar, 8, 1 > & get_shoulder_weights() const
Definition quadruped_step_period.hxx:248
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition quadruped_step_period.hxx:218
void set_centrifugal_term(const bool ¢_term)
Definition quadruped_step_period.hxx:294
void set_nb_nodes(const Scalar &nodes_)
Definition quadruped_step_period.hxx:356
const Scalar & get_dt_min() const
Definition quadruped_step_period.hxx:388
const bool & get_symmetry_term() const
Definition quadruped_step_period.hxx:277
void set_dt_min(const Scalar &dt)
Definition quadruped_step_period.hxx:392
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_step_period.hxx:99
const Scalar & get_nb_nodes() const
Definition quadruped_step_period.hxx:351
void set_dt_max(const Scalar &dt)
Definition quadruped_step_period.hxx:402
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition quadruped_step_period.hxx:222
void set_shoulder_weights(const typename MathBase::VectorXs &weights)
Definition quadruped_step_period.hxx:252
const Eigen::Matrix< Scalar, 4, 1 > & get_step_weights() const
Definition quadruped_step_period.hxx:233
void set_dt_weight(const Scalar &weight_)
Definition quadruped_step_period.hxx:318
ActionModelQuadrupedStepPeriodTpl()
Definition quadruped_step_period.hxx:8
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition quadruped_step_period.hxx:208
void set_T_gait(const Scalar &T_gait_)
Definition quadruped_step_period.hxx:306
const Scalar & get_dt_ref() const
Definition quadruped_step_period.hxx:378
void set_symmetry_term(const bool &sym_term)
Definition quadruped_step_period.hxx:282
const Scalar & get_vlim() const
Definition quadruped_step_period.hxx:365
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_step_period.hxx:52
void set_dt_bound_weight(const Scalar &weight_)
Definition quadruped_step_period.hxx:344
void set_speed_weight(const Scalar &weight_)
Definition quadruped_step_period.hxx:331
void set_shoulder_position(const typename MathBase::VectorXs &weights)
Definition quadruped_step_period.hxx:267
const Eigen::Matrix< Scalar, 8, 1 > & get_shoulder_position() const
Definition quadruped_step_period.hxx:263
void set_vlim(const Scalar &vlim_)
Definition quadruped_step_period.hxx:370
const Scalar & get_dt_bound_weight() const
Definition quadruped_step_period.hxx:338
void set_dt_ref(const Scalar &dt)
Definition quadruped_step_period.hxx:382
void set_step_weights(const typename MathBase::VectorXs &weights)
Definition quadruped_step_period.hxx:237
Definition quadruped.hpp:11