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
...@@ -317,7 +317,6 @@ namespace pinocchio ...@@ -317,7 +317,6 @@ namespace pinocchio
reduced_model.joints .reserve((size_t)njoints); reduced_model.joints .reserve((size_t)njoints);
reduced_model.jointPlacements .reserve((size_t)njoints); reduced_model.jointPlacements .reserve((size_t)njoints);
reduced_model.parents .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.names[0] = input_model.names[0];
reduced_model.joints[0] = input_model.joints[0]; reduced_model.joints[0] = input_model.joints[0];
...@@ -369,16 +368,12 @@ namespace pinocchio ...@@ -369,16 +368,12 @@ namespace pinocchio
Frame frame = Frame(joint_name, Frame frame = Frame(joint_name,
reduced_parent_joint_index, reduced_previous_frame_index, reduced_parent_joint_index, reduced_previous_frame_index,
liMi, liMi,
FIXED_JOINT); FIXED_JOINT,
input_model.inertias[joint_id]);
FrameIndex frame_id = reduced_model.addFrame(frame); 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 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++; current_index_to_lock++;
} }
else else
...@@ -436,34 +431,34 @@ namespace pinocchio ...@@ -436,34 +431,34 @@ namespace pinocchio
for(++frame_it;frame_it != input_model.frames.end(); ++frame_it) for(++frame_it;frame_it != input_model.frames.end(); ++frame_it)
{ {
const Frame & input_frame = *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(), std::vector<JointIndex>::const_iterator support_joint_it = std::find(list_of_joints_to_lock.begin(),
list_of_joints_to_lock.end(), list_of_joints_to_lock.end(),
input_frame.parent); 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 if( input_frame.type == JOINT
&& reduced_model.existFrame(input_frame.name) && 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. 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 // 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]; const Frame & joint_frame = reduced_model.frames[joint_frame_id];
Frame reduced_frame = input_frame; Frame reduced_frame = input_frame;
reduced_frame.placement = joint_frame.placement * input_frame.placement; reduced_frame.placement = joint_frame.placement * input_frame.placement;
reduced_frame.parent = joint_frame.parent; reduced_frame.parent = joint_frame.parent;
reduced_frame.previousFrame = reduced_model.getFrameId(input_model.frames[input_frame.previousFrame].name); 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 else
{ {
Frame reduced_frame = input_frame; Frame reduced_frame = input_frame;
reduced_frame.parent = reduced_model.getJointId(input_model.names[input_frame.parent]); 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_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