Adding unittest for new api

Author: Megane Millan

unittest/mjcf.cpp

@@ -1092,6 +1092,21 @@ BOOST_AUTO_TEST_CASE(build_model_no_root_joint)
   BOOST_CHECK_EQUAL(model_m.nq, 29);
 }
 
+BOOST_AUTO_TEST_CASE(build_model_with_root_joint_name)
+{
+  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.xml");
+  const std::string dir = PINOCCHIO_MODEL_DIR;
+
+  pinocchio::Model model;
+  pinocchio::mjcf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
+  BOOST_CHECK(model.names[1] == "root_joint");
+
+  pinocchio::Model model_name;
+  const std::string name_ = "freeFlyer_joint";
+  pinocchio::mjcf::buildModel(filename, pinocchio::JointModelFreeFlyer(), name_, model_name);
+  BOOST_CHECK(model_name.names[1] == name_);
+}
+
 #ifdef PINOCCHIO_WITH_URDFDOM
 /// @brief Test all the data of the humanoid model (Need to find the urdf yet)
 /// @param

unittest/python/robot_wrapper.py

@@ -12,7 +12,7 @@ class TestRobotWrapper(unittest.TestCase):
     def setUp(self):
         self.current_file = os.path.dirname(str(os.path.abspath(__file__)))
 
-    def test_mjcf(self):
+    def test_mjcf_without_root_joint(self):
         model_path = os.path.abspath(
             os.path.join(self.current_file, "../models/test_mjcf.xml")
         )
@@ -21,6 +21,59 @@ def test_mjcf(self):
         self.assertEqual(robot.nv, 5)
         self.assertEqual(robot.model.njoints, 4)
 
+    def test_mjcf_with_root_joint(self):
+        model_path = os.path.abspath(
+            os.path.join(self.current_file, "../models/test_mjcf.xml")
+        )
+        robot = pin.RobotWrapper.BuildFromMJCF(model_path, pin.JointModelFreeFlyer())
+        self.assertEqual(robot.model.names[1], "root_joint")
+
+    def test_mjcf_with_root_joint_and_root_joint_name(self):
+        model_path = os.path.abspath(
+            os.path.join(self.current_file, "../models/test_mjcf.xml")
+        )
+        name_ = "freeflyer_joint"
+        robot = pin.RobotWrapper.BuildFromMJCF(
+            model_path, pin.JointModelFreeFlyer(), name_
+        )
+        self.assertEqual(robot.model.names[1], name_)
+
+    def test_urdf_with_root_joint(self):
+        model_path = os.path.abspath(
+            os.path.join(self.current_file, "../models/3DOF_planar.urdf")
+        )
+        robot = pin.RobotWrapper.BuildFromURDF(
+            model_path, [], pin.JointModelFreeFlyer()
+        )
+        self.assertEqual(robot.model.names[1], "root_joint")
+
+    def test_urdf_with_root_joint_and_root_joint_name(self):
+        model_path = os.path.abspath(
+            os.path.join(self.current_file, "../models/3DOF_planar.urdf")
+        )
+        name_ = "freeflyer_joint"
+        robot = pin.RobotWrapper.BuildFromURDF(
+            model_path, [], pin.JointModelFreeFlyer(), name_
+        )
+        self.assertEqual(robot.model.names[1], name_)
+
+    def test_sdf_with_root_joint(self):
+        model_path = os.path.abspath(
+            os.path.join(self.current_file, "../../models/simple_humanoid.sdf")
+        )
+        robot = pin.RobotWrapper.BuildFromSDF(model_path, [], pin.JointModelFreeFlyer())
+        self.assertEqual(robot.model.names[1], "root_joint")
+
+    def test_sdf_with_root_joint_and_root_joint_name(self):
+        model_path = os.path.abspath(
+            os.path.join(self.current_file, "../../models/simple_humanoid.sdf")
+        )
+        name_ = "freeflyer_joint"
+        robot = pin.RobotWrapper.BuildFromSDF(
+            model_path, [], pin.JointModelFreeFlyer(), root_joint_name=name_
+        )
+        self.assertEqual(robot.model.names[1], name_)
+
 
 if __name__ == "__main__":
     unittest.main()

unittest/sdf.cpp

@@ -66,6 +66,26 @@ BOOST_AUTO_TEST_CASE(build_model_without_rootLink)
   BOOST_CHECK(model.nq == 69);
 }
 
+BOOST_AUTO_TEST_CASE(build_model_with_root_joint_name)
+{
+  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.sdf");
+  const std::string dir = PINOCCHIO_MODEL_DIR;
+  const std::string rootLinkName = "WAIST_LINK0";
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) contact_models;
+  pinocchio::Model model;
+  pinocchio::sdf::buildModel(
+    filename, pinocchio::JointModelFreeFlyer(), model, contact_models, rootLinkName);
+  BOOST_CHECK(model.names[1] == "root_joint");
+
+  pinocchio::Model model_name;
+  const std::string name_ = "freeFlyer_joint";
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) contact_models_name;
+  pinocchio::sdf::buildModel(
+    filename, pinocchio::JointModelFreeFlyer(), name_, model_name, contact_models_name,
+    rootLinkName);
+  BOOST_CHECK(model_name.names[1] == name_);
+}
+
 BOOST_AUTO_TEST_CASE(compare_model_with_urdf)
 {
   const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.sdf");

unittest/urdf.cpp

@@ -34,6 +34,21 @@ BOOST_AUTO_TEST_CASE(build_model)
   BOOST_CHECK(model.nq == 31);
 }
 
+BOOST_AUTO_TEST_CASE(build_model_with_root_joint_name)
+{
+  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
+  const std::string dir = PINOCCHIO_MODEL_DIR;
+
+  pinocchio::Model model;
+  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model);
+  BOOST_CHECK(model.names[1] == "root_joint");
+
+  pinocchio::Model model_name;
+  const std::string name_ = "freeFlyer_joint";
+  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), name_, model_name);
+  BOOST_CHECK(model_name.names[1] == name_);
+}
+
 BOOST_AUTO_TEST_CASE(build_model_simple_humanoid)
 {
   const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");