Unittest - update joint unittests to include spline joints

Author: MegMll

unittest/all-joints.cpp

@@ -167,6 +167,23 @@ struct init<pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
   }
 };
 
+template<typename Scalar, int Options>
+struct init<pinocchio::JointModelSplineTpl<Scalar, Options>>
+{
+  typedef pinocchio::JointModelSplineTpl<Scalar, Options> JointModel;
+
+  static JointModel run()
+  {
+    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames;
+    ctrlFrames.push_back(SE3::Identity());
+    ctrlFrames.push_back(SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.)));
+    JointModel jmodel(ctrlFrames, 1);
+
+    jmodel.setIndexes(0, 0, 0);
+    return jmodel;
+  }
+};
+
 BOOST_AUTO_TEST_SUITE(joint_model_base_test)
 
 template<typename TestDerived>

unittest/finite-differences.cpp

@@ -212,6 +212,23 @@ struct init<pinocchio::JointModelMimicTpl<Scalar, Options, JointCollection>>
   }
 };
 
+template<typename Scalar, int Options>
+struct init<pinocchio::JointModelSplineTpl<Scalar, Options>>
+{
+  typedef pinocchio::JointModelSplineTpl<Scalar, Options> JointModel;
+
+  static JointModel run()
+  {
+    PINOCCHIO_ALIGNED_STD_VECTOR(pinocchio::SE3) ctrlFrames;
+    ctrlFrames.push_back(pinocchio::SE3::Identity());
+    ctrlFrames.push_back(pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.)));
+    JointModel jmodel(ctrlFrames, 1);
+
+    jmodel.setIndexes(0, 0, 0);
+    return jmodel;
+  }
+};
+
 struct FiniteDiffJoint
 {
   void operator()(JointModelComposite & /*jmodel*/) const

unittest/joint-generic.cpp

@@ -279,6 +279,23 @@ struct init<pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
   }
 };
 
+template<typename Scalar, int Options>
+struct init<pinocchio::JointModelSplineTpl<Scalar, Options>>
+{
+  typedef pinocchio::JointModelSplineTpl<Scalar, Options> JointModel;
+
+  static JointModel run()
+  {
+    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames;
+    ctrlFrames.push_back(SE3::Identity());
+    ctrlFrames.push_back(SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.)));
+    JointModel jmodel(ctrlFrames, 1);
+
+    jmodel.setIndexes(0, 0, 0);
+    return jmodel;
+  }
+};
+
 struct TestJoint
 {
 

unittest/joint-motion-subspace.cpp

@@ -418,6 +418,23 @@ struct init<pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
   }
 };
 
+template<typename Scalar, int Options>
+struct init<pinocchio::JointModelSplineTpl<Scalar, Options>>
+{
+  typedef pinocchio::JointModelSplineTpl<Scalar, Options> JointModel;
+
+  static JointModel run()
+  {
+    PINOCCHIO_ALIGNED_STD_VECTOR(pinocchio::SE3) ctrlFrames;
+    ctrlFrames.push_back(pinocchio::SE3::Identity());
+    ctrlFrames.push_back(pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.)));
+    JointModel jmodel(ctrlFrames, 1);
+
+    jmodel.setIndexes(0, 0, 0);
+    return jmodel;
+  }
+};
+
 struct TestJointConstraint
 {
 

unittest/joint-spline.cpp

@@ -50,14 +50,14 @@ BOOST_AUTO_TEST_CASE(vsPrismatic)
 
   JointModelSpline jmodel(ctrlFrames, 1);
   JointDataSpline jdata = jmodel.createData();
-  
+
   jmodel.setIndexes(0, 0, 0);
 
   Eigen::VectorXd q(Eigen::VectorXd::Zero(1));
 
   // -------
   q << 0.2;
-  
+
   jmodel.calc(jdata, q);
 
   BOOST_CHECK(expected_configuration.rotation().isApprox(jdata.M.rotation(), 1e-12));
@@ -73,7 +73,7 @@ BOOST_AUTO_TEST_CASE(vsPrismatic)
 
   expected_configuration.translation() << 0, 0, 0.3;
   expected_v_J.linear() << 0., 0., 0.4;
-  
+
   BOOST_CHECK(expected_configuration.rotation().isApprox(jdata.M.rotation(), 1e-12));
   BOOST_CHECK(expected_configuration.translation().isApprox(jdata.M.translation(), 1e-12));
   BOOST_CHECK(expected_v_J.toVector().isApprox(((Motion)jdata.v).toVector(), 1e-12));
@@ -89,7 +89,6 @@ BOOST_AUTO_TEST_CASE(vsPrismatic)
 
 //   Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
 
-
 //   PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames;
 //   ctrlFrames.push_back(SE3::Identity());
 
@@ -132,16 +131,16 @@ BOOST_AUTO_TEST_CASE(vsPrismatic)
 //   ctrlFrames.push_back(SE3(R4, Eigen::Vector3d::Zero()));
 
 //   Model modelSpline;
-//   addJointAndBody(modelSpline, JointModelSpline(ctrlFrames), 0, SE3::Identity(), "spline_joint", inertia);
-//   Data dataSpline(modelSpline);
+//   addJointAndBody(modelSpline, JointModelSpline(ctrlFrames), 0, SE3::Identity(), "spline_joint",
+//   inertia); Data dataSpline(modelSpline);
 
 //   Model modelSph;
 //   addJointAndBody(modelSph, JointModelSphericalZYX(), 0, SE3::Identity(), "sph_joint", inertia);
 //   Data dataSph(modelSph);
 
 //   Eigen::VectorXd qSpline(Eigen::VectorXd::Zero(1));
 //   qSpline << 0.5;
-  
+
 //   Eigen::Vector3d qSph;
 //   qSph << 0, M_PI / 3, 0;
 
@@ -153,7 +152,4 @@ BOOST_AUTO_TEST_CASE(vsPrismatic)
 //   std::cout << dataSph.oMi[1] << std::endl;
 // }
 
-
-
-
-BOOST_AUTO_TEST_SUITE_END()
+BOOST_AUTO_TEST_SUITE_END()

unittest/serialization.cpp

@@ -412,6 +412,23 @@ struct init<pinocchio::JointModelMimicTpl<Scalar, Options, JointCollection>>
   }
 };
 
+template<typename Scalar, int Options>
+struct init<pinocchio::JointModelSplineTpl<Scalar, Options>>
+{
+  typedef pinocchio::JointModelSplineTpl<Scalar, Options> JointModel;
+
+  static JointModel run()
+  {
+    PINOCCHIO_ALIGNED_STD_VECTOR(pinocchio::SE3) ctrlFrames;
+    ctrlFrames.push_back(pinocchio::SE3::Identity());
+    ctrlFrames.push_back(pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.)));
+    JointModel jmodel(ctrlFrames, 1);
+
+    jmodel.setIndexes(0, 0, 0);
+    return jmodel;
+  }
+};
+
 struct TestJointModel
 {
   template<typename JointModel>