[Parsers] Fix bug when parsing the neutral config

src/multibody/model.hxx

@@ -172,7 +172,6 @@ namespace se3
     typedef std::vector<std::string>::iterator::difference_type it_diff_t;
     it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin();
     assert( (res<INT_MAX) && "Id superior to int range. Should never happen.");
-    assert( (res>=0)&&(res<(it_diff_t) joints.size()) && "The joint name you asked does not exist" );
     return Model::JointIndex(res);
   }
   

src/parsers/srdf.hpp

@@ -171,7 +171,6 @@ namespace se3
         if (v.first == "group_state")
         {
           const std::string name = v.second.get<std::string>("<xmlattr>.name");
-          std::cout << name << std::endl;
           // Ensure that it is the half_sitting tag
           if( name == "half_sitting")
           {
@@ -188,10 +187,10 @@ namespace se3
                 }
                 // Search in model the joint and its config id
                 Model::JointIndex joint_id = model.getJointId(joint_name);
-                const JointModel & joint = model.joints[joint_id];
 
                 if (joint_id != model.joints.size()) // != model.njoints
                 {
+                  const JointModel & joint = model.joints[joint_id];
                   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
                 }
@@ -203,7 +202,6 @@ namespace se3
             }
             return neutralConfig;
           }
-
           
         }
       } // BOOST_FOREACH