quadruped_step_period.hxx
Go to the documentation of this file.
1 #ifndef __quadruped_walkgen_quadruped_step_period_hxx__
2 #define __quadruped_walkgen_quadruped_step_period_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), 5, 26) {
11  B.setZero();
12  dt_ref_.setConstant(Scalar(0.02));
13  rub_max_.setZero();
14  rub_max_bool.setZero();
15  dt_min_.setConstant(Scalar(0.005));
16  dt_max_.setConstant(Scalar(0.1));
17  dt_weight_ = Scalar(1);
18  dt_bound_weight = Scalar(10);
19  state_weights_ << Scalar(1.), Scalar(1.), Scalar(150.), Scalar(35.),
20  Scalar(30.), Scalar(8.), Scalar(20.), Scalar(20.), Scalar(15.),
21  Scalar(4.), Scalar(4.), Scalar(8.);
22  shoulder_weights_.setConstant(Scalar(1));
23  pshoulder_ << Scalar(0.1946), Scalar(0.15005), Scalar(0.1946),
24  Scalar(-0.15005), Scalar(-0.1946), Scalar(0.15005), Scalar(-0.1946),
25  Scalar(-0.15005);
26  pshoulder_0 << Scalar(0.1946), Scalar(0.1946), Scalar(-0.1946),
27  Scalar(-0.1946), Scalar(0.15005), Scalar(-0.15005), Scalar(0.15005),
28  Scalar(-0.15005);
29  pshoulder_tmp.setZero();
30  pcentrifugal_tmp_1.setZero();
31  pcentrifugal_tmp_2.setZero();
32  pcentrifugal_tmp.setZero();
33  centrifugal_term = true;
34  symmetry_term = true;
35  T_gait = Scalar(0.64);
36 
37  step_weights_.setConstant(Scalar(1));
38 
39  // Optim dt
40  nb_nodes = Scalar(15.);
41  vlim = Scalar(2.);
42  beta_lim = Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) /
43  225); // apparent speed used in the cost function
44  speed_weight = Scalar(10.);
45 }
46 
47 template <typename Scalar>
49  Scalar>::~ActionModelQuadrupedStepPeriodTpl() {}
50 
51 template <typename Scalar>
53  const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
54  const Eigen::Ref<const typename MathBase::VectorXs>& x,
55  const Eigen::Ref<const typename MathBase::VectorXs>& u) {
56  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
57  throw_pretty("Invalid argument: "
58  << "x has wrong dimension (it should be " +
59  std::to_string(state_->get_nx()) + ")");
60  }
61  if (static_cast<std::size_t>(u.size()) != nu_) {
62  throw_pretty("Invalid argument: "
63  << "u has wrong dimension (it should be " +
64  std::to_string(nu_) + ")");
65  }
66 
68  static_cast<ActionDataQuadrupedStepPeriodTpl<Scalar>*>(data.get());
69 
70  d->xnext.template head<12>() = x.head(12);
71  d->xnext.template segment<8>(12) = x.segment(12, 8) + B * u.head(4);
72  d->xnext.template tail<1>() = u.tail(1);
73 
74  // Residual cost on the state and force norm
75  d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
76  d->r.template segment<8>(12) =
77  shoulder_weights_.cwiseProduct(x.segment(12, 8) - pshoulder_);
78  d->r.template segment<1>(20) = dt_weight_ * (x.tail(1) - dt_ref_);
79  d->r.template segment<4>(21) = step_weights_.cwiseProduct(u.head(4));
80  d->r.template tail<1>() = dt_weight_ * (u.tail(1) - dt_ref_);
81 
82  rub_max_ << dt_min_ - x.tail(1), x.tail(1) - dt_max_,
83  u[0] * u[0] + u[1] * u[1] - beta_lim * x[20] * x[20],
84  u[2] * u[2] + u[3] * u[3] - beta_lim * x[20] * x[20];
85 
86  rub_max_bool =
87  (rub_max_.array() >= Scalar(0.)).matrix().template cast<Scalar>();
88  rub_max_ = rub_max_.cwiseMax(Scalar(0.));
89 
90  // d->cost = Scalar(0.5) * d->r.transpose() * d->r + dt_bound_weight *
91  // Scalar(0.5) * rub_max_.head(2).squaredNorm()
92  // + speed_weight * Scalar(0.5) * rub_max_.tail(2).squaredNorm();
93  d->cost = Scalar(0.5) * d->r.transpose() * d->r +
94  dt_bound_weight * Scalar(0.5) * rub_max_.head(2).squaredNorm() +
95  speed_weight * Scalar(0.5) * rub_max_.tail(2).sum();
96 }
97 
98 template <typename Scalar>
100  const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
101  const Eigen::Ref<const typename MathBase::VectorXs>& x,
102  const Eigen::Ref<const typename MathBase::VectorXs>& u) {
103  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
104  throw_pretty("Invalid argument: "
105  << "x has wrong dimension (it should be " +
106  std::to_string(state_->get_nx()) + ")");
107  }
108  if (static_cast<std::size_t>(u.size()) != nu_) {
109  throw_pretty("Invalid argument: "
110  << "u has wrong dimension (it should be " +
111  std::to_string(nu_) + ")");
112  }
113 
115  static_cast<ActionDataQuadrupedStepPeriodTpl<Scalar>*>(data.get());
116 
117  // Cost derivatives : Lx
118  d->Lx.template head<12>() =
119  (state_weights_.array() * d->r.template head<12>().array()).matrix();
120  d->Lx.template segment<8>(12) =
121  (shoulder_weights_.array() * d->r.template segment<8>(12).array())
122  .matrix();
123  d->Lx.template tail<1>() << dt_bound_weight * (-rub_max_[0] + rub_max_[1]) -
124  beta_lim * speed_weight * x(20) *
125  rub_max_bool[2] -
126  beta_lim * speed_weight * x(20) *
127  rub_max_bool[3];
128  d->Lx.template tail<1>() += dt_weight_ * d->r.template segment<1>(20);
129 
130  // cost period <--> distance
131  // d->Lu << speed_weight*Scalar(2)*u[0]*rub_max_[2] ,
132  // speed_weight*Scalar(2)*u[1]*rub_max_[2] ,
133  // speed_weight*Scalar(2)*u[2]*rub_max_[3] ,
134  // speed_weight*Scalar(2)*u[3]*rub_max_[3] ,
135  // - speed_weight*Scalar(2)*beta_lim*u[4]*(rub_max_[2] +
136  // rub_max_[3]);
137  d->Lu << speed_weight * u[0] * rub_max_bool[2],
138  speed_weight * u[1] * rub_max_bool[2],
139  speed_weight * u[2] * rub_max_bool[3],
140  speed_weight * u[3] * rub_max_bool[3], Scalar(0.);
141 
142  d->Lu.template head<4>() +=
143  (step_weights_.array() * d->r.template segment<4>(21).array()).matrix();
144  d->Lu.template tail<1>() += dt_weight_ * d->r.template tail<1>();
145 
146  // Hessian : Lxx
147  d->Lxx.diagonal().head(12) =
148  (state_weights_.array() * state_weights_.array()).matrix();
149  d->Lxx.diagonal().segment(12, 8) =
150  (shoulder_weights_.array() * shoulder_weights_.array()).matrix();
151  d->Lxx.diagonal().tail(1) << dt_weight_ * dt_weight_ +
152  dt_bound_weight * rub_max_bool[0] +
153  dt_bound_weight * rub_max_bool[1];
154 
155  d->Lxx(20, 20) += -beta_lim * speed_weight * rub_max_bool[2] -
156  beta_lim * speed_weight * rub_max_bool[3];
157 
158  d->Luu.diagonal() << speed_weight * rub_max_bool[2],
159  speed_weight * rub_max_bool[2], speed_weight * rub_max_bool[3],
160  speed_weight * rub_max_bool[3], Scalar(0.);
161 
162  d->Luu.diagonal().head(4) +=
163  (step_weights_.array() * step_weights_.array()).matrix();
164 
165  // d->Luu.diagonal() << speed_weight*Scalar(2)*(rub_max_[2] + 2*u[0]*u[0]
166  // )*rub_max_bool[2] ,
167  // speed_weight*Scalar(2)*(rub_max_[2] + 2*u[1]*u[1]
168  // )*rub_max_bool[2] ,
169  // speed_weight*Scalar(2)*(rub_max_[3] + 2*u[2]*u[2]
170  // )*rub_max_bool[3] ,
171  // speed_weight*Scalar(2)*(rub_max_[3] + 2*u[3]*u[3]
172  // )*rub_max_bool[3] , dt_weight_*dt_weight_ -
173  // speed_weight*Scalar(2)*beta_lim*(rub_max_[2] -
174  // 2*beta_lim*u[4]*u[4])*rub_max_bool[2]
175  // -speed_weight*Scalar(2)*beta_lim*(rub_max_[3] -
176  // 2*beta_lim*u[4]*u[4])*rub_max_bool[3] ;
177  // d->Luu.diagonal().head(4) += (step_weights_.array() *
178  // step_weights_.array()).matrix() ; d->Luu.block(0,1,1,1) <<
179  // speed_weight*Scalar(4)*u[0]*u[1]*rub_max_bool[2] ; d->Luu.block(1,0,1,1) <<
180  // speed_weight*Scalar(4)*u[0]*u[1]*rub_max_bool[2] ; d->Luu.block(0,4,1,1) <<
181  // -speed_weight*Scalar(4)*beta_lim*u[0]*u[4]*rub_max_bool[2] ;
182  // d->Luu.block(4,0,1,1) <<
183  // -speed_weight*Scalar(4)*beta_lim*u[0]*u[4]*rub_max_bool[2] ;
184  // d->Luu.block(1,4,1,1) <<
185  // -speed_weight*Scalar(4)*beta_lim*u[1]*u[4]*rub_max_bool[2] ;
186  // d->Luu.block(4,1,1,1) <<
187  // -speed_weight*Scalar(4)*beta_lim*u[1]*u[4]*rub_max_bool[2] ;
188  // d->Luu.block(2,3,1,1) << speed_weight*Scalar(4)*u[2]*u[3]*rub_max_bool[3] ;
189  // d->Luu.block(3,2,1,1) << speed_weight*Scalar(4)*u[2]*u[3]*rub_max_bool[3] ;
190  // d->Luu.block(0,4,1,1) <<
191  // -speed_weight*Scalar(4)*beta_lim*u[2]*u[4]*rub_max_bool[3] ;
192  // d->Luu.block(4,0,1,1) <<
193  // -speed_weight*Scalar(4)*beta_lim*u[2]*u[4]*rub_max_bool[3] ;
194  // d->Luu.block(1,4,1,1) <<
195  // -speed_weight*Scalar(4)*beta_lim*u[3]*u[4]*rub_max_bool[3] ;
196  // d->Luu.block(4,1,1,1) <<
197  // -speed_weight*Scalar(4)*beta_lim*u[3]*u[4]*rub_max_bool[3] ;
198 
199  // Dynamic derivatives
200  d->Fx.setIdentity();
201  d->Fx.block(20, 20, 1, 1) << 0;
202  d->Fu.block(12, 0, 8, 4) = B;
203  d->Fu.block(20, 4, 1, 1) << 1;
204 }
205 
206 template <typename Scalar>
207 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
209  return boost::make_shared<ActionDataQuadrupedStepPeriodTpl<Scalar> >(this);
210 }
211 
213 // get & set parameters ////////
215 
216 template <typename Scalar>
217 const typename Eigen::Matrix<Scalar, 12, 1>&
219  return state_weights_;
220 }
221 template <typename Scalar>
223  const typename MathBase::VectorXs& weights) {
224  if (static_cast<std::size_t>(weights.size()) != 12) {
225  throw_pretty("Invalid argument: "
226  << "Weights vector has wrong dimension (it should be 12)");
227  }
228  state_weights_ = weights;
229 }
230 
231 template <typename Scalar>
232 const typename Eigen::Matrix<Scalar, 4, 1>&
234  return step_weights_;
235 }
236 template <typename Scalar>
238  const typename MathBase::VectorXs& weights) {
239  if (static_cast<std::size_t>(weights.size()) != 4) {
240  throw_pretty("Invalid argument: "
241  << "Weights vector has wrong dimension (it should be 4)");
242  }
243  step_weights_ = weights;
244 }
245 
246 template <typename Scalar>
247 const typename Eigen::Matrix<Scalar, 8, 1>&
249  return shoulder_weights_;
250 }
251 template <typename Scalar>
253  const typename MathBase::VectorXs& weights) {
254  if (static_cast<std::size_t>(weights.size()) != 8) {
255  throw_pretty("Invalid argument: "
256  << "Weights vector has wrong dimension (it should be 8)");
257  }
258  shoulder_weights_ = weights;
259 }
260 
261 template <typename Scalar>
262 const typename Eigen::Matrix<Scalar, 8, 1>&
264  return pshoulder_;
265 }
266 template <typename Scalar>
268  const typename MathBase::VectorXs& pos) {
269  if (static_cast<std::size_t>(pos.size()) != 8) {
270  throw_pretty("Invalid argument: "
271  << "Weights vector has wrong dimension (it should be 8)");
272  }
273  pshoulder_ = pos;
274 }
275 
276 template <typename Scalar>
278  const {
279  return symmetry_term;
280 }
281 template <typename Scalar>
283  const bool& sym_term) {
284  // The model need to be updated after this changed
285  symmetry_term = sym_term;
286 }
287 
288 template <typename Scalar>
290  const {
291  return centrifugal_term;
292 }
293 template <typename Scalar>
295  const bool& cent_term) {
296  // The model need to be updated after this changed
297  centrifugal_term = cent_term;
298 }
299 
300 template <typename Scalar>
302  // The model need to be updated after this changed
303  return T_gait;
304 }
305 template <typename Scalar>
307  const Scalar& T_gait_) {
308  // The model need to be updated after this changed
309  T_gait = T_gait_;
310 }
311 
312 template <typename Scalar>
314  // The model need to be updated after this changed
315  return dt_weight_;
316 }
317 template <typename Scalar>
319  const Scalar& weight_) {
320  // The model need to be updated after this changed
321  dt_weight_ = weight_;
322 }
323 
324 template <typename Scalar>
326  const {
327  // The model need to be updated after this changed
328  return speed_weight;
329 }
330 template <typename Scalar>
332  const Scalar& weight_) {
333  // The model need to be updated after this changed
334  speed_weight = weight_;
335 }
336 
337 template <typename Scalar>
339  const {
340  // The model need to be updated after this changed
341  return dt_bound_weight;
342 }
343 template <typename Scalar>
345  const Scalar& weight_) {
346  // The model need to be updated after this changed
347  dt_bound_weight = weight_;
348 }
349 
350 template <typename Scalar>
352  // The model need to be updated after this changed
353  return nb_nodes;
354 }
355 template <typename Scalar>
357  const Scalar& nodes_) {
358  // The model need to be updated after this changed
359  nb_nodes = nodes_;
360  beta_lim = Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) / 225);
361  ;
362 }
363 
364 template <typename Scalar>
366  // The model need to be updated after this changed
367  return vlim;
368 }
369 template <typename Scalar>
371  // The model need to be updated after this changed
372  vlim = vlim_;
373  beta_lim = Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) / 225);
374  ;
375 }
376 
377 template <typename Scalar>
379  return dt_ref_[0];
380 }
381 template <typename Scalar>
383  // The model need to be updated after this changed
384  dt_ref_[0] = dt;
385 }
386 
387 template <typename Scalar>
389  return dt_min_[0];
390 }
391 template <typename Scalar>
393  // The model need to be updated after this changed
394  dt_min_[0] = dt;
395 }
396 
397 template <typename Scalar>
399  return dt_max_[0];
400 }
401 template <typename Scalar>
403  // The model need to be updated after this changed
404  dt_max_[0] = dt;
405 }
406 
408 // Update current model
410 
411 template <typename Scalar>
413  const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
414  const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
415  const Eigen::Ref<const typename MathBase::MatrixXs>& S) {
416  if (static_cast<std::size_t>(l_feet.size()) != 12) {
417  throw_pretty("Invalid argument: "
418  << "l_feet matrix has wrong dimension (it should be : 3x4)");
419  }
420  if (static_cast<std::size_t>(xref.size()) != 12) {
421  throw_pretty("Invalid argument: "
422  << "Weights vector has wrong dimension (it should be " +
423  std::to_string(state_->get_nx()) + ")");
424  }
425  if (static_cast<std::size_t>(S.size()) != 4) {
426  throw_pretty("Invalid argument: "
427  << "S vector has wrong dimension (it should be 4x1)");
428  }
429 
430  xref_ = xref;
431 
432  R_tmp << cos(xref(5, 0)), -sin(xref(5, 0)), Scalar(0), sin(xref(5, 0)),
433  cos(xref(5, 0)), Scalar(0), Scalar(0), Scalar(0), Scalar(1.0);
434 
435  // Centrifual term
436  pcentrifugal_tmp_1 = xref.block(6, 0, 3, 1);
437  pcentrifugal_tmp_2 = xref.block(9, 0, 3, 1);
438  pcentrifugal_tmp = 0.5 * std::sqrt(xref(2, 0) / 9.81) *
439  pcentrifugal_tmp_1.cross(pcentrifugal_tmp_2);
440 
441  for (int i = 0; i < 4; i = i + 1) {
442  pshoulder_tmp.block(0, i, 2, 1) =
443  R_tmp.block(0, 0, 2, 2) *
444  (pshoulder_0.block(0, i, 2, 1) +
445  symmetry_term * 0.25 * T_gait * xref.block(6, 0, 2, 1) +
446  centrifugal_term * pcentrifugal_tmp.block(0, 0, 2, 1));
447  pshoulder_[2 * i] = pshoulder_tmp(0, i) + xref(0, 0);
448  pshoulder_[2 * i + 1] = pshoulder_tmp(1, i) + xref(1, 0);
449  }
450 
451  B.setZero();
452 
453  if (S(0, 0) == Scalar(1)) {
454  B.block(0, 0, 2, 2).setIdentity();
455  B.block(6, 2, 2, 2).setIdentity();
456  } else {
457  B.block(2, 0, 2, 2).setIdentity();
458  B.block(4, 2, 2, 2).setIdentity();
459  }
460 }
461 } // namespace quadruped_walkgen
462 
463 #endif
Definition: quadruped_step_period.hpp:14
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::MatrixXs > &S)
Definition: quadruped_step_period.hxx:412
const bool & get_centrifugal_term() const
Definition: quadruped_step_period.hxx:289
const Scalar & get_dt_max() const
Definition: quadruped_step_period.hxx:398
const Scalar & get_dt_weight() const
Definition: quadruped_step_period.hxx:313
const Scalar & get_speed_weight() const
Definition: quadruped_step_period.hxx:325
const Scalar & get_T_gait() const
Definition: quadruped_step_period.hxx:301
_Scalar Scalar
Definition: quadruped_step_period.hpp:16
const Eigen::Matrix< Scalar, 8, 1 > & get_shoulder_weights() const
Definition: quadruped_step_period.hxx:248
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_step_period.hxx:218
void set_centrifugal_term(const bool &cent_term)
Definition: quadruped_step_period.hxx:294
void set_nb_nodes(const Scalar &nodes_)
Definition: quadruped_step_period.hxx:356
const Scalar & get_dt_min() const
Definition: quadruped_step_period.hxx:388
const bool & get_symmetry_term() const
Definition: quadruped_step_period.hxx:277
void set_dt_min(const Scalar &dt)
Definition: quadruped_step_period.hxx:392
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_period.hxx:99
const Scalar & get_nb_nodes() const
Definition: quadruped_step_period.hxx:351
void set_dt_max(const Scalar &dt)
Definition: quadruped_step_period.hxx:402
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_period.hxx:222
void set_shoulder_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_period.hxx:252
const Eigen::Matrix< Scalar, 4, 1 > & get_step_weights() const
Definition: quadruped_step_period.hxx:233
void set_dt_weight(const Scalar &weight_)
Definition: quadruped_step_period.hxx:318
ActionModelQuadrupedStepPeriodTpl()
Definition: quadruped_step_period.hxx:8
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_step_period.hxx:208
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_step_period.hxx:306
const Scalar & get_dt_ref() const
Definition: quadruped_step_period.hxx:378
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_step_period.hxx:282
const Scalar & get_vlim() const
Definition: quadruped_step_period.hxx:365
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_period.hxx:52
void set_dt_bound_weight(const Scalar &weight_)
Definition: quadruped_step_period.hxx:344
void set_speed_weight(const Scalar &weight_)
Definition: quadruped_step_period.hxx:331
void set_shoulder_position(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_period.hxx:267
const Eigen::Matrix< Scalar, 8, 1 > & get_shoulder_position() const
Definition: quadruped_step_period.hxx:263
void set_vlim(const Scalar &vlim_)
Definition: quadruped_step_period.hxx:370
const Scalar & get_dt_bound_weight() const
Definition: quadruped_step_period.hxx:338
void set_dt_ref(const Scalar &dt)
Definition: quadruped_step_period.hxx:382
void set_step_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_step_period.hxx:237
Definition: quadruped.hpp:11
Definition: quadruped_step_period.hpp:133