Skip to content

Commit a659cef

Browse files
author
ipuch
committed
more pretty (?) : S
1 parent 56a5503 commit a659cef

File tree

1 file changed

+52
-18
lines changed

1 file changed

+52
-18
lines changed

‎include/pinocchio/multibody/joint/joint-ellipsoid.hpp‎

Lines changed: 52 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -188,23 +188,35 @@ namespace pinocchio
188188
data.M.rotation() << c1c2, -c1s2, s1,
189189
c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, -c1 * s0,
190190
-c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1;
191-
191+
192192
Scalar nx, ny, nz;
193193
nx = s1;
194194
ny = -s0 * c1;
195195
nz = c0 * c1;
196196

197197
data.M.translation() << radius_a * nx, radius_b * ny, radius_c * nz;
198+
199+
Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1;
200+
dndotx_dqdot1 = c1;
201+
dndoty_dqdot0 = - c0 * c1;
202+
dndoty_dqdot1 = s0 * s1;
203+
dndotz_dqdot0 = - c1 * s0;
204+
dndotz_dqdot1 = - c0 * s1;
198205

199206
Scalar S_11, S_21, S_31, S_12, S_22, S_32;
200-
S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + s0 * s2));
201-
S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + c2 * s0));
202-
S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c);
203-
204-
S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 * s1 * (- c0 * c2 * s1 + s0 * s2);
205-
S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c * c0 * s1 * (c0 * s1 * s2 + c2 * s0);
206-
S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0);
207-
207+
// S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + s0 * s2));
208+
S_11 = dndoty_dqdot0 * radius_b * (c0 *s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (- c0 * c2 * s1 + s0 * s2);
209+
// S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + c2 * s0));
210+
S_21 = - dndoty_dqdot0 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0);
211+
// S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c);
212+
S_31 = c1 * (- dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0);
213+
214+
//S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 * s1 * (- c0 * c2 * s1 + s0 * s2);
215+
S_12 = dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (- c0 * c2 * s1 + s0 * s2);
216+
// S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c * c0 * s1 * (c0 * s1 * s2 + c2 * s0);
217+
S_22 = - dndotx_dqdot1 * radius_a * c1 * s2 - dndoty_dqdot1 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0);
218+
//S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0);
219+
S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1;
208220

209221
data.S.matrix() << S_11, S_12, Scalar(0),
210222
S_21, S_22, Scalar(0),
@@ -315,28 +327,50 @@ namespace pinocchio
315327
Scalar c2, s2;
316328
SINCOS(data.joint_q(2), &s2, &c2);
317329

318-
// Common operations
330+
//common operations
319331
Scalar c1c2, c1s2;
320332
c1c2 = c1 * c2;
321333
c1s2 = c1 * s2;
322334

323335
data.M.rotation() << c1c2, -c1s2, s1,
324-
c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, -c1 * s0,
325-
-c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1;
336+
c0 * s2 + c2 * s0 * s1, c0 * c2 - s0 * s1 * s2, -c1 * s0,
337+
-c0 * c2 * s1 + s0 * s2, c0 * s1 * s2 + c2 * s0, c0 * c1;
326338

327339
Scalar nx, ny, nz;
328340
nx = s1;
329341
ny = -s0 * c1;
330342
nz = c0 * c1;
331343

332-
data.M.translation() << Scalar(0.01) * nx, radius_b * ny, radius_c * nz;
344+
data.M.translation() << radius_a * nx, radius_b * ny, radius_c * nz;
345+
346+
Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1;
347+
dndotx_dqdot1 = c1;
348+
dndoty_dqdot0 = - c0 * c1;
349+
dndoty_dqdot1 = s0 * s1;
350+
dndotz_dqdot0 = - c1 * s0;
351+
dndotz_dqdot1 = - c0 * s1;
333352

334-
data.S.matrix() << c1c2, s2, Scalar(0),
353+
Scalar S_11, S_21, S_31, S_12, S_22, S_32;
354+
// S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + s0 * s2));
355+
S_11 = dndoty_dqdot0 * radius_b * (c0 *s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (- c0 * c2 * s1 + s0 * s2);
356+
// S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + c2 * s0));
357+
S_21 = - dndoty_dqdot0 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0);
358+
// S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c);
359+
S_31 = c1 * (- dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0);
360+
361+
//S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 * s1 * (- c0 * c2 * s1 + s0 * s2);
362+
S_12 = dndotx_dqdot1 * radius_a * c1 * c2 + dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1) + dndotz_dqdot1 * radius_c * (- c0 * c2 * s1 + s0 * s2);
363+
// S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c * c0 * s1 * (c0 * s1 * s2 + c2 * s0);
364+
S_22 = - dndotx_dqdot1 * radius_a * c1 * s2 - dndoty_dqdot1 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0);
365+
//S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0);
366+
S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1;
367+
368+
data.S.matrix() << S_11, S_12, Scalar(0),
369+
S_21, S_22, Scalar(0),
370+
S_31, S_32, Scalar(0),
371+
c1c2, s2, Scalar(0),
335372
-c1s2, c2, Scalar(0),
336-
s1, Scalar(0), Scalar(1),
337-
c1 * (-radius_b * s0 * s1 + radius_c * c0 * c1 * s2), c1 * (radius_a - radius_c * c0 * c2), -radius_b * c1 * s0,
338-
-radius_a * s1 * s1 - radius_b * c0 * c1 + radius_c * c0 * c1 * c1 * c2, radius_b * s0 * s1 + radius_c * c0 * c1 * s2, radius_a * s1,
339-
c1 * (radius_a * s1 * s2 + radius_b * c1 * c2 * s0 - radius_c * s0), radius_a * c2 * s1 + radius_b * c1 * s0 * s2 + radius_c * c0 * s1, Scalar(0);
373+
s1, Scalar(0), Scalar(1);
340374

341375
// Velocity part
342376
data.joint_v = vs.template segment<NV>(idx_v());

0 commit comments

Comments
 (0)