GCC Code Coverage Report

Directory: ./
File: include/quadruped-walkgen/quadruped_step.hxx
Date: 2024-12-02 00:24:10
Exec Total Coverage
Lines: 0 748 0.0%
Branches: 0 1338 0.0%
Line Branch Exec Source
1 #ifndef __quadruped_walkgen_quadruped_step_hxx__
2 #define __quadruped_walkgen_quadruped_step_hxx__
3
4 #include "crocoddyl/core/utils/exception.hpp"
5
6 namespace quadruped_walkgen {
7 template <typename Scalar>
8 ✗ ActionModelQuadrupedStepTpl<Scalar>::ActionModelQuadrupedStepTpl()
9 : crocoddyl::ActionModelAbstractTpl<Scalar>(
10 ✗ boost::make_shared<crocoddyl::StateVectorTpl<Scalar> >(20), 8, 28) {
11 ✗ B.setZero();
12 ✗ state_weights_ << Scalar(1.), Scalar(1.), Scalar(150.), Scalar(35.),
13 ✗ Scalar(30.), Scalar(8.), Scalar(20.), Scalar(20.), Scalar(15.),
14 ✗ Scalar(4.), Scalar(4.), Scalar(8.);
15
16 ✗ pheuristic_ << Scalar(0.18), Scalar(0.15005), Scalar(0.18), Scalar(-0.15005),
17 ✗ Scalar(-0.21), Scalar(0.15005), Scalar(-0.21), Scalar(-0.15005);
18
19 ✗ centrifugal_term = true;
20 ✗ symmetry_term = true;
21 ✗ T_gait = Scalar(0.48);
22
23 ✗ step_weights_.setConstant(Scalar(1));
24 ✗ heuristic_weights_.setConstant(Scalar(1));
25
26 // Compute heuristic inside
27 // pshoulder_0 << Scalar(0.1946), Scalar(0.1946), Scalar(-0.1946),
28 // Scalar(-0.1946), Scalar(0.15005), Scalar(-0.15005),
29 // Scalar(0.15005), Scalar(-0.15005);
30 // pshoulder_tmp.setZero();
31 // pcentrifugal_tmp_1.setZero();
32 // pcentrifugal_tmp_2.setZero();
33 // pcentrifugal_tmp.setZero();
34
35 ✗ N_sampling = 5; // Number of point to sample the polynomial curve of the feet
36 // trajectory
37 ✗ S_.setZero(); // Usefull to compute only the trajectory for moving feet
38 ✗ position_.setZero(); // Xk+1 = Xk + Uk, Xk does not correspond to the current
39 // position of the flying feet, Delta_x is not
40 // straightforward
41
42 // Cost on the acceleration of the feet :
43 ✗ is_acc_activated_ = true;
44 ✗ acc_weight_ = Scalar(1.);
45 ✗ acc_lim_.setConstant(Scalar(50.));
46
47 ✗ delta_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
48 ✗ N_sampling - 1, 4);
49 ✗ gamma_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
50 ✗ N_sampling - 1, 3);
51 ✗ for (int k = 1; k < N_sampling; k++) {
52 ✗ delta_(k - 1, 0) =
53 ✗ (float)k / (float)N_sampling; // [1/N, 2/N, ... , (N-1)/N]
54 }
55 ✗ delta_.col(1) << delta_.col(0).pow(2);
56 ✗ delta_.col(2) << delta_.col(0).pow(3);
57 ✗ delta_.col(3) << delta_.col(0).pow(4); // Only used for speed cost
58 ✗ gamma_.col(0) =
59 ✗ 60 * delta_.col(0) - 180 * delta_.col(1) + 120 * delta_.col(2);
60 ✗ gamma_.col(1) = -36 * delta_.col(0) + 96 * delta_.col(1) - 60 * delta_.col(2);
61 ✗ gamma_.col(2) = -9 * delta_.col(0) + 18 * delta_.col(1) - 10 * delta_.col(2);
62
63 ✗ alpha_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
64 ✗ N_sampling - 1, 1); // Common for 4 feet
65 ✗ beta_x_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
66 ✗ N_sampling - 1, 4); // Depends on a0_x, v0_x of feet
67 ✗ beta_y_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
68 ✗ N_sampling - 1, 4); // Depends on a0_y, v0_y of feet
69 ✗ tmp_ones_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Ones(
70 ✗ N_sampling - 1, 1);
71
72 ✗ rb_accx_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
73 ✗ N_sampling - 1, 8);
74 ✗ rb_accy_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
75 ✗ N_sampling - 1, 8);
76 ✗ rb_accx_max_bool_ =
77 ✗ Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
78 8);
79 ✗ rb_accy_max_bool_ =
80 ✗ Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
81 8);
82
83 // Cost on the velocity of the feet :
84 ✗ is_vel_activated_ = true;
85 ✗ vel_weight_ = Scalar(1.);
86 ✗ vel_lim_.setConstant(Scalar(3.));
87
88 ✗ gamma_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
89 ✗ N_sampling - 1, 4);
90 ✗ gamma_v.col(0) = 30 * delta_.col(1) - 60 * delta_.col(2) + 30 * delta_.col(3);
91 ✗ gamma_v.col(1) = delta_.col(0);
92 ✗ gamma_v.col(2) =
93 ✗ -18 * delta_.col(1) + 32 * delta_.col(2) - 15 * delta_.col(3);
94 ✗ gamma_v.col(3) =
95 ✗ -4.5 * delta_.col(1) + 6 * delta_.col(2) - 2.5 * delta_.col(3);
96
97 ✗ alpha_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
98 ✗ N_sampling - 1, 1); // Common for 4 feet
99 ✗ beta_x_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
100 ✗ N_sampling - 1, 4); // Depends on a0_x, v0_x of feet
101 ✗ beta_y_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
102 ✗ N_sampling - 1, 4); // Depends on a0_y, v0_y of feet
103
104 ✗ rb_velx_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
105 ✗ N_sampling - 1, 8);
106 ✗ rb_vely_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
107 ✗ N_sampling - 1, 8);
108 ✗ rb_velx_max_bool_ =
109 ✗ Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
110 8);
111 ✗ rb_vely_max_bool_ =
112 ✗ Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
113 8);
114
115 // Cost on the jerk at t=0
116 ✗ is_jerk_activated_ = true;
117 ✗ jerk_weight_ = Scalar(1.);
118 ✗ alpha_j = Scalar(0.); // Common for 4 feet
119 ✗ beta_j = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
120 2, 4); // Depends on a0_x, v0_x of feet
121 ✗ rb_jerk_.setZero();
122 }
123
124 template <typename Scalar>
125 ✗ ActionModelQuadrupedStepTpl<Scalar>::~ActionModelQuadrupedStepTpl() {}
126
127 template <typename Scalar>
128 ✗ void ActionModelQuadrupedStepTpl<Scalar>::calc(
129 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
130 const Eigen::Ref<const typename MathBase::VectorXs>& x,
131 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
132 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
133 ✗ throw_pretty("Invalid argument: "
134 << "x has wrong dimension (it should be " +
135 std::to_string(state_->get_nx()) + ")");
136 }
137 ✗ if (static_cast<std::size_t>(u.size()) != nu_) {
138 ✗ throw_pretty("Invalid argument: "
139 << "u has wrong dimension (it should be " +
140 std::to_string(nu_) + ")");
141 }
142
143 ActionDataQuadrupedStepTpl<Scalar>* d =
144 ✗ static_cast<ActionDataQuadrupedStepTpl<Scalar>*>(data.get());
145
146 ✗ d->xnext.template head<12>() = x.head(12);
147 ✗ d->xnext.template tail<8>() = x.tail(8) + B * u;
148
149 // Residual cost on the state and force norm
150 ✗ d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
151 ✗ d->r.template segment<8>(12) =
152 ✗ heuristic_weights_.cwiseProduct(x.tail(8) - pheuristic_);
153 ✗ d->r.template tail<8>() = step_weights_.cwiseProduct(u);
154
155 ✗ d->cost = Scalar(0.5) * d->r.transpose() * d->r;
156
157 // Weight on the feet acceleration :
158 ✗ if (is_acc_activated_) {
159 ✗ for (int i = 0; i < 4; i++) {
160 ✗ if (S_(i) == Scalar(1.)) {
161 // position_ expressed in base frame
162 // rb_accx_max_.col(2*i) = (x(12+ 2*i) + u(2*i) - position_(0,i))*alpha_
163 // + beta_x_.col(i) - acc_lim_(0)*tmp_ones_; rb_accx_max_.col(2*i+1) =
164 // -( x(12+ 2*i) + u(2*i) - position_(0,i) )*alpha_ - beta_x_.col(i) -
165 // acc_lim_(0)*tmp_ones_;
166
167 // rb_accy_max_.col(2*i) = ( x(12+ 2*i+1) + u(2*i+1) - position_(1,i)
168 // )*alpha_ + beta_y_.col(i) - acc_lim_(1)*tmp_ones_;
169 // rb_accy_max_.col(2*i+1) = -( x(12+ 2*i+1) + u(2*i+1) - position_(1,i)
170 // )*alpha_ - beta_y_.col(i) - acc_lim_(1)*tmp_ones_;
171
172 // position_ expressed in world frame
173 ✗ rb_accx_max_.col(2 * i) =
174 ✗ (oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
175 ✗ oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(0) -
176 ✗ position_(0, i)) *
177 ✗ alpha_ +
178 ✗ beta_x_.col(i) - acc_lim_(0) * tmp_ones_;
179 ✗ rb_accx_max_.col(2 * i + 1) =
180 ✗ -(oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
181 ✗ oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(0) -
182 ✗ position_(0, i)) *
183 ✗ alpha_ +
184 ✗ beta_x_.col(i) - acc_lim_(0) * tmp_ones_;
185 ;
186
187 ✗ rb_accy_max_.col(2 * i) =
188 ✗ (oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
189 ✗ oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(1) -
190 ✗ position_(1, i)) *
191 ✗ alpha_ +
192 ✗ beta_y_.col(i) - acc_lim_(1) * tmp_ones_;
193 ✗ rb_accy_max_.col(2 * i + 1) =
194 ✗ -(oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
195 ✗ oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(1) -
196 ✗ position_(1, i)) *
197 ✗ alpha_ -
198 ✗ beta_y_.col(i) - acc_lim_(1) * tmp_ones_;
199 } else {
200 ✗ rb_accx_max_.col(2 * i).setZero();
201 ✗ rb_accx_max_.col(2 * i + 1).setZero();
202 ✗ rb_accy_max_.col(2 * i).setZero();
203 ✗ rb_accy_max_.col(2 * i + 1).setZero();
204 }
205 }
206 ✗ rb_accx_max_bool_ =
207 ✗ (rb_accx_max_ > Scalar(0.))
208 .template cast<Scalar>(); // Usefull to compute the derivatives
209 ✗ rb_accy_max_bool_ =
210 ✗ (rb_accy_max_ > Scalar(0.))
211 .template cast<Scalar>(); // Usefull to compute the derivatives
212
213 ✗ rb_accx_max_ = rb_accx_max_.cwiseMax(Scalar(0.));
214 ✗ rb_accy_max_ = rb_accy_max_.cwiseMax(Scalar(0.));
215
216 ✗ for (int foot = 0; foot < 4; foot++) {
217 ✗ if (S_(foot) == Scalar(1.)) {
218 ✗ for (int i = 0; i < (N_sampling - 1); i++) {
219 ✗ if (rb_accx_max_bool_(i, 2 * foot)) {
220 ✗ d->cost +=
221 ✗ Scalar(0.5) * acc_weight_ * pow(rb_accx_max_(i, 2 * foot), 2);
222 }
223 ✗ if (rb_accx_max_bool_(i, 2 * foot + 1)) {
224 ✗ d->cost += Scalar(0.5) * acc_weight_ *
225 ✗ pow(rb_accx_max_(i, 2 * foot + 1), 2);
226 }
227 ✗ if (rb_accy_max_bool_(i, 2 * foot)) {
228 ✗ d->cost +=
229 ✗ Scalar(0.5) * acc_weight_ * pow(rb_accy_max_(i, 2 * foot), 2);
230 }
231 ✗ if (rb_accy_max_bool_(i, 2 * foot + 1)) {
232 ✗ d->cost += Scalar(0.5) * acc_weight_ *
233 ✗ pow(rb_accy_max_(i, 2 * foot + 1), 2);
234 }
235 }
236 }
237 }
238 }
239
240 // Weight on the feet velocity
241 ✗ if (is_vel_activated_) {
242 ✗ for (int i = 0; i < 4; i++) {
243 ✗ if (S_(i) == Scalar(1.)) {
244 // position_ expressed in local frame
245 // rb_velx_max_.col(2*i) = (x(12+ 2*i) + u(2*i) -
246 // position_(0,i))*alpha_v + beta_x_v.col(i) - vel_lim_(0)*tmp_ones_;
247 // rb_velx_max_.col(2*i+1) = -( x(12+ 2*i) + u(2*i) - position_(0,i)
248 // )*alpha_v - beta_x_v.col(i) - vel_lim_(0)*tmp_ones_;
249
250 // rb_vely_max_.col(2*i) = ( x(12+ 2*i+1) + u(2*i+1) - position_(1,i)
251 // )*alpha_v + beta_y_v.col(i) - vel_lim_(1)*tmp_ones_;
252 // rb_vely_max_.col(2*i+1) = -( x(12+ 2*i+1) + u(2*i+1) - position_(1,i)
253 // )*alpha_v - beta_y_v.col(i) - vel_lim_(1)*tmp_ones_;
254
255 // position_ expressed in world frame
256 ✗ rb_velx_max_.col(2 * i) =
257 ✗ (oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
258 ✗ oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(0) -
259 ✗ position_(0, i)) *
260 ✗ alpha_v +
261 ✗ beta_x_v.col(i) - vel_lim_(0) * tmp_ones_;
262 ✗ rb_velx_max_.col(2 * i + 1) =
263 ✗ -(oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
264 ✗ oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(0) -
265 ✗ position_(0, i)) *
266 ✗ alpha_v -
267 ✗ beta_x_v.col(i) - vel_lim_(0) * tmp_ones_;
268
269 ✗ rb_vely_max_.col(2 * i) =
270 ✗ (oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
271 ✗ oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(1) -
272 ✗ position_(1, i)) *
273 ✗ alpha_v +
274 ✗ beta_y_v.col(i) - vel_lim_(1) * tmp_ones_;
275 ✗ rb_vely_max_.col(2 * i + 1) =
276 ✗ -(oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
277 ✗ oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) + oTh_(1) -
278 ✗ position_(1, i)) *
279 ✗ alpha_v -
280 ✗ beta_y_v.col(i) - vel_lim_(1) * tmp_ones_;
281 } else {
282 ✗ rb_velx_max_.col(2 * i).setZero();
283 ✗ rb_velx_max_.col(2 * i + 1).setZero();
284 ✗ rb_vely_max_.col(2 * i).setZero();
285 ✗ rb_vely_max_.col(2 * i + 1).setZero();
286 }
287 }
288 ✗ rb_velx_max_bool_ =
289 ✗ (rb_velx_max_ > Scalar(0.))
290 .template cast<Scalar>(); // Usefull to compute the derivatives
291 ✗ rb_vely_max_bool_ =
292 ✗ (rb_vely_max_ > Scalar(0.))
293 .template cast<Scalar>(); // Usefull to compute the derivatives
294
295 ✗ rb_velx_max_ = rb_velx_max_.cwiseMax(Scalar(0.));
296 ✗ rb_vely_max_ = rb_vely_max_.cwiseMax(Scalar(0.));
297
298 ✗ for (int foot = 0; foot < 4; foot++) {
299 ✗ if (S_(foot) == Scalar(1.)) {
300 ✗ for (int i = 0; i < (N_sampling - 1); i++) {
301 ✗ if (rb_velx_max_bool_(i, 2 * foot)) {
302 ✗ d->cost +=
303 ✗ Scalar(0.5) * vel_weight_ * pow(rb_velx_max_(i, 2 * foot), 2);
304 }
305 ✗ if (rb_velx_max_bool_(i, 2 * foot + 1)) {
306 ✗ d->cost += Scalar(0.5) * vel_weight_ *
307 ✗ pow(rb_velx_max_(i, 2 * foot + 1), 2);
308 }
309 ✗ if (rb_vely_max_bool_(i, 2 * foot)) {
310 ✗ d->cost +=
311 ✗ Scalar(0.5) * vel_weight_ * pow(rb_vely_max_(i, 2 * foot), 2);
312 }
313 ✗ if (rb_vely_max_bool_(i, 2 * foot + 1)) {
314 ✗ d->cost += Scalar(0.5) * vel_weight_ *
315 ✗ pow(rb_vely_max_(i, 2 * foot + 1), 2);
316 }
317 }
318 }
319 }
320 }
321
322 // Weight on the feet velocity
323 ✗ if (is_jerk_activated_) {
324 ✗ for (int i = 0; i < 4; i++) {
325 ✗ if (S_(i) == Scalar(1.)) {
326 ✗ rb_jerk_(0, i) = (oRh_(0, 0) * (x(12 + 2 * i) + u(2 * i)) +
327 ✗ oRh_(0, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) +
328 ✗ oTh_(0) - position_(0, i)) *
329 ✗ alpha_j +
330 ✗ beta_j(0, i) - jerk_(0, i);
331 ✗ rb_jerk_(1, i) = (oRh_(1, 0) * (x(12 + 2 * i) + u(2 * i)) +
332 ✗ oRh_(1, 1) * (x(12 + 2 * i + 1) + u(2 * i + 1)) +
333 ✗ oTh_(1) - position_(1, i)) *
334 ✗ alpha_j +
335 ✗ beta_j(1, i) - jerk_(1, i);
336 ✗ d->cost += Scalar(0.5) * jerk_weight_ * rb_jerk_.col(i).squaredNorm();
337 } else {
338 ✗ rb_jerk_.col(i).setZero();
339 }
340 }
341 }
342 }
343
344 template <typename Scalar>
345 ✗ void ActionModelQuadrupedStepTpl<Scalar>::calcDiff(
346 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
347 const Eigen::Ref<const typename MathBase::VectorXs>& x,
348 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
349 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
350 ✗ throw_pretty("Invalid argument: "
351 << "x has wrong dimension (it should be " +
352 std::to_string(state_->get_nx()) + ")");
353 }
354 ✗ if (static_cast<std::size_t>(u.size()) != nu_) {
355 ✗ throw_pretty("Invalid argument: "
356 << "u has wrong dimension (it should be " +
357 std::to_string(nu_) + ")");
358 }
359
360 ActionDataQuadrupedStepTpl<Scalar>* d =
361 ✗ static_cast<ActionDataQuadrupedStepTpl<Scalar>*>(data.get());
362
363 ✗ d->Lxu.setZero();
364 ✗ d->Luu.setZero();
365
366 // Cost derivatives : Lx
367 ✗ d->Lx.template head<12>() =
368 ✗ (state_weights_.array() * d->r.template head<12>().array()).matrix();
369 ✗ d->Lx.template tail<8>() =
370 ✗ (heuristic_weights_.array() * d->r.template segment<8>(12).array())
371 .matrix();
372
373 ✗ d->Lu = (step_weights_.array() * d->r.template tail<8>().array()).matrix();
374
375 // Hessian : Lxx
376 ✗ d->Lxx.diagonal().head(12) =
377 ✗ (state_weights_.array() * state_weights_.array()).matrix();
378 ✗ d->Lxx.diagonal().tail(8) =
379 ✗ (heuristic_weights_.array() * heuristic_weights_.array()).matrix();
380
381 ✗ d->Luu.diagonal() = (step_weights_.array() * step_weights_.array()).matrix();
382
383 ✗ if (is_acc_activated_) {
384 ✗ for (int foot = 0; foot < 4; foot++) {
385 ✗ if (S_[foot] == Scalar(1)) {
386 ✗ for (int i = 0; i < (N_sampling - 1); i++) {
387 // Position_ expressed in local frame
388 // if (rb_accx_max_bool_(i,2*foot)){
389
390 // d->Lu(2*foot) += acc_weight_ * alpha_(i) *
391 // rb_accx_max_(i,2*foot); d->Luu(2*foot,2*foot) += acc_weight_ *
392 // pow(alpha_(i),2);
393
394 // d->Lx(12+2*foot) += acc_weight_ * alpha_(i) *
395 // rb_accx_max_(i,2*foot); d->Lxu(12+2*foot,2*foot) += acc_weight_ *
396 // pow(alpha_(i),2); d->Lxx(12+2*foot,12+2*foot) += acc_weight_ *
397 // pow(alpha_(i),2);
398 // }
399 // if (rb_accx_max_bool_(i,2*foot+1)){
400 // d->Lu(2*foot) += - acc_weight_ * alpha_(i) *
401 // rb_accx_max_(i,2*foot+1); d->Luu(2*foot,2*foot) += acc_weight_ *
402 // pow(alpha_(i),2);
403
404 // d->Lx(12+2*foot) += - acc_weight_ * alpha_(i) *
405 // rb_accx_max_(i,2*foot+1); d->Lxu(12+2*foot,2*foot) += acc_weight_
406 // * pow(alpha_(i),2); d->Lxx(12+2*foot,12+2*foot) += acc_weight_ *
407 // pow(alpha_(i),2);
408 // }
409 // if (rb_accy_max_bool_(i,2*foot)){
410 // d->Lu(2*foot+1) += acc_weight_ * alpha_(i) *
411 // rb_accy_max_(i,2*foot); d->Luu(2*foot+1,2*foot+1) += acc_weight_
412 // * pow(alpha_(i),2);
413
414 // d->Lx(12+2*foot+1) += acc_weight_ * alpha_(i) *
415 // rb_accy_max_(i,2*foot); d->Lxu(12+2*foot+1,2*foot+1) +=
416 // acc_weight_ * pow(alpha_(i),2); d->Lxx(12+2*foot+1,12+2*foot+1)
417 // += acc_weight_ * pow(alpha_(i),2);
418 // }
419 // if (rb_accy_max_bool_(i,2*foot+1)){
420 // d->Lu(2*foot+1) += - acc_weight_ * alpha_(i) *
421 // rb_accy_max_(i,2*foot+1); d->Luu(2*foot+1,2*foot+1) +=
422 // acc_weight_ * pow(alpha_(i),2);
423
424 // d->Lx(12+2*foot+1) += - acc_weight_ * alpha_(i) *
425 // rb_accy_max_(i,2*foot+1); d->Lxu(12+2*foot+1,2*foot+1) +=
426 // acc_weight_ * pow(alpha_(i),2); d->Lxx(12+2*foot+1,12+2*foot+1)
427 // += acc_weight_ * pow(alpha_(i),2);
428 // }
429
430 // Position_ expressed in world frame
431 ✗ if (rb_accx_max_bool_(i, 2 * foot)) {
432 ✗ d->Lu(2 * foot) += acc_weight_ * oRh_(0, 0) * alpha_(i) *
433 ✗ rb_accx_max_(i, 2 * foot);
434 ✗ d->Lu(2 * foot + 1) += acc_weight_ * oRh_(0, 1) * alpha_(i) *
435 ✗ rb_accx_max_(i, 2 * foot);
436
437 ✗ d->Luu(2 * foot, 2 * foot) +=
438 ✗ acc_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_(i), 2);
439 ✗ d->Luu(2 * foot + 1, 2 * foot + 1) +=
440 ✗ acc_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_(i), 2);
441
442 ✗ d->Luu(2 * foot, 2 * foot + 1) +=
443 ✗ acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
444 ✗ d->Luu(2 * foot + 1, 2 * foot) +=
445 ✗ acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
446
447 ✗ d->Lx(12 + 2 * foot) += acc_weight_ * oRh_(0, 0) * alpha_(i) *
448 ✗ rb_accx_max_(i, 2 * foot);
449 ✗ d->Lx(12 + 2 * foot + 1) += acc_weight_ * oRh_(0, 1) * alpha_(i) *
450 ✗ rb_accx_max_(i, 2 * foot);
451
452 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
453 ✗ acc_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_(i), 2);
454 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
455 ✗ acc_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_(i), 2);
456
457 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
458 ✗ acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
459 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
460 ✗ acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
461
462 ✗ d->Lxu(12 + 2 * foot, 2 * foot) +=
463 ✗ acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(0, 0), 2);
464 ✗ d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
465 ✗ acc_weight_ * pow(alpha_(i), 2) * oRh_(0, 0) * oRh_(0, 1);
466 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
467 ✗ acc_weight_ * pow(alpha_(i), 2) * oRh_(0, 0) * oRh_(0, 1);
468 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
469 ✗ acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(0, 1), 2);
470 }
471 ✗ if (rb_accx_max_bool_(i, 2 * foot + 1)) {
472 ✗ d->Lu(2 * foot) += -acc_weight_ * oRh_(0, 0) * alpha_(i) *
473 ✗ rb_accx_max_(i, 2 * foot + 1);
474 ✗ d->Lu(2 * foot + 1) += -acc_weight_ * oRh_(0, 1) * alpha_(i) *
475 ✗ rb_accx_max_(i, 2 * foot + 1);
476
477 ✗ d->Luu(2 * foot, 2 * foot) +=
478 ✗ acc_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_(i), 2);
479 ✗ d->Luu(2 * foot + 1, 2 * foot + 1) +=
480 ✗ acc_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_(i), 2);
481
482 ✗ d->Luu(2 * foot, 2 * foot + 1) +=
483 ✗ acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
484 ✗ d->Luu(2 * foot + 1, 2 * foot) +=
485 ✗ acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
486
487 ✗ d->Lx(12 + 2 * foot) += -acc_weight_ * oRh_(0, 0) * alpha_(i) *
488 ✗ rb_accx_max_(i, 2 * foot + 1);
489 ✗ d->Lx(12 + 2 * foot + 1) += -acc_weight_ * oRh_(0, 1) * alpha_(i) *
490 ✗ rb_accx_max_(i, 2 * foot + 1);
491
492 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
493 ✗ acc_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_(i), 2);
494 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
495 ✗ acc_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_(i), 2);
496
497 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
498 ✗ acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
499 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
500 ✗ acc_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_(i), 2);
501
502 ✗ d->Lxu(12 + 2 * foot, 2 * foot) +=
503 ✗ acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(0, 0), 2);
504 ✗ d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
505 ✗ acc_weight_ * pow(alpha_(i), 2) * oRh_(0, 0) * oRh_(0, 1);
506 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
507 ✗ acc_weight_ * pow(alpha_(i), 2) * oRh_(0, 0) * oRh_(0, 1);
508 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
509 ✗ acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(0, 1), 2);
510 }
511 ✗ if (rb_accy_max_bool_(i, 2 * foot)) {
512 ✗ d->Lu(2 * foot) += acc_weight_ * oRh_(1, 0) * alpha_(i) *
513 ✗ rb_accy_max_(i, 2 * foot);
514 ✗ d->Lu(2 * foot + 1) += acc_weight_ * oRh_(1, 1) * alpha_(i) *
515 ✗ rb_accy_max_(i, 2 * foot);
516
517 ✗ d->Luu(2 * foot, 2 * foot) +=
518 ✗ acc_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_(i), 2);
519 ✗ d->Luu(2 * foot + 1, 2 * foot + 1) +=
520 ✗ acc_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_(i), 2);
521
522 ✗ d->Luu(2 * foot, 2 * foot + 1) +=
523 ✗ acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
524 ✗ d->Luu(2 * foot + 1, 2 * foot) +=
525 ✗ acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
526
527 ✗ d->Lx(12 + 2 * foot) += acc_weight_ * oRh_(1, 0) * alpha_(i) *
528 ✗ rb_accy_max_(i, 2 * foot);
529 ✗ d->Lx(12 + 2 * foot + 1) += acc_weight_ * oRh_(1, 1) * alpha_(i) *
530 ✗ rb_accy_max_(i, 2 * foot);
531
532 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
533 ✗ acc_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_(i), 2);
534 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
535 ✗ acc_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_(i), 2);
536
537 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
538 ✗ acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
539 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
540 ✗ acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
541
542 ✗ d->Lxu(12 + 2 * foot, 2 * foot) +=
543 ✗ acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(1, 0), 2);
544 ✗ d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
545 ✗ acc_weight_ * pow(alpha_(i), 2) * oRh_(1, 0) * oRh_(1, 1);
546 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
547 ✗ acc_weight_ * pow(alpha_(i), 2) * oRh_(1, 0) * oRh_(1, 1);
548 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
549 ✗ acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(1, 1), 2);
550 }
551 ✗ if (rb_accy_max_bool_(i, 2 * foot + 1)) {
552 ✗ d->Lu(2 * foot) += -acc_weight_ * oRh_(1, 0) * alpha_(i) *
553 ✗ rb_accy_max_(i, 2 * foot + 1);
554 ✗ d->Lu(2 * foot + 1) += -acc_weight_ * oRh_(1, 1) * alpha_(i) *
555 ✗ rb_accy_max_(i, 2 * foot + 1);
556
557 ✗ d->Luu(2 * foot, 2 * foot) +=
558 ✗ acc_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_(i), 2);
559 ✗ d->Luu(2 * foot + 1, 2 * foot + 1) +=
560 ✗ acc_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_(i), 2);
561
562 ✗ d->Luu(2 * foot, 2 * foot + 1) +=
563 ✗ acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
564 ✗ d->Luu(2 * foot + 1, 2 * foot) +=
565 ✗ acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
566
567 ✗ d->Lx(12 + 2 * foot) += -acc_weight_ * oRh_(1, 0) * alpha_(i) *
568 ✗ rb_accy_max_(i, 2 * foot + 1);
569 ✗ d->Lx(12 + 2 * foot + 1) += -acc_weight_ * oRh_(1, 1) * alpha_(i) *
570 ✗ rb_accy_max_(i, 2 * foot + 1);
571
572 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
573 ✗ acc_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_(i), 2);
574 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
575 ✗ acc_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_(i), 2);
576
577 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
578 ✗ acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
579 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
580 ✗ acc_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_(i), 2);
581
582 ✗ d->Lxu(12 + 2 * foot, 2 * foot) +=
583 ✗ acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(1, 0), 2);
584 ✗ d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
585 ✗ acc_weight_ * pow(alpha_(i), 2) * oRh_(1, 0) * oRh_(1, 1);
586 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
587 ✗ acc_weight_ * pow(alpha_(i), 2) * oRh_(1, 0) * oRh_(1, 1);
588 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
589 ✗ acc_weight_ * pow(alpha_(i), 2) * pow(oRh_(1, 1), 2);
590 }
591 }
592 }
593 }
594 }
595
596 ✗ if (is_vel_activated_) {
597 ✗ for (int foot = 0; foot < 4; foot++) {
598 ✗ if (S_[foot] == Scalar(1)) {
599 ✗ for (int i = 0; i < (N_sampling - 1); i++) {
600 // position_ in base frame
601 // if (rb_velx_max_bool_(i,2*foot)){
602
603 // d->Lu(2*foot) += vel_weight_ * alpha_v(i) *
604 // rb_velx_max_(i,2*foot); d->Luu(2*foot,2*foot) += vel_weight_ *
605 // pow(alpha_v(i),2);
606
607 // d->Lx(12+2*foot) += vel_weight_ * alpha_v(i) *
608 // rb_velx_max_(i,2*foot); d->Lxu(12+2*foot,2*foot) += vel_weight_ *
609 // pow(alpha_v(i),2); d->Lxx(12+2*foot,12+2*foot) += vel_weight_ *
610 // pow(alpha_v(i),2);
611 // }
612 // if (rb_velx_max_bool_(i,2*foot+1)){
613 // d->Lu(2*foot) += - vel_weight_ * alpha_v(i) *
614 // rb_velx_max_(i,2*foot+1); d->Luu(2*foot,2*foot) += vel_weight_ *
615 // pow(alpha_v(i),2);
616
617 // d->Lx(12+2*foot) += - vel_weight_ * alpha_v(i) *
618 // rb_velx_max_(i,2*foot+1); d->Lxu(12+2*foot,2*foot) += vel_weight_
619 // * pow(alpha_v(i),2); d->Lxx(12+2*foot,12+2*foot) += vel_weight_ *
620 // pow(alpha_v(i),2);
621 // }
622 // if (rb_vely_max_bool_(i,2*foot)){
623 // d->Lu(2*foot+1) += vel_weight_ * alpha_v(i) *
624 // rb_vely_max_(i,2*foot); d->Luu(2*foot+1,2*foot+1) += vel_weight_
625 // * pow(alpha_v(i),2);
626
627 // d->Lx(12+2*foot+1) += vel_weight_ * alpha_v(i) *
628 // rb_vely_max_(i,2*foot); d->Lxu(12+2*foot+1,2*foot+1) +=
629 // vel_weight_ * pow(alpha_v(i),2); d->Lxx(12+2*foot+1,12+2*foot+1)
630 // += vel_weight_ * pow(alpha_v(i),2);
631 // }
632 // if (rb_vely_max_bool_(i,2*foot+1)){
633 // d->Lu(2*foot+1) += - vel_weight_ * alpha_v(i) *
634 // rb_vely_max_(i,2*foot+1); d->Luu(2*foot+1,2*foot+1) +=
635 // vel_weight_ * pow(alpha_v(i),2);
636
637 // d->Lx(12+2*foot+1) += - vel_weight_ * alpha_v(i) *
638 // rb_vely_max_(i,2*foot+1); d->Lxu(12+2*foot+1,2*foot+1) +=
639 // vel_weight_ * pow(alpha_v(i),2); d->Lxx(12+2*foot+1,12+2*foot+1)
640 // += vel_weight_ * pow(alpha_v(i),2);
641 // }
642
643 // position_ in world frame
644 ✗ if (rb_velx_max_bool_(i, 2 * foot)) {
645 ✗ d->Lu(2 * foot) += vel_weight_ * oRh_(0, 0) * alpha_v(i) *
646 ✗ rb_velx_max_(i, 2 * foot);
647 ✗ d->Lu(2 * foot + 1) += vel_weight_ * oRh_(0, 1) * alpha_v(i) *
648 ✗ rb_velx_max_(i, 2 * foot);
649
650 ✗ d->Luu(2 * foot, 2 * foot) +=
651 ✗ vel_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_v(i), 2);
652 ✗ d->Luu(2 * foot + 1, 2 * foot + 1) +=
653 ✗ vel_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_v(i), 2);
654
655 ✗ d->Luu(2 * foot, 2 * foot + 1) +=
656 ✗ vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
657 ✗ d->Luu(2 * foot + 1, 2 * foot) +=
658 ✗ vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
659
660 ✗ d->Lx(12 + 2 * foot) += vel_weight_ * oRh_(0, 0) * alpha_v(i) *
661 ✗ rb_velx_max_(i, 2 * foot);
662 ✗ d->Lx(12 + 2 * foot + 1) += vel_weight_ * oRh_(0, 1) * alpha_v(i) *
663 ✗ rb_velx_max_(i, 2 * foot);
664
665 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
666 ✗ vel_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_v(i), 2);
667 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
668 ✗ vel_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_v(i), 2);
669
670 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
671 ✗ vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
672 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
673 ✗ vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
674
675 ✗ d->Lxu(12 + 2 * foot, 2 * foot) +=
676 ✗ vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(0, 0), 2);
677 ✗ d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
678 ✗ vel_weight_ * pow(alpha_v(i), 2) * oRh_(0, 0) * oRh_(0, 1);
679 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
680 ✗ vel_weight_ * pow(alpha_v(i), 2) * oRh_(0, 0) * oRh_(0, 1);
681 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
682 ✗ vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(0, 1), 2);
683 }
684 ✗ if (rb_velx_max_bool_(i, 2 * foot + 1)) {
685 ✗ d->Lu(2 * foot) += -vel_weight_ * oRh_(0, 0) * alpha_v(i) *
686 ✗ rb_velx_max_(i, 2 * foot + 1);
687 ✗ d->Lu(2 * foot + 1) += -vel_weight_ * oRh_(0, 1) * alpha_v(i) *
688 ✗ rb_velx_max_(i, 2 * foot + 1);
689
690 ✗ d->Luu(2 * foot, 2 * foot) +=
691 ✗ vel_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_v(i), 2);
692 ✗ d->Luu(2 * foot + 1, 2 * foot + 1) +=
693 ✗ vel_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_v(i), 2);
694
695 ✗ d->Luu(2 * foot, 2 * foot + 1) +=
696 ✗ vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
697 ✗ d->Luu(2 * foot + 1, 2 * foot) +=
698 ✗ vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
699
700 ✗ d->Lx(12 + 2 * foot) += -vel_weight_ * oRh_(0, 0) * alpha_v(i) *
701 ✗ rb_velx_max_(i, 2 * foot + 1);
702 ✗ d->Lx(12 + 2 * foot + 1) += -vel_weight_ * oRh_(0, 1) * alpha_v(i) *
703 ✗ rb_velx_max_(i, 2 * foot + 1);
704
705 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
706 ✗ vel_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_v(i), 2);
707 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
708 ✗ vel_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_v(i), 2);
709
710 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
711 ✗ vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
712 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
713 ✗ vel_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_v(i), 2);
714
715 ✗ d->Lxu(12 + 2 * foot, 2 * foot) +=
716 ✗ vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(0, 0), 2);
717 ✗ d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
718 ✗ vel_weight_ * pow(alpha_v(i), 2) * oRh_(0, 0) * oRh_(0, 1);
719 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
720 ✗ vel_weight_ * pow(alpha_v(i), 2) * oRh_(0, 0) * oRh_(0, 1);
721 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
722 ✗ vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(0, 1), 2);
723 }
724 ✗ if (rb_vely_max_bool_(i, 2 * foot)) {
725 ✗ d->Lu(2 * foot) += vel_weight_ * oRh_(1, 0) * alpha_v(i) *
726 ✗ rb_vely_max_(i, 2 * foot);
727 ✗ d->Lu(2 * foot + 1) += vel_weight_ * oRh_(1, 1) * alpha_v(i) *
728 ✗ rb_vely_max_(i, 2 * foot);
729
730 ✗ d->Luu(2 * foot, 2 * foot) +=
731 ✗ vel_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_v(i), 2);
732 ✗ d->Luu(2 * foot + 1, 2 * foot + 1) +=
733 ✗ vel_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_v(i), 2);
734
735 ✗ d->Luu(2 * foot, 2 * foot + 1) +=
736 ✗ vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
737 ✗ d->Luu(2 * foot + 1, 2 * foot) +=
738 ✗ vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
739
740 ✗ d->Lx(12 + 2 * foot) += vel_weight_ * oRh_(1, 0) * alpha_v(i) *
741 ✗ rb_vely_max_(i, 2 * foot);
742 ✗ d->Lx(12 + 2 * foot + 1) += vel_weight_ * oRh_(1, 1) * alpha_v(i) *
743 ✗ rb_vely_max_(i, 2 * foot);
744
745 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
746 ✗ vel_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_v(i), 2);
747 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
748 ✗ vel_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_v(i), 2);
749
750 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
751 ✗ vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
752 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
753 ✗ vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
754
755 ✗ d->Lxu(12 + 2 * foot, 2 * foot) +=
756 ✗ vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(1, 0), 2);
757 ✗ d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
758 ✗ vel_weight_ * pow(alpha_v(i), 2) * oRh_(1, 0) * oRh_(1, 1);
759 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
760 ✗ vel_weight_ * pow(alpha_v(i), 2) * oRh_(1, 0) * oRh_(1, 1);
761 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
762 ✗ vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(1, 1), 2);
763 }
764 ✗ if (rb_vely_max_bool_(i, 2 * foot + 1)) {
765 ✗ d->Lu(2 * foot) += -vel_weight_ * oRh_(1, 0) * alpha_v(i) *
766 ✗ rb_vely_max_(i, 2 * foot + 1);
767 ✗ d->Lu(2 * foot + 1) += -vel_weight_ * oRh_(1, 1) * alpha_v(i) *
768 ✗ rb_vely_max_(i, 2 * foot + 1);
769
770 ✗ d->Luu(2 * foot, 2 * foot) +=
771 ✗ vel_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_v(i), 2);
772 ✗ d->Luu(2 * foot + 1, 2 * foot + 1) +=
773 ✗ vel_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_v(i), 2);
774
775 ✗ d->Luu(2 * foot, 2 * foot + 1) +=
776 ✗ vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
777 ✗ d->Luu(2 * foot + 1, 2 * foot) +=
778 ✗ vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
779
780 ✗ d->Lx(12 + 2 * foot) += -vel_weight_ * oRh_(1, 0) * alpha_v(i) *
781 ✗ rb_vely_max_(i, 2 * foot + 1);
782 ✗ d->Lx(12 + 2 * foot + 1) += -vel_weight_ * oRh_(1, 1) * alpha_v(i) *
783 ✗ rb_vely_max_(i, 2 * foot + 1);
784
785 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
786 ✗ vel_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_v(i), 2);
787 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
788 ✗ vel_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_v(i), 2);
789
790 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
791 ✗ vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
792 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
793 ✗ vel_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_v(i), 2);
794
795 ✗ d->Lxu(12 + 2 * foot, 2 * foot) +=
796 ✗ vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(1, 0), 2);
797 ✗ d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
798 ✗ vel_weight_ * pow(alpha_v(i), 2) * oRh_(1, 0) * oRh_(1, 1);
799 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
800 ✗ vel_weight_ * pow(alpha_v(i), 2) * oRh_(1, 0) * oRh_(1, 1);
801 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
802 ✗ vel_weight_ * pow(alpha_v(i), 2) * pow(oRh_(1, 1), 2);
803 }
804 }
805 }
806 }
807 }
808
809 ✗ if (is_jerk_activated_) {
810 ✗ for (int foot = 0; foot < 4; foot++) {
811 ✗ if (S_[foot] == Scalar(1)) {
812 // derivatives relative to jerk on x axis
813 ✗ d->Lu(2 * foot) +=
814 ✗ jerk_weight_ * oRh_(0, 0) * alpha_j * rb_jerk_(0, foot);
815 ✗ d->Lu(2 * foot + 1) +=
816 ✗ jerk_weight_ * oRh_(0, 1) * alpha_j * rb_jerk_(0, foot);
817
818 ✗ d->Luu(2 * foot, 2 * foot) +=
819 ✗ jerk_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_j, 2);
820 ✗ d->Luu(2 * foot + 1, 2 * foot + 1) +=
821 ✗ jerk_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_j, 2);
822
823 ✗ d->Luu(2 * foot, 2 * foot + 1) +=
824 ✗ jerk_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_j, 2);
825 ✗ d->Luu(2 * foot + 1, 2 * foot) +=
826 ✗ jerk_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_j, 2);
827
828 ✗ d->Lx(12 + 2 * foot) +=
829 ✗ jerk_weight_ * oRh_(0, 0) * alpha_j * rb_jerk_(0, foot);
830 ✗ d->Lx(12 + 2 * foot + 1) +=
831 ✗ jerk_weight_ * oRh_(0, 1) * alpha_j * rb_jerk_(0, foot);
832
833 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
834 ✗ jerk_weight_ * pow(oRh_(0, 0), 2) * pow(alpha_j, 2);
835 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
836 ✗ jerk_weight_ * pow(oRh_(0, 1), 2) * pow(alpha_j, 2);
837
838 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
839 ✗ jerk_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_j, 2);
840 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
841 ✗ jerk_weight_ * oRh_(0, 0) * oRh_(0, 1) * pow(alpha_j, 2);
842
843 ✗ d->Lxu(12 + 2 * foot, 2 * foot) +=
844 ✗ jerk_weight_ * pow(alpha_j, 2) * pow(oRh_(0, 0), 2);
845 ✗ d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
846 ✗ jerk_weight_ * pow(alpha_j, 2) * oRh_(0, 0) * oRh_(0, 1);
847 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
848 ✗ jerk_weight_ * pow(alpha_j, 2) * oRh_(0, 0) * oRh_(0, 1);
849 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
850 ✗ jerk_weight_ * pow(alpha_j, 2) * pow(oRh_(0, 1), 2);
851
852 // derivatives relative to jerk on y axis
853 ✗ d->Lu(2 * foot) +=
854 ✗ jerk_weight_ * oRh_(1, 0) * alpha_j * rb_jerk_(1, foot);
855 ✗ d->Lu(2 * foot + 1) +=
856 ✗ jerk_weight_ * oRh_(1, 1) * alpha_j * rb_jerk_(1, foot);
857
858 ✗ d->Luu(2 * foot, 2 * foot) +=
859 ✗ jerk_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_j, 2);
860 ✗ d->Luu(2 * foot + 1, 2 * foot + 1) +=
861 ✗ jerk_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_j, 2);
862
863 ✗ d->Luu(2 * foot, 2 * foot + 1) +=
864 ✗ jerk_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_j, 2);
865 ✗ d->Luu(2 * foot + 1, 2 * foot) +=
866 ✗ jerk_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_j, 2);
867
868 ✗ d->Lx(12 + 2 * foot) +=
869 ✗ jerk_weight_ * oRh_(1, 0) * alpha_j * rb_jerk_(1, foot);
870 ✗ d->Lx(12 + 2 * foot + 1) +=
871 ✗ jerk_weight_ * oRh_(1, 1) * alpha_j * rb_jerk_(1, foot);
872
873 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot) +=
874 ✗ jerk_weight_ * pow(oRh_(1, 0), 2) * pow(alpha_j, 2);
875 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot + 1) +=
876 ✗ jerk_weight_ * pow(oRh_(1, 1), 2) * pow(alpha_j, 2);
877
878 ✗ d->Lxx(12 + 2 * foot, 12 + 2 * foot + 1) +=
879 ✗ jerk_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_j, 2);
880 ✗ d->Lxx(12 + 2 * foot + 1, 12 + 2 * foot) +=
881 ✗ jerk_weight_ * oRh_(1, 0) * oRh_(1, 1) * pow(alpha_j, 2);
882
883 ✗ d->Lxu(12 + 2 * foot, 2 * foot) +=
884 ✗ jerk_weight_ * pow(alpha_j, 2) * pow(oRh_(1, 0), 2);
885 ✗ d->Lxu(12 + 2 * foot, 2 * foot + 1) +=
886 ✗ jerk_weight_ * pow(alpha_j, 2) * oRh_(1, 0) * oRh_(1, 1);
887 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot) +=
888 ✗ jerk_weight_ * pow(alpha_j, 2) * oRh_(1, 0) * oRh_(1, 1);
889 ✗ d->Lxu(12 + 2 * foot + 1, 2 * foot + 1) +=
890 ✗ jerk_weight_ * pow(alpha_j, 2) * pow(oRh_(1, 1), 2);
891 }
892 }
893 }
894
895 // Dynamic derivatives
896 ✗ d->Fx.setIdentity();
897 ✗ d->Fu.block(12, 0, 8, 8) = B;
898 }
899
900 template <typename Scalar>
901 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
902 ✗ ActionModelQuadrupedStepTpl<Scalar>::createData() {
903 ✗ return boost::make_shared<ActionDataQuadrupedStepTpl<Scalar> >(this);
904 }
905
906 ////////////////////////////////
907 // get & set parameters ////////
908 ////////////////////////////////
909
910 template <typename Scalar>
911 const typename Eigen::Matrix<Scalar, 12, 1>&
912 ✗ ActionModelQuadrupedStepTpl<Scalar>::get_state_weights() const {
913 ✗ return state_weights_;
914 }
915 template <typename Scalar>
916 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_state_weights(
917 const typename MathBase::VectorXs& weights) {
918 ✗ if (static_cast<std::size_t>(weights.size()) != 12) {
919 ✗ throw_pretty("Invalid argument: "
920 << "Weights vector has wrong dimension (it should be 12)");
921 }
922 ✗ state_weights_ = weights;
923 }
924
925 template <typename Scalar>
926 const typename Eigen::Matrix<Scalar, 8, 1>&
927 ✗ ActionModelQuadrupedStepTpl<Scalar>::get_step_weights() const {
928 ✗ return step_weights_;
929 }
930 template <typename Scalar>
931 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_step_weights(
932 const typename MathBase::VectorXs& weights) {
933 ✗ if (static_cast<std::size_t>(weights.size()) != 8) {
934 ✗ throw_pretty("Invalid argument: "
935 << "Weights vector has wrong dimension (it should be 4)");
936 }
937 ✗ step_weights_ = weights;
938 }
939
940 template <typename Scalar>
941 const typename Eigen::Matrix<Scalar, 8, 1>&
942 ✗ ActionModelQuadrupedStepTpl<Scalar>::get_heuristic_weights() const {
943 ✗ return heuristic_weights_;
944 }
945 template <typename Scalar>
946 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_heuristic_weights(
947 const typename MathBase::VectorXs& weights) {
948 ✗ if (static_cast<std::size_t>(weights.size()) != 8) {
949 ✗ throw_pretty("Invalid argument: "
950 << "Weights vector has wrong dimension (it should be 8)");
951 }
952 ✗ heuristic_weights_ = weights;
953 }
954
955 template <typename Scalar>
956 ✗ const bool& ActionModelQuadrupedStepTpl<Scalar>::get_symmetry_term() const {
957 ✗ return symmetry_term;
958 }
959 template <typename Scalar>
960 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_symmetry_term(
961 const bool& sym_term) {
962 // The model need to be updated after this changed
963 ✗ symmetry_term = sym_term;
964 }
965
966 template <typename Scalar>
967 ✗ const bool& ActionModelQuadrupedStepTpl<Scalar>::get_centrifugal_term() const {
968 ✗ return centrifugal_term;
969 }
970 template <typename Scalar>
971 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_centrifugal_term(
972 const bool& cent_term) {
973 // The model need to be updated after this changed
974 ✗ centrifugal_term = cent_term;
975 }
976
977 template <typename Scalar>
978 ✗ const Scalar& ActionModelQuadrupedStepTpl<Scalar>::get_T_gait() const {
979 // The model need to be updated after this changed
980 ✗ return T_gait;
981 }
982 template <typename Scalar>
983 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_T_gait(const Scalar& T_gait_) {
984 // The model need to be updated after this changed
985 ✗ T_gait = T_gait_;
986 }
987
988 template <typename Scalar>
989 ✗ const bool& ActionModelQuadrupedStepTpl<Scalar>::get_acc_activated() const {
990 ✗ return is_acc_activated_;
991 }
992 template <typename Scalar>
993 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_acc_activated(
994 const bool& is_activated) {
995 ✗ is_acc_activated_ = is_activated;
996 }
997
998 template <typename Scalar>
999 const typename Eigen::Matrix<Scalar, 2, 1>&
1000 ✗ ActionModelQuadrupedStepTpl<Scalar>::get_acc_lim() const {
1001 ✗ return acc_lim_;
1002 }
1003 template <typename Scalar>
1004 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_acc_lim(
1005 const typename MathBase::VectorXs& acceleration_lim_) {
1006 ✗ if (static_cast<std::size_t>(acceleration_lim_.size()) != 2) {
1007 ✗ throw_pretty("Invalid argument: "
1008 << "Acceleration limit vector [ax_max, ay_max] has wrong "
1009 "dimension (it should be 2)");
1010 }
1011 ✗ acc_lim_ = acceleration_lim_;
1012 }
1013
1014 template <typename Scalar>
1015 ✗ const Scalar& ActionModelQuadrupedStepTpl<Scalar>::get_acc_weight() const {
1016 ✗ return acc_weight_;
1017 }
1018 template <typename Scalar>
1019 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_acc_weight(
1020 const Scalar& weight_) {
1021 ✗ acc_weight_ = weight_;
1022 }
1023
1024 template <typename Scalar>
1025 ✗ const bool& ActionModelQuadrupedStepTpl<Scalar>::get_vel_activated() const {
1026 ✗ return is_vel_activated_;
1027 }
1028 template <typename Scalar>
1029 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_vel_activated(
1030 const bool& is_activated) {
1031 ✗ is_vel_activated_ = is_activated;
1032 }
1033
1034 template <typename Scalar>
1035 const typename Eigen::Matrix<Scalar, 2, 1>&
1036 ✗ ActionModelQuadrupedStepTpl<Scalar>::get_vel_lim() const {
1037 ✗ return vel_lim_;
1038 }
1039 template <typename Scalar>
1040 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_vel_lim(
1041 const typename MathBase::VectorXs& velocity_lim_) {
1042 ✗ if (static_cast<std::size_t>(velocity_lim_.size()) != 2) {
1043 ✗ throw_pretty("Invalid argument: "
1044 << "Velocity limit vector [vx_max, vy_max] has wrong "
1045 "dimension (it should be 2)");
1046 }
1047 ✗ vel_lim_ = velocity_lim_;
1048 }
1049
1050 template <typename Scalar>
1051 ✗ const Scalar& ActionModelQuadrupedStepTpl<Scalar>::get_vel_weight() const {
1052 ✗ return vel_weight_;
1053 }
1054 template <typename Scalar>
1055 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_vel_weight(
1056 const Scalar& weight_) {
1057 ✗ vel_weight_ = weight_;
1058 }
1059
1060 template <typename Scalar>
1061 ✗ const Scalar& ActionModelQuadrupedStepTpl<Scalar>::get_jerk_weight() const {
1062 ✗ return jerk_weight_;
1063 }
1064 template <typename Scalar>
1065 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_jerk_weight(
1066 const Scalar& weight_) {
1067 ✗ jerk_weight_ = weight_;
1068 }
1069
1070 template <typename Scalar>
1071 ✗ const bool& ActionModelQuadrupedStepTpl<Scalar>::get_jerk_activated() const {
1072 ✗ return is_jerk_activated_;
1073 }
1074 template <typename Scalar>
1075 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_jerk_activated(
1076 const bool& is_activated) {
1077 ✗ is_jerk_activated_ = is_activated;
1078 }
1079
1080 template <typename Scalar>
1081 ✗ void ActionModelQuadrupedStepTpl<Scalar>::set_sample_feet_traj(
1082 const int& n_sample) {
1083 ✗ N_sampling = n_sample;
1084
1085 // Acceleration cost
1086 ✗ delta_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1087 ✗ N_sampling - 1, 4);
1088 ✗ gamma_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1089 ✗ N_sampling - 1, 3);
1090 ✗ for (int k = 1; k < N_sampling; k++) {
1091 ✗ delta_(k - 1, 0) =
1092 ✗ (float)k / (float)N_sampling; // [1/N, 2/N, ... , (N-1)/N]
1093 }
1094 ✗ delta_.col(1) << delta_.col(0).pow(2);
1095 ✗ delta_.col(2) << delta_.col(0).pow(3);
1096 ✗ delta_.col(3) << delta_.col(0).pow(4);
1097
1098 ✗ gamma_.col(0) =
1099 ✗ 60 * delta_.col(0) - 180 * delta_.col(1) + 120 * delta_.col(2);
1100 ✗ gamma_.col(1) = -36 * delta_.col(0) + 96 * delta_.col(1) - 60 * delta_.col(2);
1101 ✗ gamma_.col(2) = -9 * delta_.col(0) + 18 * delta_.col(1) - 10 * delta_.col(2);
1102
1103 ✗ alpha_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1104 ✗ N_sampling - 1, 1); // Common for 4 feet
1105 ✗ beta_x_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1106 ✗ N_sampling - 1, 4); // Depends on ao,vo of feet
1107 ✗ beta_y_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1108 ✗ N_sampling - 1, 4); // Depends on ao,vo of feet
1109 ✗ tmp_ones_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Ones(
1110 ✗ N_sampling - 1, 1);
1111
1112 ✗ rb_accx_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1113 ✗ N_sampling - 1, 8);
1114 ✗ rb_accy_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1115 ✗ N_sampling - 1, 8);
1116 ✗ rb_accx_max_bool_ =
1117 ✗ Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
1118 8);
1119 ✗ rb_accy_max_bool_ =
1120 ✗ Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
1121 8);
1122
1123 // Velocity cost
1124 ✗ gamma_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1125 ✗ N_sampling - 1, 4);
1126 ✗ gamma_v.col(0) = 30 * delta_.col(1) - 60 * delta_.col(2) + 30 * delta_.col(3);
1127 ✗ gamma_v.col(1) = delta_.col(0);
1128 ✗ gamma_v.col(2) =
1129 ✗ -18 * delta_.col(1) + 32 * delta_.col(2) - 15 * delta_.col(3);
1130 ✗ gamma_v.col(3) =
1131 ✗ -4.5 * delta_.col(1) + 6 * delta_.col(2) - 2.5 * delta_.col(3);
1132
1133 ✗ alpha_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1134 ✗ N_sampling - 1, 1); // Common for 4 feet
1135 ✗ beta_x_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1136 ✗ N_sampling - 1, 4); // Depends on a0_x, v0_x of feet
1137 ✗ beta_y_v = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1138 ✗ N_sampling - 1, 4); // Depends on a0_y, v0_y of feet
1139
1140 ✗ rb_velx_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1141 ✗ N_sampling - 1, 8);
1142 ✗ rb_vely_max_ = Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(
1143 ✗ N_sampling - 1, 8);
1144 ✗ rb_velx_max_bool_ =
1145 ✗ Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
1146 8);
1147 ✗ rb_vely_max_bool_ =
1148 ✗ Eigen::Array<Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(N_sampling - 1,
1149 8);
1150 }
1151
1152 ////////////////////////
1153 // Update current model
1154 ////////////////////////
1155
1156 template <typename Scalar>
1157 ✗ void ActionModelQuadrupedStepTpl<Scalar>::update_model(
1158 const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
1159 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
1160 const Eigen::Ref<const typename MathBase::VectorXs>& S,
1161 const Eigen::Ref<const typename MathBase::MatrixXs>& position,
1162 const Eigen::Ref<const typename MathBase::MatrixXs>& velocity,
1163 const Eigen::Ref<const typename MathBase::MatrixXs>& acceleration,
1164 const Eigen::Ref<const typename MathBase::MatrixXs>& jerk,
1165 const Eigen::Ref<const typename MathBase::MatrixXs>& oRh,
1166 const Eigen::Ref<const typename MathBase::MatrixXs>& oTh,
1167 const Scalar& delta_T) {
1168 ✗ if (static_cast<std::size_t>(l_feet.size()) != 12) {
1169 ✗ throw_pretty("Invalid argument: "
1170 << "l_feet matrix has wrong dimension (it should be : 3x4)");
1171 }
1172 ✗ if (static_cast<std::size_t>(xref.size()) != 12) {
1173 ✗ throw_pretty("Invalid argument: "
1174 << "Weights vector has wrong dimension (it should be " +
1175 std::to_string(state_->get_nx()) + ")");
1176 }
1177 ✗ if (static_cast<std::size_t>(S.size()) != 4) {
1178 ✗ throw_pretty("Invalid argument: "
1179 << "S vector has wrong dimension (it should be 4x1)");
1180 }
1181
1182 // Velocity : [[vx_0, vx_1, vx_2, vx_3],
1183 // [vy_0, vy_1, vy_2, vy_3],
1184 // [vz_0, vz_1, vz_2, vz_3]]
1185
1186 ✗ xref_ = xref;
1187 ✗ S_ = S;
1188 ✗ position_ = position;
1189 ✗ oRh_ = oRh;
1190 ✗ oTh_ = oTh;
1191 ✗ jerk_ = jerk;
1192
1193 /* R_tmp << cos(xref(5, 0)), -sin(xref(5, 0)), Scalar(0), sin(xref(5, 0)),
1194 cos(xref(5, 0)), Scalar(0), Scalar(0), Scalar(0), Scalar(1.0);
1195
1196 // Centrifual term
1197 pcentrifugal_tmp_1 = xref.block(6, 0, 3, 1);
1198 pcentrifugal_tmp_2 = xref.block(9, 0, 3, 1);
1199 pcentrifugal_tmp = 0.5 * std::sqrt(xref(2, 0) / 9.81) *
1200 pcentrifugal_tmp_1.cross(pcentrifugal_tmp_2);
1201
1202 for (int i = 0; i < 4; i = i + 1) {
1203 pshoulder_tmp.block(0, i, 2, 1) =
1204 R_tmp.block(0, 0, 2, 2) *
1205 (pshoulder_0.block(0, i, 2, 1) + symmetry_term * 0.25 * T_gait *
1206 xref.block(6, 0, 2, 1) + centrifugal_term * pcentrifugal_tmp.block(0, 0, 2,
1207 1)); pshoulder_[2 * i] = pshoulder_tmp(0, i) + xref(0, 0); pshoulder_[2 * i +
1208 1] = pshoulder_tmp(1, i) + xref(1, 0);
1209 } */
1210
1211 ✗ for (int i = 0; i < 4; i = i + 1) {
1212 ✗ pheuristic_[2 * i] = l_feet(0, i);
1213 ✗ pheuristic_[2 * i + 1] = l_feet(1, i);
1214 }
1215
1216 /* std::cout << pshoulder_ << std::endl; */
1217
1218 ✗ B.setZero();
1219
1220 ✗ if (S[0] == Scalar(1)) {
1221 ✗ B.block(0, 0, 2, 2).setIdentity();
1222 }
1223 ✗ if (S[1] == Scalar(1)) {
1224 ✗ B.block(2, 2, 2, 2).setIdentity();
1225 }
1226 ✗ if (S[2] == Scalar(1)) {
1227 ✗ B.block(4, 4, 2, 2).setIdentity();
1228 }
1229 ✗ if (S[3] == Scalar(1)) {
1230 ✗ B.block(6, 6, 2, 2).setIdentity();
1231 }
1232
1233 ✗ alpha_ = (1 / pow(delta_T, 2)) * gamma_.col(0);
1234 ✗ alpha_j = (60 / pow(delta_T, 3));
1235 ✗ alpha_v = (1 / delta_T) * gamma_v.col(0);
1236
1237 ✗ for (int i = 0; i < 4; i++) {
1238 ✗ if (S[i] == Scalar(1) && is_acc_activated_) {
1239 ✗ beta_x_.col(i) = acceleration(0, i) * tmp_ones_ +
1240 ✗ (velocity(0, i) / delta_T) * gamma_.col(1) +
1241 ✗ acceleration(0, i) * gamma_.col(2);
1242 ✗ beta_y_.col(i) = acceleration(1, i) * tmp_ones_ +
1243 ✗ (velocity(1, i) / delta_T) * gamma_.col(1) +
1244 ✗ acceleration(1, i) * gamma_.col(2);
1245 } else {
1246 ✗ beta_x_.col(i).setZero();
1247 ✗ beta_y_.col(i).setZero();
1248 }
1249
1250 ✗ if (S[i] == Scalar(1) && is_vel_activated_) {
1251 ✗ beta_x_v.col(i) = velocity(0, i) * tmp_ones_ +
1252 ✗ (acceleration(0, i) * delta_T) * gamma_v.col(1) +
1253 ✗ velocity(0, i) * gamma_.col(2) +
1254 ✗ (acceleration(0, i) * delta_T) * gamma_v.col(3);
1255 ✗ beta_y_v.col(i) = velocity(1, i) * tmp_ones_ +
1256 ✗ (acceleration(1, i) * delta_T) * gamma_v.col(1) +
1257 ✗ velocity(1, i) * gamma_.col(2) +
1258 ✗ (acceleration(1, i) * delta_T) * gamma_v.col(3);
1259 } else {
1260 ✗ beta_x_v.col(i).setZero();
1261 ✗ beta_y_v.col(i).setZero();
1262 }
1263
1264 ✗ if (S[i] == Scalar(1) && is_jerk_activated_) {
1265 ✗ beta_j(0, i) = -(36 * velocity(0, i)) / pow(delta_T, 2) -
1266 ✗ (9 * acceleration(0, i)) / delta_T;
1267 ✗ beta_j(1, i) = -(36 * velocity(1, i)) / pow(delta_T, 2) -
1268 ✗ (9 * acceleration(1, i)) / delta_T;
1269 } else {
1270 ✗ beta_j.col(i).setZero();
1271 }
1272 }
1273 }
1274 } // namespace quadruped_walkgen
1275
1276 #endif
1277