1 #ifndef __quadruped_walkgen_quadruped_step_period_hxx__
2 #define __quadruped_walkgen_quadruped_step_period_hxx__
4 #include "crocoddyl/core/utils/exception.hpp"
7 template <
typename Scalar>
9 : crocoddyl::ActionModelAbstractTpl<
Scalar>(
10 boost::make_shared<crocoddyl::StateVectorTpl<
Scalar> >(21), 5, 26) {
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.);
47 template <
typename Scalar>
49 Scalar>::~ActionModelQuadrupedStepPeriodTpl() {}
51 template <
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()) {
57 throw_pretty(
"Invalid argument: "
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_) {
62 throw_pretty(
"Invalid argument: "
63 <<
"u has wrong dimension (it should be " +
64 std::to_string(nu_) +
")");
70 d->xnext.template head<12>() = x.head(12);
71 d->xnext.template segment<8>(12) = x.segment(12, 8) + B * u.head(4);
72 d->xnext.template tail<1>() = u.tail(1);
75 d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
76 d->r.template segment<8>(12) =
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];
87 (rub_max_.array() >=
Scalar(0.)).matrix().template cast<Scalar>();
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();
98 template <
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()) {
104 throw_pretty(
"Invalid argument: "
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_) {
109 throw_pretty(
"Invalid argument: "
110 <<
"u has wrong dimension (it should be " +
111 std::to_string(nu_) +
")");
118 d->Lx.template head<12>() =
119 (state_weights_.array() * d->r.template head<12>().array()).matrix();
120 d->Lx.template segment<8>(12) =
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) *
128 d->Lx.template tail<1>() += dt_weight_ * d->r.template segment<1>(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.);
142 d->Lu.template head<4>() +=
143 (step_weights_.array() * d->r.template segment<4>(21).array()).matrix();
144 d->Lu.template tail<1>() += dt_weight_ * d->r.template tail<1>();
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;
206 template <
typename Scalar>
207 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
209 return boost::make_shared<ActionDataQuadrupedStepPeriodTpl<Scalar> >(
this);
216 template <
typename Scalar>
217 const typename Eigen::Matrix<Scalar, 12, 1>&
219 return state_weights_;
221 template <
typename Scalar>
223 const typename MathBase::VectorXs& weights) {
224 if (
static_cast<std::size_t
>(weights.size()) != 12) {
225 throw_pretty(
"Invalid argument: "
226 <<
"Weights vector has wrong dimension (it should be 12)");
228 state_weights_ = weights;
231 template <
typename Scalar>
232 const typename Eigen::Matrix<Scalar, 4, 1>&
234 return step_weights_;
236 template <
typename Scalar>
238 const typename MathBase::VectorXs& weights) {
239 if (
static_cast<std::size_t
>(weights.size()) != 4) {
240 throw_pretty(
"Invalid argument: "
241 <<
"Weights vector has wrong dimension (it should be 4)");
243 step_weights_ = weights;
246 template <
typename Scalar>
247 const typename Eigen::Matrix<Scalar, 8, 1>&
249 return shoulder_weights_;
251 template <
typename Scalar>
253 const typename MathBase::VectorXs& weights) {
254 if (
static_cast<std::size_t
>(weights.size()) != 8) {
255 throw_pretty(
"Invalid argument: "
256 <<
"Weights vector has wrong dimension (it should be 8)");
258 shoulder_weights_ = weights;
261 template <
typename Scalar>
262 const typename Eigen::Matrix<Scalar, 8, 1>&
266 template <
typename Scalar>
268 const typename MathBase::VectorXs& pos) {
269 if (
static_cast<std::size_t
>(pos.size()) != 8) {
270 throw_pretty(
"Invalid argument: "
271 <<
"Weights vector has wrong dimension (it should be 8)");
276 template <
typename Scalar>
279 return symmetry_term;
281 template <
typename Scalar>
283 const bool& sym_term) {
285 symmetry_term = sym_term;
288 template <
typename Scalar>
291 return centrifugal_term;
293 template <
typename Scalar>
295 const bool& cent_term) {
297 centrifugal_term = cent_term;
300 template <
typename Scalar>
305 template <
typename Scalar>
312 template <
typename Scalar>
317 template <
typename Scalar>
321 dt_weight_ = weight_;
324 template <
typename Scalar>
330 template <
typename Scalar>
334 speed_weight = weight_;
337 template <
typename Scalar>
341 return dt_bound_weight;
343 template <
typename Scalar>
347 dt_bound_weight = weight_;
350 template <
typename Scalar>
355 template <
typename Scalar>
360 beta_lim =
Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) / 225);
364 template <
typename Scalar>
369 template <
typename Scalar>
373 beta_lim =
Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) / 225);
377 template <
typename Scalar>
381 template <
typename Scalar>
387 template <
typename Scalar>
391 template <
typename Scalar>
397 template <
typename Scalar>
401 template <
typename Scalar>
411 template <
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) {
417 throw_pretty(
"Invalid argument: "
418 <<
"l_feet matrix has wrong dimension (it should be : 3x4)");
420 if (
static_cast<std::size_t
>(xref.size()) != 12) {
421 throw_pretty(
"Invalid argument: "
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) {
426 throw_pretty(
"Invalid argument: "
427 <<
"S vector has wrong dimension (it should be 4x1)");
432 R_tmp << cos(xref(5, 0)), -sin(xref(5, 0)),
Scalar(0), sin(xref(5, 0)),
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);
453 if (S(0, 0) ==
Scalar(1)) {
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_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
Definition: quadruped_step_period.hpp:133