Enable velocity checks create wrong world components for link entity in which they don't yet exist

[gabrielfpacheco]

Environment

• OS Version: Ubuntu 24.04
• Source or binary build?
  • Binary: tested with Harmonic
  • Source: tested in branches gz-sim8 and gz-sim9

Description

When using the link API to enable velocity checks for a link that does not yet have the world ECM components (WorldPose, WorldLinearVelocity, WorldAngularVelocity), typically before the physics system plugin does its first update loop, such components will be initialized with their default constructor, zeroing values that might not be zero. One more evident example is the link world pose that could have already been initialized.

• Expected behavior:

Add the following test case:

$ git diff src/Link_TEST.cc
diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc
index b2653ca2d..cde89c88a 100644
--- a/src/Link_TEST.cc
+++ b/src/Link_TEST.cc
@@ -19,9 +19,14 @@
 
 #include "gz/sim/EntityComponentManager.hh"
 #include "gz/sim/Link.hh"
+#include "gz/sim/Util.hh"
+#include "gz/sim/components/AngularVelocity.hh"
+#include "gz/sim/components/LinearVelocity.hh"
 #include "gz/sim/components/Link.hh"
+#include "gz/sim/components/Model.hh"
 #include "gz/sim/components/Name.hh"
 #include "gz/sim/components/ParentEntity.hh"
+#include "gz/sim/components/Pose.hh"
 #include "gz/sim/components/Sensor.hh"
 
 /////////////////////////////////////////////////
@@ -139,3 +144,46 @@ TEST(LinkTest, Sensors)
   EXPECT_EQ(0u, linkC.Sensors(ecm).size());
   EXPECT_EQ(gz::sim::kNullEntity, linkC.SensorByName(ecm, "invalid"));
 }
+
+TEST(LinkTest, EnableVelocityChecksComputeWrongWorldComponents)
+{More--
+  gz::sim::EntityComponentManager ecm;
+
+  gz::math::Pose3d modelWorldPose(-1.2, -3.4, -5.6, 0.1, 0.2, 0.3);
+  gz::math::Vector3d linkWorldLinvel(0.1, 0.2, -0.3);
+  gz::math::Vector3d linkWorldAngvel(0.04, -0.05, 0.06);
+
+  gz::math::Vector3d bodyLinvel = modelWorldPose.Rot().RotateVectorReverse(linkWorldLinvel);
+  gz::math::Vector3d bodyAngvel = modelWorldPose.Rot().RotateVectorReverse(linkWorldAngvel);
+
+  gz::sim::Entity modelEntity = ecm.CreateEntity();
+
+  ecm.CreateComponent(modelEntity, gz::sim::components::Model());
+  ecm.CreateComponent(modelEntity, gz::sim::components::Name("model_name"));
+  ecm.CreateComponent(modelEntity, gz::sim::components::Pose(modelWorldPose));
+
+  gz::sim::Entity linkEntity = ecm.CreateEntity();
+
+  ecm.CreateComponent(linkEntity, gz::sim::components::Link());
+  ecm.CreateComponent(linkEntity, gz::sim::components::Name("link_name"));
+  ecm.CreateComponent(linkEntity, gz::sim::components::ParentEntity(modelEntity));
+  ecm.CreateComponent(linkEntity, gz::sim::components::Pose());
+  ecm.CreateComponent(linkEntity, gz::sim::components::LinearVelocity(bodyLinvel));
+  ecm.CreateComponent(linkEntity, gz::sim::components::AngularVelocity(bodyAngvel));
+
+  EXPECT_EQ(modelWorldPose, gz::sim::worldPose(linkEntity, ecm));
+
+  gz::sim::Link link(linkEntity);
+
+  EXPECT_EQ(bodyLinvel, ecm.Component<gz::sim::components::LinearVelocity>(linkEntity)->Data());
+  EXPECT_EQ(bodyAngvel, ecm.Component<gz::sim::components::AngularVelocity>(linkEntity)->Data());
+  EXPECT_EQ(modelWorldPose, link.WorldPose(ecm).value());
+
+  link.EnableVelocityChecks(ecm, true);
+
+  EXPECT_EQ(bodyLinvel, ecm.Component<gz::sim::components::LinearVelocity>(linkEntity)->Data());
+  EXPECT_EQ(bodyAngvel, ecm.Component<gz::sim::components::AngularVelocity>(linkEntity)->Data());
+  EXPECT_EQ(modelWorldPose, link.WorldPose(ecm).value());
+  EXPECT_EQ(linkWorldLinvel, link.WorldLinearVelocity(ecm).value());
+  EXPECT_EQ(linkWorldAngvel, link.WorldAngularVelocity(ecm).value());
+}

One would expect that the link world pose would not change after simply enabling the velocity checks, but this component data is zeroed together with both world linear and angular velocities even though the body linear and angular velocities are not zero.

• Actual behavior:

The test fails as shown bellow:

[ RUN      ] LinkTest.EnableVelocityChecksComputeWrongWorldComponents
/home/gzsim/dev/gzsim/src/gz-sim/src/Link_TEST.cc:187: Failure
Expected equality of these values:
  modelWorldPose
    Which is: -1.2 -3.4 -5.6 0.1 0.2 0.3
  link.WorldPose(ecm).value()
    Which is: 0 0 0 0 0 0
/home/gzsim/dev/gzsim/src/gz-sim/src/Link_TEST.cc:188: Failure
Expected equality of these values:
  linkWorldLinvel
    Which is: 0.1 0.2 -0.3
  link.WorldLinearVelocity(ecm).value()
    Which is: 0 0 0
/home/gzsim/dev/gzsim/src/gz-sim/src/Link_TEST.cc:189: Failure
Expected equality of these values:
  linkWorldAngvel
    Which is: 0.04 -0.05 0.06
  link.WorldAngularVelocity(ecm).value()
    Which is: 0 0 0
[  FAILED  ] LinkTest.EnableVelocityChecksComputeWrongWorldComponents (0 ms)

Steps to reproduce

1. Add the unit test mentioned in the previous section to gz-sim/src/Link_TEST.cc
2. Build the package: colcon build --packages-select gz-sim9
3. Execute the test from your build folder: ./bin/UNIT_Link_TEST --gtest-filter=*EnableVelocityChecks*
4. You should verify the tests will fail because all world components are zero