Functions to enable velocity and acceleration checks on Link (#935)

Signed-off-by: Louise Poubel <[email protected]>

include/ignition/gazebo/Link.hh

@@ -185,8 +185,9 @@ namespace ignition
       /// body-fixed frame. If no offset is given, the velocity at the origin of
       /// the Link frame will be returned.
       /// \param[in] _ecm Entity-component manager.
-      /// \return Linear velocity of the link or nullopt if the link does not
-      /// have components components::WorldPose.
+      /// \return Linear velocity of the link or nullopt if the velocity checks
+      /// aren't enabled.
+      /// \sa EnableVelocityChecks
       public: std::optional<math::Vector3d> WorldLinearVelocity(
           const EntityComponentManager &_ecm) const;
 
@@ -195,28 +196,47 @@ namespace ignition
       /// \param[in] _ecm Entity-component manager.
       /// \param[in] _offset Offset of the point from the origin of the Link
       /// frame, expressed in the body-fixed frame.
-      /// \return Linear velocity of the point on the body or nullopt if the
-      /// link does not have components components::WorldPose,
-      /// components::WorldLinearVelocity and components::WorldAngularVelocity.
+      /// \return Linear velocity of the point on the body or nullopt if
+      /// velocity checks aren't enabled.
+      /// \sa EnableVelocityChecks
       public: std::optional<math::Vector3d> WorldLinearVelocity(
           const EntityComponentManager &_ecm,
           const math::Vector3d &_offset) const;
 
       /// \brief Get the angular velocity of the link in the world frame
       /// \param[in] _ecm Entity-component manager.
-      /// \return Angular velocity of the link or nullopt if the link does not
-      /// have a components::WorldAngularVelocity component.
+      /// \return Angular velocity of the link or nullopt if velocity checks
+      /// aren't enabled.
+      /// \sa EnableVelocityChecks
       public: std::optional<math::Vector3d> WorldAngularVelocity(
           const EntityComponentManager &_ecm) const;
 
+      /// \brief By default, Gazebo will not report velocities for a link, so
+      /// functions like `WorldLinearVelocity` will return nullopt. This
+      /// function can be used to enable all velocity checks.
+      /// \param[in] _ecm Mutable reference to the ECM.
+      /// \param[in] _enable True to enable checks, false to disable. Defaults
+      /// to true.
+      public: void EnableVelocityChecks(EntityComponentManager &_ecm,
+          bool _enable = true) const;
+
       /// \brief Get the linear acceleration of the body in the world frame.
       /// \param[in] _ecm Entity-component manager.
       /// \return Linear acceleration of the body in the world frame or nullopt
-      /// if the link does not have a components::WorldLinearAcceleration
-      /// component.
+      /// if acceleration checks aren't enabled.
+      /// \sa EnableAccelerationChecks
       public: std::optional<math::Vector3d> WorldLinearAcceleration(
           const EntityComponentManager &_ecm) const;
 
+      /// \brief By default, Gazebo will not report accelerations for a link, so
+      /// functions like `WorldLinearAcceleration` will return nullopt. This
+      /// function can be used to enable all acceleration checks.
+      /// \param[in] _ecm Mutable reference to the ECM.
+      /// \param[in] _enable True to enable checks, false to disable. Defaults
+      /// to true.
+      public: void EnableAccelerationChecks(EntityComponentManager &_ecm,
+          bool _enable = true) const;
+
       /// \brief Get the inertia matrix in the world frame.
       /// \param[in] _ecm Entity-component manager.
       /// \return Inertia matrix in world frame, returns nullopt if link

include/ignition/gazebo/Util.hh

@@ -175,6 +175,34 @@ namespace ignition
         const Entity &_entity,
         const EntityComponentManager &_ecm);
 
+    /// \brief Helper function to "enable" a component (i.e. create it if it
+    /// doesn't exist) or "disable" a component (i.e. remove it if it exists).
+    /// \param[in] _ecm Mutable reference to the ECM
+    /// \param[in] _entity Entity whose component is being enabled
+    /// \param[in] _enable True to enable (create), false to disable (remove).
+    /// Defaults to true.
+    /// \return True if a component was created or removed, false if nothing
+    /// changed.
+    template <class ComponentType>
+    bool IGNITION_GAZEBO_VISIBLE enableComponent(EntityComponentManager &_ecm,
+        Entity _entity, bool _enable = true)
+    {
+      bool changed{false};
+
+      auto exists = _ecm.Component<ComponentType>(_entity);
+      if (_enable && !exists)
+      {
+        _ecm.CreateComponent(_entity, ComponentType());
+        changed = true;
+      }
+      else if (!_enable && exists)
+      {
+        _ecm.RemoveComponent<ComponentType>(_entity);
+        changed = true;
+      }
+      return changed;
+    }
+
     /// \brief Environment variable holding resource paths.
     const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"};
 

src/Link.cc

@@ -32,6 +32,7 @@
 #include "ignition/gazebo/components/Pose.hh"
 #include "ignition/gazebo/components/Visual.hh"
 #include "ignition/gazebo/components/WindMode.hh"
+#include "ignition/gazebo/Util.hh"
 
 #include "ignition/gazebo/Link.hh"
 
@@ -228,6 +229,22 @@ std::optional<math::Vector3d> Link::WorldAngularVelocity(
       this->dataPtr->id);
 }
 
+//////////////////////////////////////////////////
+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);
+}
+
 //////////////////////////////////////////////////
 std::optional<math::Vector3d> Link::WorldLinearAcceleration(
     const EntityComponentManager &_ecm) const
@@ -236,6 +253,16 @@ std::optional<math::Vector3d> Link::WorldLinearAcceleration(
       this->dataPtr->id);
 }
 
+//////////////////////////////////////////////////
+void Link::EnableAccelerationChecks(EntityComponentManager &_ecm, bool _enable)
+    const
+{
+  enableComponent<components::WorldLinearAcceleration>(_ecm, this->dataPtr->id,
+      _enable);
+  enableComponent<components::LinearAcceleration>(_ecm, this->dataPtr->id,
+      _enable);
+}
+
 //////////////////////////////////////////////////
 std::optional<math::Matrix3d> Link::WorldInertiaMatrix(
     const EntityComponentManager &_ecm) const

src/Util_TEST.cc

@@ -570,3 +570,28 @@ TEST_F(UtilTest, TopLevelModel)
   // the world should have no top level model
   EXPECT_EQ(kNullEntity, topLevelModel(worldEntity, ecm));
 }
+
+/////////////////////////////////////////////////
+TEST_F(UtilTest, EnableComponent)
+{
+  EntityComponentManager ecm;
+
+  auto entity1 = ecm.CreateEntity();
+  EXPECT_EQ(nullptr, ecm.Component<components::Name>(entity1));
+
+  // Enable
+  EXPECT_TRUE(enableComponent<components::Name>(ecm, entity1));
+  EXPECT_NE(nullptr, ecm.Component<components::Name>(entity1));
+
+  // Enabling again makes no changes
+  EXPECT_FALSE(enableComponent<components::Name>(ecm, entity1, true));
+  EXPECT_NE(nullptr, ecm.Component<components::Name>(entity1));
+
+  // Disable
+  EXPECT_TRUE(enableComponent<components::Name>(ecm, entity1, false));
+  EXPECT_EQ(nullptr, ecm.Component<components::Name>(entity1));
+
+  // Disabling again makes no changes
+  EXPECT_FALSE(enableComponent<components::Name>(ecm, entity1, false));
+  EXPECT_EQ(nullptr, ecm.Component<components::Name>(entity1));
+}

src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc

@@ -127,40 +127,11 @@ void KineticEnergyMonitor::Configure(const Entity &_entity,
   transport::Node node;
   this->dataPtr->pub = node.Advertise<msgs::Double>(topic);
 
-  if (!_ecm.Component<components::WorldPose>(this->dataPtr->linkEntity))
-  {
-    _ecm.CreateComponent(this->dataPtr->linkEntity,
-        components::WorldPose());
-  }
+  Link link(this->dataPtr->linkEntity);
+  link.EnableVelocityChecks(_ecm, true);
 
-  if (!_ecm.Component<components::Inertial>(this->dataPtr->linkEntity))
-  {
-    _ecm.CreateComponent(this->dataPtr->linkEntity, components::Inertial());
-  }
-
-  // Create a world linear velocity component if one is not present.
-  if (!_ecm.Component<components::WorldLinearVelocity>(
-        this->dataPtr->linkEntity))
-  {
-    _ecm.CreateComponent(this->dataPtr->linkEntity,
-        components::WorldLinearVelocity());
-  }
-
-  // Create an angular velocity component if one is not present.
-  if (!_ecm.Component<components::AngularVelocity>(
-        this->dataPtr->linkEntity))
-  {
-    _ecm.CreateComponent(this->dataPtr->linkEntity,
-        components::AngularVelocity());
-  }
-
-  // Create an angular velocity component if one is not present.
-  if (!_ecm.Component<components::WorldAngularVelocity>(
-        this->dataPtr->linkEntity))
-  {
-    _ecm.CreateComponent(this->dataPtr->linkEntity,
-        components::WorldAngularVelocity());
-  }
+  // Create a default inertia in case the link doesn't have it
+  enableComponent<components::Inertial>(_ecm, this->dataPtr->linkEntity, true);
 }
 
 //////////////////////////////////////////////////

src/systems/lift_drag/LiftDrag.cc

@@ -498,24 +498,8 @@ void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm)
 
     if (this->dataPtr->validConfig)
     {
-      if (!_ecm.Component<components::WorldPose>(this->dataPtr->linkEntity))
-      {
-        _ecm.CreateComponent(this->dataPtr->linkEntity,
-                             components::WorldPose());
-      }
-
-      if (!_ecm.Component<components::WorldLinearVelocity>(
-              this->dataPtr->linkEntity))
-      {
-        _ecm.CreateComponent(this->dataPtr->linkEntity,
-                             components::WorldLinearVelocity());
-      }
-      if (!_ecm.Component<components::WorldAngularVelocity>(
-              this->dataPtr->linkEntity))
-      {
-        _ecm.CreateComponent(this->dataPtr->linkEntity,
-                             components::WorldAngularVelocity());
-      }
+      Link link(this->dataPtr->linkEntity);
+      link.EnableVelocityChecks(_ecm, true);
 
       if ((this->dataPtr->controlJointEntity != kNullEntity) &&
           !_ecm.Component<components::JointPosition>(

src/systems/wind_effects/WindEffects.cc

@@ -547,17 +547,8 @@ void WindEffects::PreUpdate(const UpdateInfo &_info,
   {
     if (_windMode->Data())
     {
-      // Create a WorldLinearVelocity component on the link so that
-      // physics can populate it
-      if (!_ecm.Component<components::WorldLinearVelocity>(_entity))
-      {
-        _ecm.CreateComponent(_entity,
-                             components::WorldLinearVelocity());
-      }
-      if (!_ecm.Component<components::WorldPose>(_entity))
-      {
-        _ecm.CreateComponent(_entity, components::WorldPose());
-      }
+      Link link(_entity);
+      link.EnableVelocityChecks(_ecm, true);
     }
     return true;
   });

test/integration/link.cc

@@ -256,17 +256,28 @@ TEST_F(LinkIntegrationTest, LinkVelocities)
 
   ASSERT_TRUE(link.Valid(ecm));
 
-  // Before we add components, velocity functions should return nullopt
+  // Before enabling, velocity functions should return nullopt
   EXPECT_EQ(std::nullopt, link.WorldLinearVelocity(ecm));
   EXPECT_EQ(std::nullopt, link.WorldAngularVelocity(ecm));
 
+  // After enabling, velocity functions should return default values
+  link.EnableVelocityChecks(ecm);
+
+  EXPECT_NE(nullptr, ecm.Component<components::WorldPose>(eLink));
+  EXPECT_NE(nullptr, ecm.Component<components::WorldLinearVelocity>(eLink));
+  EXPECT_NE(nullptr, ecm.Component<components::WorldAngularVelocity>(eLink));
+
+  EXPECT_EQ(math::Vector3d::Zero, link.WorldLinearVelocity(ecm));
+  EXPECT_EQ(math::Vector3d::Zero, link.WorldAngularVelocity(ecm));
+
+  // With custom velocities
   math::Pose3d pose;
   pose.Set(0, 0, 0, IGN_PI_2, 0, 0);
   math::Vector3d linVel{1.0, 0.0, 0.0};
   math::Vector3d angVel{0.0, 0.0, 2.0};
-  ecm.CreateComponent(eLink, components::WorldPose(pose));
-  ecm.CreateComponent(eLink, components::WorldLinearVelocity(linVel));
-  ecm.CreateComponent(eLink, components::WorldAngularVelocity(angVel));
+  ecm.SetComponentData<components::WorldPose>(eLink, pose);
+  ecm.SetComponentData<components::WorldLinearVelocity>(eLink, linVel);
+  ecm.SetComponentData<components::WorldAngularVelocity>(eLink, angVel);
 
   EXPECT_EQ(linVel, link.WorldLinearVelocity(ecm));
   EXPECT_EQ(angVel, link.WorldAngularVelocity(ecm));
@@ -276,6 +287,16 @@ TEST_F(LinkIntegrationTest, LinkVelocities)
   math::Vector3d angVelBody = pose.Rot().RotateVectorReverse(angVel);
   auto expLinVel = linVel + pose.Rot().RotateVector(angVelBody.Cross(offset));
   EXPECT_EQ(expLinVel, link.WorldLinearVelocity(ecm, offset));
+
+  // Disabling velocities goes back to nullopt
+  link.EnableVelocityChecks(ecm, false);
+
+  EXPECT_EQ(std::nullopt, link.WorldLinearVelocity(ecm));
+  EXPECT_EQ(std::nullopt, link.WorldAngularVelocity(ecm));
+  EXPECT_EQ(std::nullopt, link.WorldLinearVelocity(ecm, offset));
+  EXPECT_EQ(nullptr, ecm.Component<components::WorldPose>(eLink));
+  EXPECT_EQ(nullptr, ecm.Component<components::WorldLinearVelocity>(eLink));
+  EXPECT_EQ(nullptr, ecm.Component<components::WorldAngularVelocity>(eLink));
 }
 
 //////////////////////////////////////////////////
@@ -293,14 +314,25 @@ TEST_F(LinkIntegrationTest, LinkAccelerations)
 
   ASSERT_TRUE(link.Valid(ecm));
 
-  // Before we add components, velocity functions should return nullopt
+  // Before we enable acceleration, acceleration should return nullopt
   EXPECT_EQ(std::nullopt, link.WorldLinearAcceleration(ecm));
 
+  // After enabling, they should return default values
+  link.EnableAccelerationChecks(ecm);
+  EXPECT_EQ(math::Vector3d::Zero, link.WorldLinearAcceleration(ecm));
+  EXPECT_NE(nullptr, ecm.Component<components::WorldLinearAcceleration>(eLink));
+
+  // After setting acceleration, we get the value
   math::Vector3d linAccel{1.0, 0.0, 0.0};
-  math::Vector3d angAccel{0.0, 0.0, 2.0};
-  ecm.CreateComponent(eLink, components::WorldLinearAcceleration(linAccel));
+  ecm.SetComponentData<components::WorldLinearAcceleration>(eLink, linAccel);
 
   EXPECT_EQ(linAccel, link.WorldLinearAcceleration(ecm));
+
+  // Disabling accelerations goes back to nullopt
+  link.EnableAccelerationChecks(ecm, false);
+
+  EXPECT_EQ(std::nullopt, link.WorldLinearAcceleration(ecm));
+  EXPECT_EQ(nullptr, ecm.Component<components::WorldLinearAcceleration>(eLink));
 }
 
 //////////////////////////////////////////////////