diff --git a/CHANGELOG.md b/CHANGELOG.md index 8ab5b11bc6..7a24aba06b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Set NOMINMAX as a public definitions on Windows ([#2139](https://github.com/stack-of-tasks/pinocchio/pull/2139)) ### Fixed +- Order of frames in ReducedModel is now the same as in the full model ([#2160](https://github.com/stack-of-tasks/pinocchio/pull/2160)) - Remove a lot of warnings ([#2139](https://github.com/stack-of-tasks/pinocchio/pull/2139)) - `MeshcatVisualizer` doesn't crash anymore when there is no collision model defined ([#2147](https://github.com/stack-of-tasks/pinocchio/pull/2147)) - Fix MSVC build ([#2155](https://github.com/stack-of-tasks/pinocchio/pull/2155)) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 196974e155..d08f0701ab 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -339,6 +339,8 @@ namespace pinocchio // Sort indexes std::sort(list_of_joints_to_lock.begin(),list_of_joints_to_lock.end()); + typename Model::FrameVector::const_iterator frame_it = input_model.frames.begin(); + // Check that they are not two identical elements for(size_t id = 1; id < list_of_joints_to_lock.size(); ++id) { @@ -366,6 +368,7 @@ namespace pinocchio for(JointIndex joint_id = 1; joint_id < (JointIndex)input_model.njoints; ++joint_id) { + const JointIndex joint_id_to_lock = (current_index_to_lock < list_of_joints_to_lock.size()) ? list_of_joints_to_lock[current_index_to_lock] : 0; const JointIndex input_parent_joint_index = input_model.parents[joint_id]; @@ -394,6 +397,43 @@ namespace pinocchio if(joint_id == joint_id_to_lock) { // the joint should not be added to the Model but aggragated to its parent joint + //Add frames up to the joint to lock + while((*frame_it).name != joint_name) + { + ++frame_it; + const Frame & input_frame = *frame_it; + if(input_frame.name == joint_name) + break; + const std::string & support_joint_name = input_model.names[input_frame.parent]; + + std::vector::const_iterator support_joint_it = std::find(list_of_joints_to_lock.begin(), + list_of_joints_to_lock.end(), + input_frame.parent); + + if(support_joint_it != list_of_joints_to_lock.end()) + { + if( input_frame.type == JOINT + && reduced_model.existFrame(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(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, 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, false); + } + } // Compute the new placement of the joint with respect to its parent joint in the new kinematic tree. JointData joint_data = joint_input_model.createData(); @@ -437,7 +477,7 @@ namespace pinocchio = joint_input_model.jointVelocitySelector(input_model.rotorGearRatio); } } - + // Retrieve and extend the reference configurations typedef typename Model::ConfigVectorMap ConfigVectorMap; for(typename ConfigVectorMap::const_iterator config_it = input_model.referenceConfigurations.begin(); @@ -461,9 +501,8 @@ namespace pinocchio reduced_model.referenceConfigurations.insert(std::make_pair(config_name, reduced_config_vector)); } - // Add all frames - typename Model::FrameVector::const_iterator frame_it = input_model.frames.begin(); - for(++frame_it;frame_it != input_model.frames.end(); ++frame_it) + // Add all the missing frames + for(;frame_it != input_model.frames.end(); ++frame_it) { const Frame & input_frame = *frame_it; const std::string & support_joint_name = input_model.names[input_frame.parent]; diff --git a/unittest/model.cpp b/unittest/model.cpp index 5209777c10..d2995e1315 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -462,6 +462,7 @@ BOOST_AUTO_TEST_CASE(test_buildReducedModel_with_geom) BOOST_CHECK_EQUAL(go1.overrideMaterial, go2.overrideMaterial); BOOST_CHECK_EQUAL(go1.meshColor, go2.meshColor); BOOST_CHECK_EQUAL(go1.meshTexturePath, go2.meshTexturePath); + BOOST_CHECK_EQUAL(go1.parentFrame, go2.parentFrame); BOOST_CHECK_EQUAL(humanoid_model.frames[go1.parentFrame].name, reduced_humanoid_model.frames[go2.parentFrame].name); }