tests(fix): joint-generic for ellipsoid

Author: ipuch

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

@@ -245,13 +245,28 @@ namespace pinocchio
       c1c2 = c1 * c2;
       c1s2 = c1 * s2;
 
-      data.S.matrix() << c1c2, s2, Scalar(0),
-                        -c1s2, c2, Scalar(0),
-                        s1, Scalar(0), Scalar(1),
-                        c1 * (-radius_b * s0 * s1 + radius_c * c0 * c1 * s2), c1 * (radius_a - radius_c * c0 * c2), -radius_b * c1 * s0,
-                        -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,
-                        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);
+      Scalar S_11, S_21, S_31, S_12, S_22, S_32;
+      // S_11 = c1 * (-radius_b * c0 * (c0 * s2 + c2 * s0 * s1) - radius_c * s0 * (- c0 * c2 * s1 + s0 * s2));
+      S_11 = dndoty_dqdot0 * radius_b * (c0 *s2 + c2 * s0 * s1) + dndotz_dqdot0 * radius_c * (- c0 * c2 * s1 + s0 * s2);
+      // S_21 = c1 * (radius_b * c0 * (- c0 * c2 + s0 * s1 * s2) - radius_c * s0 * (c0 * s1 * s2 + c2 * s0));
+      S_21 = - dndoty_dqdot0 * radius_b * (- c0 * c2 + s0 * s1 * s2) + dndotz_dqdot0 * radius_c * (c0 * s1 * s2 + c2 * s0);
+      // S_31 = c0 * c1 * c1 * s0 * (radius_b - radius_c);
+      S_31 = c1 * (- dndoty_dqdot0 * radius_b * s0 + dndotz_dqdot0 * radius_c * c0);
+
+      //S_12 = radius_a * c1c2 * c1 + radius_b * s0 * s1 * (c0 * s2 + c2 * s0 * s1) - radius_c * c0 * s1 * (- c0 * c2 * s1 + s0 * s2);
+      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);
+      // S_22 = - radius_a * c1s2 * c1 - radius_b * s0 * s1 * (- c0 * c2 + s0 * s1 * s2) - radius_c * c0 * s1 * (c0 * s1 * s2 + c2 * s0);
+      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);
+      //S_32 = c1 * s1 * (radius_a - radius_b * s0 * s0 - radius_c * c0 * c0);
+      S_32 = dndotx_dqdot1 * radius_a * s1 - dndoty_dqdot1 * radius_b * c1 * s0 + dndotz_dqdot1 * radius_c * c0 * c1;
 
+      data.S.matrix() << S_11, S_12, Scalar(0),
+                        S_21, S_22, Scalar(0),
+                        S_31, S_32, Scalar(0),
+                        c1c2, s2, Scalar(0),
+                        -c1s2, c2, Scalar(0),
+                        s1, Scalar(0), Scalar(1);
+      
       // Velocity part
       data.v.toVector().noalias() = data.S.matrix() * data.joint_v;
 

unittest/joint-generic.cpp

@@ -264,6 +264,23 @@ struct init<pinocchio::JointModelHelicalTpl<Scalar, Options, axis>>
   }
 };
 
+template<typename Scalar, int Options>
+struct init<pinocchio::JointModelEllipsoidTpl<Scalar, Options>>
+{
+  typedef pinocchio::JointModelEllipsoidTpl<Scalar, Options> JointModel;
+
+  static JointModel run()
+  {
+    JointModel jmodel(Scalar(0.01),
+                      Scalar(0.02),
+                      Scalar(0.03)
+                      );
+
+    jmodel.setIndexes(0, 0, 0);
+    return jmodel;
+  }
+};
+
 template<typename Scalar, int Options>
 struct init<pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
 {