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