1 #ifndef __quadruped_walkgen_quadruped_nl_hxx__
2 #define __quadruped_walkgen_quadruped_nl_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;
71 implicit_integration =
true;
72 offset_com = offset_CoM;
75 template <
typename Scalar>
78 template <
typename Scalar>
80 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
81 const Eigen::Ref<const typename MathBase::VectorXs>& x,
82 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
83 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
84 throw_pretty(
"Invalid argument: "
85 <<
"x has wrong dimension (it should be " +
86 std::to_string(state_->get_nx()) +
")");
88 if (
static_cast<std::size_t
>(u.size()) != nu_) {
89 throw_pretty(
"Invalid argument: "
90 <<
"u has wrong dimension (it should be " +
91 std::to_string(nu_) +
")");
98 for (
int i = 0; i < 4; i = i + 1) {
99 if (gait(i, 0) != 0) {
100 lever_tmp = lever_arms.block(0, i, 3, 1) - x.block(0, 0, 3, 1);
101 R_tmp << 0.0, -lever_tmp[2], lever_tmp[1], lever_tmp[2], 0.0,
102 -lever_tmp[0], -lever_tmp[1], lever_tmp[0], 0.0;
103 B.block(9, 3 * i, 3, 3) << dt_ * I_inv * R_tmp;
106 psh.block(0, i, 3, 1) << x[0] - offset_com(0, 0) + pshoulder_0(0, i) -
107 pshoulder_0(1, i) * x[5] - lever_arms(0, i),
108 x[1] - offset_com(1, 0) + pshoulder_0(1, i) +
109 pshoulder_0(0, i) * x[5] - lever_arms(1, i),
110 x[2] - offset_com(2, 0) + pshoulder_0(1, i) * x[3] -
111 pshoulder_0(0, i) * x[4];
114 psh.block(0, i, 3, 1).setZero();
119 d->xnext << A.diagonal().cwiseProduct(x) + g;
120 d->xnext.template head<6>() =
121 d->xnext.template head<6>() +
122 A.topRightCorner(6, 6).diagonal().cwiseProduct(x.tail(6));
123 d->xnext.template tail<6>() =
124 d->xnext.template tail<6>() + B.block(6, 0, 6, 12) * u;
127 d->r.template head<12>() = state_weights_.cwiseProduct(x - xref_);
128 d->r.template tail<12>() = force_weights_.cwiseProduct(u - uref_);
131 for (
int i = 0; i < 4; i = i + 1) {
132 Fa_x_u.segment(6 * i, 6) << u(3 * i) - mu * u(3 * i + 2),
133 -u(3 * i) - mu * u(3 * i + 2), u(3 * i + 1) - mu * u(3 * i + 2),
134 -u(3 * i + 1) - mu * u(3 * i + 2), -u(3 * i + 2), u(3 * i + 2);
136 rub_max_ = (Fa_x_u - ub).cwiseMax(
Scalar(0.));
139 sh_ub_max_ << psh.block(0, 0, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
140 psh.block(0, 1, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
141 psh.block(0, 2, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
142 psh.block(0, 3, 3, 1).squaredNorm() - sh_hlim * sh_hlim;
144 sh_ub_max_ = sh_ub_max_.cwiseMax(
Scalar(0.));
150 d->cost = 0.5 * d->r.transpose() * d->r +
151 friction_weight_ *
Scalar(0.5) * rub_max_.squaredNorm() +
152 sh_weight *
Scalar(0.5) * sh_ub_max_.sum();
155 template <
typename Scalar>
157 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
158 const Eigen::Ref<const typename MathBase::VectorXs>& x,
159 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
160 if (
static_cast<std::size_t
>(x.size()) != state_->get_nx()) {
161 throw_pretty(
"Invalid argument: "
162 <<
"x has wrong dimension (it should be " +
163 std::to_string(state_->get_nx()) +
")");
165 if (
static_cast<std::size_t
>(u.size()) != nu_) {
166 throw_pretty(
"Invalid argument: "
167 <<
"u has wrong dimension (it should be " +
168 std::to_string(nu_) +
")");
176 d->Lx = (state_weights_.array() * d->r.template head<12>().array()).matrix();
179 d->Lxx.block(0, 0, 6, 6).setZero();
181 (state_weights_.array() * state_weights_.array()).matrix();
184 for (
int j = 0; j < 4; j = j + 1) {
185 if (sh_ub_max_[j] >
Scalar(0.)) {
186 d->Lx(0, 0) += sh_weight * psh(0, j);
187 d->Lx(1, 0) += sh_weight * psh(1, j);
188 d->Lx(2, 0) += sh_weight * psh(2, j);
189 d->Lx(3, 0) += sh_weight * pshoulder_0(1, j) * psh(2, j);
190 d->Lx(4, 0) += -sh_weight * pshoulder_0(0, j) * psh(2, j);
191 d->Lx(5, 0) += sh_weight * (-pshoulder_0(1, j) * psh(0, j) +
192 pshoulder_0(0, j) * psh(1, j));
194 d->Lxx(0, 0) += sh_weight;
195 d->Lxx(1, 1) += sh_weight;
196 d->Lxx(2, 2) += sh_weight;
197 d->Lxx(3, 3) += sh_weight * pshoulder_0(1, j) * pshoulder_0(1, j);
198 d->Lxx(3, 3) += sh_weight * pshoulder_0(0, j) * pshoulder_0(0, j);
199 d->Lxx(5, 5) += sh_weight * (pshoulder_0(1, j) * pshoulder_0(1, j) +
200 pshoulder_0(0, j) * pshoulder_0(0, j));
202 d->Lxx(0, 5) += -sh_weight * pshoulder_0(1, j);
203 d->Lxx(5, 0) += -sh_weight * pshoulder_0(1, j);
205 d->Lxx(1, 5) += sh_weight * pshoulder_0(0, j);
206 d->Lxx(5, 1) += sh_weight * pshoulder_0(0, j);
208 d->Lxx(2, 3) += sh_weight * pshoulder_0(1, j);
209 d->Lxx(2, 4) += -sh_weight * pshoulder_0(0, j);
210 d->Lxx(3, 2) += sh_weight * pshoulder_0(1, j);
211 d->Lxx(4, 2) += -sh_weight * pshoulder_0(0, j);
213 d->Lxx(3, 4) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
214 d->Lxx(4, 3) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
219 for (
int i = 0; i < 4; i = i + 1) {
220 r = friction_weight_ * rub_max_.segment(6 * i, 6);
221 d->Lu.block(i * 3, 0, 3, 1) << r(0) - r(1), r(2) - r(3),
222 -mu * (r(0) + r(1) + r(2) + r(3)) - r(4) + r(5);
225 (force_weights_.array() * d->r.template tail<12>().array()).matrix();
230 ((Fa_x_u - ub).array() >= 0.).matrix().template cast<Scalar>();
231 for (
int i = 0; i < 4; i = i + 1) {
232 r = friction_weight_ * Arr.diagonal().segment(6 * i, 6);
233 d->Luu.block(3 * i, 3 * i, 3, 3) << r(0) + r(1), 0.0, mu * (r(1) - r(0)),
234 0.0, r(2) + r(3), mu * (r(3) - r(2)), mu * (r(1) - r(0)),
235 mu * (r(3) - r(2)), mu * mu * (r(0) + r(1) + r(2) + r(3)) + r(4) + r(5);
239 (force_weights_.array() * force_weights_.array()).matrix();
244 for (
int i = 0; i < 4; i = i + 1) {
245 if (gait(i, 0) != 0) {
246 forces_3d = u.block(3 * i, 0, 3, 1);
247 d->Fx.block(9, 0, 3, 1) +=
248 -dt_ * I_inv * (base_vector_x.cross(forces_3d));
249 d->Fx.block(9, 1, 3, 1) +=
250 -dt_ * I_inv * (base_vector_y.cross(forces_3d));
251 d->Fx.block(9, 2, 3, 1) +=
252 -dt_ * I_inv * (base_vector_z.cross(forces_3d));
258 template <
typename Scalar>
259 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
261 return boost::make_shared<ActionDataQuadrupedNonLinearTpl<Scalar> >(
this);
268 template <
typename Scalar>
269 const typename Eigen::Matrix<Scalar, 12, 1>&
271 return force_weights_;
273 template <
typename Scalar>
275 const typename MathBase::VectorXs& weights) {
276 if (
static_cast<std::size_t
>(weights.size()) != state_->get_nx()) {
277 throw_pretty(
"Invalid argument: "
278 <<
"Weights vector has wrong dimension (it should be " +
279 std::to_string(state_->get_nx()) +
")");
281 force_weights_ = weights;
284 template <
typename Scalar>
285 const typename Eigen::Matrix<Scalar, 12, 1>&
287 return state_weights_;
289 template <
typename Scalar>
291 const typename MathBase::VectorXs& weights) {
292 if (
static_cast<std::size_t
>(weights.size()) != state_->get_nx()) {
293 throw_pretty(
"Invalid argument: "
294 <<
"Weights vector has wrong dimension (it should be " +
295 std::to_string(state_->get_nx()) +
")");
297 state_weights_ = weights;
300 template <
typename Scalar>
303 return friction_weight_;
305 template <
typename Scalar>
308 friction_weight_ = weight;
311 template <
typename Scalar>
315 template <
typename Scalar>
320 template <
typename Scalar>
324 template <
typename Scalar>
330 template <
typename Scalar>
334 template <
typename Scalar>
338 g[8] =
Scalar(-9.81) * dt_;
339 A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
342 template <
typename Scalar>
343 const typename Eigen::Matrix<Scalar, 3, 3>&
347 template <
typename Scalar>
349 const typename MathBase::Matrix3s& inertia_matrix) {
351 if (
static_cast<std::size_t
>(inertia_matrix.size()) != 9) {
352 throw_pretty(
"Invalid argument: "
353 <<
"gI has wrong dimension : 3x3");
358 template <
typename Scalar>
362 return min_fz_in_contact;
364 template <
typename Scalar>
368 min_fz_in_contact = min_fz;
371 template <
typename Scalar>
377 template <
typename Scalar>
382 for (
int i = 0; i < 4; i = i + 1) {
383 ub(6 * i + 5) = max_fz;
387 template <
typename Scalar>
392 template <
typename Scalar>
399 template <
typename Scalar>
404 template <
typename Scalar>
414 template <
typename Scalar>
415 const typename Eigen::Matrix<Scalar, 12, 12>&
419 template <
typename Scalar>
420 const typename Eigen::Matrix<Scalar, 12, 12>&
427 template <
typename Scalar>
430 return relative_forces;
432 template <
typename Scalar>
434 const bool& rel_forces) {
435 relative_forces = rel_forces;
437 if (relative_forces) {
438 for (
int i = 0; i < 4; i = i + 1) {
440 uref_[3 * i + 2] = (
Scalar(9.81) * mass) / (gait.sum());
450 template <
typename Scalar>
452 const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
453 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
454 const Eigen::Ref<const typename MathBase::MatrixXs>& S) {
455 if (
static_cast<std::size_t
>(l_feet.size()) != 12) {
456 throw_pretty(
"Invalid argument: "
457 <<
"l_feet matrix has wrong dimension (it should be : 3x4)");
459 if (
static_cast<std::size_t
>(xref.size()) != state_->get_nx()) {
460 throw_pretty(
"Invalid argument: "
461 <<
"Weights vector has wrong dimension (it should be " +
462 std::to_string(state_->get_nx()) +
")");
464 if (
static_cast<std::size_t
>(S.size()) != 4) {
465 throw_pretty(
"Invalid argument: "
466 <<
"S vector has wrong dimension (it should be 4x1)");
474 if (relative_forces) {
475 for (
int i = 0; i < 4; i = i + 1) {
477 uref_[3 * i + 2] = (
Scalar(9.81) * mass) / (gait.sum());
482 R_tmp << cos(xref(5, 0)), -sin(xref(5, 0)), 0, sin(xref(5, 0)),
483 cos(xref(5, 0)), 0, 0, 0, 1.0;
485 I_inv = (R_tmp.transpose() * gI * R_tmp).inverse();
486 lever_arms.block(0, 0, 2, 4) = l_feet.block(0, 0, 2, 4);
488 for (
int i = 0; i < 4; i = i + 1) {
491 ub(6 * i + 4) = -min_fz_in_contact;
494 B.block(6, 3 * i, 3, 3).diagonal() << dt_ / mass, dt_ / mass, dt_ / mass;
507 B.block(6, 3 * i, 3, 3).setZero();
508 B.block(9, 3 * i, 3, 3).setZero();
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_nl.hxx:260
void set_mass(const Scalar &m)
Definition: quadruped_nl.hxx:325
const Scalar & get_friction_weight() const
Definition: quadruped_nl.hxx:301
void set_mu(const Scalar &mu_coeff)
Definition: quadruped_nl.hxx:316
const Scalar & get_mu() const
Definition: quadruped_nl.hxx:312
void set_relative_forces(const bool &rel_forces)
Definition: quadruped_nl.hxx:433
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition: quadruped_nl.hxx:421
void set_shoulder_weight(const Scalar &weight)
Definition: quadruped_nl.hxx:405
void set_max_fz_contact(const Scalar &max_fz_)
Definition: quadruped_nl.hxx:378
const Scalar & get_dt() const
Definition: quadruped_nl.hxx:331
const Scalar & get_min_fz_contact() const
Definition: quadruped_nl.hxx:359
const Scalar & get_shoulder_hlim() const
Definition: quadruped_nl.hxx:388
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition: quadruped_nl.hxx:270
ActionModelQuadrupedNonLinearTpl(typename Eigen::Matrix< Scalar, 3, 1 > offset_CoM=Eigen::Matrix< Scalar, 3, 1 >::Zero())
Definition: quadruped_nl.hxx:8
~ActionModelQuadrupedNonLinearTpl()
Definition: quadruped_nl.hxx:76
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_nl.hxx:451
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_nl.hxx:79
void set_min_fz_contact(const Scalar &min_fz)
Definition: quadruped_nl.hxx:365
const bool & get_relative_forces() const
Definition: quadruped_nl.hxx:428
void set_friction_weight(const Scalar &weight)
Definition: quadruped_nl.hxx:306
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_nl.hxx:156
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition: quadruped_nl.hxx:344
_Scalar Scalar
Definition: quadruped_nl.hpp:16
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition: quadruped_nl.hxx:416
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition: quadruped_nl.hxx:348
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_nl.hxx:274
void set_dt(const Scalar &dt)
Definition: quadruped_nl.hxx:335
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_nl.hxx:290
const Scalar & get_max_fz_contact() const
Definition: quadruped_nl.hxx:372
void set_shoulder_hlim(const Scalar &hlim)
Definition: quadruped_nl.hxx:393
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_nl.hxx:286
const Scalar & get_mass() const
Definition: quadruped_nl.hxx:321
const Scalar & get_shoulder_weight() const
Definition: quadruped_nl.hxx:400
Definition: quadruped.hpp:11
Definition: quadruped_nl.hpp:145