Skip to content

Commit 65bc569

Browse files
author
ipuch
committed
Sdot
1 parent a659cef commit 65bc569

File tree

1 file changed

+281
-57
lines changed

1 file changed

+281
-57
lines changed

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

Lines changed: 281 additions & 57 deletions
Original file line numberDiff line numberDiff line change
@@ -252,8 +252,10 @@ namespace pinocchio
252252
-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,
253253
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);
254254

255+
// Velocity part
255256
data.v.toVector().noalias() = data.S.matrix() * data.joint_v;
256257

258+
// Compute Sdot for bias acceleration
257259
Scalar qdot0, qdot1, qdot2;
258260
qdot0 = data.joint_v(0);
259261
qdot1 = data.joint_v(1);
@@ -263,44 +265,158 @@ namespace pinocchio
263265
Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52, Sdot_62;
264266
Scalar Sdot_13, Sdot_23, Sdot_33, Sdot_43, Sdot_53, Sdot_63;
265267

266-
// Lower part (angular)
267-
Sdot_41 = - (qdot1 * c2 * s1 + qdot2 * c1 * s2);
268-
Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2;
269-
Sdot_61 = qdot1 * c1;
268+
Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1;
269+
dndotx_dqdot1 = c1;
270+
dndoty_dqdot0 = - c0 * c1;
271+
dndoty_dqdot1 = s0 * s1;
272+
dndotz_dqdot0 = - c1 * s0;
273+
dndotz_dqdot1 = - c0 * s1;
274+
275+
// Derivative of dndotXX_dqdot0 with respect to q0 and q1
276+
Scalar d_dndotx_dqdot1_dq1 = - s1; // dndotx_dqdot1 = c1;
277+
278+
Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1;
279+
Scalar d_dndoty_dqdot0_dq1 = c0 * s1;
280+
281+
Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1;
282+
Scalar d_dndoty_dqdot1_dq1 = s0 * c1;
283+
284+
Scalar d_dndotz_dqdot0_dq0 = - c1 * c0; // dndotz_dqdot0 = - c1 * s0;
285+
Scalar d_dndotz_dqdot0_dq1 = s0 * s1;
286+
287+
Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1;
288+
Scalar d_dndotz_dqdot1_dq1 = -c0 * c1;
289+
290+
291+
// Upper part (translation)
292+
// Row 1, Column 1
293+
Sdot_11 = qdot0 * (
294+
-dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2)
295+
+ dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1)
296+
+ radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0
297+
+ radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0
298+
)
299+
+ qdot1 * (
300+
dndoty_dqdot0 * radius_b * c1 * c2 * s0
301+
- dndotz_dqdot0 * radius_c * c0 * c1 * c2
302+
+ radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1
303+
+ radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1
304+
)
305+
- qdot2 * (
306+
dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2)
307+
- dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0)
308+
);
309+
310+
// Row 1, Column 2
311+
Sdot_12 = qdot0 * (
312+
-dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2)
313+
+ dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1)
314+
+ radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1
315+
+ radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1
316+
)
317+
+ qdot1 * (
318+
-dndotx_dqdot1 * radius_a * c2 * s1
319+
+ dndoty_dqdot1 * radius_b * c1 * c2 * s0
320+
- dndotz_dqdot1 * radius_c * c0 * c1 * c2
321+
+ radius_a * c1 * c2 * d_dndotx_dqdot1_dq1
322+
+ radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1
323+
+ radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1
324+
)
325+
- qdot2 * (
326+
dndotx_dqdot1 * radius_a * c1 * s2
327+
+ dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2)
328+
- dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0)
329+
);
330+
331+
// Row 1, Column 3
332+
Sdot_13 = Scalar(0);
333+
334+
// Row 2, Column 1
335+
Sdot_21 = -qdot0 * (
336+
dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0)
337+
+ dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2)
338+
+ radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0
339+
- radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0
340+
)
341+
- qdot1 * (
342+
dndoty_dqdot0 * radius_b * c1 * s0 * s2
343+
- dndotz_dqdot0 * radius_c * c0 * c1 * s2
344+
+ radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1
345+
- radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1
346+
)
347+
- qdot2 * (
348+
dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1)
349+
+ dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2)
350+
);
351+
352+
// Row 2, Column 2
353+
Sdot_22 = -qdot0 * (
354+
dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0)
355+
+ dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2)
356+
+ radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1
357+
- radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1
358+
)
359+
+ qdot1 * (
360+
dndotx_dqdot1 * radius_a * s1 * s2
361+
- dndoty_dqdot1 * radius_b * c1 * s0 * s2
362+
+ dndotz_dqdot1 * radius_c * c0 * c1 * s2
363+
- radius_a * c1 * s2 * d_dndotx_dqdot1_dq1
364+
- radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1
365+
+ radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1
366+
)
367+
- qdot2 * (
368+
dndotx_dqdot1 * radius_a * c1 * c2
369+
+ dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1)
370+
+ dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2)
371+
);
372+
373+
// Row 2, Column 3
374+
Sdot_23 = Scalar(0);
375+
376+
// Row 3, Column 1
377+
Sdot_31 = -qdot0 * c1 * (
378+
dndoty_dqdot0 * radius_b * c0
379+
+ dndotz_dqdot0 * radius_c * s0
380+
+ radius_b * s0 * d_dndoty_dqdot0_dq0
381+
- radius_c * c0 * d_dndotz_dqdot0_dq0
382+
)
383+
+ qdot1 * (
384+
-c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1)
385+
+ s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0)
386+
);
387+
388+
// Row 3, Column 2
389+
Sdot_32 = -qdot0 * c1 * (
390+
dndoty_dqdot1 * radius_b * c0
391+
+ dndotz_dqdot1 * radius_c * s0
392+
+ radius_b * s0 * d_dndoty_dqdot0_dq1
393+
- radius_c * c0 * d_dndotz_dqdot0_dq1
394+
)
395+
+ qdot1 * (
396+
dndotx_dqdot1 * radius_a * c1
397+
+ dndoty_dqdot1 * radius_b * s0 * s1
398+
- dndotz_dqdot1 * radius_c * c0 * s1
399+
+ radius_a * s1 * d_dndotx_dqdot1_dq1
400+
- radius_b * c1 * s0 * d_dndoty_dqdot1_dq1
401+
+ radius_c * c0 * c1 * d_dndotz_dqdot1_dq1
402+
);
403+
404+
// Row 3, Column 3
405+
Sdot_33 = Scalar(0);
406+
407+
// Angular part (rows 4-6)
408+
Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2);
409+
Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2;
410+
Sdot_61 = qdot1 * c1;
270411

271412
Sdot_42 = qdot2 * c2;
272-
Sdot_52 = - qdot2 * s2;
413+
Sdot_52 = -qdot2 * s2;
273414
Sdot_62 = Scalar(0);
274415

275416
Sdot_43 = Scalar(0);
276417
Sdot_53 = Scalar(0);
277418
Sdot_63 = Scalar(0);
278419

279-
// Upper part (linear)
280-
Sdot_11 = - qdot0 *c1 * (radius_b * c0 * s1 + radius_c * c0 * s0 * s2)
281-
+ qdot1 * (-2 * radius_b * c1 * c1 * s0 + radius_b * s0 - 2 * radius_c * c0 * c1 * s1 * s2)
282-
+ qdot2 * - radius_c * c0 * c1 * c1 * s2;
283-
Sdot_21 = qdot0 * c1 * s0 *(radius_b - radius_c * c1 * c2)
284-
- qdot1 * s1 * ( 2 * radius_a * c1 - radius_b * c0 + 2 * radius_c * c0 * c1 * c2)
285-
+ qdot2 * radius_c * c0 * c1 * c1 * s2;
286-
Sdot_31 = qdot0 * c0 * c1 *(radius_b * c1 * c2 - radius_c)
287-
+ qdot1 * ( - 2 * radius_a *c1 * c1 * s2 + + radius_a * s2 - 2 * radius_b *c1 * c2 * s0 * s1)
288-
+ qdot2 * c1 * (radius_a * c2 * s1 + radius_b * c1 * s0 * s2);
289-
290-
Sdot_12 = qdot0 * radius_c * c1 * c2 * s0
291-
- qdot1 * s1 * (radius_b * c1 * s0 - radius_c * c0 * s1 * s2)
292-
+ qdot2 * radius_c * c0 * c1 * c2;
293-
Sdot_22 = - qdot0 * (radius_b * c0 * c1 * s2 + radius_c * c1 *s0 * s2)
294-
+ qdot1 *(radius_b * c1 * s0 - radius_c * c0 * s1 * s2)
295-
+ qdot2 * radius_c * c0 * c1 * c2;
296-
Sdot_32 = qdot0 * (radius_b * c0 *c1*s2 +radius_c *s0*s1)
297-
- qdot1 * (- radius_a * c1 * c2 + radius_b * s0 * s1 * s2 + radius_c * c0 * c1)
298-
- qdot2 * (radius_a * s1 * s2 - radius_b * c1 * c2 * s0);
299-
300-
Sdot_13 = radius_b * (-qdot0 * c0 * c1 + qdot1 * s0 * s1);
301-
Sdot_23 = -qdot1 * radius_a * c1;
302-
Sdot_33 = Scalar(0);
303-
304420
data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13,
305421
Sdot_21, Sdot_22, Sdot_23,
306422
Sdot_31, Sdot_32, Sdot_33,
@@ -343,7 +459,8 @@ namespace pinocchio
343459

344460
data.M.translation() << radius_a * nx, radius_b * ny, radius_c * nz;
345461

346-
Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1;
462+
// First derivatives of n_dot with respect to q_dot
463+
Scalar dndotx_dqdot1, dndoty_dqdot0, dndoty_dqdot1, dndotz_dqdot0, dndotz_dqdot1;
347464
dndotx_dqdot1 = c1;
348465
dndoty_dqdot0 = - c0 * c1;
349466
dndoty_dqdot1 = s0 * s1;
@@ -385,8 +502,140 @@ namespace pinocchio
385502
Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61;
386503
Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52, Sdot_62;
387504
Scalar Sdot_13, Sdot_23, Sdot_33, Sdot_43, Sdot_53, Sdot_63;
505+
506+
// Derivative of dndotXX_dqdot0 with respect to q0 and q1
507+
Scalar d_dndotx_dqdot1_dq1 = - s1; // dndotx_dqdot1 = c1;
508+
509+
Scalar d_dndoty_dqdot0_dq0 = s0 * c1; // dndoty_dqdot0 = - c0 * c1;
510+
Scalar d_dndoty_dqdot0_dq1 = c0 * s1;
511+
512+
Scalar d_dndoty_dqdot1_dq0 = c0 * s1; // dndoty_dqdot1 = s0 * s1;
513+
Scalar d_dndoty_dqdot1_dq1 = s0 * c1;
514+
515+
Scalar d_dndotz_dqdot0_dq0 = - c1 * c0; // dndotz_dqdot0 = - c1 * s0;
516+
Scalar d_dndotz_dqdot0_dq1 = s0 * s1;
517+
518+
Scalar d_dndotz_dqdot1_dq0 = s0 * s1; // dndotz_dqdot1 = - c0 * s1;
519+
Scalar d_dndotz_dqdot1_dq1 = -c0 * c1;
520+
388521

389-
// Lower part (angular)
522+
// Upper part (translation)
523+
// Row 1, Column 1
524+
Sdot_11 = qdot0 * (
525+
-dndoty_dqdot0 * radius_b * (-c0 * c2 * s1 + s0 * s2)
526+
+ dndotz_dqdot0 * radius_c * (c0 * s2 + c2 * s0 * s1)
527+
+ radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq0
528+
+ radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq0
529+
)
530+
+ qdot1 * (
531+
dndoty_dqdot0 * radius_b * c1 * c2 * s0
532+
- dndotz_dqdot0 * radius_c * c0 * c1 * c2
533+
+ radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1
534+
+ radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1
535+
)
536+
- qdot2 * (
537+
dndoty_dqdot0 * radius_b * (-c0 * c2 + s0 * s1 * s2)
538+
- dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0)
539+
);
540+
541+
// Row 1, Column 2
542+
Sdot_12 = qdot0 * (
543+
-dndoty_dqdot1 * radius_b * (-c0 * c2 * s1 + s0 * s2)
544+
+ dndotz_dqdot1 * radius_c * (c0 * s2 + c2 * s0 * s1)
545+
+ radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot0_dq1
546+
+ radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot0_dq1
547+
)
548+
+ qdot1 * (
549+
-dndotx_dqdot1 * radius_a * c2 * s1
550+
+ dndoty_dqdot1 * radius_b * c1 * c2 * s0
551+
- dndotz_dqdot1 * radius_c * c0 * c1 * c2
552+
+ radius_a * c1 * c2 * d_dndotx_dqdot1_dq1
553+
+ radius_b * (c0 * s2 + c2 * s0 * s1) * d_dndoty_dqdot1_dq1
554+
+ radius_c * (-c0 * c2 * s1 + s0 * s2) * d_dndotz_dqdot1_dq1
555+
)
556+
- qdot2 * (
557+
dndotx_dqdot1 * radius_a * c1 * s2
558+
+ dndoty_dqdot1 * radius_b * (-c0 * c2 + s0 * s1 * s2)
559+
- dndotz_dqdot1 * radius_c * (c0 * s1 * s2 + c2 * s0)
560+
);
561+
562+
// Row 1, Column 3
563+
Sdot_13 = Scalar(0);
564+
565+
// Row 2, Column 1
566+
Sdot_21 = -qdot0 * (
567+
dndoty_dqdot0 * radius_b * (c0 * s1 * s2 + c2 * s0)
568+
+ dndotz_dqdot0 * radius_c * (-c0 * c2 + s0 * s1 * s2)
569+
+ radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq0
570+
- radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq0
571+
)
572+
- qdot1 * (
573+
dndoty_dqdot0 * radius_b * c1 * s0 * s2
574+
- dndotz_dqdot0 * radius_c * c0 * c1 * s2
575+
+ radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1
576+
- radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1
577+
)
578+
- qdot2 * (
579+
dndoty_dqdot0 * radius_b * (c0 * s2 + c2 * s0 * s1)
580+
+ dndotz_dqdot0 * radius_c * (-c0 * c2 * s1 + s0 * s2)
581+
);
582+
583+
// Row 2, Column 2
584+
Sdot_22 = -qdot0 * (
585+
dndoty_dqdot1 * radius_b * (c0 * s1 * s2 + c2 * s0)
586+
+ dndotz_dqdot1 * radius_c * (-c0 * c2 + s0 * s1 * s2)
587+
+ radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot0_dq1
588+
- radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot0_dq1
589+
)
590+
+ qdot1 * (
591+
dndotx_dqdot1 * radius_a * s1 * s2
592+
- dndoty_dqdot1 * radius_b * c1 * s0 * s2
593+
+ dndotz_dqdot1 * radius_c * c0 * c1 * s2
594+
- radius_a * c1 * s2 * d_dndotx_dqdot1_dq1
595+
- radius_b * (-c0 * c2 + s0 * s1 * s2) * d_dndoty_dqdot1_dq1
596+
+ radius_c * (c0 * s1 * s2 + c2 * s0) * d_dndotz_dqdot1_dq1
597+
)
598+
- qdot2 * (
599+
dndotx_dqdot1 * radius_a * c1 * c2
600+
+ dndoty_dqdot1 * radius_b * (c0 * s2 + c2 * s0 * s1)
601+
+ dndotz_dqdot1 * radius_c * (-c0 * c2 * s1 + s0 * s2)
602+
);
603+
604+
// Row 2, Column 3
605+
Sdot_23 = Scalar(0);
606+
607+
// Row 3, Column 1
608+
Sdot_31 = -qdot0 * c1 * (
609+
dndoty_dqdot0 * radius_b * c0
610+
+ dndotz_dqdot0 * radius_c * s0
611+
+ radius_b * s0 * d_dndoty_dqdot0_dq0
612+
- radius_c * c0 * d_dndotz_dqdot0_dq0
613+
)
614+
+ qdot1 * (
615+
-c1 * (radius_b * s0 * d_dndoty_dqdot0_dq1 - radius_c * c0 * d_dndotz_dqdot0_dq1)
616+
+ s1 * (dndoty_dqdot0 * radius_b * s0 - dndotz_dqdot0 * radius_c * c0)
617+
);
618+
619+
// Row 3, Column 2
620+
Sdot_32 = -qdot0 * c1 * (
621+
dndoty_dqdot1 * radius_b * c0
622+
+ dndotz_dqdot1 * radius_c * s0
623+
+ radius_b * s0 * d_dndoty_dqdot0_dq1
624+
- radius_c * c0 * d_dndotz_dqdot0_dq1
625+
)
626+
+ qdot1 * (
627+
dndotx_dqdot1 * radius_a * c1
628+
+ dndoty_dqdot1 * radius_b * s0 * s1
629+
- dndotz_dqdot1 * radius_c * c0 * s1
630+
+ radius_a * s1 * d_dndotx_dqdot1_dq1
631+
- radius_b * c1 * s0 * d_dndoty_dqdot1_dq1
632+
+ radius_c * c0 * c1 * d_dndotz_dqdot1_dq1
633+
);
634+
635+
// Row 3, Column 3
636+
Sdot_33 = Scalar(0);
637+
638+
// Angular part (rows 4-6)
390639
Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2);
391640
Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2;
392641
Sdot_61 = qdot1 * c1;
@@ -399,31 +648,6 @@ namespace pinocchio
399648
Sdot_53 = Scalar(0);
400649
Sdot_63 = Scalar(0);
401650

402-
// Lower part (linear)
403-
Sdot_11 = -qdot0 * c1 * (radius_b * c0 * s1 + radius_c * c0 * s0 * s2)
404-
+ qdot1 * (-2 * radius_b * c1 * c1 * s0 + radius_b * s0 - 2 * radius_c * c0 * c1 * s1 * s2)
405-
+ qdot2 * -radius_c * c0 * c1 * c1 * s2;
406-
Sdot_21 = qdot0 * c1 * s0 * (radius_b - radius_c * c1 * c2)
407-
- qdot1 * s1 * (2 * radius_a * c1 - radius_b * c0 + 2 * radius_c * c0 * c1 * c2)
408-
+ qdot2 * radius_c * c0 * c1 * c1 * s2;
409-
Sdot_31 = qdot0 * c0 * c1 * (radius_b * c1 * c2 - radius_c)
410-
+ qdot1 * (-2 * radius_a * c1 * c1 * s2 + radius_a * s2 - 2 * radius_b * c1 * c2 * s0 * s1)
411-
+ qdot2 * c1 * (radius_a * c2 * s1 + radius_b * c1 * s0 * s2);
412-
413-
Sdot_12 = qdot0 * radius_c * c1 * c2 * s0
414-
- qdot1 * s1 * (radius_b * c1 * s0 - radius_c * c0 * s1 * s2)
415-
+ qdot2 * radius_c * c0 * c1 * c2;
416-
Sdot_22 = -qdot0 * (radius_b * c0 * c1 * s2 + radius_c * c1 * s0 * s2)
417-
+ qdot1 * (radius_b * c1 * s0 - radius_c * c0 * s1 * s2)
418-
+ qdot2 * radius_c * c0 * c1 * c2;
419-
Sdot_32 = qdot0 * (radius_b * c0 * c1 * s2 + radius_c * s0 * s1)
420-
- qdot1 * (-radius_a * c1 * c2 + radius_b * s0 * s1 * s2 + radius_c * c0 * c1)
421-
- qdot2 * (radius_a * s1 * s2 - radius_b * c1 * c2 * s0);
422-
423-
Sdot_13 = radius_b * (-qdot0 * c0 * c1 + qdot1 * s0 * s1);
424-
Sdot_23 = -qdot1 * radius_a * c1;
425-
Sdot_33 = Scalar(0);
426-
427651
data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13,
428652
Sdot_21, Sdot_22, Sdot_23,
429653
Sdot_31, Sdot_32, Sdot_33,

0 commit comments

Comments
 (0)