1#ifndef __quadruped_walkgen_quadruped_step_time_hxx__
2#define __quadruped_walkgen_quadruped_step_time_hxx__
4#include "crocoddyl/core/utils/exception.hpp"
7template <
typename Scalar>
9 : crocoddyl::ActionModelAbstractTpl<
Scalar>(
13 rub_max_bool.setZero();
18 heuristicWeights.setConstant(
Scalar(0.));
19 step_weights_.setConstant(
Scalar(1));
20 pheuristic_.setZero();
32 centrifugal_term =
true;
38 beta_lim =
Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) /
40 speed_weight =
Scalar(10.);
52 alpha = MathBase::ArrayXs::Zero(nb_alpha_);
54 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
55 b_coeff = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
59 b_coeff_x0 = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
62 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
64 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
67 b_coeff_y0 = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
70 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
72 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
75 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
77 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
79 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
81 Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
84 alpha2.col(0) << alpha;
85 alpha2.col(1) << alpha.pow(2);
86 alpha2.col(2) << alpha.pow(3);
87 alpha2.col(3) << alpha.pow(4);
89 b_coeff.col(0) =
Scalar(1.0) -
Scalar(18.) * alpha2.col(1) +
90 Scalar(32.) * alpha2.col(2) -
Scalar(15.) * alpha2.col(3);
91 b_coeff.col(1) = alpha2.col(0) -
Scalar(4.5) * alpha2.col(1) +
92 Scalar(6.) * alpha2.col(2) -
Scalar(2.5) * alpha2.col(3);
93 b_coeff.col(2) =
Scalar(30.) * alpha2.col(1) -
Scalar(60.) * alpha2.col(2) +
94 Scalar(30.) * alpha2.col(3);
99template <
typename Scalar>
102template <
typename Scalar>
104 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >&
data,
105 const Eigen::Ref<const typename MathBase::VectorXs>&
x,
106 const Eigen::Ref<const typename MathBase::VectorXs>&
u) {
107 if (
static_cast<std::size_t
>(
x.size()) !=
state_->get_nx()) {
109 <<
"x has wrong dimension (it should be " +
110 std::to_string(
state_->get_nx()) +
")");
112 if (
static_cast<std::size_t
>(
u.size()) !=
nu_) {
114 <<
"u has wrong dimension (it should be " +
115 std::to_string(
nu_) +
")");
127 d->r.template
head<12>() = state_weights_.cwiseProduct(
x.head(12) - xref_);
128 d->r.template
segment<8>(12) = heuristicWeights.cwiseProduct(
132 d->r.template
tail<4>() = step_weights_.cwiseProduct(
u);
134 d->cost =
Scalar(0.5) *
d->r.transpose() *
d->r;
137 for (
int i = 0;
i < 4;
i++) {
139 rub_max_first_x.col(
i) =
x(20) * b_coeff_x0.col(
i) +
140 x(20) *
x(20) * b_coeff_x1.col(
i) +
141 u(2 *
i) * b_coeff_x2.col(
i);
142 rub_max_first_y.col(
i) =
x(20) * b_coeff_y0.col(
i) +
143 x(20) *
x(20) * b_coeff_y1.col(
i) +
144 u(2 *
i + 1) * b_coeff_y2.col(
i);
146 rub_max_first_2.col(
i) =
147 rub_max_first_x.col(
i).pow(2) + rub_max_first_y.col(
i).pow(2) -
148 x(20) *
x(20) * vlim * vlim * nb_nodes * nb_nodes;
150 rub_max_first_2.col(
i).setZero();
155 (rub_max_first_2 >
Scalar(0.))
157 rub_max_first_2 = rub_max_first_2.cwiseMax(
Scalar(0.));
159 for (
int i = 0;
i < nb_alpha_;
i++) {
160 d->cost += speed_weight *
Scalar(0.5) * rub_max_first_2.row(
i).sum();
163 rub_max_ <<
u[0] *
u[0] +
u[1] *
u[1] - beta_lim *
x[20] *
x[20],
164 u[2] *
u[2] +
u[3] *
u[3] - beta_lim *
x[20] *
x[20];
168 rub_max_ = rub_max_.cwiseMax(
Scalar(0.));
170 d->cost += speed_weight *
Scalar(0.5) * rub_max_.sum();
177 Scalar(0.5) *
d->r.head(12).transpose() *
d->r.head(12);
178 cost_[1] =
Scalar(0.5) *
d->r.segment(12, 8).transpose() *
180 cost_[2] =
Scalar(0.5) *
d->r.tail(4).transpose() *
184 for (
int i = 0;
i < 3;
i++) {
185 cost_[3] += speed_weight *
Scalar(0.5) * rub_max_first_2.row(
i).sum();
188 cost_[3] = speed_weight *
Scalar(0.5) * rub_max_.sum();
193template <
typename Scalar>
195 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >&
data,
196 const Eigen::Ref<const typename MathBase::VectorXs>&
x,
197 const Eigen::Ref<const typename MathBase::VectorXs>&
u) {
198 if (
static_cast<std::size_t
>(
x.size()) !=
state_->get_nx()) {
200 <<
"x has wrong dimension (it should be " +
201 std::to_string(
state_->get_nx()) +
")");
203 if (
static_cast<std::size_t
>(
u.size()) !=
nu_) {
205 <<
"u has wrong dimension (it should be " +
206 std::to_string(
nu_) +
")");
221 (heuristicWeights.array() *
d->r.template
segment<8>(12).array())
227 for (
int i = 0;
i < nb_alpha_;
i++) {
228 if (rub_max_first_bool(
i,
foot)) {
231 (b_coeff_x0(
i,
foot) +
233 rub_max_first_x(
i,
foot) +
235 (b_coeff_y0(
i,
foot) +
237 rub_max_first_y(
i,
foot) -
238 speed_weight *
x(20) * vlim * vlim * nb_nodes * nb_nodes;
240 speed_weight * b_coeff_x2(
i,
foot) * rub_max_first_x(
i,
foot);
241 d->Lu(2 *
foot + 1) +=
242 speed_weight * b_coeff_y2(
i,
foot) * rub_max_first_y(
i,
foot);
245 speed_weight * b_coeff_x2(
i,
foot) * b_coeff_x2(
i,
foot);
247 speed_weight * b_coeff_y2(
i,
foot) * b_coeff_y2(
i,
foot);
248 d->Lxu(20, 2 *
foot) += speed_weight *
249 (b_coeff_x0(
i,
foot) +
252 d->Lxu(20, 2 *
foot + 1) +=
254 (b_coeff_y0(
i,
foot) +
259 std::pow(b_coeff_x0(
i,
foot) +
263 rub_max_first_x(
i,
foot) +
265 std::pow(b_coeff_y0(
i,
foot) +
269 rub_max_first_y(
i,
foot) -
270 speed_weight * vlim * vlim * nb_nodes * nb_nodes;
280 << -beta_lim * speed_weight *
x(20) * rub_max_bool[0] -
281 beta_lim * speed_weight *
x(20) * rub_max_bool[1];
283 d->Lu << speed_weight *
u[0] * rub_max_bool[0],
284 speed_weight *
u[1] * rub_max_bool[0],
285 speed_weight *
u[2] * rub_max_bool[1],
286 speed_weight *
u[3] * rub_max_bool[1];
288 d->Lxx(20, 20) = -beta_lim * speed_weight * rub_max_bool[0] -
289 beta_lim * speed_weight * rub_max_bool[1];
291 d->Luu.diagonal() << speed_weight * rub_max_bool[0],
292 speed_weight * rub_max_bool[0], speed_weight * rub_max_bool[1],
293 speed_weight * rub_max_bool[1];
296 d->Lu += (step_weights_.array() *
d->r.template
tail<4>().array()).
matrix();
299 d->Lxx.diagonal().head(12) =
300 (state_weights_.array() * state_weights_.array()).
matrix();
301 d->Lxx.diagonal().segment(12, 8) =
302 (heuristicWeights.array() * heuristicWeights.array()).
matrix();
304 d->Luu.diagonal() += (step_weights_.array() * step_weights_.array()).
matrix();
308 d->Fu.block(12, 0, 8, 8) = B;
311template <
typename Scalar>
312boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
314 return boost::make_shared<ActionDataQuadrupedStepTimeTpl<Scalar> >(
this);
321template <
typename Scalar>
322const typename Eigen::Matrix<Scalar, 12, 1>&
324 return state_weights_;
326template <
typename Scalar>
328 const typename MathBase::VectorXs&
weights) {
329 if (
static_cast<std::size_t
>(
weights.size()) != 12) {
331 <<
"Weights vector has wrong dimension (it should be 12)");
336template <
typename Scalar>
337const typename Eigen::Matrix<Scalar, 4, 1>&
339 return step_weights_;
341template <
typename Scalar>
343 const typename MathBase::VectorXs&
weights) {
344 if (
static_cast<std::size_t
>(
weights.size()) != 8) {
346 <<
"Weights vector has wrong dimension (it should be 8)");
351template <
typename Scalar>
352const typename Eigen::Matrix<Scalar, 8, 1>&
354 return heuristicWeights;
356template <
typename Scalar>
358 const typename MathBase::VectorXs&
weights) {
359 if (
static_cast<std::size_t
>(
weights.size()) != 8) {
361 <<
"Weights vector has wrong dimension (it should be 8)");
366template <
typename Scalar>
368 return symmetry_term;
370template <
typename Scalar>
377template <
typename Scalar>
380 return centrifugal_term;
382template <
typename Scalar>
389template <
typename Scalar>
394template <
typename Scalar>
404template <
typename Scalar>
409template <
typename Scalar>
415template <
typename Scalar>
419template <
typename Scalar>
423 beta_lim =
Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) / 225);
427template <
typename Scalar>
431template <
typename Scalar>
434 beta_lim =
Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) / 225);
441template <
typename Scalar>
442const typename Eigen::Matrix<Scalar, 7, 1>&
449template <
typename Scalar>
453template <
typename Scalar>
464template <
typename Scalar>
466 const Eigen::Ref<const typename MathBase::MatrixXs>&
l_feet,
467 const Eigen::Ref<const typename MathBase::MatrixXs>&
velocity,
468 const Eigen::Ref<const typename MathBase::MatrixXs>&
acceleration,
469 const Eigen::Ref<const typename MathBase::MatrixXs>&
xref,
470 const Eigen::Ref<const typename MathBase::VectorXs>&
S) {
471 if (
static_cast<std::size_t
>(
l_feet.size()) != 12) {
473 <<
"l_feet matrix has wrong dimension (it should be : 3x4)");
475 if (
static_cast<std::size_t
>(
xref.size()) != 12) {
477 <<
"Weights vector has wrong dimension (it should be " +
478 std::to_string(
state_->get_nx()) +
")");
480 if (
static_cast<std::size_t
>(
S.size()) != 4) {
482 <<
"S vector has wrong dimension (it should be 4x1)");
488 for (
int i = 0;
i < 4;
i =
i + 1) {
489 pheuristic_.block(2 *
i, 0, 2, 1) =
l_feet.block(0,
i, 2, 1);
494 for (
int i = 0;
i < 4;
i++) {
497 b_coeff_x0.col(
i) = nb_nodes *
velocity(0,
i) * b_coeff.col(0);
500 b_coeff_x2.col(
i) = b_coeff.col(2);
503 b_coeff_y0.col(
i) = nb_nodes *
velocity(1,
i) * b_coeff.col(0);
506 b_coeff_y2.col(
i) = b_coeff.col(2);
508 b_coeff_x0.col(
i).setZero();
509 b_coeff_x1.col(
i).setZero();
510 b_coeff_x2.col(
i).setZero();
511 b_coeff_y0.col(
i).setZero();
512 b_coeff_y1.col(
i).setZero();
513 b_coeff_y2.col(
i).setZero();
538 B.block(0, 0, 2, 2).setIdentity();
541 B.block(2, 2, 2, 2).setIdentity();
544 B.block(4, 4, 2, 2).setIdentity();
547 B.block(6, 6, 2, 2).setIdentity();
Definition quadruped_augmented_time.hpp:14
ActionModelQuadrupedAugmentedTimeTpl()
Definition quadruped_augmented_time.hxx:9
_Scalar Scalar
Definition quadruped_augmented_time.hpp:16
const Eigen::Matrix< Scalar, 7, 1 > & get_cost() const
Definition quadruped_step_time.hxx:443
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_time.hxx:194
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition quadruped_step_time.hxx:327
void set_step_weights(const typename MathBase::VectorXs &weights)
Definition quadruped_step_time.hxx:342
const Scalar & get_speed_weight() const
Definition quadruped_step_time.hxx:405
const Scalar & get_vlim() const
Definition quadruped_step_time.hxx:428
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition quadruped_step_time.hxx:357
ActionModelQuadrupedStepTimeTpl()
Definition quadruped_step_time.hxx:8
const bool & get_centrifugal_term() const
Definition quadruped_step_time.hxx:378
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &velocity, const Eigen::Ref< const typename MathBase::MatrixXs > &acceleration, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::VectorXs > &S)
Definition quadruped_step_time.hxx:465
~ActionModelQuadrupedStepTimeTpl()
Definition quadruped_step_time.hxx:100
void set_first_step(const bool &first)
Definition quadruped_step_time.hxx:454
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition quadruped_step_time.hxx:313
void set_symmetry_term(const bool &sym_term)
Definition quadruped_step_time.hxx:371
const Scalar & get_T_gait() const
Definition quadruped_step_time.hxx:390
const bool & get_symmetry_term() const
Definition quadruped_step_time.hxx:367
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition quadruped_step_time.hxx:353
void set_vlim(const Scalar &vlim_)
Definition quadruped_step_time.hxx:432
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition quadruped_step_time.hxx:323
const Scalar & get_nb_nodes() const
Definition quadruped_step_time.hxx:416
const Eigen::Matrix< Scalar, 4, 1 > & get_step_weights() const
Definition quadruped_step_time.hxx:338
void set_nb_nodes(const Scalar &nodes_)
Definition quadruped_step_time.hxx:420
_Scalar Scalar
Definition quadruped_step_time.hpp:16
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_time.hxx:103
const bool & get_first_step() const
Definition quadruped_step_time.hxx:450
void set_centrifugal_term(const bool ¢_term)
Definition quadruped_step_time.hxx:383
void set_T_gait(const Scalar &T_gait_)
Definition quadruped_step_time.hxx:395
void set_speed_weight(const Scalar &weight_)
Definition quadruped_step_time.hxx:410
Definition quadruped.hpp:11