Add world force

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.

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<components::Inertial>(this->dataPtr->id);
+  auto worldPose = _ecm.Component<components::WorldPose>(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,

test/integration/link.cc

@@ -23,6 +23,7 @@
 #include <ignition/gazebo/components/AngularVelocity.hh>
 #include <ignition/gazebo/components/CanonicalLink.hh>
 #include <ignition/gazebo/components/Collision.hh>
+#include <ignition/gazebo/components/ExternalWorldWrenchCmd.hh>
 #include <ignition/gazebo/components/Inertial.hh>
 #include <ignition/gazebo/components/Joint.hh>
 #include <ignition/gazebo/components/LinearAcceleration.hh>
@@ -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<components::ExternalWorldWrenchCmd>(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<components::ExternalWorldWrenchCmd>(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<components::ExternalWorldWrenchCmd>(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<components::ExternalWorldWrenchCmd>(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<components::ExternalWorldWrenchCmd>(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<components::ExternalWorldWrenchCmd>(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()));
+}