[srdf][referenceconfig] unittest

stack-of-tasks · Feb 13, 2019 · 5f4036e · 5f4036e
1 parent a24594d
commit 5f4036e
Show file tree

Hide file tree

Showing 2 changed files with 43 additions and 4 deletions.
diff --git a/models/simple_humanoid.srdf b/models/simple_humanoid.srdf
@@ -37,7 +37,40 @@
         <joint name="RLEG_ANKLE_P"  value="0.0" />
         <joint name="RLEG_ANKLE_R"  value="0.0" />
     </group_state>
-
+
+  <group_state name="half_sitting2" group="all">
+        <joint name="root_joint" value="0. 0. 1. 0. 0. 0. 1." />
+        <joint name="WAIST_P" value="-1.0" />
+        <joint name="WAIST_R"  value="1.0" />
+        <joint name="CHEST"  value="0.0" />
+        <joint name="LARM_SHOULDER_P"  value="0.0" />
+        <joint name="LARM_SHOULDER_R"  value="0.0" />
+        <joint name="LARM_SHOULDER_Y"  value="0.0" />
+        <joint name="LARM_ELBOW"  value="0.0" />
+        <joint name="LARM_WRIST_Y"  value="0.0" />
+        <joint name="LARM_WRIST_P"  value="0.0" />
+        <joint name="LARM_WRIST_R"  value="0.0" />
+        <joint name="RARM_SHOULDER_P"  value="0.0" />
+        <joint name="RARM_SHOULDER_R"  value="0.0" />
+        <joint name="RARM_SHOULDER_Y"  value="0.0" />
+        <joint name="RARM_ELBOW"  value="0.0" />
+        <joint name="RARM_WRIST_Y"  value="0.0" />
+        <joint name="RARM_WRIST_P"  value="0.0" />
+        <joint name="RARM_WRIST_R"  value="0.0" />
+        <joint name="LLEG_HIP_R"  value="0.0" />
+        <joint name="LLEG_HIP_P"  value="0.0" />
+        <joint name="LLEG_HIP_Y"  value="0.0" />
+        <joint name="LLEG_KNEE"  value="0.0" />
+        <joint name="LLEG_ANKLE_P"  value="0.0" />
+        <joint name="LLEG_ANKLE_R"  value="0.0" />
+        <joint name="RLEG_HIP_R"  value="0.0" />
+        <joint name="RLEG_HIP_P"  value="0.0" />
+        <joint name="RLEG_HIP_Y"  value="0.0" />
+        <joint name="RLEG_KNEE"  value="0.0" />
+        <joint name="RLEG_ANKLE_P"  value="0.0" />
+        <joint name="RLEG_ANKLE_R"  value="0.0" />
+    </group_state>
+
     <rotor_params>
         <joint name="WAIST_P" mass="1.0" gear_ratio="0.0" />
         <joint name="WAIST_R" mass="0.0" gear_ratio="1.0" />

diff --git a/unittest/srdf.cpp b/unittest/srdf.cpp
@@ -40,7 +40,7 @@ BOOST_AUTO_TEST_CASE(test_removeCollisionPairs)
   BOOST_CHECK(num_init_col_pairs > num_col_pairs);
 }
 
-BOOST_AUTO_TEST_CASE(readNeutralConfig)
+BOOST_AUTO_TEST_CASE(readReferenceConfig)
 {
   using namespace pinocchio::urdf;
   using namespace pinocchio::srdf;
@@ -50,10 +50,16 @@ BOOST_AUTO_TEST_CASE(readNeutralConfig)
   Model model;
   buildModel(model_filename, model);
 
-  Eigen::VectorXd q = getNeutralConfiguration(model,srdf_filename,false);
-
+  loadReferenceConfigurations(model,srdf_filename,false);
+  Eigen::VectorXd q = model.referenceConfigurations["half_sitting"];
+  Eigen::VectorXd q2 = model.referenceConfigurations["half_sitting2"];
   BOOST_CHECK(q.size() == model.nq);
   BOOST_CHECK(!q.isZero());
+
+  BOOST_CHECK(q2.size() == model.nq);
+  BOOST_CHECK(!q2.isZero());
+
+
 }
 
 BOOST_AUTO_TEST_CASE(readRotorParams)