Load neutralconfiguration from srdf for joint.nq()>1 (#652)

* [parsers][srdf][neutralConfiguration] Update verbosity. If found, display joint and value. If not found, display error message. Don't display both.

* [parsers][srdf][neutralConfiguration] remove ambiguity in name of xml tag and pinocchio::Joint joint instance

* [parsers][srdf][neutralConfiguration] Load q0 value for joints with nq>1

* [srdf][unittest][neutralConfig] Add joint state to simple humanoid

* [srdf][unittest] Use simple_humanoid for loading q0.

* [srdf][neutralConfiguration] remove scalar parsing for nq()==1. Simplify Eigen::Map template

models/simple_humanoid.srdf

@@ -5,6 +5,39 @@
 -->
 <robot name="romeo">
 
+  <group_state name="half_sitting" 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" />

src/parsers/srdf.hxx

@@ -209,7 +209,7 @@ namespace pinocchio
     {
       typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
       typedef typename Model::JointModel JointModel;
-      
+
       // Check extension
       const std::string extension = filename.substr(filename.find_last_of('.')+1);
       if (extension != "srdf")
@@ -242,30 +242,33 @@ namespace pinocchio
           if( name == "half_sitting")
           {
             // Iterate over all the joint tags
-            BOOST_FOREACH(const ptree::value_type & joint, v.second)
+            BOOST_FOREACH(const ptree::value_type & joint_tag, v.second)
             {
-              if (joint.first == "joint")
+              if (joint_tag.first == "joint")
               {
-                std::string joint_name = joint.second.get<std::string>("<xmlattr>.name");
-                const Scalar joint_config = (Scalar)joint.second.get<double>("<xmlattr>.value");
-                if (verbose)
-                {
-                  std::cout << "(" << joint_name << " , " << joint_config << ")" << std::endl;
-                }
-                // Search in model the joint and its config id
+                std::string joint_name = joint_tag.second.get<std::string>("<xmlattr>.name");
                 typename Model::JointIndex joint_id = model.getJointId(joint_name);
 
+                // Search in model the joint and its config id
                 if (joint_id != model.joints.size()) // != model.njoints
                 {
                   const JointModel & joint = model.joints[joint_id];
-                  assert(joint.nq() == 1 && "SRDF:half_sitting only handles 1 DoF joints");
-                  model.neutralConfiguration(joint.idx_q()) = joint_config; // joint with 1 dof
-                  // model.neutralConfiguration.segment(joint.idx_q(),joint.nq()) = joint_config; // joint with more than 1 dof
+                  typename Model::ConfigVectorType joint_config(joint.nq());
+                  const std::string joint_val = joint_tag.second.get<std::string>("<xmlattr>.value");
+                  std::istringstream config_string(joint_val);
+                  std::vector<double> config_vec((std::istream_iterator<double>(config_string)), std::istream_iterator<double>());
+                  joint_config = Eigen::Map<typename Model::ConfigVectorType>(config_vec.data(), config_vec.size());
+                  model.neutralConfiguration.segment(joint.idx_q(),joint.nq()) = joint_config;
+                  if (verbose)
+                  {
+                    std::cout << "(" << joint_name << " , " << joint_config.transpose() << ")" << std::endl;
+                  }
                 }
                 else
                 {
                   if (verbose) std::cout << "The Joint " << joint_name << " was not found in model" << std::endl;
                 }
+
               }
             }
             return model.neutralConfiguration;

unittest/srdf.cpp

@@ -44,8 +44,8 @@ BOOST_AUTO_TEST_CASE(readNeutralConfig)
 {
   using namespace pinocchio::urdf;
   using namespace pinocchio::srdf;
-  const string model_filename = PINOCCHIO_SOURCE_DIR"/models/romeo/romeo_description/urdf/romeo_small.urdf";
-  const string srdf_filename = PINOCCHIO_SOURCE_DIR"/models/romeo/romeo_description/srdf/romeo.srdf";
+  const string model_filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
+  const string srdf_filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.srdf";
   
   Model model;
   buildModel(model_filename, model);