diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh index 8d7f228417..7aca1bec5c 100644 --- a/include/ignition/gazebo/Link.hh +++ b/include/ignition/gazebo/Link.hh @@ -262,6 +262,16 @@ namespace ignition public: void AddWorldForce(EntityComponentManager &_ecm, const math::Vector3d &_force) const; + /// \brief Add a force expressed in world coordinates and applied at + /// an offset from the center of mass of the link. + /// \param[in] _ecm Mutable Entity-component manager. + /// \param[in] _force Force to be applied expressed in world coordinates + /// \param[in] _position The point of application of the force expressed + /// in the link-fixed frame. + public: void AddWorldForce(EntityComponentManager &_ecm, + const math::Vector3d &_force, + const math::Vector3d &_position) const; + /// \brief Add a wrench expressed in world coordinates and applied to /// the link at the link's origin. This wrench is applied for one /// simulation step. diff --git a/src/Link.cc b/src/Link.cc index 41f814cd88..cd0485b398 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -326,6 +326,29 @@ void Link::AddWorldForce(EntityComponentManager &_ecm, this->AddWorldWrench(_ecm, _force, torque); } +////////////////////////////////////////////////// +void Link::AddWorldForce(EntityComponentManager &_ecm, + const math::Vector3d &_force, + const math::Vector3d &_position) const +{ + auto inertial = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.Component(this->dataPtr->id); + + // Can't apply force if the inertial's pose is not found + if (!inertial || !worldPose) + return; + + // We want the force to be applied at an offset from the center of mass, but + // ExternalWorldWrenchCmd applies the force at the link origin so we need to + // compute the resulting force and torque on the link origin. + auto posComWorldCoord = worldPose->Data().Rot().RotateVector( + _position + inertial->Data().Pose().Pos()); + + math::Vector3d torque = posComWorldCoord.Cross(_force); + + this->AddWorldWrench(_ecm, _force, torque); +} + ////////////////////////////////////////////////// void Link::AddWorldWrench(EntityComponentManager &_ecm, const math::Vector3d &_force, diff --git a/test/integration/link.cc b/test/integration/link.cc index 72b361635b..d94998d361 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -455,3 +456,92 @@ TEST_F(LinkIntegrationTest, LinkKineticEnergy) *linearVel = components::WorldLinearVelocity({math::Vector3d(10, 0, 0)}); EXPECT_DOUBLE_EQ(100.0, *link.WorldKineticEnergy(ecm)); } + +////////////////////////////////////////////////// +TEST_F(LinkIntegrationTest, LinkAddWorldForce) +{ + EntityComponentManager ecm; + EventManager eventMgr; + SdfEntityCreator creator(ecm, eventMgr); + + auto eLink = ecm.CreateEntity(); + ecm.CreateComponent(eLink, components::Link()); + + Link link(eLink); + EXPECT_EQ(eLink, link.Entity()); + + ASSERT_TRUE(link.Valid(ecm)); + + // No ExternalWorldWrenchCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLink)); + + // Add force + math::Vector3d force(0, 0, 1.0); + link.AddWorldForce(ecm, force); + + // No WorldPose or Inertial component exists so command should not work + EXPECT_EQ(nullptr, ecm.Component(eLink)); + + // create WorldPose and Inertial component and try adding force again + math::Pose3d linkWorldPose; + linkWorldPose.Set(1.0, 0.0, 0.0, 0, 0, IGN_PI_4); + math::Pose3d inertiaPose; + inertiaPose.Set(1.0, 2.0, 3.0, 0, IGN_PI_2, 0); + math::Inertiald linkInertial; + linkInertial.SetPose(inertiaPose); + ecm.CreateComponent(eLink, components::WorldPose(linkWorldPose)); + ecm.CreateComponent(eLink, components::Inertial(linkInertial)); + link.AddWorldForce(ecm, force); + + // ExternalWorldWrenchCmd component should now be created + auto wrenchComp = ecm.Component(eLink); + EXPECT_NE(nullptr, wrenchComp); + + // verify wrench values + auto wrenchMsg = wrenchComp->Data(); + + math::Vector3 expectedTorque = + linkWorldPose.Rot().RotateVector(inertiaPose.Pos()).Cross(force); + EXPECT_EQ(force, math::Vector3d( + wrenchMsg.force().x(), wrenchMsg.force().y(), wrenchMsg.force().z())); + EXPECT_EQ(expectedTorque, math::Vector3d( + wrenchMsg.torque().x(), wrenchMsg.torque().y(), wrenchMsg.torque().z())); + + // apply opposite force. Since the cmd is not processed yet, this should + // cancel out the existing wrench cmd + link.AddWorldForce(ecm, -force); + wrenchComp = ecm.Component(eLink); + EXPECT_NE(nullptr, wrenchComp); + wrenchMsg = wrenchComp->Data(); + + EXPECT_EQ(math::Vector3d::Zero, math::Vector3d( + wrenchMsg.force().x(), wrenchMsg.force().y(), wrenchMsg.force().z())); + EXPECT_EQ(math::Vector3d::Zero, math::Vector3d( + wrenchMsg.torque().x(), wrenchMsg.torque().y(), wrenchMsg.torque().z())); + + // Add world force at an offset + math::Vector3d offset{0.0, 1.0, 0.0}; + link.AddWorldForce(ecm, force, offset); + + wrenchComp = ecm.Component(eLink); + EXPECT_NE(nullptr, wrenchComp); + wrenchMsg = wrenchComp->Data(); + + expectedTorque = + linkWorldPose.Rot().RotateVector(offset + inertiaPose.Pos()).Cross(force); + EXPECT_EQ(force, math::Vector3d( + wrenchMsg.force().x(), wrenchMsg.force().y(), wrenchMsg.force().z())); + EXPECT_EQ(expectedTorque, math::Vector3d( + wrenchMsg.torque().x(), wrenchMsg.torque().y(), wrenchMsg.torque().z())); + + // apply opposite force again and verify the resulting wrench values are zero + link.AddWorldForce(ecm, -force, offset); + wrenchComp = ecm.Component(eLink); + EXPECT_NE(nullptr, wrenchComp); + wrenchMsg = wrenchComp->Data(); + + EXPECT_EQ(math::Vector3d::Zero, math::Vector3d( + wrenchMsg.force().x(), wrenchMsg.force().y(), wrenchMsg.force().z())); + EXPECT_EQ(math::Vector3d::Zero, math::Vector3d( + wrenchMsg.torque().x(), wrenchMsg.torque().y(), wrenchMsg.torque().z())); +}