1 #ifndef __quadruped_walkgen_quadruped_hxx__
2 #define __quadruped_walkgen_quadruped_hxx__
4 #include "crocoddyl/core/utils/exception.hpp"
7 template <
typename Scalar>
9 typename Eigen::Matrix<Scalar, 3, 1> offset_CoM)
10 : crocoddyl::ActionModelAbstractTpl<
Scalar>(
11 boost::make_shared<crocoddyl::StateVectorTpl<
Scalar> >(12), 12, 24) {
13 relative_forces =
false;
20 min_fz_in_contact =
Scalar(0.0);
25 g[8] =
Scalar(-9.81) * dt_;
29 A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
35 force_weights_.setConstant(0.2);
39 friction_weight_ =
Scalar(10);
43 for (
int i = 0; i < 4; i = i + 1) {
44 ub(6 * i + 5) = max_fz;
67 implicit_integration =
true;
68 offset_com = offset_CoM;
71 template <
typename Scalar>
74 template <
typename Scalar>
76 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
77 const Eigen::Ref<const typename MathBase::VectorXs>& x,
78 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
79 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
80 throw_pretty(
"Invalid argument: "
81 <<
"x has wrong dimension (it should be " +
82 std::to_string(state_->get_nx()) +
")");
84 if (
static_cast<std::size_t
>(u.size()) != nu_) {
85 throw_pretty(
"Invalid argument: "
86 <<
"u has wrong dimension (it should be " +
87 std::to_string(nu_) +
")");
93 for (
int i = 0; i < 4; i = i + 1) {
94 if (gait(i, 0) != 0) {
96 psh.block(0, i, 3, 1) << x[0] - offset_com(0, 0) + pshoulder_0(0, i) -
97 pshoulder_0(1, i) * x[5] - lever_arms(0, i),
98 x[1] - offset_com(1, 0) + pshoulder_0(1, i) +
99 pshoulder_0(0, i) * x[5] - lever_arms(1, i),
100 x[2] - offset_com(2, 0) + pshoulder_0(1, i) * x[3] -
101 pshoulder_0(0, i) * x[4];
104 psh.block(0, i, 3, 1).setZero();
109 d->xnext << A.diagonal().cwiseProduct(x) + g;
110 d->xnext.template tail<6>() =
111 d->xnext.template tail<6>() + B.block(6, 0, 6, 12) * u;
115 if (implicit_integration) {
116 d->xnext.template head<6>() =
117 d->xnext.template head<6>() +
118 A.topRightCorner(6, 6).diagonal().cwiseProduct(d->xnext.tail(6));
120 d->xnext.template head<6>() =
121 d->xnext.template head<6>() +
122 A.topRightCorner(6, 6).diagonal().cwiseProduct(x.tail(6));
126 d->r.template head<12>() = state_weights_.cwiseProduct(x - xref_);
127 d->r.template tail<12>() = force_weights_.cwiseProduct(u - uref_);
130 for (
int i = 0; i < 4; i = i + 1) {
131 Fa_x_u.segment(6 * i, 6) << u(3 * i) - mu * u(3 * i + 2),
132 -u(3 * i) - mu * u(3 * i + 2), u(3 * i + 1) - mu * u(3 * i + 2),
133 -u(3 * i + 1) - mu * u(3 * i + 2), -u(3 * i + 2), u(3 * i + 2);
135 rub_max_ = (Fa_x_u - ub).cwiseMax(0.);
138 sh_ub_max_ << psh.block(0, 0, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
139 psh.block(0, 1, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
140 psh.block(0, 2, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
141 psh.block(0, 3, 3, 1).squaredNorm() - sh_hlim * sh_hlim;
142 sh_ub_max_ = sh_ub_max_.cwiseMax(
Scalar(0.));
149 d->cost = 0.5 * d->r.transpose() * d->r +
150 friction_weight_ *
Scalar(0.5) * rub_max_.squaredNorm() +
151 sh_weight *
Scalar(0.5) * sh_ub_max_.sum();
154 template <
typename Scalar>
156 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
157 const Eigen::Ref<const typename MathBase::VectorXs>& x,
158 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
159 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
160 throw_pretty(
"Invalid argument: "
161 <<
"x has wrong dimension (it should be " +
162 std::to_string(state_->get_nx()) +
")");
164 if (
static_cast<std::size_t
>(u.size()) != nu_) {
165 throw_pretty(
"Invalid argument: "
166 <<
"u has wrong dimension (it should be " +
167 std::to_string(nu_) +
")");
174 d->Lx = (state_weights_.array() * d->r.template head<12>().array()).matrix();
177 d->Lxx.block(0, 0, 6, 6).setZero();
179 (state_weights_.array() * state_weights_.array()).matrix();
180 for (
int j = 0; j < 4; j = j + 1) {
181 if (sh_ub_max_[j] >
Scalar(0.)) {
182 d->Lx(0, 0) += sh_weight * psh(0, j);
183 d->Lx(1, 0) += sh_weight * psh(1, j);
184 d->Lx(2, 0) += sh_weight * psh(2, j);
185 d->Lx(3, 0) += sh_weight * pshoulder_0(1, j) * psh(2, j);
186 d->Lx(4, 0) += -sh_weight * pshoulder_0(0, j) * psh(2, j);
187 d->Lx(5, 0) += sh_weight * (-pshoulder_0(1, j) * psh(0, j) +
188 pshoulder_0(0, j) * psh(1, j));
190 d->Lxx(0, 0) += sh_weight;
191 d->Lxx(1, 1) += sh_weight;
192 d->Lxx(2, 2) += sh_weight;
193 d->Lxx(3, 3) += sh_weight * pshoulder_0(1, j) * pshoulder_0(1, j);
194 d->Lxx(3, 3) += sh_weight * pshoulder_0(0, j) * pshoulder_0(0, j);
195 d->Lxx(5, 5) += sh_weight * (pshoulder_0(1, j) * pshoulder_0(1, j) +
196 pshoulder_0(0, j) * pshoulder_0(0, j));
198 d->Lxx(0, 5) += -sh_weight * pshoulder_0(1, j);
199 d->Lxx(5, 0) += -sh_weight * pshoulder_0(1, j);
201 d->Lxx(1, 5) += sh_weight * pshoulder_0(0, j);
202 d->Lxx(5, 1) += sh_weight * pshoulder_0(0, j);
204 d->Lxx(2, 3) += sh_weight * pshoulder_0(1, j);
205 d->Lxx(2, 4) += -sh_weight * pshoulder_0(0, j);
206 d->Lxx(3, 2) += sh_weight * pshoulder_0(1, j);
207 d->Lxx(4, 2) += -sh_weight * pshoulder_0(0, j);
209 d->Lxx(3, 4) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
210 d->Lxx(4, 3) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
215 for (
int i = 0; i < 4; i = i + 1) {
216 r = friction_weight_ * rub_max_.segment(6 * i, 6);
217 d->Lu.block(i * 3, 0, 3, 1) << r(0) - r(1), r(2) - r(3),
218 -mu * (r(0) + r(1) + r(2) + r(3)) - r(4) + r(5);
221 (force_weights_.array() * d->r.template tail<12>().array()).matrix();
226 ((Fa_x_u - ub).array() >= 0.).matrix().template cast<Scalar>();
227 for (
int i = 0; i < 4; i = i + 1) {
228 r = friction_weight_ * Arr.diagonal().segment(6 * i, 6);
229 d->Luu.block(3 * i, 3 * i, 3, 3) << r(0) + r(1), 0.0, mu * (r(1) - r(0)),
230 0.0, r(2) + r(3), mu * (r(3) - r(2)), mu * (r(1) - r(0)),
231 mu * (r(3) - r(2)), mu * mu * (r(0) + r(1) + r(2) + r(3)) + r(4) + r(5);
235 (force_weights_.array() * force_weights_.array()).matrix();
240 if (implicit_integration) {
241 d->Fu.block(0, 0, 6, 12) << dt_ * B.block(6, 0, 6, 12);
245 template <
typename Scalar>
246 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
248 return boost::make_shared<ActionDataQuadrupedTpl<Scalar> >(
this);
255 template <
typename Scalar>
256 const typename Eigen::Matrix<Scalar, 12, 1>&
258 return force_weights_;
260 template <
typename Scalar>
262 const typename MathBase::VectorXs& weights) {
263 if (
static_cast<std::size_t
>(weights.size()) != state_->get_nx()) {
264 throw_pretty(
"Invalid argument: "
265 <<
"Weights vector has wrong dimension (it should be " +
266 std::to_string(state_->get_nx()) +
")");
268 force_weights_ = weights;
271 template <
typename Scalar>
272 const typename Eigen::Matrix<Scalar, 12, 1>&
274 return state_weights_;
276 template <
typename Scalar>
278 const typename MathBase::VectorXs& weights) {
279 if (
static_cast<std::size_t
>(weights.size()) != state_->get_nx()) {
280 throw_pretty(
"Invalid argument: "
281 <<
"Weights vector has wrong dimension (it should be " +
282 std::to_string(state_->get_nx()) +
")");
284 state_weights_ = weights;
287 template <
typename Scalar>
289 return friction_weight_;
291 template <
typename Scalar>
294 friction_weight_ = weight;
297 template <
typename Scalar>
301 template <
typename Scalar>
306 template <
typename Scalar>
310 template <
typename Scalar>
316 template <
typename Scalar>
320 template <
typename Scalar>
324 g[8] =
Scalar(-9.81) * dt_;
325 A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
328 template <
typename Scalar>
329 const typename Eigen::Matrix<Scalar, 3, 3>&
333 template <
typename Scalar>
335 const typename MathBase::Matrix3s& inertia_matrix) {
337 if (
static_cast<std::size_t
>(inertia_matrix.size()) != 9) {
338 throw_pretty(
"Invalid argument: "
339 <<
"gI has wrong dimension : 3x3");
344 template <
typename Scalar>
347 return min_fz_in_contact;
349 template <
typename Scalar>
352 min_fz_in_contact = min_fz;
355 template <
typename Scalar>
360 template <
typename Scalar>
365 for (
int i = 0; i < 4; i = i + 1) {
366 ub(6 * i + 5) = max_fz;
373 template <
typename Scalar>
374 const typename Eigen::Matrix<Scalar, 12, 12>&
378 template <
typename Scalar>
379 const typename Eigen::Matrix<Scalar, 12, 12>&
384 template <
typename Scalar>
388 template <
typename Scalar>
394 template <
typename Scalar>
398 template <
typename Scalar>
407 template <
typename Scalar>
409 return relative_forces;
411 template <
typename Scalar>
413 const bool& rel_forces) {
414 relative_forces = rel_forces;
416 if (relative_forces) {
417 for (
int i = 0; i < 4; i = i + 1) {
419 uref_[3 * i + 2] = (
Scalar(9.81) * mass) / (gait.sum());
426 template <
typename Scalar>
428 return implicit_integration;
430 template <
typename Scalar>
432 const bool& implicit) {
433 implicit_integration = implicit;
440 template <
typename Scalar>
442 const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
443 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
444 const Eigen::Ref<const typename MathBase::MatrixXs>& S) {
445 if (
static_cast<std::size_t
>(l_feet.size()) != 12) {
446 throw_pretty(
"Invalid argument: "
447 <<
"l_feet matrix has wrong dimension (it should be : 3x4)");
449 if (
static_cast<std::size_t
>(xref.size()) != state_->get_nx()) {
450 throw_pretty(
"Invalid argument: "
451 <<
"Weights vector has wrong dimension (it should be " +
452 std::to_string(state_->get_nx()) +
")");
454 if (
static_cast<std::size_t
>(S.size()) != 4) {
455 throw_pretty(
"Invalid argument: "
456 <<
"S vector has wrong dimension (it should be 4x1)");
464 if (relative_forces) {
465 for (
int i = 0; i < 4; i = i + 1) {
467 uref_[3 * i + 2] = (
Scalar(9.81) * mass) / (gait.sum());
472 R_tmp << cos(xref(5, 0)), -sin(xref(5, 0)), 0, sin(xref(5, 0)),
473 cos(xref(5, 0)), 0, 0, 0, 1.0;
475 I_inv = (R_tmp.transpose() * gI * R_tmp).inverse();
476 lever_arms.block(0, 0, 2, 4) = l_feet.block(0, 0, 2, 4);
478 for (
int i = 0; i < 4; i = i + 1) {
481 ub(6 * i + 4) = -min_fz_in_contact;
484 B.block(6, 3 * i, 3, 3).diagonal() << dt_ / mass, dt_ / mass, dt_ / mass;
485 lever_tmp = lever_arms.block(0, i, 3, 1) - xref.block(0, 0, 3, 1);
486 R_tmp << 0.0, -lever_tmp[2], lever_tmp[1], lever_tmp[2], 0.0,
487 -lever_tmp[0], -lever_tmp[1], lever_tmp[0], 0.0;
488 B.block(9, 3 * i, 3, 3) << dt_ * I_inv * R_tmp;
491 ub(6 * i + 4) =
Scalar(0.0);
492 B.block(6, 3 * i, 3, 3).setZero();
493 B.block(9, 3 * i, 3, 3).setZero();
ActionModelQuadrupedTpl(typename Eigen::Matrix< Scalar, 3, 1 > offset_CoM=Eigen::Matrix< Scalar, 3, 1 >::Zero())
Definition: quadruped.hxx:8
void set_max_fz_contact(const Scalar &max_fz_)
Definition: quadruped.hxx:361
const Scalar & get_mass() const
Definition: quadruped.hxx:307
const Scalar & get_shoulder_weight() const
Definition: quadruped.hxx:395
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition: quadruped.hxx:330
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped.hxx:261
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.hxx:155
const bool & get_implicit_integration() const
Definition: quadruped.hxx:427
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped.hxx:273
const Scalar & get_friction_weight() const
Definition: quadruped.hxx:288
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition: quadruped.hxx:257
void set_mu(const Scalar &mu_coeff)
Definition: quadruped.hxx:302
void set_friction_weight(const Scalar &weight)
Definition: quadruped.hxx:292
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.hxx:75
const Scalar & get_shoulder_hlim() const
Definition: quadruped.hxx:385
const bool & get_relative_forces() const
Definition: quadruped.hxx:408
void set_shoulder_weight(const Scalar &weight)
Definition: quadruped.hxx:399
_Scalar Scalar
Definition: quadruped.hpp:16
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped.hxx:277
void set_mass(const Scalar &m)
Definition: quadruped.hxx:311
void set_implicit_integration(const bool &implicit)
Definition: quadruped.hxx:431
const Scalar & get_min_fz_contact() const
Definition: quadruped.hxx:345
const Scalar & get_dt() const
Definition: quadruped.hxx:317
void set_dt(const Scalar &dt)
Definition: quadruped.hxx:321
void set_min_fz_contact(const Scalar &min_fz)
Definition: quadruped.hxx:350
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.hxx:441
~ActionModelQuadrupedTpl()
Definition: quadruped.hxx:72
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition: quadruped.hxx:375
void set_relative_forces(const bool &rel_forces)
Definition: quadruped.hxx:412
const Scalar & get_max_fz_contact() const
Definition: quadruped.hxx:356
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped.hxx:247
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition: quadruped.hxx:334
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition: quadruped.hxx:380
const Scalar & get_mu() const
Definition: quadruped.hxx:298
void set_shoulder_hlim(const Scalar &hlim)
Definition: quadruped.hxx:389
Definition: quadruped.hpp:11
Definition: quadruped.hpp:139