GCC Code Coverage Report

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