feat(model-graph): ellipsoid

Author: ipuch

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

@@ -111,6 +111,12 @@ namespace pinocchio
         .def_readonly("nq", &JointSphericalZYX::nq, "Number of configuration variables.")
         .def_readonly("nv", &JointSphericalZYX::nv, "Number of tangent variables.");
 
+      bp::class_<JointEllipsoid>(
+        "JointEllipsoid", "Represents an ellipsoidal joint.",
+        bp::init<>(bp::args("self"), "Default constructor."))
+        .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/parsers/graph/graph-visitor.hpp

@@ -113,6 +113,11 @@ namespace pinocchio
           return joint_calc(JointModelSphericalZYX());
         }
 
+        SE3 operator()(const JointEllipsoid &) const
+        {
+          return joint_calc(JointModelEllipsoid());
+        }
+
         SE3 operator()(const JointPlanar &) const
         {
           return joint_calc(JointModelPlanar());

include/pinocchio/parsers/graph/joints.hpp

@@ -161,6 +161,29 @@ namespace pinocchio
       }
     };
 
+    struct JointEllipsoid
+    {
+      double radius_a = 0.01;
+      double radius_b = 0.01;
+      double radius_c = 0.01;
+
+      static constexpr int nq = 3;
+      static constexpr int nv = 3;
+
+      JointEllipsoid() = default;
+      JointEllipsoid(const double r_a, const double r_b, const double r_c)
+      : radius_a(r_a)
+      , radius_b(r_b)
+      , radius_c(r_c)
+      {
+      }
+
+      bool operator==(const JointEllipsoid & other) const
+      {
+        return radius_a == other.radius_a && radius_b == other.radius_b && radius_c == other.radius_c;
+      }
+    };
+
     struct JointTranslation
     {
       static constexpr int nq = 3;
@@ -241,6 +264,7 @@ namespace pinocchio
       JointFreeFlyer,
       JointSpherical,
       JointSphericalZYX,
+      JointEllipsoid,
       JointTranslation,
       JointPlanar,
       JointHelical,

src/parsers/graph/model-graph-algo.cpp

@@ -90,6 +90,9 @@ namespace pinocchio
         // Joint Spherical ZYX
         typedef typename JointCollectionDefault::JointModelSphericalZYX JointModelSphericalZYX;
 
+        // Joint Ellipsoid
+        typedef typename JointCollectionDefault::JointModelEllipsoid JointModelEllipsoid;
+
         // Joint Translation
         typedef typename JointCollectionDefault::JointModelTranslation JointModelTranslation;
 
@@ -221,6 +224,10 @@ namespace pinocchio
         {
           return JointModelSphericalZYX();
         }
+        ReturnType operator()(const JointEllipsoid & joint) const
+        {
+          return JointModelEllipsoid(joint.radius_a, joint.radius_b, joint.radius_c);
+        }
         ReturnType operator()(const JointUniversal & joint) const
         {
           return JointModelUniversal(joint.axis1, joint.axis2);
@@ -338,6 +345,29 @@ namespace pinocchio
           model.addBodyFrame(target_vertex.name, j_id, body_pose);
         }
 
+        void operator()(const JointEllipsoid & joint, const BodyFrame & b_f)
+        {
+          if (!edge.forward)
+            PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Graph - JointEllipsoid cannot be reversed yet.");
+
+          if (boost::get<BodyFrame>(&source_vertex.frame) == nullptr)
+            PINOCCHIO_THROW_PRETTY(
+              std::invalid_argument, "Graph - Invalid joint between a body and a non body frame.");
+
+          const SE3 & joint_pose = edge.source_to_joint;
+          const SE3 & body_pose = edge.joint_to_target;
+
+          const Frame previous_body = model.frames[model.getFrameId(source_vertex.name, BODY)];
+          JointIndex j_id = model.addJoint(
+            previous_body.parentJoint,
+             cjm(joint),
+            previous_body.placement * joint_pose, edge.name);
+
+          model.addJointFrame(j_id);
+          model.appendBodyToJoint(j_id, b_f.inertia); // check this
+          model.addBodyFrame(target_vertex.name, j_id, body_pose);
+        }
+
         void operator()(const JointFixed & joint, const BodyFrame & b_f)
         {
           // Need to check what's vertex the edge is coming from. If it's a body, then we add

src/parsers/graph/model-graph.cpp

@@ -65,6 +65,10 @@ namespace pinocchio
         {
           return {JointSphericalZYX(), SE3::Identity()};
         }
+        ReturnType operator()(const JointEllipsoid &) const
+        {
+          return {JointEllipsoid(), SE3::Identity()};
+        }
         ReturnType operator()(const JointTranslation &) const
         {
           return {JointTranslation(), SE3::Identity()};

unittest/joint-ellipsoid.cpp

@@ -277,14 +277,14 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX)
 
   // Get the motion subspace matrices (which give us the Jacobians)
   JointModelSphericalZYX jmodel_s;
-  jmodel_s.setIndexes(0, 0, 0);  // Set indices for standalone joint
+  jmodel_s.setIndexes(0, 0, 0);
 
   JointDataSphericalZYX jdata_s = jmodel_s.createData();
   jmodel_s.calc(jdata_s, q_s);
 
   JointModelEllipsoid jmodel_e(0, 0, 0);
-  jmodel_e.setIndexes(0, 0, 0);  // Set indices for standalone joint
-  
+  jmodel_e.setIndexes(0, 0, 0);
+
   JointDataEllipsoid jdata_e = jmodel_e.createData();
   jmodel_e.calc(jdata_e, q_e);
 
@@ -317,56 +317,35 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX)
   forwardKinematics(modelEllipsoid, dataEllipsoid, q_e, qd_e);
 
   // Getting S with q_e from the three calcs
-  // Note: jmodel_e already has indices set from earlier
-  JointDataEllipsoid jdata_e_fk = jmodel_e.createData();
-  JointDataEllipsoid jdata_e_fk2 = jmodel_e.createData();
-  JointDataEllipsoid jdata_e_fk3 = jmodel_e.createData();
-
-  jmodel_e.calc(jdata_e_fk, q_e, qd_e);
+  JointDataEllipsoid jDataEllipsoidFK = jmodel_e.createData();
+  JointDataEllipsoid jDataEllipsoidFK2 = jmodel_e.createData();
+  JointDataEllipsoid jDataEllipsoidFK3 = jmodel_e.createData();
 
-  jmodel_e.calc(jdata_e_fk2, q_e);
+  jmodel_e.calc(jDataEllipsoidFK, q_e, qd_e);
+  jmodel_e.calc(jDataEllipsoidFK2, q_e);
+  jmodel_e.calc(jDataEllipsoidFK3, q_e);
+  jmodel_e.calc(jDataEllipsoidFK3, Blank(), qd_e);
 
-  jmodel_e.calc(jdata_e_fk3, q_e);
-  jmodel_e.calc(jdata_e_fk3, Blank(), qd_e);
-
-  BOOST_CHECK(jdata_e_fk.S.matrix().isApprox(jdata_e_fk2.S.matrix()));
-  BOOST_CHECK(jdata_e_fk.S.matrix().isApprox(jdata_e_fk3.S.matrix()));
+  BOOST_CHECK(jDataEllipsoidFK.S.matrix().isApprox(jDataEllipsoidFK2.S.matrix()));
+  BOOST_CHECK(jDataEllipsoidFK.S.matrix().isApprox(jDataEllipsoidFK3.S.matrix()));
   std::cout << "✓ Motion subspace matrices match for the three calc(...) methods" << std::endl;
 
-  JointDataSphericalZYX jdata_s_fk = jmodel_s.createData();
-  jmodel_s.calc(jdata_s_fk, q_s, qd_s);
+  JointDataSphericalZYX jDataSphereFK = jmodel_s.createData();
+  jmodel_s.calc(jDataSphereFK, q_s, qd_s);
 
   std::cout << "\n=== Joint-frame velocities (S * v) ===" << std::endl;
-  std::cout << "idx_v() for standalone joint: " << jmodel_e.idx_v() << std::endl;
-  std::cout << "qd_e: " << qd_e.transpose() << std::endl;
-  std::cout << "jdata_e_fk.joint_v: " << jdata_e_fk.joint_v.transpose() << std::endl;
-  Eigen::Matrix<double, 6, 1> joint_vel_e = jdata_e_fk.S.matrix() * qd_e;
-  std::cout << "joint_vel_e (S * qd_e): " << joint_vel_e.transpose() << std::endl;
-  std::cout << "jdata_e_fk.v (computed): " << jdata_e_fk.v.toVector().transpose() << std::endl;
-  Eigen::Matrix<double, 6, 1> manual_vel = jdata_e_fk.S.matrix() * jdata_e_fk.joint_v;
-  std::cout << "S * jdata_e_fk.joint_v: " << manual_vel.transpose() << std::endl;
-
-  Eigen::Matrix<double, 6, 1> joint_vel_s = jdata_s_fk.S.matrix() * qd_s;
-  std::cout << "Ellipsoid (S_xyz * qd_e): " << joint_vel_e.transpose() << std::endl;
-  std::cout << "SphericalZYX (S_zyx * qd_s): " << joint_vel_s.transpose() << std::endl;
-
-  std::cout << "\n=== Rotation Matrices ===" << std::endl;
-  std::cout << "Ellipsoid rotation:\n" << dataEllipsoid.oMi[1].rotation() << std::endl;
-  std::cout << "\nSphericalZYX rotation:\n" << dataSphericalZYX.oMi[1].rotation() << std::endl;
-  
-  std::cout << "\n=== Translations ===" << std::endl;
-  std::cout << "Ellipsoid translation: " << dataEllipsoid.oMi[1].translation().transpose() << std::endl;
-  std::cout << "SphericalZYX translation: " << dataSphericalZYX.oMi[1].translation().transpose() << std::endl;
-  
-  std::cout << "\n=== Spatial Velocities ===" << std::endl;
-  std::cout << "Ellipsoid v:\n" << dataEllipsoid.v[1].toVector().transpose() << std::endl;
-  std::cout << "SphericalZYX v:\n" << dataSphericalZYX.v[1].toVector().transpose() << std::endl;
+  Eigen::Matrix<double, 6, 1> joint_vel_e = jDataEllipsoidFK.S.matrix() * qd_e;
+  Eigen::Matrix<double, 6, 1> manual_vel = jDataEllipsoidFK.S.matrix() * jDataEllipsoidFK.joint_v;
+  Eigen::Matrix<double, 6, 1> joint_vel_s = jDataSphereFK.S.matrix() * qd_s;
 
   BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector()));
   std::cout << "✓ Spatial velocities match" << std::endl;
 
   BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1]));
   std::cout << "✓ Full oMi[1] matches" << std::endl;
+
+  // TODO ACCELERATION
+
 }
 
 BOOST_AUTO_TEST_SUITE_END()

unittest/model-graph.cpp

@@ -617,6 +617,32 @@ 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.
+BOOST_AUTO_TEST_CASE(test_reverse_ellipsoid)
+{
+  using namespace pinocchio::graph;
+
+  ModelGraph g = buildReversableModelGraph(JointEllipsoid());
+  ///////////////// Model
+  BOOST_CHECK_THROW(buildModel(g, "body2", pinocchio::SE3::Identity()), std::invalid_argument);
+
+  //////////////////////////////////// Forward model
+  pinocchio::Model m_forward = buildModel(g, "body1", pinocchio::SE3::Identity());
+  pinocchio::Data d_f(m_forward);
+  // config vector forward model Ellipsoid
+  Eigen::Vector3d q(m_forward.nq);
+  q << M_PI / 4, M_PI, M_PI / 2;
+  pinocchio::framesForwardKinematics(m_forward, d_f, q);
+
+  cout << "Forward Ellipsoid oMf body2: \n"
+       << d_f.oMf[m_forward.getFrameId("body2", pinocchio::BODY)].matrix() << endl;
+  BOOST_CHECK(d_f.oMf[m_forward.getFrameId("body2", pinocchio::BODY)].isApprox(
+  // trouver une valeur à mettre dedans.
+  // todo après manger :)
+  ));
+}
+
+
 /// @brief test if reversing of a composite joint is correct.
 BOOST_AUTO_TEST_CASE(test_reverse_composite)
 {