Fix initialization of world components when enabling velocity checks (#2777)

Retain already initialized world component values when
enabling velocity checks and adjust expectations
accordingly in mesh_inertia_calulation integration tests.

Signed-off-by: Gabriel Pacheco <[email protected]>
Co-authored-by: Gabriel Pacheco <[email protected]>
(cherry picked from commit 4ae75e5)

Author: 2 people

include/gz/sim/Util.hh

@@ -250,18 +250,22 @@ namespace gz
     /// \param[in] _entity Entity whose component is being enabled
     /// \param[in] _enable True to enable (create), false to disable (remove).
     /// Defaults to true.
+    /// \param[in] _comp The component to create if neeeded. Defaults to a
+    /// default-constructed component.
     /// \return True if a component was created or removed, false if nothing
     /// changed.
     template <class ComponentType>
     bool enableComponent(EntityComponentManager &_ecm,
-        Entity _entity, bool _enable = true)
+        Entity _entity,
+        bool _enable = true,
+        const ComponentType &_comp = ComponentType())
     {
       bool changed{false};
 
       auto exists = _ecm.Component<ComponentType>(_entity);
       if (_enable && !exists)
       {
-        _ecm.CreateComponent(_entity, ComponentType());
+        _ecm.CreateComponent(_entity, _comp);
         changed = true;
       }
       else if (!_enable && exists)

src/Link.cc

@@ -284,16 +284,37 @@ std::optional<math::Vector3d> Link::WorldAngularVelocity(
 void Link::EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable)
     const
 {
-  enableComponent<components::WorldLinearVelocity>(_ecm, this->dataPtr->id,
-      _enable);
-  enableComponent<components::WorldAngularVelocity>(_ecm, this->dataPtr->id,
-      _enable);
-  enableComponent<components::LinearVelocity>(_ecm, this->dataPtr->id,
-      _enable);
-  enableComponent<components::AngularVelocity>(_ecm, this->dataPtr->id,
-      _enable);
-  enableComponent<components::WorldPose>(_ecm, this->dataPtr->id,
-      _enable);
+  auto defaultWorldPose = math::Pose3d::Zero;
+  if (_enable)
+  {
+    defaultWorldPose = sim::worldPose(this->dataPtr->id, _ecm);
+  }
+
+  enableComponent(_ecm, this->dataPtr->id,
+    _enable, components::LinearVelocity());
+  enableComponent(_ecm, this->dataPtr->id,
+    _enable, components::AngularVelocity());
+  enableComponent(_ecm, this->dataPtr->id,
+    _enable, components::WorldPose(defaultWorldPose));
+
+  auto defaultWorldLinVel = math::Vector3d::Zero;
+  auto defaultWorldAngVel = math::Vector3d::Zero;
+  if (_enable)
+  {
+    // The WorldPose component is guaranteed to exist at this point
+    auto worldPose = this->WorldPose(_ecm).value();
+
+    // Compute the default world linear and angular velocities
+    defaultWorldLinVel = worldPose.Rot().RotateVector(
+      _ecm.Component<components::LinearVelocity>(this->dataPtr->id)->Data());
+    defaultWorldAngVel = worldPose.Rot().RotateVector(
+      _ecm.Component<components::AngularVelocity>(this->dataPtr->id)->Data());
+  }
+
+  enableComponent(_ecm, this->dataPtr->id,
+    _enable, components::WorldLinearVelocity(defaultWorldLinVel));
+  enableComponent(_ecm, this->dataPtr->id,
+    _enable, components::WorldAngularVelocity(defaultWorldAngVel));
 }
 
 //////////////////////////////////////////////////

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,67 @@ TEST(LinkTest, Sensors)
   EXPECT_EQ(0u, linkC.Sensors(ecm).size());
   EXPECT_EQ(gz::sim::kNullEntity, linkC.SensorByName(ecm, "invalid"));
 }
+
+TEST(LinkTest, EnableVelocityChecksCreatesAdequateWorldComponents)
+{
+  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);
+
+  auto bodyLinvel = modelWorldPose.Rot().RotateVectorReverse(linkWorldLinvel);
+  auto bodyAngvel = modelWorldPose.Rot().RotateVectorReverse(linkWorldAngvel);
+
+  // Create a model with a child link
+  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));
+
+  // The link's world pose should be correctly resolved
+  EXPECT_EQ(modelWorldPose, gz::sim::worldPose(linkEntity, ecm));
+
+  gz::sim::Link link(linkEntity);
+
+  // Body velocities should be preserved since they were in the ECM already
+  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());
+
+  // Enable velocity checks should provide the correct world components
+  // if they have not been computed and inserted in the ECM yet
+  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());
+}

src/Util_TEST.cc

@@ -726,6 +726,16 @@ TEST_F(UtilTest, EnableComponent)
   // Disabling again makes no changes
   EXPECT_FALSE(enableComponent<components::Name>(ecm, entity1, false));
   EXPECT_EQ(nullptr, ecm.Component<components::Name>(entity1));
+
+  // Creates a component with a specified value
+  EXPECT_TRUE(enableComponent<components::Name>(ecm, entity1, true,
+      components::Name("name")));
+  EXPECT_EQ("name", ecm.Component<components::Name>(entity1)->Data());
+
+  // Disabling with a dedicated value should have the same effect
+  EXPECT_TRUE(enableComponent<components::Name>(ecm, entity1, false,
+      components::Name("name")));
+  EXPECT_EQ(nullptr, ecm.Component<components::Name>(entity1));
 }
 
 /////////////////////////////////////////////////

test/integration/mesh_inertia_calculation.cc

@@ -26,6 +26,7 @@
 #include <gz/sim/Model.hh>
 #include <gz/sim/Entity.hh>
 #include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/Util.hh>
 
 #include <gz/sim/components/Inertial.hh>
 #include <gz/sim/components/Model.hh>
@@ -151,9 +152,11 @@ void cylinderColladaMeshInertiaCalculation(
   EXPECT_TRUE(
     link.WorldInertiaMatrix(*ecm).value().Equal(inertiaMatrix, 0.005));
 
-  // Check the Inertial Pose and Link Pose
-  EXPECT_EQ(link.WorldPose(*ecm).value(), gz::math::Pose3d::Zero);
-  EXPECT_EQ(link.WorldInertialPose(*ecm).value(), gz::math::Pose3d::Zero);
+  // Check the Inertial Pose and Link Pose. Their world poses should be the
+  // same since the inertial pose relative to the link is zero.
+  EXPECT_EQ(link.WorldPose(*ecm).value(), worldPose(linkEntity, *ecm));
+  EXPECT_EQ(link.WorldInertialPose(*ecm).value(),
+    worldPose(linkEntity, *ecm) * gz::math::Pose3d::Zero);
 }
 
 // Tests are disabled on Windows
@@ -242,12 +245,12 @@ void cylinderColladaMeshWithNonCenterOriginInertiaCalculation(
     link.WorldInertiaMatrix(*ecm).value().Equal(inertiaMatrix, 0.005));
 
   // Check the Inertial Pose and Link Pose
-  EXPECT_EQ(link.WorldPose(*ecm).value(), gz::math::Pose3d::Zero);
+  EXPECT_EQ(link.WorldPose(*ecm).value(), worldPose(linkEntity, *ecm));
 
   // Since the height of cylinder is 2m and origin is at center of bottom face
   // the center of mass (inertial pose) will be 1m above the ground
   EXPECT_EQ(link.WorldInertialPose(*ecm).value(),
-    gz::math::Pose3d(0, 0, 1, 0, 0, 0));
+    worldPose(linkEntity, *ecm) * gz::math::Pose3d(0, 0, 1, 0, 0, 0));
 }
 
 // Tests are disabled on Windows
@@ -348,9 +351,9 @@ void cylinderColladaOptimizedMeshInertiaCalculation(
   EXPECT_TRUE(actualIxyxzyz.Equal(
               meshInertial.MassMatrix().OffDiagonalMoments(), 3.5));
   // Check the Inertial Pose and Link Pose
-  EXPECT_EQ(link.WorldPose(*ecm).value(), gz::math::Pose3d::Zero);
+  EXPECT_EQ(link.WorldPose(*ecm).value(), worldPose(linkEntity, *ecm));
   EXPECT_TRUE(link.WorldInertialPose(*ecm).value().Equal(
-              gz::math::Pose3d::Zero, 1e-2));
+              worldPose(linkEntity, *ecm) * gz::math::Pose3d::Zero, 1e-2));
 }
 
 // Tests are disabled on Windows