Loading...
Searching...
No Matches
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
6namespace quadruped_walkgen {
7template <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
47template <typename Scalar>
49 Scalar>::~ActionModelQuadrupedStepPeriodTpl() {}
50
51template <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
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
98template <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
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
206template <typename Scalar>
207boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
209 return boost::make_shared<ActionDataQuadrupedStepPeriodTpl<Scalar> >(this);
210}
211
213// get & set parameters ////////
215
216template <typename Scalar>
217const typename Eigen::Matrix<Scalar, 12, 1>&
219 return state_weights_;
220}
221template <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
231template <typename Scalar>
232const typename Eigen::Matrix<Scalar, 4, 1>&
234 return step_weights_;
235}
236template <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
246template <typename Scalar>
247const typename Eigen::Matrix<Scalar, 8, 1>&
249 return shoulder_weights_;
250}
251template <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
261template <typename Scalar>
262const typename Eigen::Matrix<Scalar, 8, 1>&
266template <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
276template <typename Scalar>
278 const {
279 return symmetry_term;
280}
281template <typename Scalar>
283 const bool& sym_term) {
284 // The model need to be updated after this changed
285 symmetry_term = sym_term;
286}
287
288template <typename Scalar>
290 const {
291 return centrifugal_term;
292}
293template <typename Scalar>
295 const bool& cent_term) {
296 // The model need to be updated after this changed
297 centrifugal_term = cent_term;
298}
299
300template <typename Scalar>
302 // The model need to be updated after this changed
303 return T_gait;
304}
305template <typename Scalar>
307 const Scalar& T_gait_) {
308 // The model need to be updated after this changed
309 T_gait = T_gait_;
310}
311
312template <typename Scalar>
314 // The model need to be updated after this changed
315 return dt_weight_;
316}
317template <typename Scalar>
319 const Scalar& weight_) {
320 // The model need to be updated after this changed
321 dt_weight_ = weight_;
322}
323
324template <typename Scalar>
326 const {
327 // The model need to be updated after this changed
328 return speed_weight;
329}
330template <typename Scalar>
332 const Scalar& weight_) {
333 // The model need to be updated after this changed
334 speed_weight = weight_;
335}
336
337template <typename Scalar>
339 const {
340 // The model need to be updated after this changed
341 return dt_bound_weight;
342}
343template <typename Scalar>
345 const Scalar& weight_) {
346 // The model need to be updated after this changed
347 dt_bound_weight = weight_;
348}
349
350template <typename Scalar>
352 // The model need to be updated after this changed
353 return nb_nodes;
354}
355template <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
364template <typename Scalar>
366 // The model need to be updated after this changed
367 return vlim;
368}
369template <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
377template <typename Scalar>
379 return dt_ref_[0];
380}
381template <typename Scalar>
383 // The model need to be updated after this changed
384 dt_ref_[0] = dt;
385}
386
387template <typename Scalar>
389 return dt_min_[0];
390}
391template <typename Scalar>
393 // The model need to be updated after this changed
394 dt_min_[0] = dt;
395}
396
397template <typename Scalar>
399 return dt_max_[0];
400}
401template <typename Scalar>
403 // The model need to be updated after this changed
404 dt_max_[0] = dt;
405}
406
408// Update current model
410
411template <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_augmented_time.hpp:14
ActionModelQuadrupedAugmentedTimeTpl()
Definition quadruped_augmented_time.hxx:9
_Scalar Scalar
Definition quadruped_augmented_time.hpp:16
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