quadruped_nl.hxx
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_nl_hxx__
2 #define __quadruped_walkgen_quadruped_nl_hxx__
3 
4 #include "crocoddyl/core/utils/exception.hpp"
5 
6 namespace quadruped_walkgen {
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) {
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  gait.setZero();
55  base_vector_x << Scalar(1.), Scalar(0.), Scalar(0.);
56  base_vector_y << Scalar(0.), Scalar(1.), Scalar(0.);
57  base_vector_z << Scalar(0.), Scalar(0.), Scalar(1.);
58  forces_3d.setZero();
59 
60  // Used for shoulder height weight
61  pshoulder_0 << Scalar(0.1946), Scalar(0.1946), Scalar(-0.1946),
62  Scalar(-0.1946), Scalar(0.14695), Scalar(-0.14695), Scalar(0.14695),
63  Scalar(-0.14695);
64  sh_hlim = Scalar(0.27);
65  sh_weight = Scalar(10.);
66  sh_ub_max_.setZero();
67  psh.setZero();
68 
69  // Implicit integration
70  // V+ = V + dt*B*u ; P+ = P + dt*V+ != explicit : P+ = P + dt*V
71  implicit_integration = true;
72  offset_com = offset_CoM; // x, y, z offset
73 }
74 
75 template <typename Scalar>
77 
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()) + ")");
87  }
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_) + ")");
92  }
93 
95  static_cast<ActionDataQuadrupedNonLinearTpl<Scalar>*>(data.get());
96 
97  // Update B :
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;
104 
105  // Compute pdistance of the shoulder wrt contact point
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];
112  } else {
113  // Compute pdistance of the shoulder wrt contact point
114  psh.block(0, i, 3, 1).setZero();
115  }
116  };
117 
118  // Discrete dynamic : A*x + B*u + g
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;
125 
126  // Residual cost on the state and force norm
127  d->r.template head<12>() = state_weights_.cwiseProduct(x - xref_);
128  d->r.template tail<12>() = force_weights_.cwiseProduct(u - uref_);
129 
130  // Friction cone + shoulder height
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);
135  }
136  rub_max_ = (Fa_x_u - ub).cwiseMax(Scalar(0.));
137 
138  // Shoulder height weight
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;
143 
144  sh_ub_max_ = sh_ub_max_.cwiseMax(Scalar(0.));
145 
146  // Cost computation
147  // d->cost = 0.5 * d->r.transpose() * d->r + friction_weight_ *
148  // Scalar(0.5) * rub_max_.squaredNorm() + sh_weight
149  // * Scalar(0.5) * sh_ub_max_.squaredNorm() ;
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();
153 }
154 
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()) + ")");
164  }
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_) + ")");
169  }
170 
172  static_cast<ActionDataQuadrupedNonLinearTpl<Scalar>*>(data.get());
173 
174  // Cost derivatives : Lx
175 
176  d->Lx = (state_weights_.array() * d->r.template head<12>().array()).matrix();
177 
178  // Hessian : Lxx
179  d->Lxx.block(0, 0, 6, 6).setZero();
180  d->Lxx.diagonal() =
181  (state_weights_.array() * state_weights_.array()).matrix();
182 
183  // Shoulder height derivative cost
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));
193 
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));
201 
202  d->Lxx(0, 5) += -sh_weight * pshoulder_0(1, j);
203  d->Lxx(5, 0) += -sh_weight * pshoulder_0(1, j);
204 
205  d->Lxx(1, 5) += sh_weight * pshoulder_0(0, j);
206  d->Lxx(5, 1) += sh_weight * pshoulder_0(0, j);
207 
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);
212 
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);
215  }
216  }
217 
218  // Cost derivative : Lu
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);
223  }
224  d->Lu = d->Lu +
225  (force_weights_.array() * d->r.template tail<12>().array()).matrix();
226 
227  // Hessian : Luu
228  // Matrix friction cone hessian (20x12)
229  Arr.diagonal() =
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);
236  }
237  d->Luu.diagonal() =
238  d->Luu.diagonal() +
239  (force_weights_.array() * force_weights_.array()).matrix();
240 
241  // Dynamic derivatives
242  d->Fx << A;
243 
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));
253  }
254  }
255  d->Fu << B;
256 }
257 
258 template <typename Scalar>
259 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
261  return boost::make_shared<ActionDataQuadrupedNonLinearTpl<Scalar> >(this);
262 }
263 
265 // get & set parameters ////////
267 
268 template <typename Scalar>
269 const typename Eigen::Matrix<Scalar, 12, 1>&
271  return force_weights_;
272 }
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()) + ")");
280  }
281  force_weights_ = weights;
282 }
283 
284 template <typename Scalar>
285 const typename Eigen::Matrix<Scalar, 12, 1>&
287  return state_weights_;
288 }
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()) + ")");
296  }
297  state_weights_ = weights;
298 }
299 
300 template <typename Scalar>
302  const {
303  return friction_weight_;
304 }
305 template <typename Scalar>
307  const Scalar& weight) {
308  friction_weight_ = weight;
309 }
310 
311 template <typename Scalar>
313  return mu;
314 }
315 template <typename Scalar>
317  mu = mu_coeff;
318 }
319 
320 template <typename Scalar>
322  return mass;
323 }
324 template <typename Scalar>
326  // The model need to be updated after this changed
327  mass = m;
328 }
329 
330 template <typename Scalar>
332  return dt_;
333 }
334 template <typename Scalar>
336  // The model need to be updated after this changed
337  dt_ = dt;
338  g[8] = Scalar(-9.81) * dt_;
339  A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
340 }
341 
342 template <typename Scalar>
343 const typename Eigen::Matrix<Scalar, 3, 3>&
345  return gI;
346 }
347 template <typename Scalar>
349  const typename MathBase::Matrix3s& inertia_matrix) {
350  // The model need to be updated after this changed
351  if (static_cast<std::size_t>(inertia_matrix.size()) != 9) {
352  throw_pretty("Invalid argument: "
353  << "gI has wrong dimension : 3x3");
354  }
355  gI = inertia_matrix;
356 }
357 
358 template <typename Scalar>
360  const {
361  // The model need to be updated after this changed
362  return min_fz_in_contact;
363 }
364 template <typename Scalar>
366  const Scalar& min_fz) {
367  // The model need to be updated after this changed
368  min_fz_in_contact = min_fz;
369 }
370 
371 template <typename Scalar>
373  const {
374  // The model need to be updated after this changed
375  return max_fz;
376 }
377 template <typename Scalar>
379  const Scalar& max_fz_) {
380  // The model need to be updated after this changed
381  max_fz = max_fz_;
382  for (int i = 0; i < 4; i = i + 1) {
383  ub(6 * i + 5) = max_fz;
384  }
385 }
386 
387 template <typename Scalar>
389  const {
390  return sh_hlim;
391 }
392 template <typename Scalar>
394  const Scalar& hlim) {
395  // The model need to be updated after this changed
396  sh_hlim = hlim;
397 }
398 
399 template <typename Scalar>
401  const {
402  return sh_weight;
403 }
404 template <typename Scalar>
406  const Scalar& weight) {
407  // The model need to be updated after this changed
408  sh_weight = weight;
409 }
410 
414 template <typename Scalar>
415 const typename Eigen::Matrix<Scalar, 12, 12>&
417  return A;
418 }
419 template <typename Scalar>
420 const typename Eigen::Matrix<Scalar, 12, 12>&
422  return B;
423 }
424 
425 // to modify the cost on the command : || fz - m*g/nb contact ||^2
426 // --> set to True
427 template <typename Scalar>
429  const {
430  return relative_forces;
431 }
432 template <typename Scalar>
434  const bool& rel_forces) {
435  relative_forces = rel_forces;
436  uref_.setZero();
437  if (relative_forces) {
438  for (int i = 0; i < 4; i = i + 1) {
439  if (gait[i] == 1) {
440  uref_[3 * i + 2] = (Scalar(9.81) * mass) / (gait.sum());
441  }
442  }
443  }
444 }
445 
447 // Update current model
449 
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)");
458  }
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()) + ")");
463  }
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)");
467  }
468 
469  xref_ = xref;
470  gait = S;
471 
472  // Set ref u vector according to nb of contact
473  uref_.setZero();
474  if (relative_forces) {
475  for (int i = 0; i < 4; i = i + 1) {
476  if (gait[i] == 1) {
477  uref_[3 * i + 2] = (Scalar(9.81) * mass) / (gait.sum());
478  }
479  }
480  }
481 
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;
484 
485  I_inv = (R_tmp.transpose() * gI * R_tmp).inverse(); // I_inv
486  lever_arms.block(0, 0, 2, 4) = l_feet.block(0, 0, 2, 4);
487 
488  for (int i = 0; i < 4; i = i + 1) {
489  if (S(i, 0) != 0) {
490  // set limit for normal force, (foot in contact with the ground)
491  ub(6 * i + 4) = -min_fz_in_contact;
492 
493  // B update
494  B.block(6, 3 * i, 3, 3).diagonal() << dt_ / mass, dt_ / mass, dt_ / mass;
495 
496  // Assuption 1 : levers arms not depends on the state, but on the
497  // predicted position (xfref)
498  // --> B will be updated with the update_B method for each calc function
499 
500  // lever_tmp = lever_arms.block(0,i,3,1) - xref.block(0,0,3,1) ;
501  // R_tmp << 0.0, -lever_tmp[2], lever_tmp[1],
502  // lever_tmp[2], 0.0, -lever_tmp[0], -lever_tmp[1], lever_tmp[0], 0.0 ;
503  // B.block(9 , 3*i , 3,3) << dt_ * R* R_tmp;
504  } else {
505  // set limit for normal force at 0.0
506  ub(6 * i + 4) = 0.0;
507  B.block(6, 3 * i, 3, 3).setZero();
508  B.block(9, 3 * i, 3, 3).setZero();
509  };
510  };
511 }
512 } // namespace quadruped_walkgen
513 
514 #endif
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