1#ifndef __quadruped_walkgen_quadruped_nl_hxx__
2#define __quadruped_walkgen_quadruped_nl_hxx__
4#include "crocoddyl/core/utils/exception.hpp"
7template <
typename Scalar>
9 typename Eigen::Matrix<Scalar, 3, 1>
offset_CoM)
10 : crocoddyl::ActionModelAbstractTpl<
Scalar>(
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;
75template <
typename Scalar>
78template <
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()) {
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_) {
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;
122 A.topRightCorner(6, 6).diagonal().cwiseProduct(
x.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);
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();
155template <
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()) {
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_) {
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);
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));
258template <
typename Scalar>
259boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
261 return boost::make_shared<ActionDataQuadrupedNonLinearTpl<Scalar> >(
this);
268template <
typename Scalar>
269const typename Eigen::Matrix<Scalar, 12, 1>&
271 return force_weights_;
273template <
typename Scalar>
275 const typename MathBase::VectorXs&
weights) {
276 if (
static_cast<std::size_t
>(
weights.size()) !=
state_->get_nx()) {
278 <<
"Weights vector has wrong dimension (it should be " +
279 std::to_string(
state_->get_nx()) +
")");
284template <
typename Scalar>
285const typename Eigen::Matrix<Scalar, 12, 1>&
287 return state_weights_;
289template <
typename Scalar>
291 const typename MathBase::VectorXs&
weights) {
292 if (
static_cast<std::size_t
>(
weights.size()) !=
state_->get_nx()) {
294 <<
"Weights vector has wrong dimension (it should be " +
295 std::to_string(
state_->get_nx()) +
")");
300template <
typename Scalar>
303 return friction_weight_;
305template <
typename Scalar>
308 friction_weight_ =
weight;
311template <
typename Scalar>
315template <
typename Scalar>
320template <
typename Scalar>
324template <
typename Scalar>
330template <
typename Scalar>
334template <
typename Scalar>
338 g[8] =
Scalar(-9.81) * dt_;
339 A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
342template <
typename Scalar>
343const typename Eigen::Matrix<Scalar, 3, 3>&
347template <
typename Scalar>
353 <<
"gI has wrong dimension : 3x3");
358template <
typename Scalar>
362 return min_fz_in_contact;
364template <
typename Scalar>
368 min_fz_in_contact =
min_fz;
371template <
typename Scalar>
377template <
typename Scalar>
382 for (
int i = 0;
i < 4;
i =
i + 1) {
383 ub(6 *
i + 5) = max_fz;
387template <
typename Scalar>
392template <
typename Scalar>
399template <
typename Scalar>
404template <
typename Scalar>
414template <
typename Scalar>
415const typename Eigen::Matrix<Scalar, 12, 12>&
419template <
typename Scalar>
420const typename Eigen::Matrix<Scalar, 12, 12>&
427template <
typename Scalar>
430 return relative_forces;
432template <
typename Scalar>
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());
450template <
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) {
457 <<
"l_feet matrix has wrong dimension (it should be : 3x4)");
459 if (
static_cast<std::size_t
>(
xref.size()) !=
state_->get_nx()) {
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) {
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());
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();
Definition quadruped_augmented_time.hpp:14
ActionModelQuadrupedAugmentedTimeTpl()
Definition quadruped_augmented_time.hxx:9
_Scalar Scalar
Definition quadruped_augmented_time.hpp:16
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