1 #ifndef __quadruped_walkgen_quadruped_step_time_hxx__
2 #define __quadruped_walkgen_quadruped_step_time_hxx__
4 #include "crocoddyl/core/utils/exception.hpp"
7 template <
typename Scalar>
9 : crocoddyl::ActionModelAbstractTpl<
Scalar>(
10 boost::make_shared<crocoddyl::StateVectorTpl<
Scalar> >(21), 8, 29) {
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);
99 template <
typename Scalar>
102 template <
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()) {
108 throw_pretty(
"Invalid argument: "
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_) {
113 throw_pretty(
"Invalid argument: "
114 <<
"u has wrong dimension (it should be " +
115 std::to_string(nu_) +
")");
122 d->xnext.template head<12>() = x.head(12);
123 d->xnext.template segment<8>(12) = x.segment(12, 8) + B * u;
124 d->xnext.template tail<1>() = x.tail(1);
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.))
156 .
template cast<Scalar>();
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];
167 (rub_max_.array() >=
Scalar(0.)).matrix().template cast<Scalar>();
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();
193 template <
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()) {
199 throw_pretty(
"Invalid argument: "
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_) {
204 throw_pretty(
"Invalid argument: "
205 <<
"u has wrong dimension (it should be " +
206 std::to_string(nu_) +
")");
218 d->Lx.template head<12>() =
219 (state_weights_.array() * d->r.template head<12>().array()).matrix();
220 d->Lx.template segment<8>(12) =
221 (heuristicWeights.array() * d->r.template segment<8>(12).array())
225 for (
int foot = 0; foot < 4; foot++) {
226 if (S_[foot] ==
Scalar(1)) {
227 for (
int i = 0; i < nb_alpha_; i++) {
228 if (rub_max_first_bool(i, foot)) {
231 (b_coeff_x0(i, foot) +
232 Scalar(2) * x(20) * b_coeff_x1(i, foot)) *
233 rub_max_first_x(i, foot) +
235 (b_coeff_y0(i, foot) +
236 Scalar(2) * x(20) * b_coeff_y1(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);
244 d->Luu(2 * foot, 2 * foot) +=
245 speed_weight * b_coeff_x2(i, foot) * b_coeff_x2(i, foot);
246 d->Luu(2 * foot + 1, 2 * foot + 1) +=
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) +
250 Scalar(2) * x(20) * b_coeff_x1(i, foot)) *
252 d->Lxu(20, 2 * foot + 1) +=
254 (b_coeff_y0(i, foot) +
255 Scalar(2) * x(20) * b_coeff_y1(i, foot)) *
259 std::pow(b_coeff_x0(i, foot) +
260 Scalar(2) * x(20) * b_coeff_x1(i, foot),
262 speed_weight *
Scalar(2) * b_coeff_x1(i, foot) *
263 rub_max_first_x(i, foot) +
265 std::pow(b_coeff_y0(i, foot) +
266 Scalar(2) * x(20) * b_coeff_x1(i, foot),
268 speed_weight *
Scalar(2) * b_coeff_y1(i, foot) *
269 rub_max_first_y(i, foot) -
270 speed_weight * vlim * vlim * nb_nodes * nb_nodes;
279 d->Lx.template tail<1>()
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;
311 template <
typename Scalar>
312 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
314 return boost::make_shared<ActionDataQuadrupedStepTimeTpl<Scalar> >(
this);
321 template <
typename Scalar>
322 const typename Eigen::Matrix<Scalar, 12, 1>&
324 return state_weights_;
326 template <
typename Scalar>
328 const typename MathBase::VectorXs& weights) {
329 if (
static_cast<std::size_t
>(weights.size()) != 12) {
330 throw_pretty(
"Invalid argument: "
331 <<
"Weights vector has wrong dimension (it should be 12)");
333 state_weights_ = weights;
336 template <
typename Scalar>
337 const typename Eigen::Matrix<Scalar, 4, 1>&
339 return step_weights_;
341 template <
typename Scalar>
343 const typename MathBase::VectorXs& weights) {
344 if (
static_cast<std::size_t
>(weights.size()) != 8) {
345 throw_pretty(
"Invalid argument: "
346 <<
"Weights vector has wrong dimension (it should be 8)");
348 step_weights_ = weights;
351 template <
typename Scalar>
352 const typename Eigen::Matrix<Scalar, 8, 1>&
354 return heuristicWeights;
356 template <
typename Scalar>
358 const typename MathBase::VectorXs& weights) {
359 if (
static_cast<std::size_t
>(weights.size()) != 8) {
360 throw_pretty(
"Invalid argument: "
361 <<
"Weights vector has wrong dimension (it should be 8)");
363 heuristicWeights = weights;
366 template <
typename Scalar>
368 return symmetry_term;
370 template <
typename Scalar>
372 const bool& sym_term) {
374 symmetry_term = sym_term;
377 template <
typename Scalar>
380 return centrifugal_term;
382 template <
typename Scalar>
384 const bool& cent_term) {
386 centrifugal_term = cent_term;
389 template <
typename Scalar>
394 template <
typename Scalar>
404 template <
typename Scalar>
409 template <
typename Scalar>
412 speed_weight = weight_;
415 template <
typename Scalar>
419 template <
typename Scalar>
423 beta_lim =
Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) / 225);
427 template <
typename Scalar>
431 template <
typename Scalar>
434 beta_lim =
Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) / 225);
441 template <
typename Scalar>
442 const typename Eigen::Matrix<Scalar, 7, 1>&
449 template <
typename Scalar>
453 template <
typename Scalar>
464 template <
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) {
472 throw_pretty(
"Invalid argument: "
473 <<
"l_feet matrix has wrong dimension (it should be : 3x4)");
475 if (
static_cast<std::size_t
>(xref.size()) != 12) {
476 throw_pretty(
"Invalid argument: "
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) {
481 throw_pretty(
"Invalid argument: "
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);
499 nb_nodes * nb_nodes * acceleration(0, i) * b_coeff.col(1);
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);
505 nb_nodes * nb_nodes * acceleration(1, i) * b_coeff.col(1);
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();
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
Definition: quadruped_step_time.hpp:151