GCC Code Coverage Report

Directory: ./
File: include/quadruped-walkgen/quadruped_step_time.hxx
Date: 2024-12-02 00:24:10
Exec Total Coverage
Lines: 0 279 0.0%
Branches: 0 800 0.0%
Line Branch Exec Source
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>
8 ✗ ActionModelQuadrupedStepTimeTpl<Scalar>::ActionModelQuadrupedStepTimeTpl()
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>
100 ✗ ActionModelQuadrupedStepTimeTpl<Scalar>::~ActionModelQuadrupedStepTimeTpl() {}
101
102 template <typename Scalar>
103 ✗ void ActionModelQuadrupedStepTimeTpl<Scalar>::calc(
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
118 ActionDataQuadrupedStepTimeTpl<Scalar>* d =
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>
194 ✗ void ActionModelQuadrupedStepTimeTpl<Scalar>::calcDiff(
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
209 ActionDataQuadrupedStepTimeTpl<Scalar>* d =
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> >
313 ✗ ActionModelQuadrupedStepTimeTpl<Scalar>::createData() {
314 ✗ return boost::make_shared<ActionDataQuadrupedStepTimeTpl<Scalar> >(this);
315 }
316
317 ////////////////////////////////
318 // get & set parameters ////////
319 ////////////////////////////////
320
321 template <typename Scalar>
322 const typename Eigen::Matrix<Scalar, 12, 1>&
323 ✗ ActionModelQuadrupedStepTimeTpl<Scalar>::get_state_weights() const {
324 ✗ return state_weights_;
325 }
326 template <typename Scalar>
327 ✗ void ActionModelQuadrupedStepTimeTpl<Scalar>::set_state_weights(
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>&
338 ✗ ActionModelQuadrupedStepTimeTpl<Scalar>::get_step_weights() const {
339 ✗ return step_weights_;
340 }
341 template <typename Scalar>
342 ✗ void ActionModelQuadrupedStepTimeTpl<Scalar>::set_step_weights(
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>&
353 ✗ ActionModelQuadrupedStepTimeTpl<Scalar>::get_heuristic_weights() const {
354 ✗ return heuristicWeights;
355 }
356 template <typename Scalar>
357 ✗ void ActionModelQuadrupedStepTimeTpl<Scalar>::set_heuristic_weights(
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>
367 ✗ const bool& ActionModelQuadrupedStepTimeTpl<Scalar>::get_symmetry_term() const {
368 ✗ return symmetry_term;
369 }
370 template <typename Scalar>
371 ✗ void ActionModelQuadrupedStepTimeTpl<Scalar>::set_symmetry_term(
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>
378 ✗ const bool& ActionModelQuadrupedStepTimeTpl<Scalar>::get_centrifugal_term()
379 const {
380 ✗ return centrifugal_term;
381 }
382 template <typename Scalar>
383 ✗ void ActionModelQuadrupedStepTimeTpl<Scalar>::set_centrifugal_term(
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>
390 ✗ const Scalar& ActionModelQuadrupedStepTimeTpl<Scalar>::get_T_gait() const {
391 // The model need to be updated after this changed
392 ✗ return T_gait;
393 }
394 template <typename Scalar>
395 ✗ void ActionModelQuadrupedStepTimeTpl<Scalar>::set_T_gait(
396 const Scalar& T_gait_) {
397 // The model need to be updated after this changed
398 ✗ T_gait = T_gait_;
399 }
400
401 /////////////////////////////////////////////
402 // Get and modify param in speed cost //
403 /////////////////////////////////////////////
404 template <typename Scalar>
405 ✗ const Scalar& ActionModelQuadrupedStepTimeTpl<Scalar>::get_speed_weight()
406 const {
407 ✗ return speed_weight;
408 }
409 template <typename Scalar>
410 ✗ void ActionModelQuadrupedStepTimeTpl<Scalar>::set_speed_weight(
411 const Scalar& weight_) {
412 ✗ speed_weight = weight_;
413 }
414
415 template <typename Scalar>
416 ✗ const Scalar& ActionModelQuadrupedStepTimeTpl<Scalar>::get_nb_nodes() const {
417 ✗ return nb_nodes;
418 }
419 template <typename Scalar>
420 ✗ void ActionModelQuadrupedStepTimeTpl<Scalar>::set_nb_nodes(
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>
428 ✗ const Scalar& ActionModelQuadrupedStepTimeTpl<Scalar>::get_vlim() const {
429 ✗ return vlim;
430 }
431 template <typename Scalar>
432 ✗ void ActionModelQuadrupedStepTimeTpl<Scalar>::set_vlim(const Scalar& vlim_) {
433 ✗ vlim = vlim_;
434 ✗ beta_lim = Scalar((64 * nb_nodes * nb_nodes * vlim * vlim) / 225);
435 ;
436 }
437
438 ///////////////
439 // Log cost //
440 ///////////////
441 template <typename Scalar>
442 const typename Eigen::Matrix<Scalar, 7, 1>&
443 ✗ ActionModelQuadrupedStepTimeTpl<Scalar>::get_cost() const {
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>
450 ✗ const bool& ActionModelQuadrupedStepTimeTpl<Scalar>::get_first_step() const {
451 ✗ return first_step;
452 }
453 template <typename Scalar>
454 ✗ void ActionModelQuadrupedStepTimeTpl<Scalar>::set_first_step(
455 const bool& first) {
456 // The model need to be updated after this changed
457 ✗ first_step = first;
458 }
459
460 ////////////////////////
461 // Update current model
462 ////////////////////////
463
464 template <typename Scalar>
465 ✗ void ActionModelQuadrupedStepTimeTpl<Scalar>::update_model(
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
553