From 638e97ca21f37c08f970d4301e2143de9cf014e4 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 29 Jul 2021 19:15:28 -0700 Subject: [PATCH] Functions to enable velocity and acceleration checks on Link (#935) Signed-off-by: Louise Poubel --- include/ignition/gazebo/Link.hh | 38 +++++++++++---- include/ignition/gazebo/Util.hh | 28 +++++++++++ src/Link.cc | 27 +++++++++++ src/Util_TEST.cc | 25 ++++++++++ .../KineticEnergyMonitor.cc | 37 ++------------- src/systems/lift_drag/LiftDrag.cc | 20 +------- src/systems/wind_effects/WindEffects.cc | 13 +----- test/integration/link.cc | 46 ++++++++++++++++--- 8 files changed, 156 insertions(+), 78 deletions(-) diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh index 1f6ad18cd6..8d7f228417 100644 --- a/include/ignition/gazebo/Link.hh +++ b/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 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 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 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 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 diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 6f9591970a..d83240f98b 100644 --- a/include/ignition/gazebo/Util.hh +++ b/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 + bool IGNITION_GAZEBO_VISIBLE enableComponent(EntityComponentManager &_ecm, + Entity _entity, bool _enable = true) + { + bool changed{false}; + + auto exists = _ecm.Component(_entity); + if (_enable && !exists) + { + _ecm.CreateComponent(_entity, ComponentType()); + changed = true; + } + else if (!_enable && exists) + { + _ecm.RemoveComponent(_entity); + changed = true; + } + return changed; + } + /// \brief Environment variable holding resource paths. const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"}; diff --git a/src/Link.cc b/src/Link.cc index 6289b7909f..41f814cd88 100644 --- a/src/Link.cc +++ b/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 Link::WorldAngularVelocity( this->dataPtr->id); } +////////////////////////////////////////////////// +void Link::EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable) + const +{ + enableComponent(_ecm, this->dataPtr->id, + _enable); + enableComponent(_ecm, this->dataPtr->id, + _enable); + enableComponent(_ecm, this->dataPtr->id, + _enable); + enableComponent(_ecm, this->dataPtr->id, + _enable); + enableComponent(_ecm, this->dataPtr->id, + _enable); +} + ////////////////////////////////////////////////// std::optional Link::WorldLinearAcceleration( const EntityComponentManager &_ecm) const @@ -236,6 +253,16 @@ std::optional Link::WorldLinearAcceleration( this->dataPtr->id); } +////////////////////////////////////////////////// +void Link::EnableAccelerationChecks(EntityComponentManager &_ecm, bool _enable) + const +{ + enableComponent(_ecm, this->dataPtr->id, + _enable); + enableComponent(_ecm, this->dataPtr->id, + _enable); +} + ////////////////////////////////////////////////// std::optional Link::WorldInertiaMatrix( const EntityComponentManager &_ecm) const diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 2283468c2d..7b117cdeda 100644 --- a/src/Util_TEST.cc +++ b/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(entity1)); + + // Enable + EXPECT_TRUE(enableComponent(ecm, entity1)); + EXPECT_NE(nullptr, ecm.Component(entity1)); + + // Enabling again makes no changes + EXPECT_FALSE(enableComponent(ecm, entity1, true)); + EXPECT_NE(nullptr, ecm.Component(entity1)); + + // Disable + EXPECT_TRUE(enableComponent(ecm, entity1, false)); + EXPECT_EQ(nullptr, ecm.Component(entity1)); + + // Disabling again makes no changes + EXPECT_FALSE(enableComponent(ecm, entity1, false)); + EXPECT_EQ(nullptr, ecm.Component(entity1)); +} diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc index 60b1ab327e..962c53351c 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.cc +++ b/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(topic); - if (!_ecm.Component(this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - components::WorldPose()); - } + Link link(this->dataPtr->linkEntity); + link.EnableVelocityChecks(_ecm, true); - if (!_ecm.Component(this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, components::Inertial()); - } - - // Create a world linear velocity component if one is not present. - if (!_ecm.Component( - this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - components::WorldLinearVelocity()); - } - - // Create an angular velocity component if one is not present. - if (!_ecm.Component( - this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - components::AngularVelocity()); - } - - // Create an angular velocity component if one is not present. - if (!_ecm.Component( - this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - components::WorldAngularVelocity()); - } + // Create a default inertia in case the link doesn't have it + enableComponent(_ecm, this->dataPtr->linkEntity, true); } ////////////////////////////////////////////////// diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index 1d1194bc7e..a9e63d1764 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/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(this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - components::WorldPose()); - } - - if (!_ecm.Component( - this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - components::WorldLinearVelocity()); - } - if (!_ecm.Component( - 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( diff --git a/src/systems/wind_effects/WindEffects.cc b/src/systems/wind_effects/WindEffects.cc index 72c1f25b93..215bc57ac4 100644 --- a/src/systems/wind_effects/WindEffects.cc +++ b/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(_entity)) - { - _ecm.CreateComponent(_entity, - components::WorldLinearVelocity()); - } - if (!_ecm.Component(_entity)) - { - _ecm.CreateComponent(_entity, components::WorldPose()); - } + Link link(_entity); + link.EnableVelocityChecks(_ecm, true); } return true; }); diff --git a/test/integration/link.cc b/test/integration/link.cc index bb112a6448..4b69e71c0c 100644 --- a/test/integration/link.cc +++ b/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(eLink)); + EXPECT_NE(nullptr, ecm.Component(eLink)); + EXPECT_NE(nullptr, ecm.Component(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(eLink, pose); + ecm.SetComponentData(eLink, linVel); + ecm.SetComponentData(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(eLink)); + EXPECT_EQ(nullptr, ecm.Component(eLink)); + EXPECT_EQ(nullptr, ecm.Component(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(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(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(eLink)); } //////////////////////////////////////////////////