quadruped_step.hxx
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_step_hxx__
2 #define __quadruped_walkgen_quadruped_step_hxx__
3 
4 #include "crocoddyl/core/utils/exception.hpp"
5 
6 namespace quadruped_walkgen {
7 template <typename Scalar>
9  : crocoddyl::ActionModelAbstractTpl<Scalar>(
10  boost::make_shared<crocoddyl::StateVectorTpl<Scalar> >(20), 8, 28) {
11  B.setZero();
12  state_weights_ << Scalar(1.), Scalar(1.), Scalar(150.), Scalar(35.),
13  Scalar(30.), Scalar(8.), Scalar(20.), Scalar(20.), Scalar(15.),
14  Scalar(4.), Scalar(4.), Scalar(8.);
15 
16  pheuristic_ << Scalar(0.18), Scalar(0.15005), Scalar(0.18), Scalar(-0.15005),
17  Scalar(-0.21), Scalar(0.15005), Scalar(-0.21), Scalar(-0.15005);
18 
19  centrifugal_term = true;
20  symmetry_term = true;
21  T_gait = Scalar(0.48);
22 
23  step_weights_.setConstant(Scalar(1));
24  heuristic_weights_.setConstant(Scalar(1));
25 
26  // Compute heuristic inside
27  // pshoulder_0 << Scalar(0.1946), Scalar(0.1946), Scalar(-0.1946),
28  // Scalar(-0.1946), Scalar(0.15005), Scalar(-0.15005),
29  // Scalar(0.15005), Scalar(-0.15005);
30  // pshoulder_tmp.setZero();
31  // pcentrifugal_tmp_1.setZero();
32  // pcentrifugal_tmp_2.setZero();
33  // pcentrifugal_tmp.setZero();
34 
35  N_sampling = 5; // Number of point to sample the polynomial curve of the feet
36  // trajectory
37  S_.setZero(); // Usefull to compute only the trajectory for moving feet
38  position_.setZero(); // Xk+1 = Xk + Uk, Xk does not correspond to the current
39  // position of the flying feet, Delta_x is not
40  // straightforward
41 
42  // Cost on the acceleration of the feet :
43  is_acc_activated_ = true;
44  acc_weight_ = Scalar(1.);
45  acc_lim_.setConstant(Scalar(50.));
46 
47  delta_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
48  N_sampling - 1, 4);
49  gamma_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
50  N_sampling - 1, 3);
51  for (int k = 1; k < N_sampling; k++) {
52  delta_(k - 1, 0) =
53  (float)k / (float)N_sampling; // [1/N, 2/N, ... , (N-1)/N]
54  }
55  delta_.col(1) << delta_.col(0).pow(2);
56  delta_.col(2) << delta_.col(0).pow(3);
57  delta_.col(3) << delta_.col(0).pow(4); // Only used for speed cost
58  gamma_.col(0) =
59  60 * delta_.col(0) - 180 * delta_.col(1) + 120 * delta_.col(2);
60  gamma_.col(1) = -36 * delta_.col(0) + 96 * delta_.col(1) - 60 * delta_.col(2);
61  gamma_.col(2) = -9 * delta_.col(0) + 18 * delta_.col(1) - 10 * delta_.col(2);
62 
63  alpha_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
64  N_sampling - 1, 1); // Common for 4 feet
65  beta_x_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
66  N_sampling - 1, 4); // Depends on a0_x, v0_x of feet
67  beta_y_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
68  N_sampling - 1, 4); // Depends on a0_y, v0_y of feet
69  tmp_ones_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Ones(
70  N_sampling - 1, 1);
71 
72  rb_accx_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
73  N_sampling - 1, 8);
74  rb_accy_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
75  N_sampling - 1, 8);
76  rb_accx_max_bool_ =
77  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
78  8);
79  rb_accy_max_bool_ =
80  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
81  8);
82 
83  // Cost on the velocity of the feet :
84  is_vel_activated_ = true;
85  vel_weight_ = Scalar(1.);
86  vel_lim_.setConstant(Scalar(3.));
87 
88  gamma_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
89  N_sampling - 1, 4);
90  gamma_v.col(0) = 30 * delta_.col(1) - 60 * delta_.col(2) + 30 * delta_.col(3);
91  gamma_v.col(1) = delta_.col(0);
92  gamma_v.col(2) =
93  -18 * delta_.col(1) + 32 * delta_.col(2) - 15 * delta_.col(3);
94  gamma_v.col(3) =
95  -4.5 * delta_.col(1) + 6 * delta_.col(2) - 2.5 * delta_.col(3);
96 
97  alpha_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
98  N_sampling - 1, 1); // Common for 4 feet
99  beta_x_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
100  N_sampling - 1, 4); // Depends on a0_x, v0_x of feet
101  beta_y_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
102  N_sampling - 1, 4); // Depends on a0_y, v0_y of feet
103 
104  rb_velx_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
105  N_sampling - 1, 8);
106  rb_vely_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
107  N_sampling - 1, 8);
108  rb_velx_max_bool_ =
109  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
110  8);
111  rb_vely_max_bool_ =
112  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
113  8);
114 
115  // Cost on the jerk at t=0
116  is_jerk_activated_ = true;
117  jerk_weight_ = Scalar(1.);
118  alpha_j = Scalar(0.); // Common for 4 feet
119  beta_j = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
120  2, 4); // Depends on a0_x, v0_x of feet
121  rb_jerk_.setZero();
122 }
123 
124 template <typename Scalar>
126 
127 template <typename Scalar>
129  const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
130  const Eigen::Ref<const typename MathBase::VectorXs>& x,
131  const Eigen::Ref<const typename MathBase::VectorXs>& u) {
132  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
133  throw_pretty("Invalid argument: "
134  << "x has wrong dimension (it should be " +
135  std::to_string(state_->get_nx()) + ")");
136  }
137  if (static_cast<std::size_t>(u.size()) != nu_) {
138  throw_pretty("Invalid argument: "
139  << "u has wrong dimension (it should be " +
140  std::to_string(nu_) + ")");
141  }
142 
144  static_cast<ActionDataQuadrupedStepTpl<Scalar>*>(data.get());
145 
146  d->xnext.template head<12>() = x.head(12);
147  d->xnext.template tail<8>() = x.tail(8) + B * u;
148 
149  // Residual cost on the state and force norm
150  d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
151  d->r.template segment<8>(12) =
152  heuristic_weights_.cwiseProduct(x.tail(8) - pheuristic_);
153  d->r.template tail<8>() = step_weights_.cwiseProduct(u);
154 
155  d->cost = Scalar(0.5) * d->r.transpose() * d->r;
156 
157  // Weight on the feet acceleration :
158  if (is_acc_activated_) {
159  for (int i = 0; i < 4; i++) {
160  if (S_(i) == Scalar(1.)) {
161  // position_ expressed in base frame
162  // rb_accx_max_.col(2*i) = (x(12+ 2*i) + u(2*i) - position_(0,i))*alpha_
163  // + beta_x_.col(i) - acc_lim_(0)*tmp_ones_; rb_accx_max_.col(2*i+1) =
164  // -( x(12+ 2*i) + u(2*i) - position_(0,i) )*alpha_ - beta_x_.col(i) -
165  // acc_lim_(0)*tmp_ones_;
166 
167  // rb_accy_max_.col(2*i) = ( x(12+ 2*i+1) + u(2*i+1) - position_(1,i)
168  // )*alpha_ + beta_y_.col(i) - acc_lim_(1)*tmp_ones_;
169  // rb_accy_max_.col(2*i+1) = -( x(12+ 2*i+1) + u(2*i+1) - position_(1,i)
170  // )*alpha_ - beta_y_.col(i) - acc_lim_(1)*tmp_ones_;
171 
172  // position_ expressed in world frame
173  rb_accx_max_.col(2 * i) =
174  (oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
175  oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(0) -
176  position_(0, i)) *
177  alpha_ +
178  beta_x_.col(i) - acc_lim_(0) * tmp_ones_;
179  rb_accx_max_.col(2 * i + 1) =
180  -(oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
181  oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(0) -
182  position_(0, i)) *
183  alpha_ +
184  beta_x_.col(i) - acc_lim_(0) * tmp_ones_;
185  ;
186 
187  rb_accy_max_.col(2 * i) =
188  (oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
189  oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(1) -
190  position_(1, i)) *
191  alpha_ +
192  beta_y_.col(i) - acc_lim_(1) * tmp_ones_;
193  rb_accy_max_.col(2 * i + 1) =
194  -(oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
195  oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(1) -
196  position_(1, i)) *
197  alpha_ -
198  beta_y_.col(i) - acc_lim_(1) * tmp_ones_;
199  } else {
200  rb_accx_max_.col(2 * i).setZero();
201  rb_accx_max_.col(2 * i + 1).setZero();
202  rb_accy_max_.col(2 * i).setZero();
203  rb_accy_max_.col(2 * i + 1).setZero();
204  }
205  }
206  rb_accx_max_bool_ =
207  (rb_accx_max_ > Scalar(0.))
208  .template cast<Scalar>(); // Usefull to compute the derivatives
209  rb_accy_max_bool_ =
210  (rb_accy_max_ > Scalar(0.))
211  .template cast<Scalar>(); // Usefull to compute the derivatives
212 
213  rb_accx_max_ = rb_accx_max_.cwiseMax(Scalar(0.));
214  rb_accy_max_ = rb_accy_max_.cwiseMax(Scalar(0.));
215 
216  for (int foot = 0; foot < 4; foot++) {
217  if (S_(foot) == Scalar(1.)) {
218  for (int i = 0; i < (N_sampling - 1); i++) {
219  if (rb_accx_max_bool_(i, 2 * foot)) {
220  d->cost +=
221  Scalar(0.5) * acc_weight_ * pow(rb_accx_max_(i, 2 * foot), 2);
222  }
223  if (rb_accx_max_bool_(i, 2 * foot + 1)) {
224  d->cost += Scalar(0.5) * acc_weight_ *
225  pow(rb_accx_max_(i, 2 * foot + 1), 2);
226  }
227  if (rb_accy_max_bool_(i, 2 * foot)) {
228  d->cost +=
229  Scalar(0.5) * acc_weight_ * pow(rb_accy_max_(i, 2 * foot), 2);
230  }
231  if (rb_accy_max_bool_(i, 2 * foot + 1)) {
232  d->cost += Scalar(0.5) * acc_weight_ *
233  pow(rb_accy_max_(i, 2 * foot + 1), 2);
234  }
235  }
236  }
237  }
238  }
239 
240  // Weight on the feet velocity
241  if (is_vel_activated_) {
242  for (int i = 0; i < 4; i++) {
243  if (S_(i) == Scalar(1.)) {
244  // position_ expressed in local frame
245  // rb_velx_max_.col(2*i) = (x(12+ 2*i) + u(2*i) -
246  // position_(0,i))*alpha_v + beta_x_v.col(i) - vel_lim_(0)*tmp_ones_;
247  // rb_velx_max_.col(2*i+1) = -( x(12+ 2*i) + u(2*i) - position_(0,i)
248  // )*alpha_v - beta_x_v.col(i) - vel_lim_(0)*tmp_ones_;
249 
250  // rb_vely_max_.col(2*i) = ( x(12+ 2*i+1) + u(2*i+1) - position_(1,i)
251  // )*alpha_v + beta_y_v.col(i) - vel_lim_(1)*tmp_ones_;
252  // rb_vely_max_.col(2*i+1) = -( x(12+ 2*i+1) + u(2*i+1) - position_(1,i)
253  // )*alpha_v - beta_y_v.col(i) - vel_lim_(1)*tmp_ones_;
254 
255  // position_ expressed in world frame
256  rb_velx_max_.col(2 * i) =
257  (oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
258  oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(0) -
259  position_(0, i)) *
260  alpha_v +
261  beta_x_v.col(i) - vel_lim_(0) * tmp_ones_;
262  rb_velx_max_.col(2 * i + 1) =
263  -(oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
264  oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(0) -
265  position_(0, i)) *
266  alpha_v -
267  beta_x_v.col(i) - vel_lim_(0) * tmp_ones_;
268 
269  rb_vely_max_.col(2 * i) =
270  (oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
271  oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(1) -
272  position_(1, i)) *
273  alpha_v +
274  beta_y_v.col(i) - vel_lim_(1) * tmp_ones_;
275  rb_vely_max_.col(2 * i + 1) =
276  -(oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
277  oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(1) -
278  position_(1, i)) *
279  alpha_v -
280  beta_y_v.col(i) - vel_lim_(1) * tmp_ones_;
281  } else {
282  rb_velx_max_.col(2 * i).setZero();
283  rb_velx_max_.col(2 * i + 1).setZero();
284  rb_vely_max_.col(2 * i).setZero();
285  rb_vely_max_.col(2 * i + 1).setZero();
286  }
287  }
288  rb_velx_max_bool_ =
289  (rb_velx_max_ > Scalar(0.))
290  .template cast<Scalar>(); // Usefull to compute the derivatives
291  rb_vely_max_bool_ =
292  (rb_vely_max_ > Scalar(0.))
293  .template cast<Scalar>(); // Usefull to compute the derivatives
294 
295  rb_velx_max_ = rb_velx_max_.cwiseMax(Scalar(0.));
296  rb_vely_max_ = rb_vely_max_.cwiseMax(Scalar(0.));
297 
298  for (int foot = 0; foot < 4; foot++) {
299  if (S_(foot) == Scalar(1.)) {
300  for (int i = 0; i < (N_sampling - 1); i++) {
301  if (rb_velx_max_bool_(i, 2 * foot)) {
302  d->cost +=
303  Scalar(0.5) * vel_weight_ * pow(rb_velx_max_(i, 2 * foot), 2);
304  }
305  if (rb_velx_max_bool_(i, 2 * foot + 1)) {
306  d->cost += Scalar(0.5) * vel_weight_ *
307  pow(rb_velx_max_(i, 2 * foot + 1), 2);
308  }
309  if (rb_vely_max_bool_(i, 2 * foot)) {
310  d->cost +=
311  Scalar(0.5) * vel_weight_ * pow(rb_vely_max_(i, 2 * foot), 2);
312  }
313  if (rb_vely_max_bool_(i, 2 * foot + 1)) {
314  d->cost += Scalar(0.5) * vel_weight_ *
315  pow(rb_vely_max_(i, 2 * foot + 1), 2);
316  }
317  }
318  }
319  }
320  }
321 
322  // Weight on the feet velocity
323  if (is_jerk_activated_) {
324  for (int i = 0; i < 4; i++) {
325  if (S_(i) == Scalar(1.)) {
326  rb_jerk_(0, i) = (oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
327  oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) +
328  oTh_(0) - position_(0, i)) *
329  alpha_j +
330  beta_j(0, i) - jerk_(0, i);
331  rb_jerk_(1, i) = (oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
332  oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) +
333  oTh_(1) - position_(1, i)) *
334  alpha_j +
335  beta_j(1, i) - jerk_(1, i);
336  d->cost += Scalar(0.5) * jerk_weight_ * rb_jerk_.col(i).squaredNorm();
337  } else {
338  rb_jerk_.col(i).setZero();
339  }
340  }
341  }
342 }
343 
344 template <typename Scalar>
346  const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
347  const Eigen::Ref<const typename MathBase::VectorXs>& x,
348  const Eigen::Ref<const typename MathBase::VectorXs>& u) {
349  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
350  throw_pretty("Invalid argument: "
351  << "x has wrong dimension (it should be " +
352  std::to_string(state_->get_nx()) + ")");
353  }
354  if (static_cast<std::size_t>(u.size()) != nu_) {
355  throw_pretty("Invalid argument: "
356  << "u has wrong dimension (it should be " +
357  std::to_string(nu_) + ")");
358  }
359 
361  static_cast<ActionDataQuadrupedStepTpl<Scalar>*>(data.get());
362 
363  d->Lxu.setZero();
364  d->Luu.setZero();
365 
366  // Cost derivatives : Lx
367  d->Lx.template head<12>() =
368  (state_weights_.array() * d->r.template head<12>().array()).matrix();
369  d->Lx.template tail<8>() =
370  (heuristic_weights_.array() * d->r.template segment<8>(12).array())
371  .matrix();
372 
373  d->Lu = (step_weights_.array() * d->r.template tail<8>().array()).matrix();
374 
375  // Hessian : Lxx
376  d->Lxx.diagonal().head(12) =
377  (state_weights_.array() * state_weights_.array()).matrix();
378  d->Lxx.diagonal().tail(8) =
379  (heuristic_weights_.array() * heuristic_weights_.array()).matrix();
380 
381  d->Luu.diagonal() = (step_weights_.array() * step_weights_.array()).matrix();
382 
383  if (is_acc_activated_) {
384  for (int foot = 0; foot < 4; foot++) {
385  if (S_[foot] == Scalar(1)) {
386  for (int i = 0; i < (N_sampling - 1); i++) {
387  // Position_ expressed in local frame
388  // if (rb_accx_max_bool_(i,2*foot)){
389 
390  // d->Lu(2*foot) += acc_weight_ * alpha_(i) *
391  // rb_accx_max_(i,2*foot); d->Luu(2*foot,2*foot) += acc_weight_ *
392  // pow(alpha_(i),2);
393 
394  // d->Lx(12+2*foot) += acc_weight_ * alpha_(i) *
395  // rb_accx_max_(i,2*foot); d->Lxu(12+2*foot,2*foot) += acc_weight_ *
396  // pow(alpha_(i),2); d->Lxx(12+2*foot,12+2*foot) += acc_weight_ *
397  // pow(alpha_(i),2);
398  // }
399  // if (rb_accx_max_bool_(i,2*foot+1)){
400  // d->Lu(2*foot) += - acc_weight_ * alpha_(i) *
401  // rb_accx_max_(i,2*foot+1); d->Luu(2*foot,2*foot) += acc_weight_ *
402  // pow(alpha_(i),2);
403 
404  // d->Lx(12+2*foot) += - acc_weight_ * alpha_(i) *
405  // rb_accx_max_(i,2*foot+1); d->Lxu(12+2*foot,2*foot) += acc_weight_
406  // * pow(alpha_(i),2); d->Lxx(12+2*foot,12+2*foot) += acc_weight_ *
407  // pow(alpha_(i),2);
408  // }
409  // if (rb_accy_max_bool_(i,2*foot)){
410  // d->Lu(2*foot+1) += acc_weight_ * alpha_(i) *
411  // rb_accy_max_(i,2*foot); d->Luu(2*foot+1,2*foot+1) += acc_weight_
412  // * pow(alpha_(i),2);
413 
414  // d->Lx(12+2*foot+1) += acc_weight_ * alpha_(i) *
415  // rb_accy_max_(i,2*foot); d->Lxu(12+2*foot+1,2*foot+1) +=
416  // acc_weight_ * pow(alpha_(i),2); d->Lxx(12+2*foot+1,12+2*foot+1)
417  // += acc_weight_ * pow(alpha_(i),2);
418  // }
419  // if (rb_accy_max_bool_(i,2*foot+1)){
420  // d->Lu(2*foot+1) += - acc_weight_ * alpha_(i) *
421  // rb_accy_max_(i,2*foot+1); d->Luu(2*foot+1,2*foot+1) +=
422  // acc_weight_ * pow(alpha_(i),2);
423 
424  // d->Lx(12+2*foot+1) += - acc_weight_ * alpha_(i) *
425  // rb_accy_max_(i,2*foot+1); d->Lxu(12+2*foot+1,2*foot+1) +=
426  // acc_weight_ * pow(alpha_(i),2); d->Lxx(12+2*foot+1,12+2*foot+1)
427  // += acc_weight_ * pow(alpha_(i),2);
428  // }
429 
430  // Position_ expressed in world frame
431  if (rb_accx_max_bool_(i, 2 * foot)) {
432  d->Lu(2 * foot) += acc_weight_ * oRh_(0, 0) * alpha_(i) *
433  rb_accx_max_(i, 2 * foot);
434  d->Lu(2 * foot + 1) += acc_weight_ * oRh_(0, 1) * alpha_(i) *
435  rb_accx_max_(i, 2 * foot);
436 
437  d->Luu(2 * foot, 2 * foot) +=
438  acc_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_(i), 2);
439  d->Luu(2 * foot + 1, 2 * foot + 1) +=
440  acc_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_(i), 2);
441 
442  d->Luu(2 * foot, 2 * foot + 1) +=
443  acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
444  d->Luu(2 * foot + 1, 2 * foot) +=
445  acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
446 
447  d->Lx(12 + 2 * foot) += acc_weight_ * oRh_(0, 0) * alpha_(i) *
448  rb_accx_max_(i, 2 * foot);
449  d->Lx(12 + 2 * foot + 1) += acc_weight_ * oRh_(0, 1) * alpha_(i) *
450  rb_accx_max_(i, 2 * foot);
451 
452  d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
453  acc_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_(i), 2);
454  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
455  acc_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_(i), 2);
456 
457  d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
458  acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
459  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
460  acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
461 
462  d->Lxu(12 + 2 * foot, 2 * foot) +=
463  acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(0, 0), 2);
464  d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
465  acc_weight_ * pow(alpha_(i), 2) * oRh_(0, 0) * oRh_(0, 1);
466  d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
467  acc_weight_ * pow(alpha_(i), 2) * oRh_(0, 0) * oRh_(0, 1);
468  d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
469  acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(0, 1), 2);
470  }
471  if (rb_accx_max_bool_(i, 2 * foot + 1)) {
472  d->Lu(2 * foot) += -acc_weight_ * oRh_(0, 0) * alpha_(i) *
473  rb_accx_max_(i, 2 * foot + 1);
474  d->Lu(2 * foot + 1) += -acc_weight_ * oRh_(0, 1) * alpha_(i) *
475  rb_accx_max_(i, 2 * foot + 1);
476 
477  d->Luu(2 * foot, 2 * foot) +=
478  acc_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_(i), 2);
479  d->Luu(2 * foot + 1, 2 * foot + 1) +=
480  acc_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_(i), 2);
481 
482  d->Luu(2 * foot, 2 * foot + 1) +=
483  acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
484  d->Luu(2 * foot + 1, 2 * foot) +=
485  acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
486 
487  d->Lx(12 + 2 * foot) += -acc_weight_ * oRh_(0, 0) * alpha_(i) *
488  rb_accx_max_(i, 2 * foot + 1);
489  d->Lx(12 + 2 * foot + 1) += -acc_weight_ * oRh_(0, 1) * alpha_(i) *
490  rb_accx_max_(i, 2 * foot + 1);
491 
492  d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
493  acc_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_(i), 2);
494  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
495  acc_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_(i), 2);
496 
497  d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
498  acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
499  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
500  acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
501 
502  d->Lxu(12 + 2 * foot, 2 * foot) +=
503  acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(0, 0), 2);
504  d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
505  acc_weight_ * pow(alpha_(i), 2) * oRh_(0, 0) * oRh_(0, 1);
506  d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
507  acc_weight_ * pow(alpha_(i), 2) * oRh_(0, 0) * oRh_(0, 1);
508  d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
509  acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(0, 1), 2);
510  }
511  if (rb_accy_max_bool_(i, 2 * foot)) {
512  d->Lu(2 * foot) += acc_weight_ * oRh_(1, 0) * alpha_(i) *
513  rb_accy_max_(i, 2 * foot);
514  d->Lu(2 * foot + 1) += acc_weight_ * oRh_(1, 1) * alpha_(i) *
515  rb_accy_max_(i, 2 * foot);
516 
517  d->Luu(2 * foot, 2 * foot) +=
518  acc_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_(i), 2);
519  d->Luu(2 * foot + 1, 2 * foot + 1) +=
520  acc_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_(i), 2);
521 
522  d->Luu(2 * foot, 2 * foot + 1) +=
523  acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
524  d->Luu(2 * foot + 1, 2 * foot) +=
525  acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
526 
527  d->Lx(12 + 2 * foot) += acc_weight_ * oRh_(1, 0) * alpha_(i) *
528  rb_accy_max_(i, 2 * foot);
529  d->Lx(12 + 2 * foot + 1) += acc_weight_ * oRh_(1, 1) * alpha_(i) *
530  rb_accy_max_(i, 2 * foot);
531 
532  d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
533  acc_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_(i), 2);
534  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
535  acc_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_(i), 2);
536 
537  d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
538  acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
539  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
540  acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
541 
542  d->Lxu(12 + 2 * foot, 2 * foot) +=
543  acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(1, 0), 2);
544  d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
545  acc_weight_ * pow(alpha_(i), 2) * oRh_(1, 0) * oRh_(1, 1);
546  d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
547  acc_weight_ * pow(alpha_(i), 2) * oRh_(1, 0) * oRh_(1, 1);
548  d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
549  acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(1, 1), 2);
550  }
551  if (rb_accy_max_bool_(i, 2 * foot + 1)) {
552  d->Lu(2 * foot) += -acc_weight_ * oRh_(1, 0) * alpha_(i) *
553  rb_accy_max_(i, 2 * foot + 1);
554  d->Lu(2 * foot + 1) += -acc_weight_ * oRh_(1, 1) * alpha_(i) *
555  rb_accy_max_(i, 2 * foot + 1);
556 
557  d->Luu(2 * foot, 2 * foot) +=
558  acc_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_(i), 2);
559  d->Luu(2 * foot + 1, 2 * foot + 1) +=
560  acc_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_(i), 2);
561 
562  d->Luu(2 * foot, 2 * foot + 1) +=
563  acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
564  d->Luu(2 * foot + 1, 2 * foot) +=
565  acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
566 
567  d->Lx(12 + 2 * foot) += -acc_weight_ * oRh_(1, 0) * alpha_(i) *
568  rb_accy_max_(i, 2 * foot + 1);
569  d->Lx(12 + 2 * foot + 1) += -acc_weight_ * oRh_(1, 1) * alpha_(i) *
570  rb_accy_max_(i, 2 * foot + 1);
571 
572  d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
573  acc_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_(i), 2);
574  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
575  acc_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_(i), 2);
576 
577  d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
578  acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
579  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
580  acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
581 
582  d->Lxu(12 + 2 * foot, 2 * foot) +=
583  acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(1, 0), 2);
584  d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
585  acc_weight_ * pow(alpha_(i), 2) * oRh_(1, 0) * oRh_(1, 1);
586  d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
587  acc_weight_ * pow(alpha_(i), 2) * oRh_(1, 0) * oRh_(1, 1);
588  d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
589  acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(1, 1), 2);
590  }
591  }
592  }
593  }
594  }
595 
596  if (is_vel_activated_) {
597  for (int foot = 0; foot < 4; foot++) {
598  if (S_[foot] == Scalar(1)) {
599  for (int i = 0; i < (N_sampling - 1); i++) {
600  // position_ in base frame
601  // if (rb_velx_max_bool_(i,2*foot)){
602 
603  // d->Lu(2*foot) += vel_weight_ * alpha_v(i) *
604  // rb_velx_max_(i,2*foot); d->Luu(2*foot,2*foot) += vel_weight_ *
605  // pow(alpha_v(i),2);
606 
607  // d->Lx(12+2*foot) += vel_weight_ * alpha_v(i) *
608  // rb_velx_max_(i,2*foot); d->Lxu(12+2*foot,2*foot) += vel_weight_ *
609  // pow(alpha_v(i),2); d->Lxx(12+2*foot,12+2*foot) += vel_weight_ *
610  // pow(alpha_v(i),2);
611  // }
612  // if (rb_velx_max_bool_(i,2*foot+1)){
613  // d->Lu(2*foot) += - vel_weight_ * alpha_v(i) *
614  // rb_velx_max_(i,2*foot+1); d->Luu(2*foot,2*foot) += vel_weight_ *
615  // pow(alpha_v(i),2);
616 
617  // d->Lx(12+2*foot) += - vel_weight_ * alpha_v(i) *
618  // rb_velx_max_(i,2*foot+1); d->Lxu(12+2*foot,2*foot) += vel_weight_
619  // * pow(alpha_v(i),2); d->Lxx(12+2*foot,12+2*foot) += vel_weight_ *
620  // pow(alpha_v(i),2);
621  // }
622  // if (rb_vely_max_bool_(i,2*foot)){
623  // d->Lu(2*foot+1) += vel_weight_ * alpha_v(i) *
624  // rb_vely_max_(i,2*foot); d->Luu(2*foot+1,2*foot+1) += vel_weight_
625  // * pow(alpha_v(i),2);
626 
627  // d->Lx(12+2*foot+1) += vel_weight_ * alpha_v(i) *
628  // rb_vely_max_(i,2*foot); d->Lxu(12+2*foot+1,2*foot+1) +=
629  // vel_weight_ * pow(alpha_v(i),2); d->Lxx(12+2*foot+1,12+2*foot+1)
630  // += vel_weight_ * pow(alpha_v(i),2);
631  // }
632  // if (rb_vely_max_bool_(i,2*foot+1)){
633  // d->Lu(2*foot+1) += - vel_weight_ * alpha_v(i) *
634  // rb_vely_max_(i,2*foot+1); d->Luu(2*foot+1,2*foot+1) +=
635  // vel_weight_ * pow(alpha_v(i),2);
636 
637  // d->Lx(12+2*foot+1) += - vel_weight_ * alpha_v(i) *
638  // rb_vely_max_(i,2*foot+1); d->Lxu(12+2*foot+1,2*foot+1) +=
639  // vel_weight_ * pow(alpha_v(i),2); d->Lxx(12+2*foot+1,12+2*foot+1)
640  // += vel_weight_ * pow(alpha_v(i),2);
641  // }
642 
643  // position_ in world frame
644  if (rb_velx_max_bool_(i, 2 * foot)) {
645  d->Lu(2 * foot) += vel_weight_ * oRh_(0, 0) * alpha_v(i) *
646  rb_velx_max_(i, 2 * foot);
647  d->Lu(2 * foot + 1) += vel_weight_ * oRh_(0, 1) * alpha_v(i) *
648  rb_velx_max_(i, 2 * foot);
649 
650  d->Luu(2 * foot, 2 * foot) +=
651  vel_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_v(i), 2);
652  d->Luu(2 * foot + 1, 2 * foot + 1) +=
653  vel_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_v(i), 2);
654 
655  d->Luu(2 * foot, 2 * foot + 1) +=
656  vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
657  d->Luu(2 * foot + 1, 2 * foot) +=
658  vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
659 
660  d->Lx(12 + 2 * foot) += vel_weight_ * oRh_(0, 0) * alpha_v(i) *
661  rb_velx_max_(i, 2 * foot);
662  d->Lx(12 + 2 * foot + 1) += vel_weight_ * oRh_(0, 1) * alpha_v(i) *
663  rb_velx_max_(i, 2 * foot);
664 
665  d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
666  vel_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_v(i), 2);
667  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
668  vel_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_v(i), 2);
669 
670  d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
671  vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
672  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
673  vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
674 
675  d->Lxu(12 + 2 * foot, 2 * foot) +=
676  vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(0, 0), 2);
677  d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
678  vel_weight_ * pow(alpha_v(i), 2) * oRh_(0, 0) * oRh_(0, 1);
679  d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
680  vel_weight_ * pow(alpha_v(i), 2) * oRh_(0, 0) * oRh_(0, 1);
681  d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
682  vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(0, 1), 2);
683  }
684  if (rb_velx_max_bool_(i, 2 * foot + 1)) {
685  d->Lu(2 * foot) += -vel_weight_ * oRh_(0, 0) * alpha_v(i) *
686  rb_velx_max_(i, 2 * foot + 1);
687  d->Lu(2 * foot + 1) += -vel_weight_ * oRh_(0, 1) * alpha_v(i) *
688  rb_velx_max_(i, 2 * foot + 1);
689 
690  d->Luu(2 * foot, 2 * foot) +=
691  vel_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_v(i), 2);
692  d->Luu(2 * foot + 1, 2 * foot + 1) +=
693  vel_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_v(i), 2);
694 
695  d->Luu(2 * foot, 2 * foot + 1) +=
696  vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
697  d->Luu(2 * foot + 1, 2 * foot) +=
698  vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
699 
700  d->Lx(12 + 2 * foot) += -vel_weight_ * oRh_(0, 0) * alpha_v(i) *
701  rb_velx_max_(i, 2 * foot + 1);
702  d->Lx(12 + 2 * foot + 1) += -vel_weight_ * oRh_(0, 1) * alpha_v(i) *
703  rb_velx_max_(i, 2 * foot + 1);
704 
705  d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
706  vel_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_v(i), 2);
707  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
708  vel_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_v(i), 2);
709 
710  d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
711  vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
712  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
713  vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
714 
715  d->Lxu(12 + 2 * foot, 2 * foot) +=
716  vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(0, 0), 2);
717  d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
718  vel_weight_ * pow(alpha_v(i), 2) * oRh_(0, 0) * oRh_(0, 1);
719  d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
720  vel_weight_ * pow(alpha_v(i), 2) * oRh_(0, 0) * oRh_(0, 1);
721  d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
722  vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(0, 1), 2);
723  }
724  if (rb_vely_max_bool_(i, 2 * foot)) {
725  d->Lu(2 * foot) += vel_weight_ * oRh_(1, 0) * alpha_v(i) *
726  rb_vely_max_(i, 2 * foot);
727  d->Lu(2 * foot + 1) += vel_weight_ * oRh_(1, 1) * alpha_v(i) *
728  rb_vely_max_(i, 2 * foot);
729 
730  d->Luu(2 * foot, 2 * foot) +=
731  vel_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_v(i), 2);
732  d->Luu(2 * foot + 1, 2 * foot + 1) +=
733  vel_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_v(i), 2);
734 
735  d->Luu(2 * foot, 2 * foot + 1) +=
736  vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
737  d->Luu(2 * foot + 1, 2 * foot) +=
738  vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
739 
740  d->Lx(12 + 2 * foot) += vel_weight_ * oRh_(1, 0) * alpha_v(i) *
741  rb_vely_max_(i, 2 * foot);
742  d->Lx(12 + 2 * foot + 1) += vel_weight_ * oRh_(1, 1) * alpha_v(i) *
743  rb_vely_max_(i, 2 * foot);
744 
745  d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
746  vel_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_v(i), 2);
747  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
748  vel_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_v(i), 2);
749 
750  d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
751  vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
752  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
753  vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
754 
755  d->Lxu(12 + 2 * foot, 2 * foot) +=
756  vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(1, 0), 2);
757  d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
758  vel_weight_ * pow(alpha_v(i), 2) * oRh_(1, 0) * oRh_(1, 1);
759  d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
760  vel_weight_ * pow(alpha_v(i), 2) * oRh_(1, 0) * oRh_(1, 1);
761  d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
762  vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(1, 1), 2);
763  }
764  if (rb_vely_max_bool_(i, 2 * foot + 1)) {
765  d->Lu(2 * foot) += -vel_weight_ * oRh_(1, 0) * alpha_v(i) *
766  rb_vely_max_(i, 2 * foot + 1);
767  d->Lu(2 * foot + 1) += -vel_weight_ * oRh_(1, 1) * alpha_v(i) *
768  rb_vely_max_(i, 2 * foot + 1);
769 
770  d->Luu(2 * foot, 2 * foot) +=
771  vel_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_v(i), 2);
772  d->Luu(2 * foot + 1, 2 * foot + 1) +=
773  vel_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_v(i), 2);
774 
775  d->Luu(2 * foot, 2 * foot + 1) +=
776  vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
777  d->Luu(2 * foot + 1, 2 * foot) +=
778  vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
779 
780  d->Lx(12 + 2 * foot) += -vel_weight_ * oRh_(1, 0) * alpha_v(i) *
781  rb_vely_max_(i, 2 * foot + 1);
782  d->Lx(12 + 2 * foot + 1) += -vel_weight_ * oRh_(1, 1) * alpha_v(i) *
783  rb_vely_max_(i, 2 * foot + 1);
784 
785  d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
786  vel_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_v(i), 2);
787  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
788  vel_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_v(i), 2);
789 
790  d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
791  vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
792  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
793  vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
794 
795  d->Lxu(12 + 2 * foot, 2 * foot) +=
796  vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(1, 0), 2);
797  d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
798  vel_weight_ * pow(alpha_v(i), 2) * oRh_(1, 0) * oRh_(1, 1);
799  d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
800  vel_weight_ * pow(alpha_v(i), 2) * oRh_(1, 0) * oRh_(1, 1);
801  d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
802  vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(1, 1), 2);
803  }
804  }
805  }
806  }
807  }
808 
809  if (is_jerk_activated_) {
810  for (int foot = 0; foot < 4; foot++) {
811  if (S_[foot] == Scalar(1)) {
812  // derivatives relative to jerk on x axis
813  d->Lu(2 * foot) +=
814  jerk_weight_ * oRh_(0, 0) * alpha_j * rb_jerk_(0, foot);
815  d->Lu(2 * foot + 1) +=
816  jerk_weight_ * oRh_(0, 1) * alpha_j * rb_jerk_(0, foot);
817 
818  d->Luu(2 * foot, 2 * foot) +=
819  jerk_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_j, 2);
820  d->Luu(2 * foot + 1, 2 * foot + 1) +=
821  jerk_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_j, 2);
822 
823  d->Luu(2 * foot, 2 * foot + 1) +=
824  jerk_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_j, 2);
825  d->Luu(2 * foot + 1, 2 * foot) +=
826  jerk_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_j, 2);
827 
828  d->Lx(12 + 2 * foot) +=
829  jerk_weight_ * oRh_(0, 0) * alpha_j * rb_jerk_(0, foot);
830  d->Lx(12 + 2 * foot + 1) +=
831  jerk_weight_ * oRh_(0, 1) * alpha_j * rb_jerk_(0, foot);
832 
833  d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
834  jerk_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_j, 2);
835  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
836  jerk_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_j, 2);
837 
838  d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
839  jerk_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_j, 2);
840  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
841  jerk_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_j, 2);
842 
843  d->Lxu(12 + 2 * foot, 2 * foot) +=
844  jerk_weight_ * pow(alpha_j, 2) * pow(oRh_(0, 0), 2);
845  d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
846  jerk_weight_ * pow(alpha_j, 2) * oRh_(0, 0) * oRh_(0, 1);
847  d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
848  jerk_weight_ * pow(alpha_j, 2) * oRh_(0, 0) * oRh_(0, 1);
849  d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
850  jerk_weight_ * pow(alpha_j, 2) * pow(oRh_(0, 1), 2);
851 
852  // derivatives relative to jerk on y axis
853  d->Lu(2 * foot) +=
854  jerk_weight_ * oRh_(1, 0) * alpha_j * rb_jerk_(1, foot);
855  d->Lu(2 * foot + 1) +=
856  jerk_weight_ * oRh_(1, 1) * alpha_j * rb_jerk_(1, foot);
857 
858  d->Luu(2 * foot, 2 * foot) +=
859  jerk_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_j, 2);
860  d->Luu(2 * foot + 1, 2 * foot + 1) +=
861  jerk_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_j, 2);
862 
863  d->Luu(2 * foot, 2 * foot + 1) +=
864  jerk_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_j, 2);
865  d->Luu(2 * foot + 1, 2 * foot) +=
866  jerk_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_j, 2);
867 
868  d->Lx(12 + 2 * foot) +=
869  jerk_weight_ * oRh_(1, 0) * alpha_j * rb_jerk_(1, foot);
870  d->Lx(12 + 2 * foot + 1) +=
871  jerk_weight_ * oRh_(1, 1) * alpha_j * rb_jerk_(1, foot);
872 
873  d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
874  jerk_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_j, 2);
875  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
876  jerk_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_j, 2);
877 
878  d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
879  jerk_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_j, 2);
880  d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
881  jerk_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_j, 2);
882 
883  d->Lxu(12 + 2 * foot, 2 * foot) +=
884  jerk_weight_ * pow(alpha_j, 2) * pow(oRh_(1, 0), 2);
885  d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
886  jerk_weight_ * pow(alpha_j, 2) * oRh_(1, 0) * oRh_(1, 1);
887  d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
888  jerk_weight_ * pow(alpha_j, 2) * oRh_(1, 0) * oRh_(1, 1);
889  d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
890  jerk_weight_ * pow(alpha_j, 2) * pow(oRh_(1, 1), 2);
891  }
892  }
893  }
894 
895  // Dynamic derivatives
896  d->Fx.setIdentity();
897  d->Fu.block(12, 0, 8, 8) = B;
898 }
899 
900 template <typename Scalar>
901 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
903  return boost::make_shared<ActionDataQuadrupedStepTpl<Scalar> >(this);
904 }
905 
907 // get & set parameters ////////
909 
910 template <typename Scalar>
911 const typename Eigen::Matrix<Scalar, 12, 1>&
913  return state_weights_;
914 }
915 template <typename Scalar>
917  const typename MathBase::VectorXs& weights) {
918  if (static_cast<std::size_t>(weights.size()) != 12) {
919  throw_pretty("Invalid argument: "
920  << "Weights vector has wrong dimension (it should be 12)");
921  }
922  state_weights_ = weights;
923 }
924 
925 template <typename Scalar>
926 const typename Eigen::Matrix<Scalar, 8, 1>&
928  return step_weights_;
929 }
930 template <typename Scalar>
932  const typename MathBase::VectorXs& weights) {
933  if (static_cast<std::size_t>(weights.size()) != 8) {
934  throw_pretty("Invalid argument: "
935  << "Weights vector has wrong dimension (it should be 4)");
936  }
937  step_weights_ = weights;
938 }
939 
940 template <typename Scalar>
941 const typename Eigen::Matrix<Scalar, 8, 1>&
943  return heuristic_weights_;
944 }
945 template <typename Scalar>
947  const typename MathBase::VectorXs& weights) {
948  if (static_cast<std::size_t>(weights.size()) != 8) {
949  throw_pretty("Invalid argument: "
950  << "Weights vector has wrong dimension (it should be 8)");
951  }
952  heuristic_weights_ = weights;
953 }
954 
955 template <typename Scalar>
957  return symmetry_term;
958 }
959 template <typename Scalar>
961  const bool& sym_term) {
962  // The model need to be updated after this changed
963  symmetry_term = sym_term;
964 }
965 
966 template <typename Scalar>
968  return centrifugal_term;
969 }
970 template <typename Scalar>
972  const bool& cent_term) {
973  // The model need to be updated after this changed
974  centrifugal_term = cent_term;
975 }
976 
977 template <typename Scalar>
979  // The model need to be updated after this changed
980  return T_gait;
981 }
982 template <typename Scalar>
984  // The model need to be updated after this changed
985  T_gait = T_gait_;
986 }
987 
988 template <typename Scalar>
990  return is_acc_activated_;
991 }
992 template <typename Scalar>
994  const bool& is_activated) {
995  is_acc_activated_ = is_activated;
996 }
997 
998 template <typename Scalar>
999 const typename Eigen::Matrix<Scalar, 2, 1>&
1001  return acc_lim_;
1002 }
1003 template <typename Scalar>
1005  const typename MathBase::VectorXs& acceleration_lim_) {
1006  if (static_cast<std::size_t>(acceleration_lim_.size()) != 2) {
1007  throw_pretty("Invalid argument: "
1008  << "Acceleration limit vector [ax_max, ay_max] has wrong "
1009  "dimension (it should be 2)");
1010  }
1011  acc_lim_ = acceleration_lim_;
1012 }
1013 
1014 template <typename Scalar>
1016  return acc_weight_;
1017 }
1018 template <typename Scalar>
1020  const Scalar& weight_) {
1021  acc_weight_ = weight_;
1022 }
1023 
1024 template <typename Scalar>
1026  return is_vel_activated_;
1027 }
1028 template <typename Scalar>
1030  const bool& is_activated) {
1031  is_vel_activated_ = is_activated;
1032 }
1033 
1034 template <typename Scalar>
1035 const typename Eigen::Matrix<Scalar, 2, 1>&
1037  return vel_lim_;
1038 }
1039 template <typename Scalar>
1041  const typename MathBase::VectorXs& velocity_lim_) {
1042  if (static_cast<std::size_t>(velocity_lim_.size()) != 2) {
1043  throw_pretty("Invalid argument: "
1044  << "Velocity limit vector [vx_max, vy_max] has wrong "
1045  "dimension (it should be 2)");
1046  }
1047  vel_lim_ = velocity_lim_;
1048 }
1049 
1050 template <typename Scalar>
1052  return vel_weight_;
1053 }
1054 template <typename Scalar>
1056  const Scalar& weight_) {
1057  vel_weight_ = weight_;
1058 }
1059 
1060 template <typename Scalar>
1062  return jerk_weight_;
1063 }
1064 template <typename Scalar>
1066  const Scalar& weight_) {
1067  jerk_weight_ = weight_;
1068 }
1069 
1070 template <typename Scalar>
1072  return is_jerk_activated_;
1073 }
1074 template <typename Scalar>
1076  const bool& is_activated) {
1077  is_jerk_activated_ = is_activated;
1078 }
1079 
1080 template <typename Scalar>
1082  const int& n_sample) {
1083  N_sampling = n_sample;
1084 
1085  // Acceleration cost
1086  delta_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1087  N_sampling - 1, 4);
1088  gamma_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1089  N_sampling - 1, 3);
1090  for (int k = 1; k < N_sampling; k++) {
1091  delta_(k - 1, 0) =
1092  (float)k / (float)N_sampling; // [1/N, 2/N, ... , (N-1)/N]
1093  }
1094  delta_.col(1) << delta_.col(0).pow(2);
1095  delta_.col(2) << delta_.col(0).pow(3);
1096  delta_.col(3) << delta_.col(0).pow(4);
1097 
1098  gamma_.col(0) =
1099  60 * delta_.col(0) - 180 * delta_.col(1) + 120 * delta_.col(2);
1100  gamma_.col(1) = -36 * delta_.col(0) + 96 * delta_.col(1) - 60 * delta_.col(2);
1101  gamma_.col(2) = -9 * delta_.col(0) + 18 * delta_.col(1) - 10 * delta_.col(2);
1102 
1103  alpha_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1104  N_sampling - 1, 1); // Common for 4 feet
1105  beta_x_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1106  N_sampling - 1, 4); // Depends on ao,vo of feet
1107  beta_y_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1108  N_sampling - 1, 4); // Depends on ao,vo of feet
1109  tmp_ones_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Ones(
1110  N_sampling - 1, 1);
1111 
1112  rb_accx_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1113  N_sampling - 1, 8);
1114  rb_accy_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1115  N_sampling - 1, 8);
1116  rb_accx_max_bool_ =
1117  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
1118  8);
1119  rb_accy_max_bool_ =
1120  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
1121  8);
1122 
1123  // Velocity cost
1124  gamma_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1125  N_sampling - 1, 4);
1126  gamma_v.col(0) = 30 * delta_.col(1) - 60 * delta_.col(2) + 30 * delta_.col(3);
1127  gamma_v.col(1) = delta_.col(0);
1128  gamma_v.col(2) =
1129  -18 * delta_.col(1) + 32 * delta_.col(2) - 15 * delta_.col(3);
1130  gamma_v.col(3) =
1131  -4.5 * delta_.col(1) + 6 * delta_.col(2) - 2.5 * delta_.col(3);
1132 
1133  alpha_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1134  N_sampling - 1, 1); // Common for 4 feet
1135  beta_x_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1136  N_sampling - 1, 4); // Depends on a0_x, v0_x of feet
1137  beta_y_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1138  N_sampling - 1, 4); // Depends on a0_y, v0_y of feet
1139 
1140  rb_velx_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1141  N_sampling - 1, 8);
1142  rb_vely_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1143  N_sampling - 1, 8);
1144  rb_velx_max_bool_ =
1145  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
1146  8);
1147  rb_vely_max_bool_ =
1148  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
1149  8);
1150 }
1151 
1153 // Update current model
1155 
1156 template <typename Scalar>
1158  const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
1159  const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
1160  const Eigen::Ref<const typename MathBase::VectorXs>& S,
1161  const Eigen::Ref<const typename MathBase::MatrixXs>& position,
1162  const Eigen::Ref<const typename MathBase::MatrixXs>& velocity,
1163  const Eigen::Ref<const typename MathBase::MatrixXs>& acceleration,
1164  const Eigen::Ref<const typename MathBase::MatrixXs>& jerk,
1165  const Eigen::Ref<const typename MathBase::MatrixXs>& oRh,
1166  const Eigen::Ref<const typename MathBase::MatrixXs>& oTh,
1167  const Scalar& delta_T) {
1168  if (static_cast<std::size_t>(l_feet.size()) != 12) {
1169  throw_pretty("Invalid argument: "
1170  << "l_feet matrix has wrong dimension (it should be : 3x4)");
1171  }
1172  if (static_cast<std::size_t>(xref.size()) != 12) {
1173  throw_pretty("Invalid argument: "
1174  << "Weights vector has wrong dimension (it should be " +
1175  std::to_string(state_->get_nx()) + ")");
1176  }
1177  if (static_cast<std::size_t>(S.size()) != 4) {
1178  throw_pretty("Invalid argument: "
1179  << "S vector has wrong dimension (it should be 4x1)");
1180  }
1181 
1182  // Velocity : [[vx_0, vx_1, vx_2, vx_3],
1183  // [vy_0, vy_1, vy_2, vy_3],
1184  // [vz_0, vz_1, vz_2, vz_3]]
1185 
1186  xref_ = xref;
1187  S_ = S;
1188  position_ = position;
1189  oRh_ = oRh;
1190  oTh_ = oTh;
1191  jerk_ = jerk;
1192 
1193  /* R_tmp << cos(xref(5, 0)), -sin(xref(5, 0)), Scalar(0), sin(xref(5, 0)),
1194  cos(xref(5, 0)), Scalar(0), Scalar(0), Scalar(0), Scalar(1.0);
1195 
1196  // Centrifual term
1197  pcentrifugal_tmp_1 = xref.block(6, 0, 3, 1);
1198  pcentrifugal_tmp_2 = xref.block(9, 0, 3, 1);
1199  pcentrifugal_tmp = 0.5 * std::sqrt(xref(2, 0) / 9.81) *
1200  pcentrifugal_tmp_1.cross(pcentrifugal_tmp_2);
1201 
1202  for (int i = 0; i < 4; i = i + 1) {
1203  pshoulder_tmp.block(0, i, 2, 1) =
1204  R_tmp.block(0, 0, 2, 2) *
1205  (pshoulder_0.block(0, i, 2, 1) + symmetry_term * 0.25 * T_gait *
1206  xref.block(6, 0, 2, 1) + centrifugal_term * pcentrifugal_tmp.block(0, 0, 2,
1207  1)); pshoulder_[2 * i] = pshoulder_tmp(0, i) + xref(0, 0); pshoulder_[2 * i +
1208  1] = pshoulder_tmp(1, i) + xref(1, 0);
1209  } */
1210 
1211  for (int i = 0; i < 4; i = i + 1) {
1212  pheuristic_[2 * i] = l_feet(0, i);
1213  pheuristic_[2 * i + 1] = l_feet(1, i);
1214  }
1215 
1216  /* std::cout << pshoulder_ << std::endl; */
1217 
1218  B.setZero();
1219 
1220  if (S[0] == Scalar(1)) {
1221  B.block(0, 0, 2, 2).setIdentity();
1222  }
1223  if (S[1] == Scalar(1)) {
1224  B.block(2, 2, 2, 2).setIdentity();
1225  }
1226  if (S[2] == Scalar(1)) {
1227  B.block(4, 4, 2, 2).setIdentity();
1228  }
1229  if (S[3] == Scalar(1)) {
1230  B.block(6, 6, 2, 2).setIdentity();
1231  }
1232 
1233  alpha_ = (1 / pow(delta_T, 2)) * gamma_.col(0);
1234  alpha_j = (60 / pow(delta_T, 3));
1235  alpha_v = (1 / delta_T) * gamma_v.col(0);
1236 
1237  for (int i = 0; i < 4; i++) {
1238  if (S[i] == Scalar(1) && is_acc_activated_) {
1239  beta_x_.col(i) = acceleration(0, i) * tmp_ones_ +
1240  (velocity(0, i) / delta_T) * gamma_.col(1) +
1241  acceleration(0, i) * gamma_.col(2);
1242  beta_y_.col(i) = acceleration(1, i) * tmp_ones_ +
1243  (velocity(1, i) / delta_T) * gamma_.col(1) +
1244  acceleration(1, i) * gamma_.col(2);
1245  } else {
1246  beta_x_.col(i).setZero();
1247  beta_y_.col(i).setZero();
1248  }
1249 
1250  if (S[i] == Scalar(1) && is_vel_activated_) {
1251  beta_x_v.col(i) = velocity(0, i) * tmp_ones_ +
1252  (acceleration(0, i) * delta_T) * gamma_v.col(1) +
1253  velocity(0, i) * gamma_.col(2) +
1254  (acceleration(0, i) * delta_T) * gamma_v.col(3);
1255  beta_y_v.col(i) = velocity(1, i) * tmp_ones_ +
1256  (acceleration(1, i) * delta_T) * gamma_v.col(1) +
1257  velocity(1, i) * gamma_.col(2) +
1258  (acceleration(1, i) * delta_T) * gamma_v.col(3);
1259  } else {
1260  beta_x_v.col(i).setZero();
1261  beta_y_v.col(i).setZero();
1262  }
1263 
1264  if (S[i] == Scalar(1) && is_jerk_activated_) {
1265  beta_j(0, i) = -(36 * velocity(0, i)) / pow(delta_T, 2) -
1266  (9 * acceleration(0, i)) / delta_T;
1267  beta_j(1, i) = -(36 * velocity(1, i)) / pow(delta_T, 2) -
1268  (9 * acceleration(1, i)) / delta_T;
1269  } else {
1270  beta_j.col(i).setZero();
1271  }
1272  }
1273 }
1274 } // namespace quadruped_walkgen
1275 
1276 #endif
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_vel_weight
const Scalar & get_vel_weight() const
Definition: quadruped_step.hxx:1051
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_heuristic_weights
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step.hxx:946
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_jerk_weight
void set_jerk_weight(const Scalar &weight_)
Definition: quadruped_step.hxx:1065
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_heuristic_weights
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_step.hxx:942
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_acc_lim
void set_acc_lim(const typename MathBase::VectorXs &acceleration_lim_)
Definition: quadruped_step.hxx:1004
quadruped_walkgen::ActionModelQuadrupedStepTpl::Scalar
_Scalar Scalar
Definition: quadruped_step.hpp:16
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_state_weights
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_step.hxx:912
quadruped_walkgen
Definition: quadruped.hpp:11
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_sample_feet_traj
void set_sample_feet_traj(const int &n_sample)
Definition: quadruped_step.hxx:1081
quadruped_walkgen::ActionModelQuadrupedStepTpl::~ActionModelQuadrupedStepTpl
~ActionModelQuadrupedStepTpl()
Definition: quadruped_step.hxx:125
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_vel_activated
const bool & get_vel_activated() const
Definition: quadruped_step.hxx:1025
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_vel_weight
void set_vel_weight(const Scalar &weight_)
Definition: quadruped_step.hxx:1055
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_state_weights
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step.hxx:916
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_acc_weight
void set_acc_weight(const Scalar &weight_)
Definition: quadruped_step.hxx:1019
quadruped_walkgen::ActionModelQuadrupedStepTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_step.hxx:902
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_jerk_activated
const bool & get_jerk_activated() const
Definition: quadruped_step.hxx:1071
quadruped_walkgen::ActionModelQuadrupedStepTpl::calcDiff
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_step.hxx:345
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_step_weights
const Eigen::Matrix< Scalar, 8, 1 > & get_step_weights() const
Definition: quadruped_step.hxx:927
quadruped_walkgen::ActionDataQuadrupedStepTpl
Definition: quadruped_step.hpp:177
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_symmetry_term
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_step.hxx:960
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_step_weights
void set_step_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step.hxx:931
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_centrifugal_term
const bool & get_centrifugal_term() const
Definition: quadruped_step.hxx:967
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_T_gait
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_step.hxx:983
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_vel_activated
void set_vel_activated(const bool &is_activated)
Definition: quadruped_step.hxx:1029
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_acc_activated
void set_acc_activated(const bool &is_activated)
Definition: quadruped_step.hxx:993
quadruped_walkgen::ActionModelQuadrupedStepTpl::ActionModelQuadrupedStepTpl
ActionModelQuadrupedStepTpl()
Definition: quadruped_step.hxx:8
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_acc_lim
const Eigen::Matrix< Scalar, 2, 1 > & get_acc_lim() const
Definition: quadruped_step.hxx:1000
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_jerk_weight
const Scalar & get_jerk_weight() const
Definition: quadruped_step.hxx:1061
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_centrifugal_term
void set_centrifugal_term(const bool &cent_term)
Definition: quadruped_step.hxx:971
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_jerk_activated
void set_jerk_activated(const bool &is_activated)
Definition: quadruped_step.hxx:1075
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_symmetry_term
const bool & get_symmetry_term() const
Definition: quadruped_step.hxx:956
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_acc_weight
const Scalar & get_acc_weight() const
Definition: quadruped_step.hxx:1015
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_vel_lim
const Eigen::Matrix< Scalar, 2, 1 > & get_vel_lim() const
Definition: quadruped_step.hxx:1036
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_T_gait
const Scalar & get_T_gait() const
Definition: quadruped_step.hxx:978
quadruped_walkgen::ActionModelQuadrupedStepTpl::get_acc_activated
const bool & get_acc_activated() const
Definition: quadruped_step.hxx:989
quadruped_walkgen::ActionModelQuadrupedStepTpl::set_vel_lim
void set_vel_lim(const typename MathBase::VectorXs &velocity_lim_)
Definition: quadruped_step.hxx:1040
quadruped_walkgen::ActionModelQuadrupedStepTpl::update_model
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::VectorXs > &S, const Eigen::Ref< const typename MathBase::MatrixXs > &position, const Eigen::Ref< const typename MathBase::MatrixXs > &velocity, const Eigen::Ref< const typename MathBase::MatrixXs > &acceleration, const Eigen::Ref< const typename MathBase::MatrixXs > &jerk, const Eigen::Ref< const typename MathBase::MatrixXs > &oRh, const Eigen::Ref< const typename MathBase::MatrixXs > &oTh, const Scalar &delta_T)
Definition: quadruped_step.hxx:1157
quadruped_walkgen::ActionModelQuadrupedStepTpl::calc
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_step.hxx:128