quadruped_augmented.hxx
Go to the documentation of this file.
1#ifndef __quadruped_walkgen_quadruped_augmented_hxx__
2#define __quadruped_walkgen_quadruped_augmented_hxx__
3
4#include "crocoddyl/core/utils/exception.hpp"
5
6namespace quadruped_walkgen {
7template <typename Scalar>
9 typename Eigen::Matrix<Scalar, 3, 1> offset_CoM)
10 : crocoddyl::ActionModelAbstractTpl<Scalar>(
11 boost::make_shared<crocoddyl::StateVectorTpl<Scalar> >(20), 12, 32) {
12 // Relative forces to compute the norm mof the command
13 relative_forces = true;
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_in_contact = Scalar(25.0);
22
23 // Matrix model initialization
24 g.setZero();
25 g[8] = Scalar(-9.81) * dt_;
26 gI.setZero();
27 gI.diagonal() << Scalar(0.00578574), Scalar(0.01938108), Scalar(0.02476124);
28 A.setIdentity();
29 A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
30 B.setZero();
31 lever_arms.setZero();
32 R.setZero();
33
34 // Weight vectors initialization
35 force_weights_.setConstant(Scalar(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 heuristic_weights_.setConstant(Scalar(1));
41 stop_weights_.setConstant(Scalar(1));
42 // pshoulder_ << Scalar(0.1946), Scalar(0.15005), Scalar(0.1946),
43 // Scalar(-0.15005), Scalar(-0.1946), Scalar(0.15005),
44 // Scalar(-0.1946), Scalar(-0.15005);
45 pshoulder_0 << Scalar(0.18), Scalar(0.18), Scalar(-0.21), Scalar(-0.21),
46 Scalar(0.14695), Scalar(-0.14695), Scalar(0.14695), Scalar(-0.14695);
47 // pshoulder_tmp.setZero();
48 // pcentrifugal_tmp_1.setZero();
49 // pcentrifugal_tmp_2.setZero();
50 // pcentrifugal_tmp.setZero();
51 // UpperBound vector
52 ub.setZero();
53 for (int i = 0; i < 4; i = i + 1) {
54 ub(6 * i + 5) = max_fz_in_contact;
55 }
56
57 // Temporary vector used
58 Fa_x_u.setZero();
59 rub_.setZero();
60 rub_max_.setZero();
61 Arr.setZero();
62 r.setZero();
63 lever_tmp.setZero();
64 R_tmp.setZero();
65 gait.setZero();
66 base_vector_x << Scalar(1.), Scalar(0.), Scalar(0.);
67 base_vector_y << Scalar(0.), Scalar(1.), Scalar(0.);
68 base_vector_z << Scalar(0.), Scalar(0.), Scalar(1.);
69 forces_3d.setZero();
70 gait_double.setZero();
71
72 // bool to add heuristic for foot position
73 centrifugal_term = true;
74 symmetry_term = true;
75 T_gait = Scalar(0.48);
76
77 // // Used for shoulder height weight
78 // pshoulder_0 << Scalar(0.1946) , Scalar(0.1946) , Scalar(-0.1946),
79 // Scalar(-0.1946) ,
80 // Scalar(0.15005) , Scalar(-0.15005) , Scalar(0.15005) ,
81 // Scalar(-0.15005) ;
82 sh_hlim = Scalar(0.27);
83 sh_weight.setConstant(Scalar(1.));
84 sh_ub_max_.setZero();
85 psh.setZero();
86 pheuristic_.setZero();
87 offset_com = offset_CoM; // x, y, z offset
88
89 shoulder_reference_position = false; // Using predicted trajectory of the CoM
90}
91
92template <typename Scalar>
94
95template <typename Scalar>
97 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
98 const Eigen::Ref<const typename MathBase::VectorXs>& x,
99 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
100 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
101 throw_pretty("Invalid argument: "
102 << "x has wrong dimension (it should be " +
103 std::to_string(state_->get_nx()) + ")");
104 }
105 if (static_cast<std::size_t>(u.size()) != nu_) {
106 throw_pretty("Invalid argument: "
107 << "u has wrong dimension (it should be " +
108 std::to_string(nu_) + ")");
109 }
110
112 static_cast<ActionDataQuadrupedAugmentedTpl<Scalar>*>(data.get());
113
114 // Update B :
115 for (int i = 0; i < 4; i = i + 1) {
116 lever_tmp.setZero();
117 if (gait(i, 0) != 0) {
118 lever_tmp.head(2) = x.block(12 + 2 * i, 0, 2, 1);
119 lever_tmp += -x.block(0, 0, 3, 1);
120 R_tmp << Scalar(0.0), -lever_tmp[2], lever_tmp[1], lever_tmp[2],
121 Scalar(0.0), -lever_tmp[0], -lever_tmp[1], lever_tmp[0], Scalar(0.0);
122 B.block(9, 3 * i, 3, 3) << dt_ * R * R_tmp;
123
124 // Compute pdistance of the shoulder wrt contact point
125 if (shoulder_reference_position) {
126 // Ref vector as reference for the shoulder trajectory, roll and pitch
127 // at first
128 psh.block(0, i, 3, 1) << xref_(0, 0) - offset_com(0, 0) +
129 pshoulder_0(0, i) * cos(xref_(5, 0)) -
130 pshoulder_0(1, i) * sin(xref_(5, 0)) -
131 x(12 + 2 * i),
132 xref_(1, 0) - offset_com(1, 0) +
133 pshoulder_0(0, i) * sin(xref_(5, 0)) +
134 pshoulder_0(1, i) * cos(xref_(5, 0)) - x(12 + 2 * i + 1),
135 xref_(2, 0) - offset_com(2, 0);
136 } else {
137 // psh.block(0, i, 3, 1) << x(0) + pshoulder_0(0, i) - pshoulder_0(1, i)
138 // * x(5) - x(12 + 2 * i),
139 // x(1) + pshoulder_0(1, i) + pshoulder_0(0, i)
140 // * x(5) - x(12 + 2 * i + 1), x(2) -
141 // offset_com + pshoulder_0(1, i) * x(3) -
142 // pshoulder_0(0, i) * x(4);
143 // Correction, no approximation for yaw
144 psh.block(0, i, 3, 1)
145 << x(0) - offset_com(0, 0) + pshoulder_0(0, i) * cos(x(5)) -
146 pshoulder_0(1, i) * sin(x(5)) - x(12 + 2 * i),
147 x(1) - offset_com(1, 0) + pshoulder_0(0, i) * sin(x(5)) +
148 pshoulder_0(1, i) * cos(x(5)) - x(12 + 2 * i + 1),
149 x(2) - offset_com(2, 0) + pshoulder_0(1, i) * x(3) -
150 pshoulder_0(0, i) * x(4);
151 }
152
153 } else {
154 // Compute pdistance of the shoulder wrt contact point
155 psh.block(0, i, 3, 1).setZero();
156 }
157 };
158
159 // Discrete dynamic : A*x + B*u + g
160 d->xnext.template head<12>() =
161 A.diagonal().cwiseProduct(x.block(0, 0, 12, 1)) + g;
162 d->xnext.template head<6>() =
163 d->xnext.template head<6>() +
164 A.topRightCorner(6, 6).diagonal().cwiseProduct(x.block(6, 0, 6, 1));
165 d->xnext.template segment<6>(6) =
166 d->xnext.template segment<6>(6) + B.block(6, 0, 6, 12) * u;
167 d->xnext.template tail<8>() = x.tail(8);
168
169 // Residual cost on the state and force norm
170 d->r.template head<12>() = state_weights_.cwiseProduct(x.head(12) - xref_);
171 d->r.template segment<8>(12) =
172 ((heuristic_weights_.cwiseProduct(x.tail(8) - pheuristic_)).array() *
173 gait_double.array())
174 .matrix();
175 d->r.template tail<12>() = force_weights_.cwiseProduct(u - uref_);
176
177 // Friction cone
178 for (int i = 0; i < 4; i = i + 1) {
179 Fa_x_u.segment(6 * i, 6) << u(3 * i) - mu * u(3 * i + 2),
180 -u(3 * i) - mu * u(3 * i + 2), u(3 * i + 1) - mu * u(3 * i + 2),
181 -u(3 * i + 1) - mu * u(3 * i + 2), -u(3 * i + 2), u(3 * i + 2);
182 }
183 rub_max_ = (Fa_x_u - ub).cwiseMax(Scalar(0.));
184
185 // Shoulder height weight
186 sh_ub_max_ << Scalar(0.5) * sh_weight(0) *
187 (psh.block(0, 0, 3, 1).squaredNorm() - sh_hlim * sh_hlim),
188 Scalar(0.5) * sh_weight(1) *
189 (psh.block(0, 1, 3, 1).squaredNorm() - sh_hlim * sh_hlim),
190 Scalar(0.5) * sh_weight(2) *
191 (psh.block(0, 2, 3, 1).squaredNorm() - sh_hlim * sh_hlim),
192 Scalar(0.5) * sh_weight(3) *
193 (psh.block(0, 3, 3, 1).squaredNorm() - sh_hlim * sh_hlim);
194
195 sh_ub_max_ = sh_ub_max_.cwiseMax(Scalar(0.));
196
197 // Cost computation
198 // d->cost = Scalar(0.5) * d->r.transpose() * d->r + friction_weight_ *
199 // Scalar(0.5) * rub_max_.squaredNorm()
200 // + Scalar(0.5)*( (stop_weights_.cwiseProduct(x.tail(8) - pref_)
201 // ).array() * gait_double.array()
202 // ).matrix().squaredNorm() ;
203
204 d->cost =
205 Scalar(0.5) * d->r.transpose() * d->r +
206 friction_weight_ * Scalar(0.5) * rub_max_.squaredNorm() +
207 Scalar(0.5) * ((stop_weights_.cwiseProduct(x.tail(8) - pstop_)).array() *
208 gait_double.array())
209 .matrix()
210 .squaredNorm() +
211 sh_ub_max_.sum();
212}
213
214template <typename Scalar>
216 const boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >& data,
217 const Eigen::Ref<const typename MathBase::VectorXs>& x,
218 const Eigen::Ref<const typename MathBase::VectorXs>& u) {
219 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
220 throw_pretty("Invalid argument: "
221 << "x has wrong dimension (it should be " +
222 std::to_string(state_->get_nx()) + ")");
223 }
224 if (static_cast<std::size_t>(u.size()) != nu_) {
225 throw_pretty("Invalid argument: "
226 << "u has wrong dimension (it should be " +
227 std::to_string(nu_) + ")");
228 }
229
231 static_cast<ActionDataQuadrupedAugmentedTpl<Scalar>*>(data.get());
232
233 // Cost derivatives : Lx
234 d->Lx.setZero();
235 d->Lx.template head<12>() =
236 (state_weights_.array() * d->r.template head<12>().array()).matrix();
237 d->Lx.template tail<8>() =
238 (heuristic_weights_.array() * d->r.template segment<8>(12).array())
239 .matrix() +
240 ((stop_weights_.cwiseProduct(x.tail(8) - pstop_)).array() *
241 gait_double.array() * stop_weights_.array())
242 .matrix();
243
244 // Cost derivative : Lu
245 for (int i = 0; i < 4; i = i + 1) {
246 r = friction_weight_ * rub_max_.segment(6 * i, 6);
247 d->Lu.block(i * 3, 0, 3, 1) << r(0) - r(1), r(2) - r(3),
248 -mu * (r(0) + r(1) + r(2) + r(3)) - r(4) + r(5);
249 }
250 d->Lu = d->Lu +
251 (force_weights_.array() * d->r.template tail<12>().array()).matrix();
252
253 // Hessian : Lxx
254 // Hessian : Lxx
255 d->Lxx.setZero();
256
257 d->Lxx.diagonal().head(12) =
258 (state_weights_.array() * state_weights_.array()).matrix();
259 d->Lxx.diagonal().tail(8) =
260 (gait_double.array() * heuristic_weights_.array() *
261 heuristic_weights_.array())
262 .matrix();
263 d->Lxx.diagonal().tail(8) +=
264 (gait_double.array() * stop_weights_.array() * stop_weights_.array())
265 .matrix();
266
267 // Shoulder height derivative cost
268 for (int j = 0; j < 4; j = j + 1) {
269 if (sh_ub_max_[j] > Scalar(0.)) {
270 if (shoulder_reference_position) {
271 d->Lx(12 + 2 * j, 0) += -sh_weight(j) * psh(0, j);
272 d->Lx(12 + 2 * j + 1, 0) += -sh_weight(j) * psh(1, j);
273
274 d->Lxx(12 + 2 * j, 12 + 2 * j) += sh_weight(j);
275 d->Lxx(12 + 2 * j + 1, 12 + 2 * j + 1) += sh_weight(j);
276
277 } else {
278 // Approximation of small even for yaw (wrong)
279 // d->Lx(0, 0) += sh_weight(j) * psh(0, j);
280 // d->Lx(1, 0) += sh_weight(j) * psh(1, j);
281 // d->Lx(2, 0) += sh_weight(j) * psh(2, j);
282 // d->Lx(3, 0) += sh_weight(j) * pshoulder_0(1, j) * psh(2, j);
283 // d->Lx(4, 0) += -sh_weight(j) * pshoulder_0(0, j) * psh(2, j);
284 // d->Lx(5, 0) += sh_weight(j) * (-pshoulder_0(1, j) * psh(0, j) +
285 // pshoulder_0(0, j) * psh(1, j));
286
287 // d->Lx(12 + 2 * j, 0) += -sh_weight(j) * psh(0, j);
288 // d->Lx(12 + 2 * j + 1, 0) += -sh_weight(j) * psh(1, j);
289
290 // d->Lxx(0, 0) += sh_weight(j);
291 // d->Lxx(1, 1) += sh_weight(j);
292 // d->Lxx(2, 2) += sh_weight(j);
293 // d->Lxx(3, 3) += sh_weight(j) * pshoulder_0(1, j) * pshoulder_0(1, j);
294 // d->Lxx(3, 3) += sh_weight(j) * pshoulder_0(0, j) * pshoulder_0(0, j);
295 // d->Lxx(5, 5) += sh_weight(j) * (pshoulder_0(1, j) * pshoulder_0(1, j)
296 // + pshoulder_0(0, j) * pshoulder_0(0, j));
297
298 // d->Lxx(12 + 2 * j, 12 + 2 * j) += sh_weight(j);
299 // d->Lxx(12 + 2 * j + 1, 12 + 2 * j + 1) += sh_weight(j);
300
301 // d->Lxx(0, 5) += -sh_weight(j) * pshoulder_0(1, j);
302 // d->Lxx(5, 0) += -sh_weight(j) * pshoulder_0(1, j);
303
304 // d->Lxx(1, 5) += sh_weight(j) * pshoulder_0(0, j);
305 // d->Lxx(5, 1) += sh_weight(j) * pshoulder_0(0, j);
306
307 // d->Lxx(2, 3) += sh_weight(j) * pshoulder_0(1, j);
308 // d->Lxx(2, 4) += -sh_weight(j) * pshoulder_0(0, j);
309 // d->Lxx(3, 2) += sh_weight(j) * pshoulder_0(1, j);
310 // d->Lxx(4, 2) += -sh_weight(j) * pshoulder_0(0, j);
311
312 // d->Lxx(3, 4) += -sh_weight(j) * pshoulder_0(1, j) * pshoulder_0(0,
313 // j); d->Lxx(4, 3) += -sh_weight(j) * pshoulder_0(1, j) *
314 // pshoulder_0(0, j);
315
316 // d->Lxx(0, 12 + 2 * j) += -sh_weight(j);
317 // d->Lxx(12 + 2 * j, 0) += -sh_weight(j);
318
319 // d->Lxx(5, 12 + 2 * j) += sh_weight(j) * pshoulder_0(1, j);
320 // d->Lxx(12 + 2 * j, 5) += sh_weight(j) * pshoulder_0(1, j);
321
322 // d->Lxx(1, 12 + 2 * j + 1) += -sh_weight(j);
323 // d->Lxx(12 + 2 * j + 1, 1) += -sh_weight(j);
324
325 // d->Lxx(5, 12 + 2 * j + 1) += -sh_weight(j) * pshoulder_0(0, j);
326 // d->Lxx(12 + 2 * j + 1, 5) += -sh_weight(j) * pshoulder_0(0, j);
327 d->Lx(0, 0) += sh_weight(j) * psh(0, j);
328 d->Lx(1, 0) += sh_weight(j) * psh(1, j);
329 d->Lx(2, 0) += sh_weight(j) * psh(2, j);
330 d->Lx(3, 0) += sh_weight(j) * pshoulder_0(1, j) * psh(2, j);
331 d->Lx(4, 0) += -sh_weight(j) * pshoulder_0(0, j) * psh(2, j);
332 d->Lx(5, 0) +=
333 sh_weight(j) *
334 ((-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j)) *
335 psh(0, j) +
336 (cos(x(5)) * pshoulder_0(0, j) - sin(x(5)) * pshoulder_0(1, j)) *
337 psh(1, j));
338
339 d->Lx(12 + 2 * j, 0) += -sh_weight(j) * psh(0, j);
340 d->Lx(12 + 2 * j + 1, 0) += -sh_weight(j) * psh(1, j);
341
342 d->Lxx(0, 0) += sh_weight(j);
343 d->Lxx(1, 1) += sh_weight(j);
344 d->Lxx(2, 2) += sh_weight(j);
345 d->Lxx(3, 3) += sh_weight(j) * pshoulder_0(1, j) * pshoulder_0(1, j);
346 d->Lxx(3, 3) += sh_weight(j) * pshoulder_0(0, j) * pshoulder_0(0, j);
347 d->Lxx(5, 5) +=
348 sh_weight(j) *
349 ((-cos(x(5)) * pshoulder_0(0, j) + sin(x(5)) * pshoulder_0(1, j)) *
350 psh(0, j) +
351 (-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j)) *
352 (-sin(x(5)) * pshoulder_0(0, j) -
353 cos(x(5)) * pshoulder_0(1, j)) +
354 (-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j)) *
355 psh(1, j) +
356 (cos(x(5)) * pshoulder_0(0, j) - sin(x(5)) * pshoulder_0(1, j)) *
357 (cos(x(5)) * pshoulder_0(0, j) -
358 sin(x(5)) * pshoulder_0(1, j)));
359
360 d->Lxx(12 + 2 * j, 12 + 2 * j) += sh_weight(j);
361 d->Lxx(12 + 2 * j + 1, 12 + 2 * j + 1) += sh_weight(j);
362
363 d->Lxx(0, 5) += sh_weight(j) * (-sin(x(5)) * pshoulder_0(0, j) -
364 cos(x(5)) * pshoulder_0(1, j));
365 d->Lxx(5, 0) += sh_weight(j) * (-sin(x(5)) * pshoulder_0(0, j) -
366 cos(x(5)) * pshoulder_0(1, j));
367
368 d->Lxx(1, 5) += sh_weight(j) * (cos(x(5)) * pshoulder_0(0, j) -
369 sin(x(5)) * pshoulder_0(1, j));
370 d->Lxx(5, 1) += sh_weight(j) * (cos(x(5)) * pshoulder_0(0, j) -
371 sin(x(5)) * pshoulder_0(1, j));
372
373 d->Lxx(2, 3) += sh_weight(j) * pshoulder_0(1, j);
374 d->Lxx(2, 4) += -sh_weight(j) * pshoulder_0(0, j);
375 d->Lxx(3, 2) += sh_weight(j) * pshoulder_0(1, j);
376 d->Lxx(4, 2) += -sh_weight(j) * pshoulder_0(0, j);
377
378 d->Lxx(3, 4) += -sh_weight(j) * pshoulder_0(1, j) * pshoulder_0(0, j);
379 d->Lxx(4, 3) += -sh_weight(j) * pshoulder_0(1, j) * pshoulder_0(0, j);
380
381 d->Lxx(0, 12 + 2 * j) += -sh_weight(j);
382 d->Lxx(12 + 2 * j, 0) += -sh_weight(j);
383
384 d->Lxx(5, 12 + 2 * j) +=
385 -sh_weight(j) *
386 (-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j));
387 d->Lxx(12 + 2 * j, 5) +=
388 -sh_weight(j) *
389 (-sin(x(5)) * pshoulder_0(0, j) - cos(x(5)) * pshoulder_0(1, j));
390
391 d->Lxx(1, 12 + 2 * j + 1) += -sh_weight(j);
392 d->Lxx(12 + 2 * j + 1, 1) += -sh_weight(j);
393
394 d->Lxx(5, 12 + 2 * j + 1) +=
395 -sh_weight(j) *
396 (cos(x(5)) * pshoulder_0(0, j) - sin(x(5)) * pshoulder_0(1, j));
397 d->Lxx(12 + 2 * j + 1, 5) +=
398 -sh_weight(j) *
399 (cos(x(5)) * pshoulder_0(0, j) - sin(x(5)) * pshoulder_0(1, j));
400 }
401 }
402 }
403
404 // Hessian : Luu
405 // Matrix friction cone hessian (20x12)
406 Arr.diagonal() =
407 ((Fa_x_u - ub).array() >= 0.).matrix().template cast<Scalar>();
408 for (int i = 0; i < 4; i = i + 1) {
409 r = friction_weight_ * Arr.diagonal().segment(6 * i, 6);
410 d->Luu.block(3 * i, 3 * i, 3, 3) << r(0) + r(1), 0.0, mu * (r(1) - r(0)),
411 0.0, r(2) + r(3), mu * (r(3) - r(2)), mu * (r(1) - r(0)),
412 mu * (r(3) - r(2)), mu * mu * (r(0) + r(1) + r(2) + r(3)) + r(4) + r(5);
413 }
414 d->Luu.diagonal() =
415 d->Luu.diagonal() +
416 (force_weights_.array() * force_weights_.array()).matrix();
417
418 // Dynamic derivatives
419 d->Fx.setZero();
420 d->Fx.block(0, 0, 12, 12) << A;
421 d->Fx.block(12, 12, 8, 8) << Eigen::Matrix<Scalar, 8, 8>::Identity();
422
423 for (int i = 0; i < 4; i = i + 1) {
424 if (gait(i, 0) != 0) {
425 forces_3d = u.block(3 * i, 0, 3, 1);
426 d->Fx.block(9, 0, 3, 1) += -dt_ * R * (base_vector_x.cross(forces_3d));
427 d->Fx.block(9, 1, 3, 1) += -dt_ * R * (base_vector_y.cross(forces_3d));
428 d->Fx.block(9, 2, 3, 1) += -dt_ * R * (base_vector_z.cross(forces_3d));
429
430 d->Fx.block(9, 12 + 2 * i, 3, 1) +=
431 dt_ * R * (base_vector_x.cross(forces_3d));
432 d->Fx.block(9, 12 + 2 * i + 1, 3, 1) +=
433 dt_ * R * (base_vector_y.cross(forces_3d));
434 }
435 }
436 // d->Fu << Eigen::Matrix<Scalar, 20, 12>::Zero() ;
437 d->Fu.block(0, 0, 12, 12) << B;
438}
439
440template <typename Scalar>
441boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
443 return boost::make_shared<ActionDataQuadrupedAugmentedTpl<Scalar> >(this);
444}
445
447// get & set parameters ////////
449
450template <typename Scalar>
451const typename Eigen::Matrix<Scalar, 12, 1>&
453 return force_weights_;
454}
455template <typename Scalar>
457 const typename MathBase::VectorXs& weights) {
458 if (static_cast<std::size_t>(weights.size()) != 12) {
459 throw_pretty("Invalid argument: "
460 << "Weights vector has wrong dimension (it should be 12)");
461 }
462 force_weights_ = weights;
463}
464
465template <typename Scalar>
466const typename Eigen::Matrix<Scalar, 12, 1>&
468 return state_weights_;
469}
470template <typename Scalar>
472 const typename MathBase::VectorXs& weights) {
473 if (static_cast<std::size_t>(weights.size()) != 12) {
474 throw_pretty("Invalid argument: "
475 << "Weights vector has wrong dimension (it should be 12)");
476 }
477 state_weights_ = weights;
478}
479
480template <typename Scalar>
481const typename Eigen::Matrix<Scalar, 8, 1>&
483 return heuristic_weights_;
484}
485template <typename Scalar>
487 const typename MathBase::VectorXs& weights) {
488 if (static_cast<std::size_t>(weights.size()) != 8) {
489 throw_pretty("Invalid argument: "
490 << "Weights vector has wrong dimension (it should be 8)");
491 }
492 heuristic_weights_ = weights;
493}
494
495template <typename Scalar>
496const typename Eigen::Matrix<Scalar, 8, 1>&
498 return stop_weights_;
499}
500template <typename Scalar>
502 const typename MathBase::VectorXs& weights) {
503 if (static_cast<std::size_t>(weights.size()) != 8) {
504 throw_pretty("Invalid argument: "
505 << "Weights vector has wrong dimension (it should be 8)");
506 }
507 stop_weights_ = weights;
508}
509
510template <typename Scalar>
512 const {
513 return friction_weight_;
514}
515template <typename Scalar>
517 const Scalar& weight) {
518 friction_weight_ = weight;
519}
520
521template <typename Scalar>
523 return mu;
524}
525template <typename Scalar>
527 mu = mu_coeff;
528}
529
530template <typename Scalar>
532 return mass;
533}
534template <typename Scalar>
536 // The model need to be updated after this changed
537 mass = m;
538}
539
540template <typename Scalar>
542 return dt_;
543}
544template <typename Scalar>
546 // The model need to be updated after this changed
547 dt_ = dt;
548 g[8] = Scalar(-9.81) * dt_;
549 A.topRightCorner(6, 6) << Eigen::Matrix<Scalar, 6, 6>::Identity() * dt_;
550}
551
552template <typename Scalar>
553const typename Eigen::Matrix<Scalar, 3, 3>&
555 return gI;
556}
557template <typename Scalar>
559 const typename MathBase::Matrix3s& inertia_matrix) {
560 // The model need to be updated after this changed
561 if (static_cast<std::size_t>(inertia_matrix.size()) != 9) {
562 throw_pretty("Invalid argument: "
563 << "gI has wrong dimension : 3x3");
564 }
565 gI = inertia_matrix;
566}
567
568template <typename Scalar>
570 const {
571 // The model need to be updated after this changed
572 return min_fz_in_contact;
573}
574template <typename Scalar>
576 const Scalar& min_fz) {
577 // The model need to be updated after this changed
578 min_fz_in_contact = min_fz;
579}
580
581template <typename Scalar>
583 const {
584 // The model need to be updated after this changed
585 return max_fz_in_contact;
586}
587template <typename Scalar>
589 const Scalar& max_fz) {
590 // The model need to be updated after this changed
591 max_fz_in_contact = max_fz;
592 for (int i = 0; i < 4; i = i + 1) {
593 ub(6 * i + 5) = max_fz_in_contact;
594 }
595}
596
597template <typename Scalar>
599 const {
600 return symmetry_term;
601}
602template <typename Scalar>
604 const bool& sym_term) {
605 // The model need to be updated after this changed
606 symmetry_term = sym_term;
607}
608
609template <typename Scalar>
611 const {
612 return centrifugal_term;
613}
614template <typename Scalar>
616 const bool& cent_term) {
617 // The model need to be updated after this changed
618 centrifugal_term = cent_term;
619}
620
621template <typename Scalar>
623 Scalar>::get_shoulder_reference_position() const {
624 return shoulder_reference_position;
625}
626template <typename Scalar>
628 const bool& reference) {
629 shoulder_reference_position = reference;
630}
631
632template <typename Scalar>
634 // The model need to be updated after this changed
635 return T_gait;
636}
637template <typename Scalar>
639 const Scalar& T_gait_) {
640 // The model need to be updated after this changed
641 T_gait = T_gait_;
642}
643
644template <typename Scalar>
646 const {
647 return sh_hlim;
648}
649template <typename Scalar>
651 const Scalar& hlim) {
652 // The model need to be updated after this changed
653 sh_hlim = hlim;
654}
655
656template <typename Scalar>
657const typename Eigen::Matrix<Scalar, 4, 1>&
659 return sh_weight;
660}
661template <typename Scalar>
663 const typename Eigen::Matrix<Scalar, 4, 1>& weight) {
664 // The model need to be updated after this changed
665 sh_weight = weight;
666}
667
671template <typename Scalar>
672const typename Eigen::Matrix<Scalar, 12, 12>&
674 return A;
675}
676template <typename Scalar>
677const typename Eigen::Matrix<Scalar, 12, 12>&
679 return B;
680}
681
682// to modify the cost on the command : || fz - m*g/nb contact ||^2
683// --> set to True
684template <typename Scalar>
686 const {
687 return relative_forces;
688}
689template <typename Scalar>
691 const bool& rel_forces) {
692 relative_forces = rel_forces;
693 uref_.setZero();
694 if (relative_forces) {
695 for (int i = 0; i < 4; i = i + 1) {
696 if (gait[i] == 1) {
697 uref_[3 * i + 2] = (Scalar(9.81) * mass) / (gait.sum());
698 }
699 }
700 }
701}
702
704// Update current model
706
707template <typename Scalar>
709 const Eigen::Ref<const typename MathBase::MatrixXs>& l_feet,
710 const Eigen::Ref<const typename MathBase::MatrixXs>& l_stop,
711 const Eigen::Ref<const typename MathBase::MatrixXs>& xref,
712 const Eigen::Ref<const typename MathBase::MatrixXs>& S) {
713 if (static_cast<std::size_t>(l_feet.size()) != 12) {
714 throw_pretty("Invalid argument: "
715 << "l_feet matrix has wrong dimension (it should be : 3x4)");
716 }
717 if (static_cast<std::size_t>(xref.size()) != 12) {
718 throw_pretty("Invalid argument: "
719 << "xref vector has wrong dimension (it should be 12 )");
720 }
721 if (static_cast<std::size_t>(S.size()) != 4) {
722 throw_pretty("Invalid argument: "
723 << "S vector has wrong dimension (it should be 4x1)");
724 }
725
726 xref_ = xref;
727 gait = S;
728
729 uref_.setZero();
730 if (relative_forces) {
731 for (int i = 0; i < 4; i = i + 1) {
732 if (gait[i] == 1) {
733 uref_[3 * i + 2] = (Scalar(9.81) * mass) / (gait.sum());
734 }
735 }
736 }
737
738 for (int i = 0; i < 4; i = i + 1) {
739 gait_double(2 * i, 0) = gait(i, 0);
740 gait_double(2 * i + 1, 0) = gait(i, 0);
741
742 pheuristic_.block(2 * i, 0, 2, 1) = l_feet.block(0, i, 2, 1);
743 pstop_.block(2 * i, 0, 2, 1) = l_stop.block(0, i, 2, 1);
744 }
745
746 R_tmp << cos(xref(5, 0)), -sin(xref(5, 0)), Scalar(0), sin(xref(5, 0)),
747 cos(xref(5, 0)), Scalar(0), Scalar(0), Scalar(0), Scalar(1.0);
748
749 // Centrifual term
750 // pcentrifugal_tmp_1 = xref.block(6, 0, 3, 1);
751 // pcentrifugal_tmp_2 = xref.block(9, 0, 3, 1);
752 // pcentrifugal_tmp = 0.5 * std::sqrt(xref(2, 0) / 9.81) *
753 // pcentrifugal_tmp_1.cross(pcentrifugal_tmp_2);
754
755 // for (int i = 0; i < 4; i = i + 1) {
756 // pshoulder_tmp.block(0, i, 2, 1) =
757 // R_tmp.block(0, 0, 2, 2) *
758 // (pshoulder_0.block(0, i, 2, 1) + symmetry_term * 0.25 * T_gait *
759 // xref.block(6, 0, 2, 1) +
760 // centrifugal_term * pcentrifugal_tmp.block(0, 0, 2, 1));
761 // }
762
763 R = (R_tmp.transpose() * gI * R_tmp).inverse(); // I_inv
764
765 for (int i = 0; i < 4; i = i + 1) {
766 // pshoulder_[2 * i] = pshoulder_tmp(0, i) + xref(0, 0);
767 // pshoulder_[2 * i + 1] = pshoulder_tmp(1, i) + xref(1, 0);
768
769 if (S(i, 0) != 0) {
770 // set limit for normal force, (foot in contact with the ground)
771 ub[5 * i + 4] = -min_fz_in_contact;
772
773 // B update
774 B.block(6, 3 * i, 3, 3).diagonal() << dt_ / mass, dt_ / mass, dt_ / mass;
775
776 // Assuption 1 : levers arms not depends on the state, but on the
777 // predicted position (xfref)
778 // --> B will be updated with the update_B method for each calc function
779
780 // lever_tmp = lever_arms.block(0,i,3,1) - xref.block(0,0,3,1) ;
781 // R_tmp << 0.0, -lever_tmp[2], lever_tmp[1],
782 // lever_tmp[2], 0.0, -lever_tmp[0], -lever_tmp[1], lever_tmp[0], 0.0 ;
783 // B.block(9 , 3*i , 3,3) << dt_ * R* R_tmp;
784 } else {
785 // set limit for normal force at 0.0
786 ub[5 * i + 4] = Scalar(0.0);
787 B.block(6, 3 * i, 3, 3).setZero();
788 B.block(9, 3 * i, 3, 3).setZero();
789 };
790 };
791}
792} // namespace quadruped_walkgen
793
794#endif
Definition: quadruped_augmented.hpp:14
const bool & get_centrifugal_term() const
Definition: quadruped_augmented.hxx:610
const Scalar & get_max_fz_contact() const
Definition: quadruped_augmented.hxx:582
void set_T_gait(const Scalar &T_gait_)
Definition: quadruped_augmented.hxx:638
const Eigen::Matrix< Scalar, 8, 1 > & get_heuristic_weights() const
Definition: quadruped_augmented.hxx:482
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const typename MathBase::VectorXs > &x, const Eigen::Ref< const typename MathBase::VectorXs > &u)
Definition: quadruped_augmented.hxx:96
void set_heuristic_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:486
const Scalar & get_shoulder_hlim() const
Definition: quadruped_augmented.hxx:645
void set_min_fz_contact(const Scalar &min_fz)
Definition: quadruped_augmented.hxx:575
void set_stop_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:501
const Scalar & get_T_gait() const
Definition: quadruped_augmented.hxx:633
const Eigen::Matrix< Scalar, 12, 12 > & get_A() const
Definition: quadruped_augmented.hxx:673
void set_friction_weight(const Scalar &weight)
Definition: quadruped_augmented.hxx:516
const Scalar & get_min_fz_contact() const
Definition: quadruped_augmented.hxx:569
const Eigen::Matrix< Scalar, 4, 1 > & get_shoulder_contact_weight() const
Definition: quadruped_augmented.hxx:658
~ActionModelQuadrupedAugmentedTpl()
Definition: quadruped_augmented.hxx:93
void update_model(const Eigen::Ref< const typename MathBase::MatrixXs > &l_feet, const Eigen::Ref< const typename MathBase::MatrixXs > &l_stop, const Eigen::Ref< const typename MathBase::MatrixXs > &xref, const Eigen::Ref< const typename MathBase::MatrixXs > &S)
Definition: quadruped_augmented.hxx:708
const Eigen::Matrix< Scalar, 12, 1 > & get_state_weights() const
Definition: quadruped_augmented.hxx:467
const Scalar & get_friction_weight() const
Definition: quadruped_augmented.hxx:511
const Scalar & get_mu() const
Definition: quadruped_augmented.hxx:522
void set_max_fz_contact(const Scalar &max_fz)
Definition: quadruped_augmented.hxx:588
void set_relative_forces(const bool &rel_forces)
Definition: quadruped_augmented.hxx:690
void set_gI(const typename MathBase::Matrix3s &inertia_matrix)
Definition: quadruped_augmented.hxx:558
const Scalar & get_dt() const
Definition: quadruped_augmented.hxx:541
virtual boost::shared_ptr< ActionDataAbstract > createData()
Definition: quadruped_augmented.hxx:442
void set_centrifugal_term(const bool &cent_term)
Definition: quadruped_augmented.hxx:615
void set_dt(const Scalar &dt)
Definition: quadruped_augmented.hxx:545
const bool & get_symmetry_term() const
Definition: quadruped_augmented.hxx:598
const Eigen::Matrix< Scalar, 3, 3 > & get_gI() const
Definition: quadruped_augmented.hxx:554
void set_shoulder_hlim(const Scalar &hlim)
Definition: quadruped_augmented.hxx:650
void set_force_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:456
void set_mu(const Scalar &mu_coeff)
Definition: quadruped_augmented.hxx:526
void set_state_weights(const typename MathBase::VectorXs &weights)
Definition: quadruped_augmented.hxx:471
_Scalar Scalar
Definition: quadruped_augmented.hpp:16
ActionModelQuadrupedAugmentedTpl(typename Eigen::Matrix< Scalar, 3, 1 > offset_CoM=Eigen::Matrix< Scalar, 3, 1 >::Zero())
Definition: quadruped_augmented.hxx:8
void set_mass(const Scalar &m)
Definition: quadruped_augmented.hxx:535
void set_symmetry_term(const bool &sym_term)
Definition: quadruped_augmented.hxx:603
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const typename MathBase::VectorXs > &x, const Eigen::Ref< const typename MathBase::VectorXs > &u)
Definition: quadruped_augmented.hxx:215
void set_shoulder_contact_weight(const typename Eigen::Matrix< Scalar, 4, 1 > &weight)
Definition: quadruped_augmented.hxx:662
const Eigen::Matrix< Scalar, 12, 12 > & get_B() const
Definition: quadruped_augmented.hxx:678
void set_shoulder_reference_position(const bool &reference)
Definition: quadruped_augmented.hxx:627
const Eigen::Matrix< Scalar, 12, 1 > & get_force_weights() const
Definition: quadruped_augmented.hxx:452
const bool & get_relative_forces() const
Definition: quadruped_augmented.hxx:685
const Scalar & get_mass() const
Definition: quadruped_augmented.hxx:531
const Eigen::Matrix< Scalar, 8, 1 > & get_stop_weights() const
Definition: quadruped_augmented.hxx:497
Definition: quadruped.hpp:11
Definition: quadruped_augmented.hpp:183