quadruped.hxx
Go to the documentation of this file.
1#ifndef __quadruped_walkgen_quadruped_hxx__
2#define __quadruped_walkgen_quadruped_hxx__
3
4#include "crocoddyl/core/utils/exception.hpp"
5
6namespace quadruped_walkgen {
7template <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) {
12 // Relative forces to compute the norm mof the command
13 relative_forces = false;
14 uref_.setZero();
15
16 // Model parameters
17 mu = Scalar(1);
18 dt_ = Scalar(0.02);
19 mass = Scalar(2.50000279);
20 min_fz_in_contact = Scalar(0.0);
21 max_fz = Scalar(25.);
22
23 // Matrix model initialization
24 g.setZero();
25 g[8] = Scalar(-9.81) * dt_;
26 gI.setZero();
27 gI.diagonal() << Scalar(3.09249e-2), Scalar(5.106100e-2), Scalar(6.939757e-2);
28 A.setIdentity();
29 A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
30 B.setZero();
31 lever_arms.setZero();
32 I_inv.setZero();
33
34 // Weight vectors initialization
35 force_weights_.setConstant(0.2);
36 state_weights_ << Scalar(1.), Scalar(1.), Scalar(150.), Scalar(35.),
37 Scalar(30.), Scalar(8.), Scalar(20.), Scalar(20.), Scalar(15.),
38 Scalar(4.), Scalar(4.), Scalar(8.);
39 friction_weight_ = Scalar(10);
40
41 // UpperBound vector
42 ub.setZero();
43 for (int i = 0; i < 4; i = i + 1) {
44 ub(6 * i + 5) = max_fz;
45 }
46
47 // Temporary vector used
48 Fa_x_u.setZero();
49 rub_max_.setZero();
50 Arr.setZero();
51 r.setZero();
52 lever_tmp.setZero();
53 R_tmp.setZero();
54
55 // Used for shoulder height weight
56 pshoulder_0 << Scalar(0.1946), Scalar(0.1946), Scalar(-0.1946),
57 Scalar(-0.1946), Scalar(0.14695), Scalar(-0.14695), Scalar(0.14695),
58 Scalar(-0.14695);
59 sh_hlim = Scalar(0.27);
60 sh_weight = Scalar(10.);
61 sh_ub_max_.setZero();
62 psh.setZero();
63 gait.setZero();
64
65 // Implicit integration
66 // V+ = V + dt*B*u ; P+ = P + dt*V+ != explicit : P+ = P + dt*V
67 implicit_integration = true;
68 offset_com = offset_CoM; // x, y, z offset
69}
70
71template <typename Scalar>
73
74template <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()) + ")");
83 }
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_) + ")");
88 }
89
91 static_cast<ActionDataQuadrupedTpl<Scalar>*>(data.get());
92
93 for (int i = 0; i < 4; i = i + 1) {
94 if (gait(i, 0) != 0) {
95 // Compute pdistance of the shoulder wrt contact point
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];
102 } else {
103 // Compute pdistance of the shoulder wrt contact point
104 psh.block(0, i, 3, 1).setZero();
105 }
106 }
107
108 // Discrete dynamic : A*x + B*u + g
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;
112
113 // Explicit : d->xnext.template head<6>() = d->xnext.template head<6>() +
114 // A.topRightCorner(6,6).diagonal().cwiseProduct(d->xnext.tail(6)) ;
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));
119 } else {
120 d->xnext.template head<6>() =
121 d->xnext.template head<6>() +
122 A.topRightCorner(6, 6).diagonal().cwiseProduct(x.tail(6));
123 }
124
125 // Residual cost on the state and force norm
126 d->r.template head<12>() = state_weights_.cwiseProduct(x - xref_);
127 d->r.template tail<12>() = force_weights_.cwiseProduct(u - uref_);
128
129 // Friction cone
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);
134 }
135 rub_max_ = (Fa_x_u - ub).cwiseMax(0.);
136
137 // Shoulder height weight
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.));
143
144 // Cost computation
145 // d->cost = 0.5 * d->r.transpose() * d->r + friction_weight_ *
146 // Scalar(0.5) * rub_max_.squaredNorm() + sh_weight
147 // * Scalar(0.5) * sh_ub_max_.squaredNorm() ;
148
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();
152}
153
154template <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()) + ")");
163 }
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_) + ")");
168 }
169
171 static_cast<ActionDataQuadrupedTpl<Scalar>*>(data.get());
172
173 // Cost derivatives : Lx
174 d->Lx = (state_weights_.array() * d->r.template head<12>().array()).matrix();
175
176 // Hessian : Lxx
177 d->Lxx.block(0, 0, 6, 6).setZero();
178 d->Lxx.diagonal() =
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));
189
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));
197
198 d->Lxx(0, 5) += -sh_weight * pshoulder_0(1, j);
199 d->Lxx(5, 0) += -sh_weight * pshoulder_0(1, j);
200
201 d->Lxx(1, 5) += sh_weight * pshoulder_0(0, j);
202 d->Lxx(5, 1) += sh_weight * pshoulder_0(0, j);
203
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);
208
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);
211 }
212 }
213
214 // Cost derivative : Lu
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);
219 }
220 d->Lu = d->Lu +
221 (force_weights_.array() * d->r.template tail<12>().array()).matrix();
222
223 // Hessian : Luu
224 // Matrix friction cone hessian (20x12)
225 Arr.diagonal() =
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);
232 }
233 d->Luu.diagonal() =
234 d->Luu.diagonal() +
235 (force_weights_.array() * force_weights_.array()).matrix();
236
237 // Dynamic derivatives
238 d->Fx << A;
239 d->Fu << B;
240 if (implicit_integration) {
241 d->Fu.block(0, 0, 6, 12) << dt_ * B.block(6, 0, 6, 12);
242 }
243}
244
245template <typename Scalar>
246boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
248 return boost::make_shared<ActionDataQuadrupedTpl<Scalar> >(this);
249}
250
252// get & set parameters ////////
254
255template <typename Scalar>
256const typename Eigen::Matrix<Scalar, 12, 1>&
258 return force_weights_;
259}
260template <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()) + ")");
267 }
268 force_weights_ = weights;
269}
270
271template <typename Scalar>
272const typename Eigen::Matrix<Scalar, 12, 1>&
274 return state_weights_;
275}
276template <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()) + ")");
283 }
284 state_weights_ = weights;
285}
286
287template <typename Scalar>
289 return friction_weight_;
290}
291template <typename Scalar>
293 const Scalar& weight) {
294 friction_weight_ = weight;
295}
296
297template <typename Scalar>
299 return mu;
300}
301template <typename Scalar>
303 mu = mu_coeff;
304}
305
306template <typename Scalar>
308 return mass;
309}
310template <typename Scalar>
312 // The model need to be updated after this changed
313 mass = m;
314}
315
316template <typename Scalar>
318 return dt_;
319}
320template <typename Scalar>
322 // The model need to be updated after this changed
323 dt_ = dt;
324 g[8] = Scalar(-9.81) * dt_;
325 A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
326}
327
328template <typename Scalar>
329const typename Eigen::Matrix<Scalar, 3, 3>&
331 return gI;
332}
333template <typename Scalar>
335 const typename MathBase::Matrix3s& inertia_matrix) {
336 // The model need to be updated after this changed
337 if (static_cast<std::size_t>(inertia_matrix.size()) != 9) {
338 throw_pretty("Invalid argument: "
339 << "gI has wrong dimension : 3x3");
340 }
341 gI = inertia_matrix;
342}
343
344template <typename Scalar>
346 // The model need to be updated after this changed
347 return min_fz_in_contact;
348}
349template <typename Scalar>
351 // The model need to be updated after this changed
352 min_fz_in_contact = min_fz;
353}
354
355template <typename Scalar>
357 // The model need to be updated after this changed
358 return max_fz;
359}
360template <typename Scalar>
362 const Scalar& max_fz_) {
363 // The model need to be updated after this changed
364 max_fz = max_fz_;
365 for (int i = 0; i < 4; i = i + 1) {
366 ub(6 * i + 5) = max_fz;
367 }
368}
369
373template <typename Scalar>
374const typename Eigen::Matrix<Scalar, 12, 12>&
376 return A;
377}
378template <typename Scalar>
379const typename Eigen::Matrix<Scalar, 12, 12>&
381 return B;
382}
383
384template <typename Scalar>
386 return sh_hlim;
387}
388template <typename Scalar>
390 // The model need to be updated after this changed
391 sh_hlim = hlim;
392}
393
394template <typename Scalar>
396 return sh_weight;
397}
398template <typename Scalar>
400 const Scalar& weight) {
401 // The model need to be updated after this changed
402 sh_weight = weight;
403}
404
405// to modify the cost on the command : || fz - m*g/nb contact ||^2
406// --> set to True
407template <typename Scalar>
409 return relative_forces;
410}
411template <typename Scalar>
413 const bool& rel_forces) {
414 relative_forces = rel_forces;
415 uref_.setZero();
416 if (relative_forces) {
417 for (int i = 0; i < 4; i = i + 1) {
418 if (gait[i] == 1) {
419 uref_[3 * i + 2] = (Scalar(9.81) * mass) / (gait.sum());
420 }
421 }
422 }
423}
424
425// To set implicit integration
426template <typename Scalar>
428 return implicit_integration;
429}
430template <typename Scalar>
432 const bool& implicit) {
433 implicit_integration = implicit;
434}
435
437// Update current model
439
440template <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)");
448 }
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()) + ")");
453 }
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)");
457 }
458
459 xref_ = xref;
460 gait = S;
461
462 // Set ref u vector according to nb of contact
463 uref_.setZero();
464 if (relative_forces) {
465 for (int i = 0; i < 4; i = i + 1) {
466 if (gait[i] == 1) {
467 uref_[3 * i + 2] = (Scalar(9.81) * mass) / (gait.sum());
468 }
469 }
470 }
471
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;
474
475 I_inv = (R_tmp.transpose() * gI * R_tmp).inverse(); // I_inv
476 lever_arms.block(0, 0, 2, 4) = l_feet.block(0, 0, 2, 4);
477
478 for (int i = 0; i < 4; i = i + 1) {
479 if (S(i, 0) != 0) {
480 // set limit for normal force, (foot in contact with the ground)
481 ub(6 * i + 4) = -min_fz_in_contact;
482
483 // B update
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;
489 } else {
490 // set limit for normal force at 0.0
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();
494 };
495 };
496}
497} // namespace quadruped_walkgen
498
499#endif
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