Add force offset support to ApplyLinkWrench system and to Link API

include/gz/sim/Link.hh

@@ -306,6 +306,19 @@ namespace gz
                                  const math::Vector3d &_force,
                                  const math::Vector3d &_torque) const;
 
+      /// \brief Add a wrench expressed in world coordinates and applied to
+      /// the link at the given offset from its center of mass. This wrench
+      /// is applied for one simulation step.
+      /// \param[in] _ecm Mutable Entity-component manager.
+      /// \param[in] _force Force to be applied expressed in world coordinates
+      /// \param[in] _offset The point of application of the force expressed
+      /// in the link-fixed frame and relative to the center of mass.
+      /// \param[in] _torque Torque to be applied expressed in world coordinates
+      public: void AddWorldWrench(EntityComponentManager &_ecm,
+                                    const math::Vector3d &_force,
+                                    const math::Vector3d &_offset,
+                                    const math::Vector3d &_torque) const;
+
       /// \brief Pointer to private data.
       private: std::unique_ptr<LinkPrivate> dataPtr;
     };

src/Link.cc

@@ -432,3 +432,57 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm,
               msgs::Convert(linkWrenchComp->Data().torque()) + _torque);
   }
 }
+
+//////////////////////////////////////////////////
+void Link::AddWorldWrench(EntityComponentManager &_ecm,
+                          const math::Vector3d &_force,
+                          const math::Vector3d &_offset,
+                          const math::Vector3d &_torque) const
+{
+  auto inertial = _ecm.Component<components::Inertial>(this->dataPtr->id);
+  auto worldPoseComp = _ecm.Component<components::WorldPose>(this->dataPtr->id);
+
+  // Can't apply force if the inertial's pose is not found
+  if (!inertial)
+  {
+    gzdbg << "Inertial Component not found" << std::endl;
+    return;
+  }
+
+  math::Pose3d linkWorldPose;
+  if (worldPoseComp)
+  {
+    linkWorldPose = worldPoseComp->Data();
+  }
+  else
+  {
+    linkWorldPose = worldPose(this->dataPtr->id, _ecm);
+  }
+
+  // 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 = linkWorldPose.Rot().RotateVector(
+    _offset + inertial->Data().Pose().Pos());
+
+  math::Vector3d torqueWithOffset = _torque + posComWorldCoord.Cross(_force);
+
+  auto linkWrenchComp =
+    _ecm.Component<components::ExternalWorldWrenchCmd>(this->dataPtr->id);
+
+  if (!linkWrenchComp)
+  {
+    components::ExternalWorldWrenchCmd wrench;
+    msgs::Set(wrench.Data().mutable_force(), _force);
+    msgs::Set(wrench.Data().mutable_torque(), torqueWithOffset);
+    _ecm.CreateComponent(this->dataPtr->id, wrench);
+  }
+  else
+  {
+    msgs::Set(linkWrenchComp->Data().mutable_force(),
+      msgs::Convert(linkWrenchComp->Data().force()) + _force);
+
+    msgs::Set(linkWrenchComp->Data().mutable_torque(),
+      msgs::Convert(linkWrenchComp->Data().torque()) + torqueWithOffset);
+  }
+}

src/systems/apply_link_wrench/ApplyLinkWrench.cc

@@ -80,16 +80,17 @@ class gz::sim::systems::ApplyLinkWrenchPrivate
 /// \param[in] _msg Entity message. If it's a link, that link is returned. If
 /// it's a model, its canonical link is returned.
 /// \param[out] Force to apply.
+/// \param[out] Offset of the force application point relative to the
+/// link's center of mass.
 /// \param[out] Torque to apply.
 /// \return Target link entity.
 Link decomposeMessage(const EntityComponentManager &_ecm,
     const msgs::EntityWrench &_msg, math::Vector3d &_force,
-    math::Vector3d &_torque)
+    math::Vector3d &_offset, math::Vector3d &_torque)
 {
   if (_msg.wrench().has_force_offset())
   {
-    gzwarn << "Force offset currently not supported, it will be ignored."
-            << std::endl;
+    _offset = msgs::Convert(_msg.wrench().force_offset());
   }
 
   if (_msg.wrench().has_force())
@@ -270,8 +271,9 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info,
     auto msg = this->dataPtr->newWrenches.front();
 
     math::Vector3d force;
+    math::Vector3d offset;
     math::Vector3d torque;
-    auto link = decomposeMessage(_ecm, msg, force, torque);
+    auto link = decomposeMessage(_ecm, msg, force, offset, torque);
     if (!link.Valid(_ecm))
     {
       gzerr << "Entity not found." << std::endl
@@ -280,11 +282,12 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info,
       continue;
     }
 
-    link.AddWorldWrench(_ecm, force, torque);
+    link.AddWorldWrench(_ecm, force, offset, torque);
 
     if (this->dataPtr->verbose)
     {
-      gzdbg << "Applying wrench [" << force << " " << torque << "] to entity ["
+      gzdbg << "Applying wrench [" << force << " " << torque
+            << "] with force offset [" << offset << "] to entity ["
             << link.Entity() << "] for 1 time step." << std::endl;
     }
 
@@ -295,15 +298,16 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info,
   for (auto msg : this->dataPtr->persistentWrenches)
   {
     math::Vector3d force;
+    math::Vector3d offset;
     math::Vector3d torque;
-    auto link = decomposeMessage(_ecm, msg, force, torque);
+    auto link = decomposeMessage(_ecm, msg, force, offset, torque);
     if (!link.Valid(_ecm))
     {
       // Not an error, persistent wrenches can be applied preemptively before
       // an entity is inserted
       continue;
     }
-    link.AddWorldWrench(_ecm, force, torque);
+    link.AddWorldWrench(_ecm, force, offset, torque);
   }
 }
 

test/integration/link.cc

@@ -608,6 +608,33 @@ TEST_F(LinkIntegrationTest, LinkAddWorldForce)
   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()));
+
+  // apply world force at an offset and world torque
+  math::Vector3d torque{1.0, 0.0, 0.0};
+  link.AddWorldWrench(ecm, force, offset, torque);
+
+  wrenchComp = ecm.Component<components::ExternalWorldWrenchCmd>(eLink);
+  EXPECT_NE(nullptr, wrenchComp);
+  wrenchMsg = wrenchComp->Data();
+
+  expectedTorque =
+      torque +
+      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 wrench again and verify the resulting wrench values are zero
+  link.AddWorldWrench(ecm, -force, offset, -torque);
+  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(