quadruped_augmented.hxx
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_augmented_hxx__
2 #define __quadruped_walkgen_quadruped_augmented_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> >(20), 12, 32) {
12  // Relative forces to compute the norm mof the command
13  relative_forces = true;
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_in_contact = Scalar(25.0);
22 
23  // Matrix model initialization
24  g.setZero();
25  g[8] = Scalar(-9.81) * dt_;
26  gI.setZero();
27  gI.diagonal() << Scalar(0.00578574), Scalar(0.01938108), Scalar(0.02476124);
28  A.setIdentity();
29  A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
30  B.setZero();
31  lever_arms.setZero();
32  R.setZero();
33 
34  // Weight vectors initialization
35  force_weights_.setConstant(Scalar(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  heuristic_weights_.setConstant(Scalar(1));
41  stop_weights_.setConstant(Scalar(1));
42  // pshoulder_ << Scalar(0.1946), Scalar(0.15005), Scalar(0.1946),
43  // Scalar(-0.15005), Scalar(-0.1946), Scalar(0.15005),
44  // Scalar(-0.1946), Scalar(-0.15005);
45  pshoulder_0 << Scalar(0.18), Scalar(0.18), Scalar(-0.21), Scalar(-0.21),
46  Scalar(0.14695), Scalar(-0.14695), Scalar(0.14695), Scalar(-0.14695);
47  // pshoulder_tmp.setZero();
48  // pcentrifugal_tmp_1.setZero();
49  // pcentrifugal_tmp_2.setZero();
50  // pcentrifugal_tmp.setZero();
51  // UpperBound vector
52  ub.setZero();
53  for (int i = 0; i < 4; i = i + 1) {
54  ub(6 * i + 5) = max_fz_in_contact;
55  }
56 
57  // Temporary vector used
58  Fa_x_u.setZero();
59  rub_.setZero();
60  rub_max_.setZero();
61  Arr.setZero();
62  r.setZero();
63  lever_tmp.setZero();
64  R_tmp.setZero();
65  gait.setZero();
66  base_vector_x << Scalar(1.), Scalar(0.), Scalar(0.);
67  base_vector_y << Scalar(0.), Scalar(1.), Scalar(0.);
68  base_vector_z << Scalar(0.), Scalar(0.), Scalar(1.);
69  forces_3d.setZero();
70  gait_double.setZero();
71 
72  // bool to add heuristic for foot position
73  centrifugal_term = true;
74  symmetry_term = true;
75  T_gait = Scalar(0.48);
76 
77  // // Used for shoulder height weight
78  // pshoulder_0 << Scalar(0.1946) , Scalar(0.1946) , Scalar(-0.1946),
79  // Scalar(-0.1946) ,
80  // Scalar(0.15005) , Scalar(-0.15005) , Scalar(0.15005) ,
81  // Scalar(-0.15005) ;
82  sh_hlim = Scalar(0.27);
83  sh_weight.setConstant(Scalar(1.));
84  sh_ub_max_.setZero();
85  psh.setZero();
86  pheuristic_.setZero();
87  offset_com = offset_CoM; // x, y, z offset
88 
89  shoulder_reference_position = false; // Using predicted trajectory of the CoM
90 }
91 
92 template <typename Scalar>
94 
95 template <typename Scalar>
97  const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
98  const Eigen::Ref<const typename MathBase::VectorXs>& x,
99  const Eigen::Ref<const typename MathBase::VectorXs>& u) {
100  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
101  throw_pretty("Invalid argument: "
102  << "x has wrong dimension (it should be " +
103  std::to_string(state_->get_nx()) + ")");
104  }
105  if (static_cast<std::size_t>(u.size()) != nu_) {
106  throw_pretty("Invalid argument: "
107  << "u has wrong dimension (it should be " +
108  std::to_string(nu_) + ")");
109  }
110 
112  static_cast<ActionDataQuadrupedAugmentedTpl<Scalar>*>(data.get());
113 
114  // Update B :
115  for (int i = 0; i < 4; i = i + 1) {
116  lever_tmp.setZero();
117  if (gait(i, 0) != 0) {
118  lever_tmp.head(2) = x.block(12 + 2 * i, 0, 2, 1);
119  lever_tmp += -x.block(0, 0, 3, 1);
120  R_tmp << Scalar(0.0), -lever_tmp[2], lever_tmp[1], lever_tmp[2],
121  Scalar(0.0), -lever_tmp[0], -lever_tmp[1], lever_tmp[0], Scalar(0.0);
122  B.block(9, 3 * i, 3, 3) << dt_ * R * R_tmp;
123 
124  // Compute pdistance of the shoulder wrt contact point
125  if (shoulder_reference_position) {
126  // Ref vector as reference for the shoulder trajectory, roll and pitch
127  // at first
128  psh.block(0, i, 3, 1) << xref_(0, 0) - offset_com(0, 0) +
129  pshoulder_0(0, i) * cos(xref_(5, 0)) -
130  pshoulder_0(1, i) * sin(xref_(5, 0)) -
131  x(12 + 2 * i),
132  xref_(1, 0) - offset_com(1, 0) +
133  pshoulder_0(0, i) * sin(xref_(5, 0)) +
134  pshoulder_0(1, i) * cos(xref_(5, 0)) - x(12 + 2 * i + 1),
135  xref_(2, 0) - offset_com(2, 0);
136  } else {
137  // psh.block(0, i, 3, 1) << x(0) + pshoulder_0(0, i) - pshoulder_0(1, i)
138  // * x(5) - x(12 + 2 * i),
139  // x(1) + pshoulder_0(1, i) + pshoulder_0(0, i)
140  // * x(5) - x(12 + 2 * i + 1), x(2) -
141  // offset_com + pshoulder_0(1, i) * x(3) -
142  // pshoulder_0(0, i) * x(4);
143  // Correction, no approximation for yaw
144  psh.block(0, i, 3, 1)
145  << x(0) - offset_com(0, 0) + pshoulder_0(0, i) * cos(x(5)) -
146  pshoulder_0(1, i) * sin(x(5)) - x(12 + 2 * i),
147  x(1) - offset_com(1, 0) + pshoulder_0(0, i) * sin(x(5)) +
148  pshoulder_0(1, i) * cos(x(5)) - x(12 + 2 * i + 1),
149  x(2) - offset_com(2, 0) + pshoulder_0(1, i) * x(3) -
150  pshoulder_0(0, i) * x(4);
151  }
152 
153  } else {
154  // Compute pdistance of the shoulder wrt contact point
155  psh.block(0, i, 3, 1).setZero();
156  }
157  };
158 
159  // Discrete dynamic : A*x + B*u + g
160  d->xnext.template head<12>() =
161  A.diagonal().cwiseProduct(x.block(0, 0, 12, 1)) + g;
162  d->xnext.template head<6>() =
163  d->xnext.template head<6>() +
164  A.topRightCorner(6, 6).diagonal().cwiseProduct(x.block(6, 0, 6, 1));
165  d->xnext.template segment<6>(6) =
166  d->xnext.template segment<6>(6) + B.block(6, 0, 6, 12) * u;
167  d->xnext.template tail<8>() = x.tail(8);
168 
169  // Residual cost on the state and force norm
170  d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
171  d->r.template segment<8>(12) =
172  ((heuristic_weights_.cwiseProduct(x.tail(8) - pheuristic_)).array() *
173  gait_double.array())
174  .matrix();
175  d->r.template tail<12>() = force_weights_.cwiseProduct(u - uref_);
176 
177  // Friction cone
178  for (int i = 0; i < 4; i = i + 1) {
179  Fa_x_u.segment(6 * i, 6) << u(3 * i) - mu * u(3 * i + 2),
180  -u(3 * i) - mu * u(3 * i + 2), u(3 * i + 1) - mu * u(3 * i + 2),
181  -u(3 * i + 1) - mu * u(3 * i + 2), -u(3 * i + 2), u(3 * i + 2);
182  }
183  rub_max_ = (Fa_x_u - ub).cwiseMax(Scalar(0.));
184 
185  // Shoulder height weight
186  sh_ub_max_ << Scalar(0.5) * sh_weight(0) *
187  (psh.block(0, 0, 3, 1).squaredNorm() - sh_hlim * sh_hlim),
188  Scalar(0.5) * sh_weight(1) *
189  (psh.block(0, 1, 3, 1).squaredNorm() - sh_hlim * sh_hlim),
190  Scalar(0.5) * sh_weight(2) *
191  (psh.block(0, 2, 3, 1).squaredNorm() - sh_hlim * sh_hlim),
192  Scalar(0.5) * sh_weight(3) *
193  (psh.block(0, 3, 3, 1).squaredNorm() - sh_hlim * sh_hlim);
194 
195  sh_ub_max_ = sh_ub_max_.cwiseMax(Scalar(0.));
196 
197  // Cost computation
198  // d->cost = Scalar(0.5) * d->r.transpose() * d->r + friction_weight_ *
199  // Scalar(0.5) * rub_max_.squaredNorm()
200  // + Scalar(0.5)*( (stop_weights_.cwiseProduct(x.tail(8) - pref_)
201  // ).array() * gait_double.array()
202  // ).matrix().squaredNorm() ;
203 
204  d->cost =
205  Scalar(0.5) * d->r.transpose() * d->r +
206  friction_weight_ * Scalar(0.5) * rub_max_.squaredNorm() +
207  Scalar(0.5) * ((stop_weights_.cwiseProduct(x.tail(8) - pstop_)).array() *
208  gait_double.array())
209  .matrix()
210  .squaredNorm() +
211  sh_ub_max_.sum();
212 }
213 
214 template <typename Scalar>
216  const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
217  const Eigen::Ref<const typename MathBase::VectorXs>& x,
218  const Eigen::Ref<const typename MathBase::VectorXs>& u) {
219  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
220  throw_pretty("Invalid argument: "
221  << "x has wrong dimension (it should be " +
222  std::to_string(state_->get_nx()) + ")");
223  }
224  if (static_cast<std::size_t>(u.size()) != nu_) {
225  throw_pretty("Invalid argument: "
226  << "u has wrong dimension (it should be " +
227  std::to_string(nu_) + ")");
228  }
229 
231  static_cast<ActionDataQuadrupedAugmentedTpl<Scalar>*>(data.get());
232 
233  // Cost derivatives : Lx
234  d->Lx.setZero();
235  d->Lx.template head<12>() =
236  (state_weights_.array() * d->r.template head<12>().array()).matrix();
237  d->Lx.template tail<8>() =
238  (heuristic_weights_.array() * d->r.template segment<8>(12).array())
239  .matrix() +
240  ((stop_weights_.cwiseProduct(x.tail(8) - pstop_)).array() *
241  gait_double.array() * stop_weights_.array())
242  .matrix();
243 
244  // Cost derivative : Lu
245  for (int i = 0; i < 4; i = i + 1) {
246  r = friction_weight_ * rub_max_.segment(6 * i, 6);
247  d->Lu.block(i * 3, 0, 3, 1) << r(0) - r(1), r(2) - r(3),
248  -mu * (r(0) + r(1) + r(2) + r(3)) - r(4) + r(5);
249  }
250  d->Lu = d->Lu +
251  (force_weights_.array() * d->r.template tail<12>().array()).matrix();
252 
253  // Hessian : Lxx
254  // Hessian : Lxx
255  d->Lxx.setZero();
256 
257  d->Lxx.diagonal().head(12) =
258  (state_weights_.array() * state_weights_.array()).matrix();
259  d->Lxx.diagonal().tail(8) =
260  (gait_double.array() * heuristic_weights_.array() *
261  heuristic_weights_.array())
262  .matrix();
263  d->Lxx.diagonal().tail(8) +=
264  (gait_double.array() * stop_weights_.array() * stop_weights_.array())
265  .matrix();
266 
267  // Shoulder height derivative cost
268  for (int j = 0; j < 4; j = j + 1) {
269  if (sh_ub_max_[j] > Scalar(0.)) {
270  if (shoulder_reference_position) {
271  d->Lx(12 + 2 * j, 0) += -sh_weight(j) * psh(0, j);
272  d->Lx(12 + 2 * j + 1, 0) += -sh_weight(j) * psh(1, j);
273 
274  d->Lxx(12 + 2 * j, 12 + 2 * j) += sh_weight(j);
275  d->Lxx(12 + 2 * j + 1, 12 + 2 * j + 1) += sh_weight(j);
276 
277  } else {
278  // Approximation of small even for yaw (wrong)
279  // d->Lx(0, 0) += sh_weight(j) * psh(0, j);
280  // d->Lx(1, 0) += sh_weight(j) * psh(1, j);
281  // d->Lx(2, 0) += sh_weight(j) * psh(2, j);
282  // d->Lx(3, 0) += sh_weight(j) * pshoulder_0(1, j) * psh(2, j);
283  // d->Lx(4, 0) += -sh_weight(j) * pshoulder_0(0, j) * psh(2, j);
284  // d->Lx(5, 0) += sh_weight(j) * (-pshoulder_0(1, j) * psh(0, j) +
285  // pshoulder_0(0, j) * psh(1, j));
286 
287  // d->Lx(12 + 2 * j, 0) += -sh_weight(j) * psh(0, j);
288  // d->Lx(12 + 2 * j + 1, 0) += -sh_weight(j) * psh(1, j);
289 
290  // d->Lxx(0, 0) += sh_weight(j);
291  // d->Lxx(1, 1) += sh_weight(j);
292  // d->Lxx(2, 2) += sh_weight(j);
293  // d->Lxx(3, 3) += sh_weight(j) * pshoulder_0(1, j) * pshoulder_0(1, j);
294  // d->Lxx(3, 3) += sh_weight(j) * pshoulder_0(0, j) * pshoulder_0(0, j);
295  // d->Lxx(5, 5) += sh_weight(j) * (pshoulder_0(1, j) * pshoulder_0(1, j)
296  // + pshoulder_0(0, j) * pshoulder_0(0, j));
297 
298  // d->Lxx(12 + 2 * j, 12 + 2 * j) += sh_weight(j);
299  // d->Lxx(12 + 2 * j + 1, 12 + 2 * j + 1) += sh_weight(j);
300 
301  // d->Lxx(0, 5) += -sh_weight(j) * pshoulder_0(1, j);
302  // d->Lxx(5, 0) += -sh_weight(j) * pshoulder_0(1, j);
303 
304  // d->Lxx(1, 5) += sh_weight(j) * pshoulder_0(0, j);
305  // d->Lxx(5, 1) += sh_weight(j) * pshoulder_0(0, j);
306 
307  // d->Lxx(2, 3) += sh_weight(j) * pshoulder_0(1, j);
308  // d->Lxx(2, 4) += -sh_weight(j) * pshoulder_0(0, j);
309  // d->Lxx(3, 2) += sh_weight(j) * pshoulder_0(1, j);
310  // d->Lxx(4, 2) += -sh_weight(j) * pshoulder_0(0, j);
311 
312  // d->Lxx(3, 4) += -sh_weight(j) * pshoulder_0(1, j) * pshoulder_0(0,
313  // j); d->Lxx(4, 3) += -sh_weight(j) * pshoulder_0(1, j) *
314  // pshoulder_0(0, j);
315 
316  // d->Lxx(0, 12 + 2 * j) += -sh_weight(j);
317  // d->Lxx(12 + 2 * j, 0) += -sh_weight(j);
318 
319  // d->Lxx(5, 12 + 2 * j) += sh_weight(j) * pshoulder_0(1, j);
320  // d->Lxx(12 + 2 * j, 5) += sh_weight(j) * pshoulder_0(1, j);
321 
322  // d->Lxx(1, 12 + 2 * j + 1) += -sh_weight(j);
323  // d->Lxx(12 + 2 * j + 1, 1) += -sh_weight(j);
324 
325  // d->Lxx(5, 12 + 2 * j + 1) += -sh_weight(j) * pshoulder_0(0, j);
326  // d->Lxx(12 + 2 * j + 1, 5) += -sh_weight(j) * pshoulder_0(0, j);
327  d->Lx(0, 0) += sh_weight(j) * psh(0, j);
328  d->Lx(1, 0) += sh_weight(j) * psh(1, j);
329  d->Lx(2, 0) += sh_weight(j) * psh(2, j);
330  d->Lx(3, 0) += sh_weight(j) * pshoulder_0(1, j) * psh(2, j);
331  d->Lx(4, 0) += -sh_weight(j) * pshoulder_0(0, j) * psh(2, j);
332  d->Lx(5, 0) +=
333  sh_weight(j) *
334  ((-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j)) *
335  psh(0, j) +
336  (cos(x(5)) * pshoulder_0(0, j) - sin(x(5)) * pshoulder_0(1, j)) *
337  psh(1, j));
338 
339  d->Lx(12 + 2 * j, 0) += -sh_weight(j) * psh(0, j);
340  d->Lx(12 + 2 * j + 1, 0) += -sh_weight(j) * psh(1, j);
341 
342  d->Lxx(0, 0) += sh_weight(j);
343  d->Lxx(1, 1) += sh_weight(j);
344  d->Lxx(2, 2) += sh_weight(j);
345  d->Lxx(3, 3) += sh_weight(j) * pshoulder_0(1, j) * pshoulder_0(1, j);
346  d->Lxx(3, 3) += sh_weight(j) * pshoulder_0(0, j) * pshoulder_0(0, j);
347  d->Lxx(5, 5) +=
348  sh_weight(j) *
349  ((-cos(x(5)) * pshoulder_0(0, j) + sin(x(5)) * pshoulder_0(1, j)) *
350  psh(0, j) +
351  (-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j)) *
352  (-sin(x(5)) * pshoulder_0(0, j) -
353  cos(x(5)) * pshoulder_0(1, j)) +
354  (-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j)) *
355  psh(1, j) +
356  (cos(x(5)) * pshoulder_0(0, j) - sin(x(5)) * pshoulder_0(1, j)) *
357  (cos(x(5)) * pshoulder_0(0, j) -
358  sin(x(5)) * pshoulder_0(1, j)));
359 
360  d->Lxx(12 + 2 * j, 12 + 2 * j) += sh_weight(j);
361  d->Lxx(12 + 2 * j + 1, 12 + 2 * j + 1) += sh_weight(j);
362 
363  d->Lxx(0, 5) += sh_weight(j) * (-sin(x(5)) * pshoulder_0(0, j) -
364  cos(x(5)) * pshoulder_0(1, j));
365  d->Lxx(5, 0) += sh_weight(j) * (-sin(x(5)) * pshoulder_0(0, j) -
366  cos(x(5)) * pshoulder_0(1, j));
367 
368  d->Lxx(1, 5) += sh_weight(j) * (cos(x(5)) * pshoulder_0(0, j) -
369  sin(x(5)) * pshoulder_0(1, j));
370  d->Lxx(5, 1) += sh_weight(j) * (cos(x(5)) * pshoulder_0(0, j) -
371  sin(x(5)) * pshoulder_0(1, j));
372 
373  d->Lxx(2, 3) += sh_weight(j) * pshoulder_0(1, j);
374  d->Lxx(2, 4) += -sh_weight(j) * pshoulder_0(0, j);
375  d->Lxx(3, 2) += sh_weight(j) * pshoulder_0(1, j);
376  d->Lxx(4, 2) += -sh_weight(j) * pshoulder_0(0, j);
377 
378  d->Lxx(3, 4) += -sh_weight(j) * pshoulder_0(1, j) * pshoulder_0(0, j);
379  d->Lxx(4, 3) += -sh_weight(j) * pshoulder_0(1, j) * pshoulder_0(0, j);
380 
381  d->Lxx(0, 12 + 2 * j) += -sh_weight(j);
382  d->Lxx(12 + 2 * j, 0) += -sh_weight(j);
383 
384  d->Lxx(5, 12 + 2 * j) +=
385  -sh_weight(j) *
386  (-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j));
387  d->Lxx(12 + 2 * j, 5) +=
388  -sh_weight(j) *
389  (-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j));
390 
391  d->Lxx(1, 12 + 2 * j + 1) += -sh_weight(j);
392  d->Lxx(12 + 2 * j + 1, 1) += -sh_weight(j);
393 
394  d->Lxx(5, 12 + 2 * j + 1) +=
395  -sh_weight(j) *
396  (cos(x(5)) * pshoulder_0(0, j) - sin(x(5)) * pshoulder_0(1, j));
397  d->Lxx(12 + 2 * j + 1, 5) +=
398  -sh_weight(j) *
399  (cos(x(5)) * pshoulder_0(0, j) - sin(x(5)) * pshoulder_0(1, j));
400  }
401  }
402  }
403 
404  // Hessian : Luu
405  // Matrix friction cone hessian (20x12)
406  Arr.diagonal() =
407  ((Fa_x_u - ub).array() >= 0.).matrix().template cast<Scalar>();
408  for (int i = 0; i < 4; i = i + 1) {
409  r = friction_weight_ * Arr.diagonal().segment(6 * i, 6);
410  d->Luu.block(3 * i, 3 * i, 3, 3) << r(0) + r(1), 0.0, mu * (r(1) - r(0)),
411  0.0, r(2) + r(3), mu * (r(3) - r(2)), mu * (r(1) - r(0)),
412  mu * (r(3) - r(2)), mu * mu * (r(0) + r(1) + r(2) + r(3)) + r(4) + r(5);
413  }
414  d->Luu.diagonal() =
415  d->Luu.diagonal() +
416  (force_weights_.array() * force_weights_.array()).matrix();
417 
418  // Dynamic derivatives
419  d->Fx.setZero();
420  d->Fx.block(0, 0, 12, 12) << A;
421  d->Fx.block(12, 12, 8, 8) << Eigen::Matrix<Scalar, 8, 8>::Identity();
422 
423  for (int i = 0; i < 4; i = i + 1) {
424  if (gait(i, 0) != 0) {
425  forces_3d = u.block(3 * i, 0, 3, 1);
426  d->Fx.block(9, 0, 3, 1) += -dt_ * R * (base_vector_x.cross(forces_3d));
427  d->Fx.block(9, 1, 3, 1) += -dt_ * R * (base_vector_y.cross(forces_3d));
428  d->Fx.block(9, 2, 3, 1) += -dt_ * R * (base_vector_z.cross(forces_3d));
429 
430  d->Fx.block(9, 12 + 2 * i, 3, 1) +=
431  dt_ * R * (base_vector_x.cross(forces_3d));
432  d->Fx.block(9, 12 + 2 * i + 1, 3, 1) +=
433  dt_ * R * (base_vector_y.cross(forces_3d));
434  }
435  }
436  // d->Fu << Eigen::Matrix<Scalar, 20, 12>::Zero() ;
437  d->Fu.block(0, 0, 12, 12) << B;
438 }
439 
440 template <typename Scalar>
441 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
443  return boost::make_shared<ActionDataQuadrupedAugmentedTpl<Scalar> >(this);
444 }
445 
447 // get & set parameters ////////
449 
450 template <typename Scalar>
451 const typename Eigen::Matrix<Scalar, 12, 1>&
453  return force_weights_;
454 }
455 template <typename Scalar>
457  const typename MathBase::VectorXs& weights) {
458  if (static_cast<std::size_t>(weights.size()) != 12) {
459  throw_pretty("Invalid argument: "
460  << "Weights vector has wrong dimension (it should be 12)");
461  }
462  force_weights_ = weights;
463 }
464 
465 template <typename Scalar>
466 const typename Eigen::Matrix<Scalar, 12, 1>&
468  return state_weights_;
469 }
470 template <typename Scalar>
472  const typename MathBase::VectorXs& weights) {
473  if (static_cast<std::size_t>(weights.size()) != 12) {
474  throw_pretty("Invalid argument: "
475  << "Weights vector has wrong dimension (it should be 12)");
476  }
477  state_weights_ = weights;
478 }
479 
480 template <typename Scalar>
481 const typename Eigen::Matrix<Scalar, 8, 1>&
483  return heuristic_weights_;
484 }
485 template <typename Scalar>
487  const typename MathBase::VectorXs& weights) {
488  if (static_cast<std::size_t>(weights.size()) != 8) {
489  throw_pretty("Invalid argument: "
490  << "Weights vector has wrong dimension (it should be 8)");
491  }
492  heuristic_weights_ = weights;
493 }
494 
495 template <typename Scalar>
496 const typename Eigen::Matrix<Scalar, 8, 1>&
498  return stop_weights_;
499 }
500 template <typename Scalar>
502  const typename MathBase::VectorXs& weights) {
503  if (static_cast<std::size_t>(weights.size()) != 8) {
504  throw_pretty("Invalid argument: "
505  << "Weights vector has wrong dimension (it should be 8)");
506  }
507  stop_weights_ = weights;
508 }
509 
510 template <typename Scalar>
512  const {
513  return friction_weight_;
514 }
515 template <typename Scalar>
517  const Scalar& weight) {
518  friction_weight_ = weight;
519 }
520 
521 template <typename Scalar>
523  return mu;
524 }
525 template <typename Scalar>
527  mu = mu_coeff;
528 }
529 
530 template <typename Scalar>
532  return mass;
533 }
534 template <typename Scalar>
536  // The model need to be updated after this changed
537  mass = m;
538 }
539 
540 template <typename Scalar>
542  return dt_;
543 }
544 template <typename Scalar>
546  // The model need to be updated after this changed
547  dt_ = dt;
548  g[8] = Scalar(-9.81) * dt_;
549  A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
550 }
551 
552 template <typename Scalar>
553 const typename Eigen::Matrix<Scalar, 3, 3>&
555  return gI;
556 }
557 template <typename Scalar>
559  const typename MathBase::Matrix3s& inertia_matrix) {
560  // The model need to be updated after this changed
561  if (static_cast<std::size_t>(inertia_matrix.size()) != 9) {
562  throw_pretty("Invalid argument: "
563  << "gI has wrong dimension : 3x3");
564  }
565  gI = inertia_matrix;
566 }
567 
568 template <typename Scalar>
570  const {
571  // The model need to be updated after this changed
572  return min_fz_in_contact;
573 }
574 template <typename Scalar>
576  const Scalar& min_fz) {
577  // The model need to be updated after this changed
578  min_fz_in_contact = min_fz;
579 }
580 
581 template <typename Scalar>
583  const {
584  // The model need to be updated after this changed
585  return max_fz_in_contact;
586 }
587 template <typename Scalar>
589  const Scalar& max_fz) {
590  // The model need to be updated after this changed
591  max_fz_in_contact = max_fz;
592  for (int i = 0; i < 4; i = i + 1) {
593  ub(6 * i + 5) = max_fz_in_contact;
594  }
595 }
596 
597 template <typename Scalar>
599  const {
600  return symmetry_term;
601 }
602 template <typename Scalar>
604  const bool& sym_term) {
605  // The model need to be updated after this changed
606  symmetry_term = sym_term;
607 }
608 
609 template <typename Scalar>
611  const {
612  return centrifugal_term;
613 }
614 template <typename Scalar>
616  const bool& cent_term) {
617  // The model need to be updated after this changed
618  centrifugal_term = cent_term;
619 }
620 
621 template <typename Scalar>
623  Scalar>::get_shoulder_reference_position() const {
624  return shoulder_reference_position;
625 }
626 template <typename Scalar>
628  const bool& reference) {
629  shoulder_reference_position = reference;
630 }
631 
632 template <typename Scalar>
634  // The model need to be updated after this changed
635  return T_gait;
636 }
637 template <typename Scalar>
639  const Scalar& T_gait_) {
640  // The model need to be updated after this changed
641  T_gait = T_gait_;
642 }
643 
644 template <typename Scalar>
646  const {
647  return sh_hlim;
648 }
649 template <typename Scalar>
651  const Scalar& hlim) {
652  // The model need to be updated after this changed
653  sh_hlim = hlim;
654 }
655 
656 template <typename Scalar>
657 const typename Eigen::Matrix<Scalar, 4, 1>&
659  return sh_weight;
660 }
661 template <typename Scalar>
663  const typename Eigen::Matrix<Scalar, 4, 1>& weight) {
664  // The model need to be updated after this changed
665  sh_weight = weight;
666 }
667 
671 template <typename Scalar>
672 const typename Eigen::Matrix<Scalar, 12, 12>&
674  return A;
675 }
676 template <typename Scalar>
677 const typename Eigen::Matrix<Scalar, 12, 12>&
679  return B;
680 }
681 
682 // to modify the cost on the command : || fz - m*g/nb contact ||^2
683 // --> set to True
684 template <typename Scalar>
686  const {
687  return relative_forces;
688 }
689 template <typename Scalar>
691  const bool& rel_forces) {
692  relative_forces = rel_forces;
693  uref_.setZero();
694  if (relative_forces) {
695  for (int i = 0; i < 4; i = i + 1) {
696  if (gait[i] == 1) {
697  uref_[3 * i + 2] = (Scalar(9.81) * mass) / (gait.sum());
698  }
699  }
700  }
701 }
702 
704 // Update current model
706 
707 template <typename Scalar>
709  const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
710  const Eigen::Ref<const typename MathBase::MatrixXs>& l_stop,
711  const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
712  const Eigen::Ref<const typename MathBase::MatrixXs>& S) {
713  if (static_cast<std::size_t>(l_feet.size()) != 12) {
714  throw_pretty("Invalid argument: "
715  << "l_feet matrix has wrong dimension (it should be : 3x4)");
716  }
717  if (static_cast<std::size_t>(xref.size()) != 12) {
718  throw_pretty("Invalid argument: "
719  << "xref vector has wrong dimension (it should be 12 )");
720  }
721  if (static_cast<std::size_t>(S.size()) != 4) {
722  throw_pretty("Invalid argument: "
723  << "S vector has wrong dimension (it should be 4x1)");
724  }
725 
726  xref_ = xref;
727  gait = S;
728 
729  uref_.setZero();
730  if (relative_forces) {
731  for (int i = 0; i < 4; i = i + 1) {
732  if (gait[i] == 1) {
733  uref_[3 * i + 2] = (Scalar(9.81) * mass) / (gait.sum());
734  }
735  }
736  }
737 
738  for (int i = 0; i < 4; i = i + 1) {
739  gait_double(2 * i, 0) = gait(i, 0);
740  gait_double(2 * i + 1, 0) = gait(i, 0);
741 
742  pheuristic_.block(2 * i, 0, 2, 1) = l_feet.block(0, i, 2, 1);
743  pstop_.block(2 * i, 0, 2, 1) = l_stop.block(0, i, 2, 1);
744  }
745 
746  R_tmp << cos(xref(5, 0)), -sin(xref(5, 0)), Scalar(0), sin(xref(5, 0)),
747  cos(xref(5, 0)), Scalar(0), Scalar(0), Scalar(0), Scalar(1.0);
748 
749  // Centrifual term
750  // pcentrifugal_tmp_1 = xref.block(6, 0, 3, 1);
751  // pcentrifugal_tmp_2 = xref.block(9, 0, 3, 1);
752  // pcentrifugal_tmp = 0.5 * std::sqrt(xref(2, 0) / 9.81) *
753  // pcentrifugal_tmp_1.cross(pcentrifugal_tmp_2);
754 
755  // for (int i = 0; i < 4; i = i + 1) {
756  // pshoulder_tmp.block(0, i, 2, 1) =
757  // R_tmp.block(0, 0, 2, 2) *
758  // (pshoulder_0.block(0, i, 2, 1) + symmetry_term * 0.25 * T_gait *
759  // xref.block(6, 0, 2, 1) +
760  // centrifugal_term * pcentrifugal_tmp.block(0, 0, 2, 1));
761  // }
762 
763  R = (R_tmp.transpose() * gI * R_tmp).inverse(); // I_inv
764 
765  for (int i = 0; i < 4; i = i + 1) {
766  // pshoulder_[2 * i] = pshoulder_tmp(0, i) + xref(0, 0);
767  // pshoulder_[2 * i + 1] = pshoulder_tmp(1, i) + xref(1, 0);
768 
769  if (S(i, 0) != 0) {
770  // set limit for normal force, (foot in contact with the ground)
771  ub[5 * i + 4] = -min_fz_in_contact;
772 
773  // B update
774  B.block(6, 3 * i, 3, 3).diagonal() << dt_ / mass, dt_ / mass, dt_ / mass;
775 
776  // Assuption 1 : levers arms not depends on the state, but on the
777  // predicted position (xfref)
778  // --> B will be updated with the update_B method for each calc function
779 
780  // lever_tmp = lever_arms.block(0,i,3,1) - xref.block(0,0,3,1) ;
781  // R_tmp << 0.0, -lever_tmp[2], lever_tmp[1],
782  // lever_tmp[2], 0.0, -lever_tmp[0], -lever_tmp[1], lever_tmp[0], 0.0 ;
783  // B.block(9 , 3*i , 3,3) << dt_ * R* R_tmp;
784  } else {
785  // set limit for normal force at 0.0
786  ub[5 * i + 4] = Scalar(0.0);
787  B.block(6, 3 * i, 3, 3).setZero();
788  B.block(9, 3 * i, 3, 3).setZero();
789  };
790  };
791 }
792 } // namespace quadruped_walkgen
793 
794 #endif
Definition: quadruped_augmented.hpp:14
const bool & get_centrifugal_term() const
Definition: quadruped_augmented.hxx:610
const Scalar & get_max_fz_contact() const
Definition: quadruped_augmented.hxx:582
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_augmented.hxx:638
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_augmented.hxx:482
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_augmented.hxx:96
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:486
const Scalar & get_shoulder_hlim() const
Definition: quadruped_augmented.hxx:645
void set_min_fz_contact(const Scalar &min_fz)
Definition: quadruped_augmented.hxx:575
void set_stop_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:501
const Scalar & get_T_gait() const
Definition: quadruped_augmented.hxx:633
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition: quadruped_augmented.hxx:673
void set_friction_weight(const Scalar &weight)
Definition: quadruped_augmented.hxx:516
const Scalar & get_min_fz_contact() const
Definition: quadruped_augmented.hxx:569
const Eigen::Matrix< Scalar, 4, 1 > & get_shoulder_contact_weight() const
Definition: quadruped_augmented.hxx:658
~ActionModelQuadrupedAugmentedTpl()
Definition: quadruped_augmented.hxx:93
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &l_stop, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::MatrixXs > &S)
Definition: quadruped_augmented.hxx:708
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_augmented.hxx:467
const Scalar & get_friction_weight() const
Definition: quadruped_augmented.hxx:511
const Scalar & get_mu() const
Definition: quadruped_augmented.hxx:522
void set_max_fz_contact(const Scalar &max_fz)
Definition: quadruped_augmented.hxx:588
void set_relative_forces(const bool &rel_forces)
Definition: quadruped_augmented.hxx:690
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition: quadruped_augmented.hxx:558
const Scalar & get_dt() const
Definition: quadruped_augmented.hxx:541
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_augmented.hxx:442
void set_centrifugal_term(const bool &cent_term)
Definition: quadruped_augmented.hxx:615
void set_dt(const Scalar &dt)
Definition: quadruped_augmented.hxx:545
const bool & get_symmetry_term() const
Definition: quadruped_augmented.hxx:598
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition: quadruped_augmented.hxx:554
void set_shoulder_hlim(const Scalar &hlim)
Definition: quadruped_augmented.hxx:650
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:456
void set_mu(const Scalar &mu_coeff)
Definition: quadruped_augmented.hxx:526
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:471
_Scalar Scalar
Definition: quadruped_augmented.hpp:16
ActionModelQuadrupedAugmentedTpl(typename Eigen::Matrix< Scalar, 3, 1 > offset_CoM=Eigen::Matrix< Scalar, 3, 1 >::Zero())
Definition: quadruped_augmented.hxx:8
void set_mass(const Scalar &m)
Definition: quadruped_augmented.hxx:535
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_augmented.hxx:603
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_augmented.hxx:215
void set_shoulder_contact_weight(const typename Eigen::Matrix< Scalar, 4, 1 > &weight)
Definition: quadruped_augmented.hxx:662
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition: quadruped_augmented.hxx:678
void set_shoulder_reference_position(const bool &reference)
Definition: quadruped_augmented.hxx:627
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition: quadruped_augmented.hxx:452
const bool & get_relative_forces() const
Definition: quadruped_augmented.hxx:685
const Scalar & get_mass() const
Definition: quadruped_augmented.hxx:531
const Eigen::Matrix< Scalar, 8, 1 > & get_stop_weights() const
Definition: quadruped_augmented.hxx:497
Definition: quadruped.hpp:11
Definition: quadruped_augmented.hpp:183