Commit a8c2ad85 authored by jcarpent's avatar jcarpent
Browse files

[Parsers] Fix bug when parsing the neutral config

parent c64edb24
......@@ -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);
}
......
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment