diff --git a/CHANGELOG.md b/CHANGELOG.md index b789835b97..d6f58f5bc3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -28,6 +28,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Remove pixi 0.57 warnings ([#2802](https://github.com/stack-of-tasks/pinocchio/pull/2802)) - CMake: use `sdformat.cmake` from `jrl-cmakemodules` ([#2800](https://github.com/stack-of-tasks/pinocchio/pull/2800)) - Fix doxygen document hierarchy ([#2811](https://github.com/stack-of-tasks/pinocchio/pull/2811)) +- Fix `appendModel` issues ([#2807](https://github.com/stack-of-tasks/pinocchio/pull/2807)): + - Bad inertia transformation when multiple frame with inertia have non null placement + - Undefined behavior when more than two frame was attached to universe ### Removed - Remove CMake < 3.22 details ([#2790](https://github.com/stack-of-tasks/pinocchio/pull/2790)) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 6997e6207d..f5ee5235f4 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -70,13 +70,13 @@ namespace pinocchio parentFrame < model.frames.size(), std::invalid_argument, "parentFrame is greater than the size of the frames vector."); - const Frame & pframe = model.frames[parentFrame]; - JointIndex jid = pframe.parentJoint; + const auto pframe_placement = model.frames[parentFrame].placement; + JointIndex jid = model.frames[parentFrame].parentJoint; assert(jid < model.joints.size()); // If inertia is not NaN, add it. if (modelAB.inertias[0] == modelAB.inertias[0]) - model.appendBodyToJoint(jid, modelAB.inertias[0], pframe.placement * pfMAB); + model.appendBodyToJoint(jid, modelAB.inertias[0], pframe_placement * pfMAB); // Add all frames whose parent is this joint. for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid) @@ -101,15 +101,10 @@ namespace pinocchio } // Modify frame placement - frame.placement = pframe.placement * pfMAB * frame.placement; - // Some frames may have some inertia attached to them. In this case, we need to remove it - // from the parent joint. To prevent introducing NaNs, we check if the frame inertia is - // not NaN and is not zero. - if (frame.inertia == frame.inertia && frame.inertia != Inertia::Zero()) - { - model.inertias[frame.parentJoint] -= frame.inertia; - } - model.addFrame(frame); + frame.placement = pframe_placement * pfMAB * frame.placement; + // Inertias are already computed in model.appendBodyToJoint call. + // No need to append them again. + model.addFrame(frame, false); } } @@ -130,7 +125,7 @@ namespace pinocchio { go.parentFrame = parentFrame; } - go.placement = (pframe.placement * pfMAB) * go.placement; + go.placement = (pframe_placement * pfMAB) * go.placement; geomModel.addGeometryObject(go); } } @@ -248,14 +243,9 @@ namespace pinocchio modelAB, model, modelAB.frames[frame.parentFrame].name, modelAB.frames[frame.parentFrame].type); } - // Some frames may have some inertia attached to them. In this case, we need to remove - // it from the parent joint. To prevent introducing NaNs, we check if the frame inertia - // is not NaN and is not zero. - if (frame.inertia == frame.inertia && frame.inertia != Inertia::Zero()) - { - model.inertias[frame.parentJoint] -= frame.inertia; - } - model.addFrame(frame); + // Inertias are already computed in modelAB.inertias[joint_id_in]. + // No need to append them again. + model.addFrame(frame, false); } } // Add all geometries whose parent is this joint. diff --git a/unittest/model.cpp b/unittest/model.cpp index 4c65560761..7d67a0c75b 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -1098,4 +1098,75 @@ BOOST_AUTO_TEST_CASE(test_has_configuration_limit_mimic) BOOST_CHECK_EQUAL(model.hasConfigurationLimitInTangent().size(), 2); } +// Non regression test for: https://github.com/stack-of-tasks/pinocchio/issues/2805 +// When a frame with non null placement have inertia, the inertia was not well transformed +// in the support joint. +BOOST_AUTO_TEST_CASE(test_append_model_issue_2805) +{ + // Create robot with a fixed joint + Model model_fixed_joint; + Inertia I(1000, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity() * 166); + SE3 translation(Eigen::Matrix3d::Identity(), Eigen::Vector3d::UnitX()); + auto root_joint_index = model_fixed_joint.addJoint( + 0, pinocchio::JointModel(pinocchio::JointModelFreeFlyer()), SE3::Identity(), "root_joint"); + auto root_joint_frame_index = + model_fixed_joint.addFrame(Frame("root_joint", root_joint_index, 0, SE3::Identity(), JOINT)); + auto link_1_index = model_fixed_joint.addFrame( + Frame("link_1", root_joint_index, root_joint_frame_index, SE3::Identity(), BODY, I)); + auto joint1_fixed_index = model_fixed_joint.addFrame( + Frame("urdf_joint_1fixed", root_joint_index, link_1_index, translation, FIXED_JOINT)); + model_fixed_joint.addFrame( + Frame("link_2", root_joint_index, joint1_fixed_index, translation, BODY, I)); + + Model model_root; + auto new_model = appendModel(model_root, model_fixed_joint, 0, SE3::Identity()); + + BOOST_CHECK(new_model.inertias[1].isApprox(model_fixed_joint.inertias[1])); +} + +// Non regression test for: https://github.com/stack-of-tasks/pinocchio/issues/2805 +// Same test as above but verify the special case of frame attached to universe frame. +BOOST_AUTO_TEST_CASE(test_append_model_universe_with_inertia_issue_2805) +{ + Inertia I(1000, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity() * 166); + SE3 translation_root_joint(Eigen::Matrix3d::Identity(), Eigen::Vector3d::UnitX()); + SE3 translation_fixed_link_1(Eigen::Matrix3d::Identity(), Eigen::Vector3d::UnitY()); + SE3 translation_fixed_link_2(Eigen::Matrix3d::Identity(), Eigen::Vector3d(1., 0., 1.)); + SE3 translation_AB(Eigen::Matrix3d::Identity(), Eigen::Vector3d::UnitZ()); + + // Base model + Model model_a; + auto model_a_root_joint_index = model_a.addJoint( + 0, pinocchio::JointModel(pinocchio::JointModelFreeFlyer()), SE3::Identity(), "root_joint"); + auto model_a_root_joint_frame = model_a.addFrame( + Frame("root_joint", model_a_root_joint_index, 0, translation_root_joint, JOINT)); + + //  Model with frame and inertia attached to universe. + // To reproduce the issue, we must have two frame with inertia attached to universe. + // With only one frame, some computation will give a 0 mass inertia that will hide the bug. + Model model_b; + model_b.addFrame(Frame("fixed_link_1", 0, 0, translation_fixed_link_1, BODY, I)); + model_b.addFrame(Frame("fixed_link_2", 0, 0, translation_fixed_link_2, BODY, I)); + + Model expected_model; + auto expected_model_root_joint_index = expected_model.addJoint( + 0, pinocchio::JointModel(pinocchio::JointModelFreeFlyer()), SE3::Identity(), "root_joint"); + auto expected_model_root_joint_frame_index = expected_model.addFrame( + Frame("root_joint", expected_model_root_joint_index, 0, translation_root_joint, JOINT)); + auto fixed_link_1_frame_index = expected_model.addFrame(Frame( + "fixed_link_1", expected_model_root_joint_index, expected_model_root_joint_frame_index, + translation_root_joint.act(translation_AB.act(translation_fixed_link_1)), BODY, I)); + auto fixed_link_2_frame_index = expected_model.addFrame(Frame( + "fixed_link_2", expected_model_root_joint_index, expected_model_root_joint_frame_index, + translation_root_joint.act(translation_AB.act(translation_fixed_link_2)), BODY, I)); + + // Test frame position and inertias attached to root_joint + auto new_model = appendModel(model_a, model_b, model_a_root_joint_frame, translation_AB); + BOOST_CHECK(new_model.frames[fixed_link_1_frame_index].placement.isApprox( + expected_model.frames[fixed_link_1_frame_index].placement)); + BOOST_CHECK(new_model.frames[fixed_link_2_frame_index].placement.isApprox( + expected_model.frames[fixed_link_2_frame_index].placement)); + BOOST_CHECK(new_model.inertias[1].isApprox(expected_model.inertias[1])); +} + BOOST_AUTO_TEST_SUITE_END()