Verified Commit f18f5ca2 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo/models: fix the case where the list of joints is empty

parent deb9ed3b
......@@ -300,7 +300,7 @@ namespace pinocchio
typedef typename Model::JointData JointData;
typedef typename Model::Frame Frame;
typedef typename Model::SE3 SE3;
// Sort indexes
std::sort(list_of_joints_to_lock.begin(),list_of_joints_to_lock.end());
......@@ -317,7 +317,6 @@ namespace pinocchio
reduced_model.joints .reserve((size_t)njoints);
reduced_model.jointPlacements .reserve((size_t)njoints);
reduced_model.parents .reserve((size_t)njoints);
reduced_model.inertias .reserve((size_t)njoints);
reduced_model.names[0] = input_model.names[0];
reduced_model.joints[0] = input_model.joints[0];
......@@ -369,16 +368,12 @@ namespace pinocchio
Frame frame = Frame(joint_name,
reduced_parent_joint_index, reduced_previous_frame_index,
liMi,
FIXED_JOINT);
FIXED_JOINT,
input_model.inertias[joint_id]);
FrameIndex frame_id = reduced_model.addFrame(frame);
reduced_model.frames[frame_id].previousFrame = frame_id; // a bit weird, but this is a solution for missing parent frame
// Add the Inertia of the link supported by joint_id
reduced_model.appendBodyToJoint(reduced_parent_joint_index,
input_model.inertias[joint_id],
frame.placement);
current_index_to_lock++;
}
else
......@@ -436,34 +431,34 @@ namespace pinocchio
for(++frame_it;frame_it != input_model.frames.end(); ++frame_it)
{
const Frame & input_frame = *frame_it;
const std::string & input_joint_name = input_model.names[input_frame.parent];
const std::string & support_joint_name = input_model.names[input_frame.parent];
std::vector<JointIndex>::const_iterator joint_id_it = std::find(list_of_joints_to_lock.begin(),
list_of_joints_to_lock.end(),
input_frame.parent);
std::vector<JointIndex>::const_iterator support_joint_it = std::find(list_of_joints_to_lock.begin(),
list_of_joints_to_lock.end(),
input_frame.parent);
if(joint_id_it != list_of_joints_to_lock.end())
if(support_joint_it != list_of_joints_to_lock.end())
{
if( input_frame.type == JOINT
&& reduced_model.existFrame(input_frame.name)
&& input_joint_name == input_frame.name)
&& support_joint_name == input_frame.name)
continue; // this means that the Joint is now fixed and has been replaced by a Frame. No need to add a new one.
// The joint has been removed and replaced by a Frame
const FrameIndex joint_frame_id = reduced_model.getFrameId(input_joint_name);
const FrameIndex joint_frame_id = reduced_model.getFrameId(support_joint_name);
const Frame & joint_frame = reduced_model.frames[joint_frame_id];
Frame reduced_frame = input_frame;
reduced_frame.placement = joint_frame.placement * input_frame.placement;
reduced_frame.parent = joint_frame.parent;
reduced_frame.previousFrame = reduced_model.getFrameId(input_model.frames[input_frame.previousFrame].name);
reduced_model.addFrame(reduced_frame);
reduced_model.addFrame(reduced_frame, false);
}
else
{
Frame reduced_frame = input_frame;
reduced_frame.parent = reduced_model.getJointId(input_model.names[input_frame.parent]);
reduced_frame.previousFrame = reduced_model.getFrameId(input_model.frames[input_frame.previousFrame].name);
reduced_model.addFrame(reduced_frame);
reduced_model.addFrame(reduced_frame, false);
}
}
}
......
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