GCC Code Coverage Report

Directory: ./
File: include/quadruped-walkgen/quadruped_nl.hxx
Date: 2024-12-02 00:24:10
Exec Total Coverage
Lines: 0 244 0.0%
Branches: 0 808 0.0%
Line Branch Exec Source
1 #ifndef __quadruped_walkgen_quadruped_nl_hxx__
2 #define __quadruped_walkgen_quadruped_nl_hxx__
3
4 #include "crocoddyl/core/utils/exception.hpp"
5
6 namespace quadruped_walkgen {
7 template <typename Scalar>
8 ✗ ActionModelQuadrupedNonLinearTpl<Scalar>::ActionModelQuadrupedNonLinearTpl(
9 typename Eigen::Matrix<Scalar, 3, 1> offset_CoM)
10 : crocoddyl::ActionModelAbstractTpl<Scalar>(
11 ✗ boost::make_shared<crocoddyl::StateVectorTpl<Scalar> >(12), 12, 24) {
12 // Relative forces to compute the norm mof the command
13 ✗ relative_forces = false;
14 ✗ uref_.setZero();
15
16 // Model parameters
17 ✗ mu = Scalar(1);
18 ✗ dt_ = Scalar(0.02);
19 ✗ mass = Scalar(2.50000279);
20 ✗ min_fz_in_contact = Scalar(0.0);
21 ✗ max_fz = Scalar(25.);
22
23 // Matrix model initialization
24 ✗ g.setZero();
25 ✗ g[8] = Scalar(-9.81) * dt_;
26 ✗ gI.setZero();
27 ✗ gI.diagonal() << Scalar(3.09249e-2), Scalar(5.106100e-2), Scalar(6.939757e-2);
28 ✗ A.setIdentity();
29 ✗ A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
30 ✗ B.setZero();
31 ✗ lever_arms.setZero();
32 ✗ I_inv.setZero();
33
34 // Weight vectors initialization
35 ✗ force_weights_.setConstant(0.2);
36 ✗ state_weights_ << Scalar(1.), Scalar(1.), Scalar(150.), Scalar(35.),
37 ✗ Scalar(30.), Scalar(8.), Scalar(20.), Scalar(20.), Scalar(15.),
38 ✗ Scalar(4.), Scalar(4.), Scalar(8.);
39 ✗ friction_weight_ = Scalar(10);
40
41 // UpperBound vector
42 ✗ ub.setZero();
43 ✗ for (int i = 0; i < 4; i = i + 1) {
44 ✗ ub(6 * i + 5) = max_fz;
45 }
46
47 // Temporary vector used
48 ✗ Fa_x_u.setZero();
49 ✗ rub_max_.setZero();
50 ✗ Arr.setZero();
51 ✗ r.setZero();
52 ✗ lever_tmp.setZero();
53 ✗ R_tmp.setZero();
54 ✗ gait.setZero();
55 ✗ base_vector_x << Scalar(1.), Scalar(0.), Scalar(0.);
56 ✗ base_vector_y << Scalar(0.), Scalar(1.), Scalar(0.);
57 ✗ base_vector_z << Scalar(0.), Scalar(0.), Scalar(1.);
58 ✗ forces_3d.setZero();
59
60 // Used for shoulder height weight
61 ✗ pshoulder_0 << Scalar(0.1946), Scalar(0.1946), Scalar(-0.1946),
62 ✗ Scalar(-0.1946), Scalar(0.14695), Scalar(-0.14695), Scalar(0.14695),
63 ✗ Scalar(-0.14695);
64 ✗ sh_hlim = Scalar(0.27);
65 ✗ sh_weight = Scalar(10.);
66 ✗ sh_ub_max_.setZero();
67 ✗ psh.setZero();
68
69 // Implicit integration
70 // V+ = V + dt*B*u ; P+ = P + dt*V+ != explicit : P+ = P + dt*V
71 ✗ implicit_integration = true;
72 ✗ offset_com = offset_CoM; // x, y, z offset
73 }
74
75 template <typename Scalar>
76 ✗ ActionModelQuadrupedNonLinearTpl<Scalar>::~ActionModelQuadrupedNonLinearTpl() {}
77
78 template <typename Scalar>
79 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::calc(
80 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
81 const Eigen::Ref<const typename MathBase::VectorXs>& x,
82 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
83 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
84 ✗ throw_pretty("Invalid argument: "
85 << "x has wrong dimension (it should be " +
86 std::to_string(state_->get_nx()) + ")");
87 }
88 ✗ if (static_cast<std::size_t>(u.size()) != nu_) {
89 ✗ throw_pretty("Invalid argument: "
90 << "u has wrong dimension (it should be " +
91 std::to_string(nu_) + ")");
92 }
93
94 ActionDataQuadrupedNonLinearTpl<Scalar>* d =
95 ✗ static_cast<ActionDataQuadrupedNonLinearTpl<Scalar>*>(data.get());
96
97 // Update B :
98 ✗ for (int i = 0; i < 4; i = i + 1) {
99 ✗ if (gait(i, 0) != 0) {
100 ✗ lever_tmp = lever_arms.block(0, i, 3, 1) - x.block(0, 0, 3, 1);
101 ✗ R_tmp << 0.0, -lever_tmp[2], lever_tmp[1], lever_tmp[2], 0.0,
102 ✗ -lever_tmp[0], -lever_tmp[1], lever_tmp[0], 0.0;
103 ✗ B.block(9, 3 * i, 3, 3) << dt_ * I_inv * R_tmp;
104
105 // Compute pdistance of the shoulder wrt contact point
106 ✗ psh.block(0, i, 3, 1) << x[0] - offset_com(0, 0) + pshoulder_0(0, i) -
107 ✗ pshoulder_0(1, i) * x[5] - lever_arms(0, i),
108 ✗ x[1] - offset_com(1, 0) + pshoulder_0(1, i) +
109 ✗ pshoulder_0(0, i) * x[5] - lever_arms(1, i),
110 ✗ x[2] - offset_com(2, 0) + pshoulder_0(1, i) * x[3] -
111 ✗ pshoulder_0(0, i) * x[4];
112 } else {
113 // Compute pdistance of the shoulder wrt contact point
114 ✗ psh.block(0, i, 3, 1).setZero();
115 }
116 };
117
118 // Discrete dynamic : A*x + B*u + g
119 ✗ d->xnext << A.diagonal().cwiseProduct(x) + g;
120 ✗ d->xnext.template head<6>() =
121 ✗ d->xnext.template head<6>() +
122 ✗ A.topRightCorner(6, 6).diagonal().cwiseProduct(x.tail(6));
123 ✗ d->xnext.template tail<6>() =
124 ✗ d->xnext.template tail<6>() + B.block(6, 0, 6, 12) * u;
125
126 // Residual cost on the state and force norm
127 ✗ d->r.template head<12>() = state_weights_.cwiseProduct(x - xref_);
128 ✗ d->r.template tail<12>() = force_weights_.cwiseProduct(u - uref_);
129
130 // Friction cone + shoulder height
131 ✗ for (int i = 0; i < 4; i = i + 1) {
132 ✗ Fa_x_u.segment(6 * i, 6) << u(3 * i) - mu * u(3 * i + 2),
133 ✗ -u(3 * i) - mu * u(3 * i + 2), u(3 * i + 1) - mu * u(3 * i + 2),
134 ✗ -u(3 * i + 1) - mu * u(3 * i + 2), -u(3 * i + 2), u(3 * i + 2);
135 }
136 ✗ rub_max_ = (Fa_x_u - ub).cwiseMax(Scalar(0.));
137
138 // Shoulder height weight
139 ✗ sh_ub_max_ << psh.block(0, 0, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
140 ✗ psh.block(0, 1, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
141 ✗ psh.block(0, 2, 3, 1).squaredNorm() - sh_hlim * sh_hlim,
142 ✗ psh.block(0, 3, 3, 1).squaredNorm() - sh_hlim * sh_hlim;
143
144 ✗ sh_ub_max_ = sh_ub_max_.cwiseMax(Scalar(0.));
145
146 // Cost computation
147 // d->cost = 0.5 * d->r.transpose() * d->r + friction_weight_ *
148 // Scalar(0.5) * rub_max_.squaredNorm() + sh_weight
149 // * Scalar(0.5) * sh_ub_max_.squaredNorm() ;
150 ✗ d->cost = 0.5 * d->r.transpose() * d->r +
151 ✗ friction_weight_ * Scalar(0.5) * rub_max_.squaredNorm() +
152 ✗ sh_weight * Scalar(0.5) * sh_ub_max_.sum();
153 }
154
155 template <typename Scalar>
156 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::calcDiff(
157 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
158 const Eigen::Ref<const typename MathBase::VectorXs>& x,
159 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
160 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
161 ✗ throw_pretty("Invalid argument: "
162 << "x has wrong dimension (it should be " +
163 std::to_string(state_->get_nx()) + ")");
164 }
165 ✗ if (static_cast<std::size_t>(u.size()) != nu_) {
166 ✗ throw_pretty("Invalid argument: "
167 << "u has wrong dimension (it should be " +
168 std::to_string(nu_) + ")");
169 }
170
171 ActionDataQuadrupedNonLinearTpl<Scalar>* d =
172 ✗ static_cast<ActionDataQuadrupedNonLinearTpl<Scalar>*>(data.get());
173
174 // Cost derivatives : Lx
175
176 ✗ d->Lx = (state_weights_.array() * d->r.template head<12>().array()).matrix();
177
178 // Hessian : Lxx
179 ✗ d->Lxx.block(0, 0, 6, 6).setZero();
180 ✗ d->Lxx.diagonal() =
181 ✗ (state_weights_.array() * state_weights_.array()).matrix();
182
183 // Shoulder height derivative cost
184 ✗ for (int j = 0; j < 4; j = j + 1) {
185 ✗ if (sh_ub_max_[j] > Scalar(0.)) {
186 ✗ d->Lx(0, 0) += sh_weight * psh(0, j);
187 ✗ d->Lx(1, 0) += sh_weight * psh(1, j);
188 ✗ d->Lx(2, 0) += sh_weight * psh(2, j);
189 ✗ d->Lx(3, 0) += sh_weight * pshoulder_0(1, j) * psh(2, j);
190 ✗ d->Lx(4, 0) += -sh_weight * pshoulder_0(0, j) * psh(2, j);
191 ✗ d->Lx(5, 0) += sh_weight * (-pshoulder_0(1, j) * psh(0, j) +
192 ✗ pshoulder_0(0, j) * psh(1, j));
193
194 ✗ d->Lxx(0, 0) += sh_weight;
195 ✗ d->Lxx(1, 1) += sh_weight;
196 ✗ d->Lxx(2, 2) += sh_weight;
197 ✗ d->Lxx(3, 3) += sh_weight * pshoulder_0(1, j) * pshoulder_0(1, j);
198 ✗ d->Lxx(3, 3) += sh_weight * pshoulder_0(0, j) * pshoulder_0(0, j);
199 ✗ d->Lxx(5, 5) += sh_weight * (pshoulder_0(1, j) * pshoulder_0(1, j) +
200 ✗ pshoulder_0(0, j) * pshoulder_0(0, j));
201
202 ✗ d->Lxx(0, 5) += -sh_weight * pshoulder_0(1, j);
203 ✗ d->Lxx(5, 0) += -sh_weight * pshoulder_0(1, j);
204
205 ✗ d->Lxx(1, 5) += sh_weight * pshoulder_0(0, j);
206 ✗ d->Lxx(5, 1) += sh_weight * pshoulder_0(0, j);
207
208 ✗ d->Lxx(2, 3) += sh_weight * pshoulder_0(1, j);
209 ✗ d->Lxx(2, 4) += -sh_weight * pshoulder_0(0, j);
210 ✗ d->Lxx(3, 2) += sh_weight * pshoulder_0(1, j);
211 ✗ d->Lxx(4, 2) += -sh_weight * pshoulder_0(0, j);
212
213 ✗ d->Lxx(3, 4) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
214 ✗ d->Lxx(4, 3) += -sh_weight * pshoulder_0(1, j) * pshoulder_0(0, j);
215 }
216 }
217
218 // Cost derivative : Lu
219 ✗ for (int i = 0; i < 4; i = i + 1) {
220 ✗ r = friction_weight_ * rub_max_.segment(6 * i, 6);
221 ✗ d->Lu.block(i * 3, 0, 3, 1) << r(0) - r(1), r(2) - r(3),
222 ✗ -mu * (r(0) + r(1) + r(2) + r(3)) - r(4) + r(5);
223 }
224 ✗ d->Lu = d->Lu +
225 ✗ (force_weights_.array() * d->r.template tail<12>().array()).matrix();
226
227 // Hessian : Luu
228 // Matrix friction cone hessian (20x12)
229 ✗ Arr.diagonal() =
230 ✗ ((Fa_x_u - ub).array() >= 0.).matrix().template cast<Scalar>();
231 ✗ for (int i = 0; i < 4; i = i + 1) {
232 ✗ r = friction_weight_ * Arr.diagonal().segment(6 * i, 6);
233 ✗ d->Luu.block(3 * i, 3 * i, 3, 3) << r(0) + r(1), 0.0, mu * (r(1) - r(0)),
234 ✗ 0.0, r(2) + r(3), mu * (r(3) - r(2)), mu * (r(1) - r(0)),
235 ✗ mu * (r(3) - r(2)), mu * mu * (r(0) + r(1) + r(2) + r(3)) + r(4) + r(5);
236 }
237 ✗ d->Luu.diagonal() =
238 ✗ d->Luu.diagonal() +
239 ✗ (force_weights_.array() * force_weights_.array()).matrix();
240
241 // Dynamic derivatives
242 ✗ d->Fx << A;
243
244 ✗ for (int i = 0; i < 4; i = i + 1) {
245 ✗ if (gait(i, 0) != 0) {
246 ✗ forces_3d = u.block(3 * i, 0, 3, 1);
247 ✗ d->Fx.block(9, 0, 3, 1) +=
248 ✗ -dt_ * I_inv * (base_vector_x.cross(forces_3d));
249 ✗ d->Fx.block(9, 1, 3, 1) +=
250 ✗ -dt_ * I_inv * (base_vector_y.cross(forces_3d));
251 ✗ d->Fx.block(9, 2, 3, 1) +=
252 ✗ -dt_ * I_inv * (base_vector_z.cross(forces_3d));
253 }
254 }
255 ✗ d->Fu << B;
256 }
257
258 template <typename Scalar>
259 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
260 ✗ ActionModelQuadrupedNonLinearTpl<Scalar>::createData() {
261 ✗ return boost::make_shared<ActionDataQuadrupedNonLinearTpl<Scalar> >(this);
262 }
263
264 ////////////////////////////////
265 // get & set parameters ////////
266 ////////////////////////////////
267
268 template <typename Scalar>
269 const typename Eigen::Matrix<Scalar, 12, 1>&
270 ✗ ActionModelQuadrupedNonLinearTpl<Scalar>::get_force_weights() const {
271 ✗ return force_weights_;
272 }
273 template <typename Scalar>
274 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::set_force_weights(
275 const typename MathBase::VectorXs& weights) {
276 ✗ if (static_cast<std::size_t>(weights.size()) != state_->get_nx()) {
277 ✗ throw_pretty("Invalid argument: "
278 << "Weights vector has wrong dimension (it should be " +
279 std::to_string(state_->get_nx()) + ")");
280 }
281 ✗ force_weights_ = weights;
282 }
283
284 template <typename Scalar>
285 const typename Eigen::Matrix<Scalar, 12, 1>&
286 ✗ ActionModelQuadrupedNonLinearTpl<Scalar>::get_state_weights() const {
287 ✗ return state_weights_;
288 }
289 template <typename Scalar>
290 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::set_state_weights(
291 const typename MathBase::VectorXs& weights) {
292 ✗ if (static_cast<std::size_t>(weights.size()) != state_->get_nx()) {
293 ✗ throw_pretty("Invalid argument: "
294 << "Weights vector has wrong dimension (it should be " +
295 std::to_string(state_->get_nx()) + ")");
296 }
297 ✗ state_weights_ = weights;
298 }
299
300 template <typename Scalar>
301 ✗ const Scalar& ActionModelQuadrupedNonLinearTpl<Scalar>::get_friction_weight()
302 const {
303 ✗ return friction_weight_;
304 }
305 template <typename Scalar>
306 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::set_friction_weight(
307 const Scalar& weight) {
308 ✗ friction_weight_ = weight;
309 }
310
311 template <typename Scalar>
312 ✗ const Scalar& ActionModelQuadrupedNonLinearTpl<Scalar>::get_mu() const {
313 ✗ return mu;
314 }
315 template <typename Scalar>
316 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::set_mu(const Scalar& mu_coeff) {
317 ✗ mu = mu_coeff;
318 }
319
320 template <typename Scalar>
321 ✗ const Scalar& ActionModelQuadrupedNonLinearTpl<Scalar>::get_mass() const {
322 ✗ return mass;
323 }
324 template <typename Scalar>
325 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::set_mass(const Scalar& m) {
326 // The model need to be updated after this changed
327 ✗ mass = m;
328 }
329
330 template <typename Scalar>
331 ✗ const Scalar& ActionModelQuadrupedNonLinearTpl<Scalar>::get_dt() const {
332 ✗ return dt_;
333 }
334 template <typename Scalar>
335 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::set_dt(const Scalar& dt) {
336 // The model need to be updated after this changed
337 ✗ dt_ = dt;
338 ✗ g[8] = Scalar(-9.81) * dt_;
339 ✗ A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
340 }
341
342 template <typename Scalar>
343 const typename Eigen::Matrix<Scalar, 3, 3>&
344 ✗ ActionModelQuadrupedNonLinearTpl<Scalar>::get_gI() const {
345 ✗ return gI;
346 }
347 template <typename Scalar>
348 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::set_gI(
349 const typename MathBase::Matrix3s& inertia_matrix) {
350 // The model need to be updated after this changed
351 ✗ if (static_cast<std::size_t>(inertia_matrix.size()) != 9) {
352 ✗ throw_pretty("Invalid argument: "
353 << "gI has wrong dimension : 3x3");
354 }
355 ✗ gI = inertia_matrix;
356 }
357
358 template <typename Scalar>
359 ✗ const Scalar& ActionModelQuadrupedNonLinearTpl<Scalar>::get_min_fz_contact()
360 const {
361 // The model need to be updated after this changed
362 ✗ return min_fz_in_contact;
363 }
364 template <typename Scalar>
365 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::set_min_fz_contact(
366 const Scalar& min_fz) {
367 // The model need to be updated after this changed
368 ✗ min_fz_in_contact = min_fz;
369 }
370
371 template <typename Scalar>
372 ✗ const Scalar& ActionModelQuadrupedNonLinearTpl<Scalar>::get_max_fz_contact()
373 const {
374 // The model need to be updated after this changed
375 ✗ return max_fz;
376 }
377 template <typename Scalar>
378 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::set_max_fz_contact(
379 const Scalar& max_fz_) {
380 // The model need to be updated after this changed
381 ✗ max_fz = max_fz_;
382 ✗ for (int i = 0; i < 4; i = i + 1) {
383 ✗ ub(6 * i + 5) = max_fz;
384 }
385 }
386
387 template <typename Scalar>
388 ✗ const Scalar& ActionModelQuadrupedNonLinearTpl<Scalar>::get_shoulder_hlim()
389 const {
390 ✗ return sh_hlim;
391 }
392 template <typename Scalar>
393 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::set_shoulder_hlim(
394 const Scalar& hlim) {
395 // The model need to be updated after this changed
396 ✗ sh_hlim = hlim;
397 }
398
399 template <typename Scalar>
400 ✗ const Scalar& ActionModelQuadrupedNonLinearTpl<Scalar>::get_shoulder_weight()
401 const {
402 ✗ return sh_weight;
403 }
404 template <typename Scalar>
405 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::set_shoulder_weight(
406 const Scalar& weight) {
407 // The model need to be updated after this changed
408 ✗ sh_weight = weight;
409 }
410
411 ///////////////////////////
412 //// get A & B matrix /////
413 ///////////////////////////
414 template <typename Scalar>
415 const typename Eigen::Matrix<Scalar, 12, 12>&
416 ✗ ActionModelQuadrupedNonLinearTpl<Scalar>::get_A() const {
417 ✗ return A;
418 }
419 template <typename Scalar>
420 const typename Eigen::Matrix<Scalar, 12, 12>&
421 ✗ ActionModelQuadrupedNonLinearTpl<Scalar>::get_B() const {
422 ✗ return B;
423 }
424
425 // to modify the cost on the command : || fz - m*g/nb contact ||^2
426 // --> set to True
427 template <typename Scalar>
428 ✗ const bool& ActionModelQuadrupedNonLinearTpl<Scalar>::get_relative_forces()
429 const {
430 ✗ return relative_forces;
431 }
432 template <typename Scalar>
433 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::set_relative_forces(
434 const bool& rel_forces) {
435 ✗ relative_forces = rel_forces;
436 ✗ uref_.setZero();
437 ✗ if (relative_forces) {
438 ✗ for (int i = 0; i < 4; i = i + 1) {
439 ✗ if (gait[i] == 1) {
440 ✗ uref_[3 * i + 2] = (Scalar(9.81) * mass) / (gait.sum());
441 }
442 }
443 }
444 }
445
446 ////////////////////////
447 // Update current model
448 ////////////////////////
449
450 template <typename Scalar>
451 ✗ void ActionModelQuadrupedNonLinearTpl<Scalar>::update_model(
452 const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
453 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
454 const Eigen::Ref<const typename MathBase::MatrixXs>& S) {
455 ✗ if (static_cast<std::size_t>(l_feet.size()) != 12) {
456 ✗ throw_pretty("Invalid argument: "
457 << "l_feet matrix has wrong dimension (it should be : 3x4)");
458 }
459 ✗ if (static_cast<std::size_t>(xref.size()) != state_->get_nx()) {
460 ✗ throw_pretty("Invalid argument: "
461 << "Weights vector has wrong dimension (it should be " +
462 std::to_string(state_->get_nx()) + ")");
463 }
464 ✗ if (static_cast<std::size_t>(S.size()) != 4) {
465 ✗ throw_pretty("Invalid argument: "
466 << "S vector has wrong dimension (it should be 4x1)");
467 }
468
469 ✗ xref_ = xref;
470 ✗ gait = S;
471
472 // Set ref u vector according to nb of contact
473 ✗ uref_.setZero();
474 ✗ if (relative_forces) {
475 ✗ for (int i = 0; i < 4; i = i + 1) {
476 ✗ if (gait[i] == 1) {
477 ✗ uref_[3 * i + 2] = (Scalar(9.81) * mass) / (gait.sum());
478 }
479 }
480 }
481
482 ✗ R_tmp << cos(xref(5, 0)), -sin(xref(5, 0)), 0, sin(xref(5, 0)),
483 ✗ cos(xref(5, 0)), 0, 0, 0, 1.0;
484
485 ✗ I_inv = (R_tmp.transpose() * gI * R_tmp).inverse(); // I_inv
486 ✗ lever_arms.block(0, 0, 2, 4) = l_feet.block(0, 0, 2, 4);
487
488 ✗ for (int i = 0; i < 4; i = i + 1) {
489 ✗ if (S(i, 0) != 0) {
490 // set limit for normal force, (foot in contact with the ground)
491 ✗ ub(6 * i + 4) = -min_fz_in_contact;
492
493 // B update
494 ✗ B.block(6, 3 * i, 3, 3).diagonal() << dt_ / mass, dt_ / mass, dt_ / mass;
495
496 // Assuption 1 : levers arms not depends on the state, but on the
497 // predicted position (xfref)
498 // --> B will be updated with the update_B method for each calc function
499
500 // lever_tmp = lever_arms.block(0,i,3,1) - xref.block(0,0,3,1) ;
501 // R_tmp << 0.0, -lever_tmp[2], lever_tmp[1],
502 // lever_tmp[2], 0.0, -lever_tmp[0], -lever_tmp[1], lever_tmp[0], 0.0 ;
503 // B.block(9 , 3*i , 3,3) << dt_ * R* R_tmp;
504 } else {
505 // set limit for normal force at 0.0
506 ✗ ub(6 * i + 4) = 0.0;
507 ✗ B.block(6, 3 * i, 3, 3).setZero();
508 ✗ B.block(9, 3 * i, 3, 3).setZero();
509 };
510 };
511 }
512 } // namespace quadruped_walkgen
513
514 #endif
515