tests(joint-ellipsoid): Sdot as finite difference

Author: ipuch

bindings/python/parsers/graph/expose-edges.cpp

@@ -36,6 +36,9 @@ namespace pinocchio
     const int JointSphericalZYX::nq;
     const int JointSphericalZYX::nv;
 
+    const int JointEllipsoid::nq;
+    const int JointEllipsoid::nv;
+
     const int JointTranslation::nq;
     const int JointTranslation::nv;
 
@@ -117,6 +120,15 @@ namespace pinocchio
         .def_readonly("nq", &JointEllipsoid::nq, "Number of configuration variables.")
         .def_readonly("nv", &JointEllipsoid::nv, "Number of tangent variables.");
 
+      bp::class_<JointEllipsoid>(
+        "JointEllipsoid", "Represents an ellipsoidal joint.",
+        bp::init<>(bp::args("self", "radius_a", "radius_b", "radius_c"), "Constructor with radii."))
+        .def_readwrite("radius_a", &JointEllipsoid::radius_a, "Semi-axis length in the x direction.")
+        .def_readwrite("radius_b", &JointEllipsoid::radius_b, "Semi-axis length in the y direction.")
+        .def_readwrite("radius_c", &JointEllipsoid::radius_c, "Semi-axis length in the z direction.")
+        .def_readonly("nq", &JointEllipsoid::nq, "Number of configuration variables.")
+        .def_readonly("nv", &JointEllipsoid::nv, "Number of tangent variables.");
+
       bp::class_<JointTranslation>(
         "JointTranslation", "Represents a translation joint.",
         bp::init<>(bp::args("self"), "Default constructor."))

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

@@ -12,7 +12,7 @@
 #include "pinocchio/math/matrix.hpp"
 #include "pinocchio/spatial/inertia.hpp"
 #include "pinocchio/spatial/skew.hpp"
-#include "pinocchio/multibody/joint-motion-subspace.hpp" // because we are dense.
+#include "pinocchio/multibody/joint-motion-subspace.hpp"
 
 namespace pinocchio
 {
@@ -452,28 +452,23 @@ namespace pinocchio
       const Scalar c1s2 = c1 * s2;
 
       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;
 

unittest/joint-ellipsoid.cpp

@@ -36,6 +36,63 @@ void addJointAndBody(
   model.appendBodyToJoint(idx, Y);
 }
 
+/// \brief Compute Sdot (motion subspace derivative) via finite differences
+template<typename JointModel>
+Eigen::Matrix<double, 6, JointModel::NV> finiteDiffSdot(
+  const JointModel & jmodel,
+  typename JointModel::JointDataDerived & jdata,
+  const typename JointModel::ConfigVector_t & q,
+  const typename JointModel::TangentVector_t & v,
+  double eps = 1e-8)
+{
+  typedef typename LieGroup<JointModel>::type LieGroupType;
+  typedef typename JointModel::ConfigVector_t ConfigVector_t;
+  typedef typename JointModel::TangentVector_t TangentVector_t;
+  
+  const Eigen::DenseIndex nv = jmodel.nv();
+  
+  Eigen::Matrix<double, 6, JointModel::NV> Sdot_fd;
+  Sdot_fd.setZero();
+  
+  ConfigVector_t q_integrated(q);
+  TangentVector_t v_integrate(nv);
+  v_integrate.setZero();
+  
+  for (Eigen::DenseIndex k = 0; k < nv; ++k) 
+  {
+    // Integrate along kth direction
+    v_integrate[k] = eps;
+    q_integrated = LieGroupType().integrate(q, v_integrate);
+    
+    // Compute S at q + eps * e_k
+    jmodel.calc(jdata, q_integrated);
+    const Eigen::Matrix<double, 6, JointModel::NV> S_plus = jdata.S.matrix();
+    
+    // Integrate in negative direction
+    v_integrate[k] = -eps;
+    q_integrated = LieGroupType().integrate(q, v_integrate);
+    
+    // Compute S at q - eps * e_k
+    jmodel.calc(jdata, q_integrated);
+    const Eigen::Matrix<double, 6, JointModel::NV> S_minus = jdata.S.matrix();
+    
+    // Compute dS/dq_k via central differences
+    Eigen::Matrix<double, 6, JointModel::NV> dS_dqk = (S_plus - S_minus) / (2.0 * eps);
+    
+    // Accumulate: Sdot += (dS/dq_k) * v_k
+    Sdot_fd += dS_dqk * v[k];
+    
+    // Reset
+    v_integrate[k] = 0.;
+  }
+  
+  return Sdot_fd;
+}
+
+// ============================================================================
+// Test Suite
+// ============================================================================
+
 BOOST_AUTO_TEST_SUITE(JointEllipsoid)
 
 BOOST_AUTO_TEST_CASE(vsSphericalZYX)
@@ -71,31 +128,8 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX)
   // Extract XYZ Euler angles from the rotation matrix
   // For XYZ convention: R = Rx(x) * Ry(y) * Rz(z)
   // We need to solve for x, y, z
-
   Eigen::Vector3d q_e;
-
-  // From the Ellipsoid rotation matrix structure:
-  // R(0,2) = sin(y)
-  // R(1,2) = -sin(x)*cos(y)
-  // R(2,2) = cos(x)*cos(y)
-
-  double sy = R(0, 2);
-  q_e(1) = std::asin(sy); // y angle
-
-  double cy = std::cos(q_e(1));
-
-  if (std::abs(cy) > 1e-6)
-  {
-    // Not at singularity
-    q_e(0) = std::atan2(-R(1, 2) / cy, R(2, 2) / cy); // x angle
-    q_e(2) = std::atan2(-R(0, 1) / cy, R(0, 0) / cy); // z angle
-  }
-  else
-  {
-    // Gimbal lock - choose x = 0
-    q_e(0) = 0.0;
-    q_e(2) = std::atan2(R(1, 0), R(1, 1));
-  }
+  q_e = R.eulerAngles(0, 1, 2); // XYZ convention
 
   // Get the motion subspace matrices (which give us the Jacobians)
   JointModelSphericalZYX jmodel_s;
@@ -291,4 +325,36 @@ BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz)
   BOOST_CHECK(aAbaEllipsoid.isApprox(qddot_ellipsoid));
 }
 
+BOOST_AUTO_TEST_CASE(testSdotFiniteDifferences)
+{
+  using namespace pinocchio;
+  
+  // Ellipsoid parameters
+  double radius_a = 2.0;
+  double radius_b = 1.5;
+  double radius_c = 1.0;
+
+  JointModelEllipsoid jmodel(radius_a, radius_b, radius_c);
+  jmodel.setIndexes(0, 0, 0);
+  
+  JointDataEllipsoid jdata = jmodel.createData();
+  
+  // Test configuration and velocity
+  typedef JointModelEllipsoid::ConfigVector_t ConfigVector_t;
+  typedef JointModelEllipsoid::TangentVector_t TangentVector_t;
+  typedef LieGroup<JointModelEllipsoid>::type LieGroupType;
+  
+  ConfigVector_t q = LieGroupType().random();
+  TangentVector_t v = TangentVector_t::Random();
+
+  // Compute analytical Sdot
+  const double eps = 1e-8;
+  jmodel.calc(jdata, q, v);
+  const Eigen::Matrix<double, 6, 3> Sdot_ref = jdata.Sdot.matrix();
+  
+  // Compute Sdot via finite differences using helper function
+  const Eigen::Matrix<double, 6, 3> Sdot_fd = finiteDiffSdot(jmodel, jdata, q, v, eps);
+  
+  BOOST_CHECK(Sdot_ref.isApprox(Sdot_fd, sqrt(eps)));
+}
 BOOST_AUTO_TEST_SUITE_END()

unittest/model-graph.cpp

@@ -617,7 +617,7 @@ BOOST_AUTO_TEST_CASE(test_reverse_spherical_zyx)
     d_f.oMf[m_forward.getFrameId("body1", pinocchio::BODY)]));
 }
 
-/// @brief test if reversing of a composite joint is correct.
+/// @brief test if reversing of an ellipsoid joint is correct.
 BOOST_AUTO_TEST_CASE(test_reverse_ellipsoid)
 {
   using namespace pinocchio::graph;