Merge pull request #2807 from jorisv/topic/repro-append-model-issue

Fix appendModel issue when a frame have non null transformation

Author: jorisv

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))

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.

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()