GCC Code Coverage Report

Directory: ./
File: include/quadruped-walkgen/quadruped_augmented.hxx
Date: 2024-12-02 00:24:10
Exec Total Coverage
Lines: 0 365 0.0%
Branches: 0 1032 0.0%
Line Branch Exec Source
1 #ifndef __quadruped_walkgen_quadruped_augmented_hxx__
2 #define __quadruped_walkgen_quadruped_augmented_hxx__
3
4 #include "crocoddyl/core/utils/exception.hpp"
5
6 namespace quadruped_walkgen {
7 template <typename Scalar>
8 ✗ ActionModelQuadrupedAugmentedTpl<Scalar>::ActionModelQuadrupedAugmentedTpl(
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
92 template <typename Scalar>
93 ✗ ActionModelQuadrupedAugmentedTpl<Scalar>::~ActionModelQuadrupedAugmentedTpl() {}
94
95 template <typename Scalar>
96 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::calc(
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
111 ActionDataQuadrupedAugmentedTpl<Scalar>* d =
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
214 template <typename Scalar>
215 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::calcDiff(
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
230 ActionDataQuadrupedAugmentedTpl<Scalar>* d =
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
440 template <typename Scalar>
441 boost::shared_ptr<crocoddyl::ActionDataAbstractTpl<Scalar> >
442 ✗ ActionModelQuadrupedAugmentedTpl<Scalar>::createData() {
443 ✗ return boost::make_shared<ActionDataQuadrupedAugmentedTpl<Scalar> >(this);
444 }
445
446 ////////////////////////////////
447 // get & set parameters ////////
448 ////////////////////////////////
449
450 template <typename Scalar>
451 const typename Eigen::Matrix<Scalar, 12, 1>&
452 ✗ ActionModelQuadrupedAugmentedTpl<Scalar>::get_force_weights() const {
453 ✗ return force_weights_;
454 }
455 template <typename Scalar>
456 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_force_weights(
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
465 template <typename Scalar>
466 const typename Eigen::Matrix<Scalar, 12, 1>&
467 ✗ ActionModelQuadrupedAugmentedTpl<Scalar>::get_state_weights() const {
468 ✗ return state_weights_;
469 }
470 template <typename Scalar>
471 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_state_weights(
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
480 template <typename Scalar>
481 const typename Eigen::Matrix<Scalar, 8, 1>&
482 ✗ ActionModelQuadrupedAugmentedTpl<Scalar>::get_heuristic_weights() const {
483 ✗ return heuristic_weights_;
484 }
485 template <typename Scalar>
486 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_heuristic_weights(
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
495 template <typename Scalar>
496 const typename Eigen::Matrix<Scalar, 8, 1>&
497 ✗ ActionModelQuadrupedAugmentedTpl<Scalar>::get_stop_weights() const {
498 ✗ return stop_weights_;
499 }
500 template <typename Scalar>
501 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_stop_weights(
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
510 template <typename Scalar>
511 ✗ const Scalar& ActionModelQuadrupedAugmentedTpl<Scalar>::get_friction_weight()
512 const {
513 ✗ return friction_weight_;
514 }
515 template <typename Scalar>
516 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_friction_weight(
517 const Scalar& weight) {
518 ✗ friction_weight_ = weight;
519 }
520
521 template <typename Scalar>
522 ✗ const Scalar& ActionModelQuadrupedAugmentedTpl<Scalar>::get_mu() const {
523 ✗ return mu;
524 }
525 template <typename Scalar>
526 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_mu(const Scalar& mu_coeff) {
527 ✗ mu = mu_coeff;
528 }
529
530 template <typename Scalar>
531 ✗ const Scalar& ActionModelQuadrupedAugmentedTpl<Scalar>::get_mass() const {
532 ✗ return mass;
533 }
534 template <typename Scalar>
535 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_mass(const Scalar& m) {
536 // The model need to be updated after this changed
537 ✗ mass = m;
538 }
539
540 template <typename Scalar>
541 ✗ const Scalar& ActionModelQuadrupedAugmentedTpl<Scalar>::get_dt() const {
542 ✗ return dt_;
543 }
544 template <typename Scalar>
545 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_dt(const Scalar& dt) {
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
552 template <typename Scalar>
553 const typename Eigen::Matrix<Scalar, 3, 3>&
554 ✗ ActionModelQuadrupedAugmentedTpl<Scalar>::get_gI() const {
555 ✗ return gI;
556 }
557 template <typename Scalar>
558 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_gI(
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
568 template <typename Scalar>
569 ✗ const Scalar& ActionModelQuadrupedAugmentedTpl<Scalar>::get_min_fz_contact()
570 const {
571 // The model need to be updated after this changed
572 ✗ return min_fz_in_contact;
573 }
574 template <typename Scalar>
575 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_min_fz_contact(
576 const Scalar& min_fz) {
577 // The model need to be updated after this changed
578 ✗ min_fz_in_contact = min_fz;
579 }
580
581 template <typename Scalar>
582 ✗ const Scalar& ActionModelQuadrupedAugmentedTpl<Scalar>::get_max_fz_contact()
583 const {
584 // The model need to be updated after this changed
585 ✗ return max_fz_in_contact;
586 }
587 template <typename Scalar>
588 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_max_fz_contact(
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
597 template <typename Scalar>
598 ✗ const bool& ActionModelQuadrupedAugmentedTpl<Scalar>::get_symmetry_term()
599 const {
600 ✗ return symmetry_term;
601 }
602 template <typename Scalar>
603 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_symmetry_term(
604 const bool& sym_term) {
605 // The model need to be updated after this changed
606 ✗ symmetry_term = sym_term;
607 }
608
609 template <typename Scalar>
610 ✗ const bool& ActionModelQuadrupedAugmentedTpl<Scalar>::get_centrifugal_term()
611 const {
612 ✗ return centrifugal_term;
613 }
614 template <typename Scalar>
615 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_centrifugal_term(
616 const bool& cent_term) {
617 // The model need to be updated after this changed
618 ✗ centrifugal_term = cent_term;
619 }
620
621 template <typename Scalar>
622 ✗ const bool& ActionModelQuadrupedAugmentedTpl<
623 Scalar>::get_shoulder_reference_position() const {
624 ✗ return shoulder_reference_position;
625 }
626 template <typename Scalar>
627 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_shoulder_reference_position(
628 const bool& reference) {
629 ✗ shoulder_reference_position = reference;
630 }
631
632 template <typename Scalar>
633 ✗ const Scalar& ActionModelQuadrupedAugmentedTpl<Scalar>::get_T_gait() const {
634 // The model need to be updated after this changed
635 ✗ return T_gait;
636 }
637 template <typename Scalar>
638 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_T_gait(
639 const Scalar& T_gait_) {
640 // The model need to be updated after this changed
641 ✗ T_gait = T_gait_;
642 }
643
644 template <typename Scalar>
645 ✗ const Scalar& ActionModelQuadrupedAugmentedTpl<Scalar>::get_shoulder_hlim()
646 const {
647 ✗ return sh_hlim;
648 }
649 template <typename Scalar>
650 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_shoulder_hlim(
651 const Scalar& hlim) {
652 // The model need to be updated after this changed
653 ✗ sh_hlim = hlim;
654 }
655
656 template <typename Scalar>
657 const typename Eigen::Matrix<Scalar, 4, 1>&
658 ✗ ActionModelQuadrupedAugmentedTpl<Scalar>::get_shoulder_contact_weight() const {
659 ✗ return sh_weight;
660 }
661 template <typename Scalar>
662 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_shoulder_contact_weight(
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
668 ///////////////////////////
669 //// get A & B matrix /////
670 ///////////////////////////
671 template <typename Scalar>
672 const typename Eigen::Matrix<Scalar, 12, 12>&
673 ✗ ActionModelQuadrupedAugmentedTpl<Scalar>::get_A() const {
674 ✗ return A;
675 }
676 template <typename Scalar>
677 const typename Eigen::Matrix<Scalar, 12, 12>&
678 ✗ ActionModelQuadrupedAugmentedTpl<Scalar>::get_B() const {
679 ✗ return B;
680 }
681
682 // to modify the cost on the command : || fz - m*g/nb contact ||^2
683 // --> set to True
684 template <typename Scalar>
685 ✗ const bool& ActionModelQuadrupedAugmentedTpl<Scalar>::get_relative_forces()
686 const {
687 ✗ return relative_forces;
688 }
689 template <typename Scalar>
690 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::set_relative_forces(
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
703 ////////////////////////
704 // Update current model
705 ////////////////////////
706
707 template <typename Scalar>
708 ✗ void ActionModelQuadrupedAugmentedTpl<Scalar>::update_model(
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
795