quadruped_step_time.hxx
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_step_time_hxx__
2 #define __quadruped_walkgen_quadruped_step_time_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> >(21), 8, 29) {
11  B.setZero(); // x_next = x + B * u
12  rub_max_.setZero();
13  rub_max_bool.setZero();
14 
15  state_weights_ << Scalar(1.), Scalar(1.), Scalar(150.), Scalar(35.),
16  Scalar(30.), Scalar(8.), Scalar(20.), Scalar(20.), Scalar(15.),
17  Scalar(4.), Scalar(4.), Scalar(8.);
18  heuristicWeights.setConstant(Scalar(0.));
19  step_weights_.setConstant(Scalar(1));
20  pheuristic_.setZero();
21 
22  // Compute heuristic inside update Model
23  // pshoulder_0 << Scalar(0.1946) , Scalar(0.1946) , Scalar(-0.1946),
24  // Scalar(-0.1946) ,
25  // Scalar(0.15005) , Scalar(-0.15005) , Scalar(0.15005) ,
26  // Scalar(-0.15005) ;
27  // pshoulder_tmp.setZero() ;
28  // pcentrifugal_tmp_1.setZero() ;
29  // pcentrifugal_tmp_2.setZero() ;
30  // pcentrifugal_tmp.setZero() ;
31  // T_gait = Scalar(0.64) ;
32  centrifugal_term = true;
33  symmetry_term = true;
34 
35  // Weight on the speed ot the feet
36  nb_nodes = Scalar(15.);
37  vlim = Scalar(2.);
38  beta_lim = Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) /
39  225); // apparent speed used in the cost function
40  speed_weight = Scalar(10.);
41 
42  // Logging cost
43  cost_.setZero();
44  log_cost = true;
45 
46  // indicates whether it t the 1st step, otherwise the cost function is much
47  // simpler (acc, speed = 0)
48  first_step = false;
49 
50  // Coefficients for sample velocity of the feet
51  nb_alpha_ = 4;
52  alpha = MathBase::ArrayXs::Zero(nb_alpha_);
53  alpha2 =
54  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
55  b_coeff = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
56  nb_alpha_, 3); // Constant for all feet, avoid re-computing them
57 
58  // Cost = DT * b0(alpha) + DT**2 * b1(alpha) + DX * b2(alpha) for x velocity
59  b_coeff_x0 = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
60  nb_alpha_, 4); // col(i) --> foot i
61  b_coeff_x1 =
62  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
63  b_coeff_x2 =
64  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
65 
66  // Cost = DT * b0(alpha) + DT**2 * b1(alpha) + DX * b2(alpha) for y velocity
67  b_coeff_y0 = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
68  nb_alpha_, 4); // col(i) --> foot i
69  b_coeff_y1 =
70  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
71  b_coeff_y2 =
72  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
73 
74  rub_max_first_x =
75  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
76  rub_max_first_y =
77  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
78  rub_max_first_2 =
79  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
80  rub_max_first_bool =
81  Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(nb_alpha_, 4);
82 
83  alpha.setLinSpaced(nb_alpha_, Scalar(0.0), Scalar(1.0));
84  alpha2.col(0) << alpha;
85  alpha2.col(1) << alpha.pow(2);
86  alpha2.col(2) << alpha.pow(3);
87  alpha2.col(3) << alpha.pow(4);
88 
89  b_coeff.col(0) = Scalar(1.0) - Scalar(18.) * alpha2.col(1) +
90  Scalar(32.) * alpha2.col(2) - Scalar(15.) * alpha2.col(3);
91  b_coeff.col(1) = alpha2.col(0) - Scalar(4.5) * alpha2.col(1) +
92  Scalar(6.) * alpha2.col(2) - Scalar(2.5) * alpha2.col(3);
93  b_coeff.col(2) = Scalar(30.) * alpha2.col(1) - Scalar(60.) * alpha2.col(2) +
94  Scalar(30.) * alpha2.col(3);
95 
96  lfeet.setZero();
97 }
98 
99 template <typename Scalar>
101 
102 template <typename Scalar>
104  const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
105  const Eigen::Ref<const typename MathBase::VectorXs>& x,
106  const Eigen::Ref<const typename MathBase::VectorXs>& u) {
107  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
108  throw_pretty("Invalid argument: "
109  << "x has wrong dimension (it should be " +
110  std::to_string(state_->get_nx()) + ")");
111  }
112  if (static_cast<std::size_t>(u.size()) != nu_) {
113  throw_pretty("Invalid argument: "
114  << "u has wrong dimension (it should be " +
115  std::to_string(nu_) + ")");
116  }
117 
119  static_cast<ActionDataQuadrupedStepTimeTpl<Scalar>*>(data.get());
120 
121  // Update position of the feet
122  d->xnext.template head<12>() = x.head(12);
123  d->xnext.template segment<8>(12) = x.segment(12, 8) + B * u;
124  d->xnext.template tail<1>() = x.tail(1);
125 
126  // Residual cost on the state and force norm
127  d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
128  d->r.template segment<8>(12) = heuristicWeights.cwiseProduct(
129  x.segment(12, 8) -
130  pheuristic_); // Not used, set to 0, S matrix is for moving feet and not
131  // feet already on the ground
132  d->r.template tail<4>() = step_weights_.cwiseProduct(u);
133 
134  d->cost = Scalar(0.5) * d->r.transpose() * d->r;
135 
136  if (first_step) {
137  for (int i = 0; i < 4; i++) {
138  if (S_[i] == Scalar(1)) {
139  rub_max_first_x.col(i) = x(20) * b_coeff_x0.col(i) +
140  x(20) * x(20) * b_coeff_x1.col(i) +
141  u(2 * i) * b_coeff_x2.col(i);
142  rub_max_first_y.col(i) = x(20) * b_coeff_y0.col(i) +
143  x(20) * x(20) * b_coeff_y1.col(i) +
144  u(2 * i + 1) * b_coeff_y2.col(i);
145 
146  rub_max_first_2.col(i) =
147  rub_max_first_x.col(i).pow(2) + rub_max_first_y.col(i).pow(2) -
148  x(20) * x(20) * vlim * vlim * nb_nodes * nb_nodes;
149  } else {
150  rub_max_first_2.col(i).setZero();
151  }
152  }
153 
154  rub_max_first_bool =
155  (rub_max_first_2 > Scalar(0.))
156  .template cast<Scalar>(); // Usefull to compute the derivatives
157  rub_max_first_2 = rub_max_first_2.cwiseMax(Scalar(0.)); // Remove <0 terms
158 
159  for (int i = 0; i < nb_alpha_; i++) {
160  d->cost += speed_weight * Scalar(0.5) * rub_max_first_2.row(i).sum();
161  }
162  } else {
163  rub_max_ << u[0] * u[0] + u[1] * u[1] - beta_lim * x[20] * x[20],
164  u[2] * u[2] + u[3] * u[3] - beta_lim * x[20] * x[20];
165 
166  rub_max_bool =
167  (rub_max_.array() >= Scalar(0.)).matrix().template cast<Scalar>();
168  rub_max_ = rub_max_.cwiseMax(Scalar(0.));
169 
170  d->cost += speed_weight * Scalar(0.5) * rub_max_.sum();
171  }
172 
173  if (log_cost) {
174  cost_[3] = 0;
175  // Length to be consistent with others models
176  cost_[0] =
177  Scalar(0.5) * d->r.head(12).transpose() * d->r.head(12); // State cost
178  cost_[1] = Scalar(0.5) * d->r.segment(12, 8).transpose() *
179  d->r.segment(12, 8); // heuristic cost
180  cost_[2] = Scalar(0.5) * d->r.tail(4).transpose() *
181  d->r.tail(4); // Delta feet cost
182 
183  if (first_step) {
184  for (int i = 0; i < 3; i++) {
185  cost_[3] += speed_weight * Scalar(0.5) * rub_max_first_2.row(i).sum();
186  }
187  } else {
188  cost_[3] = speed_weight * Scalar(0.5) * rub_max_.sum();
189  }
190  }
191 }
192 
193 template <typename Scalar>
195  const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
196  const Eigen::Ref<const typename MathBase::VectorXs>& x,
197  const Eigen::Ref<const typename MathBase::VectorXs>& u) {
198  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
199  throw_pretty("Invalid argument: "
200  << "x has wrong dimension (it should be " +
201  std::to_string(state_->get_nx()) + ")");
202  }
203  if (static_cast<std::size_t>(u.size()) != nu_) {
204  throw_pretty("Invalid argument: "
205  << "u has wrong dimension (it should be " +
206  std::to_string(nu_) + ")");
207  }
208 
210  static_cast<ActionDataQuadrupedStepTimeTpl<Scalar>*>(data.get());
211 
212  d->Lx.setZero();
213  d->Lu.setZero();
214  d->Lxu.setZero();
215  d->Lxx.setZero();
216  d->Luu.setZero();
217  // Cost derivatives : Lx
218  d->Lx.template head<12>() =
219  (state_weights_.array() * d->r.template head<12>().array()).matrix();
220  d->Lx.template segment<8>(12) =
221  (heuristicWeights.array() * d->r.template segment<8>(12).array())
222  .matrix();
223 
224  if (first_step) {
225  for (int foot = 0; foot < 4; foot++) {
226  if (S_[foot] == Scalar(1)) {
227  for (int i = 0; i < nb_alpha_; i++) {
228  if (rub_max_first_bool(i, foot)) {
229  d->Lx(20) +=
230  speed_weight *
231  (b_coeff_x0(i, foot) +
232  Scalar(2) * x(20) * b_coeff_x1(i, foot)) *
233  rub_max_first_x(i, foot) +
234  speed_weight *
235  (b_coeff_y0(i, foot) +
236  Scalar(2) * x(20) * b_coeff_y1(i, foot)) *
237  rub_max_first_y(i, foot) -
238  speed_weight * x(20) * vlim * vlim * nb_nodes * nb_nodes;
239  d->Lu(2 * foot) +=
240  speed_weight * b_coeff_x2(i, foot) * rub_max_first_x(i, foot);
241  d->Lu(2 * foot + 1) +=
242  speed_weight * b_coeff_y2(i, foot) * rub_max_first_y(i, foot);
243 
244  d->Luu(2 * foot, 2 * foot) +=
245  speed_weight * b_coeff_x2(i, foot) * b_coeff_x2(i, foot);
246  d->Luu(2 * foot + 1, 2 * foot + 1) +=
247  speed_weight * b_coeff_y2(i, foot) * b_coeff_y2(i, foot);
248  d->Lxu(20, 2 * foot) += speed_weight *
249  (b_coeff_x0(i, foot) +
250  Scalar(2) * x(20) * b_coeff_x1(i, foot)) *
251  b_coeff_x2(i, foot);
252  d->Lxu(20, 2 * foot + 1) +=
253  speed_weight *
254  (b_coeff_y0(i, foot) +
255  Scalar(2) * x(20) * b_coeff_y1(i, foot)) *
256  b_coeff_y2(i, foot);
257  d->Lxx(20, 20) +=
258  speed_weight *
259  std::pow(b_coeff_x0(i, foot) +
260  Scalar(2) * x(20) * b_coeff_x1(i, foot),
261  2) +
262  speed_weight * Scalar(2) * b_coeff_x1(i, foot) *
263  rub_max_first_x(i, foot) +
264  speed_weight *
265  std::pow(b_coeff_y0(i, foot) +
266  Scalar(2) * x(20) * b_coeff_x1(i, foot),
267  2) +
268  speed_weight * Scalar(2) * b_coeff_y1(i, foot) *
269  rub_max_first_y(i, foot) -
270  speed_weight * vlim * vlim * nb_nodes * nb_nodes;
271  }
272  }
273  }
274  }
275 
276  }
277 
278  else {
279  d->Lx.template tail<1>()
280  << -beta_lim * speed_weight * x(20) * rub_max_bool[0] -
281  beta_lim * speed_weight * x(20) * rub_max_bool[1];
282 
283  d->Lu << speed_weight * u[0] * rub_max_bool[0],
284  speed_weight * u[1] * rub_max_bool[0],
285  speed_weight * u[2] * rub_max_bool[1],
286  speed_weight * u[3] * rub_max_bool[1];
287 
288  d->Lxx(20, 20) = -beta_lim * speed_weight * rub_max_bool[0] -
289  beta_lim * speed_weight * rub_max_bool[1];
290 
291  d->Luu.diagonal() << speed_weight * rub_max_bool[0],
292  speed_weight * rub_max_bool[0], speed_weight * rub_max_bool[1],
293  speed_weight * rub_max_bool[1];
294  }
295 
296  d->Lu += (step_weights_.array() * d->r.template tail<4>().array()).matrix();
297 
298  // Hessian : Lxx
299  d->Lxx.diagonal().head(12) =
300  (state_weights_.array() * state_weights_.array()).matrix();
301  d->Lxx.diagonal().segment(12, 8) =
302  (heuristicWeights.array() * heuristicWeights.array()).matrix();
303 
304  d->Luu.diagonal() += (step_weights_.array() * step_weights_.array()).matrix();
305 
306  // Dynamic derivatives
307  d->Fx.setIdentity();
308  d->Fu.block(12, 0, 8, 8) = B;
309 }
310 
311 template <typename Scalar>
312 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
314  return boost::make_shared<ActionDataQuadrupedStepTimeTpl<Scalar> >(this);
315 }
316 
318 // get & set parameters ////////
320 
321 template <typename Scalar>
322 const typename Eigen::Matrix<Scalar, 12, 1>&
324  return state_weights_;
325 }
326 template <typename Scalar>
328  const typename MathBase::VectorXs& weights) {
329  if (static_cast<std::size_t>(weights.size()) != 12) {
330  throw_pretty("Invalid argument: "
331  << "Weights vector has wrong dimension (it should be 12)");
332  }
333  state_weights_ = weights;
334 }
335 
336 template <typename Scalar>
337 const typename Eigen::Matrix<Scalar, 4, 1>&
339  return step_weights_;
340 }
341 template <typename Scalar>
343  const typename MathBase::VectorXs& weights) {
344  if (static_cast<std::size_t>(weights.size()) != 8) {
345  throw_pretty("Invalid argument: "
346  << "Weights vector has wrong dimension (it should be 8)");
347  }
348  step_weights_ = weights;
349 }
350 
351 template <typename Scalar>
352 const typename Eigen::Matrix<Scalar, 8, 1>&
354  return heuristicWeights;
355 }
356 template <typename Scalar>
358  const typename MathBase::VectorXs& weights) {
359  if (static_cast<std::size_t>(weights.size()) != 8) {
360  throw_pretty("Invalid argument: "
361  << "Weights vector has wrong dimension (it should be 8)");
362  }
363  heuristicWeights = weights;
364 }
365 
366 template <typename Scalar>
368  return symmetry_term;
369 }
370 template <typename Scalar>
372  const bool& sym_term) {
373  // The model need to be updated after this changed
374  symmetry_term = sym_term;
375 }
376 
377 template <typename Scalar>
379  const {
380  return centrifugal_term;
381 }
382 template <typename Scalar>
384  const bool& cent_term) {
385  // The model need to be updated after this changed
386  centrifugal_term = cent_term;
387 }
388 
389 template <typename Scalar>
391  // The model need to be updated after this changed
392  return T_gait;
393 }
394 template <typename Scalar>
396  const Scalar& T_gait_) {
397  // The model need to be updated after this changed
398  T_gait = T_gait_;
399 }
400 
402 // Get and modify param in speed cost //
404 template <typename Scalar>
406  const {
407  return speed_weight;
408 }
409 template <typename Scalar>
411  const Scalar& weight_) {
412  speed_weight = weight_;
413 }
414 
415 template <typename Scalar>
417  return nb_nodes;
418 }
419 template <typename Scalar>
421  const Scalar& nodes_) {
422  nb_nodes = nodes_;
423  beta_lim = Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) / 225);
424  ;
425 }
426 
427 template <typename Scalar>
429  return vlim;
430 }
431 template <typename Scalar>
433  vlim = vlim_;
434  beta_lim = Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) / 225);
435  ;
436 }
437 
439 // Log cost //
441 template <typename Scalar>
442 const typename Eigen::Matrix<Scalar, 7, 1>&
444  return cost_;
445 }
446 
447 // indicates whether it t the 1st step, otherwise the cost function is much
448 // simpler (acc, speed = 0)
449 template <typename Scalar>
451  return first_step;
452 }
453 template <typename Scalar>
455  const bool& first) {
456  // The model need to be updated after this changed
457  first_step = first;
458 }
459 
461 // Update current model
463 
464 template <typename Scalar>
466  const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
467  const Eigen::Ref<const typename MathBase::MatrixXs>& velocity,
468  const Eigen::Ref<const typename MathBase::MatrixXs>& acceleration,
469  const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
470  const Eigen::Ref<const typename MathBase::VectorXs>& S) {
471  if (static_cast<std::size_t>(l_feet.size()) != 12) {
472  throw_pretty("Invalid argument: "
473  << "l_feet matrix has wrong dimension (it should be : 3x4)");
474  }
475  if (static_cast<std::size_t>(xref.size()) != 12) {
476  throw_pretty("Invalid argument: "
477  << "Weights vector has wrong dimension (it should be " +
478  std::to_string(state_->get_nx()) + ")");
479  }
480  if (static_cast<std::size_t>(S.size()) != 4) {
481  throw_pretty("Invalid argument: "
482  << "S vector has wrong dimension (it should be 4x1)");
483  }
484  // Velocity : [[vx_0, vx_1, vx_2, vx_3],
485  // [vy_0, vy_1, vy_2, vy_3],
486  // [vz_0, vz_1, vz_2, vz_3]]
487 
488  for (int i = 0; i < 4; i = i + 1) {
489  pheuristic_.block(2 * i, 0, 2, 1) = l_feet.block(0, i, 2, 1);
490  }
491  xref_ = xref;
492  S_ = S;
493 
494  for (int i = 0; i < 4; i++) {
495  if (S[i] == Scalar(1)) {
496  // Coeff for x velocity
497  b_coeff_x0.col(i) = nb_nodes * velocity(0, i) * b_coeff.col(0);
498  b_coeff_x1.col(i) =
499  nb_nodes * nb_nodes * acceleration(0, i) * b_coeff.col(1);
500  b_coeff_x2.col(i) = b_coeff.col(2);
501 
502  // Coeff for y velocity
503  b_coeff_y0.col(i) = nb_nodes * velocity(1, i) * b_coeff.col(0);
504  b_coeff_y1.col(i) =
505  nb_nodes * nb_nodes * acceleration(1, i) * b_coeff.col(1);
506  b_coeff_y2.col(i) = b_coeff.col(2);
507  } else {
508  b_coeff_x0.col(i).setZero();
509  b_coeff_x1.col(i).setZero();
510  b_coeff_x2.col(i).setZero();
511  b_coeff_y0.col(i).setZero();
512  b_coeff_y1.col(i).setZero();
513  b_coeff_y2.col(i).setZero();
514  }
515  }
516 
517  // Compute heuristic inside update_model
518  // R_tmp << cos(xref(5,0)) ,-sin(xref(5,0)) , Scalar(0),
519  // sin(xref(5,0)), cos(xref(5,0)), Scalar(0),
520  // Scalar(0),Scalar(0),Scalar(1.0) ;
521  // // Centrifual term
522  // pcentrifugal_tmp_1 = xref.block(6,0,3,1) ;
523  // pcentrifugal_tmp_2 = xref.block(9,0,3,1) ;
524  // pcentrifugal_tmp = 0.5*std::sqrt(xref(2,0)/9.81) *
525  // pcentrifugal_tmp_1.cross(pcentrifugal_tmp_2) ;
526 
527  // for (int i=0; i<4; i=i+1){
528  // pshoulder_tmp.block(0,i,2,1) =
529  // R_tmp.block(0,0,2,2)*(pshoulder_0.block(0,i,2,1) + symmetry_term *
530  // 0.25*T_gait*xref.block(6,0,2,1) + centrifugal_term *
531  // pcentrifugal_tmp.block(0,0,2,1) ); pshoulder_[2*i] = pshoulder_tmp(0,i) +
532  // xref(0,0); pshoulder_[2*i+1] = pshoulder_tmp(1,i) + xref(1,0);
533  // }
534 
535  B.setZero();
536  // Set B matrix according to the moving feet : S = gait - gait_old
537  if (S[0] == Scalar(1)) {
538  B.block(0, 0, 2, 2).setIdentity();
539  }
540  if (S[1] == Scalar(1)) {
541  B.block(2, 2, 2, 2).setIdentity();
542  }
543  if (S[2] == Scalar(1)) {
544  B.block(4, 4, 2, 2).setIdentity();
545  }
546  if (S[3] == Scalar(1)) {
547  B.block(6, 6, 2, 2).setIdentity();
548  }
549 }
550 } // namespace quadruped_walkgen
551 
552 #endif
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_vlim
void set_vlim(const Scalar &vlim_)
Definition: quadruped_step_time.hxx:432
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_centrifugal_term
void set_centrifugal_term(const bool &cent_term)
Definition: quadruped_step_time.hxx:383
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_nb_nodes
const Scalar & get_nb_nodes() const
Definition: quadruped_step_time.hxx:416
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::update_model
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &velocity, const Eigen::Ref< const typename MathBase::MatrixXs > &acceleration, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::VectorXs > &S)
Definition: quadruped_step_time.hxx:465
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_state_weights
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_step_time.hxx:323
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::Scalar
_Scalar Scalar
Definition: quadruped_step_time.hpp:16
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::~ActionModelQuadrupedStepTimeTpl
~ActionModelQuadrupedStepTimeTpl()
Definition: quadruped_step_time.hxx:100
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_vlim
const Scalar & get_vlim() const
Definition: quadruped_step_time.hxx:428
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_T_gait
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_step_time.hxx:395
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_first_step
const bool & get_first_step() const
Definition: quadruped_step_time.hxx:450
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_speed_weight
void set_speed_weight(const Scalar &weight_)
Definition: quadruped_step_time.hxx:410
quadruped_walkgen
Definition: quadruped.hpp:11
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_step_weights
const Eigen::Matrix< Scalar, 4, 1 > & get_step_weights() const
Definition: quadruped_step_time.hxx:338
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_heuristic_weights
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_step_time.hxx:353
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_nb_nodes
void set_nb_nodes(const Scalar &nodes_)
Definition: quadruped_step_time.hxx:420
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_step_time.hxx:313
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::ActionModelQuadrupedStepTimeTpl
ActionModelQuadrupedStepTimeTpl()
Definition: quadruped_step_time.hxx:8
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_symmetry_term
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_step_time.hxx:371
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::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_time.hxx:103
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_heuristic_weights
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_time.hxx:357
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_speed_weight
const Scalar & get_speed_weight() const
Definition: quadruped_step_time.hxx:405
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::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_time.hxx:194
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_cost
const Eigen::Matrix< Scalar, 7, 1 > & get_cost() const
Definition: quadruped_step_time.hxx:443
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_state_weights
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_time.hxx:327
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_step_weights
void set_step_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_time.hxx:342
quadruped_walkgen::ActionDataQuadrupedStepTimeTpl
Definition: quadruped_step_time.hpp:150
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_centrifugal_term
const bool & get_centrifugal_term() const
Definition: quadruped_step_time.hxx:378
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::set_first_step
void set_first_step(const bool &first)
Definition: quadruped_step_time.hxx:454
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_symmetry_term
const bool & get_symmetry_term() const
Definition: quadruped_step_time.hxx:367
quadruped_walkgen::ActionModelQuadrupedStepTimeTpl::get_T_gait
const Scalar & get_T_gait() const
Definition: quadruped_step_time.hxx:390