Add force offset support to ApplyLinkWrench system and to Link API (#2026)

Adds support for specifying the application point of a force in the ApplyLinkWrench system. This is done through an offset expressed in the link frame (thus, relative to the link origin).

This uses the force_offset field that was already present in the Wrench message, but wasn't being used in the system. Also, an overloaded method AddWorldWrench was added to the Link interface.

---------

Signed-off-by: Henrique-BO <[email protected]>

Author: Henrique-BO

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 an offset from the link's origin. 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] _torque Torque to be applied expressed in world coordinates
+      /// \param[in] _offset The point of application of the force expressed
+      /// in the link frame
+      public: void AddWorldWrench(EntityComponentManager &_ecm,
+                                  const math::Vector3d &_force,
+                                  const math::Vector3d &_torque,
+                                  const math::Vector3d &_offset) const;
+
       /// \brief Pointer to private data.
       private: std::unique_ptr<LinkPrivate> dataPtr;
     };

src/Link.cc

@@ -412,23 +412,46 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm,
                          const math::Vector3d &_force,
                          const math::Vector3d &_torque) const
 {
-  auto linkWrenchComp =
-      _ecm.Component<components::ExternalWorldWrenchCmd>(this->dataPtr->id);
+  this->AddWorldWrench(_ecm, _force, _torque, math::Vector3d::Zero);
+}
+
+//////////////////////////////////////////////////
+void Link::AddWorldWrench(EntityComponentManager &_ecm,
+                          const math::Vector3d &_force,
+                          const math::Vector3d &_torque,
+                          const math::Vector3d &_offset) const
+{
+  math::Pose3d linkWorldPose;
+  auto worldPoseComp = _ecm.Component<components::WorldPose>(this->dataPtr->id);
+  if (worldPoseComp)
+  {
+    linkWorldPose = worldPoseComp->Data();
+  }
+  else
+  {
+    linkWorldPose = worldPose(this->dataPtr->id, _ecm);
+  }
 
-  components::ExternalWorldWrenchCmd wrench;
+  // We want the force to be applied at an offset from the link origin, so we
+  // must compute the resulting force and torque on the link origin.
+  auto posComWorldCoord = linkWorldPose.Rot().RotateVector(_offset);
+  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(), _torque);
+    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::Convert(linkWrenchComp->Data().force()) + _force);
 
     msgs::Set(linkWrenchComp->Data().mutable_torque(),
-              msgs::Convert(linkWrenchComp->Data().torque()) + _torque);
+      msgs::Convert(linkWrenchComp->Data().torque()) + torqueWithOffset);
   }
 }

src/systems/apply_link_wrench/ApplyLinkWrench.cc

@@ -81,15 +81,16 @@ class gz::sim::systems::ApplyLinkWrenchPrivate
 /// it's a model, its canonical link is returned.
 /// \param[out] Force to apply.
 /// \param[out] Torque to apply.
+/// \param[out] Offset of the force application point expressed in the link
+/// frame.
 /// \return Target link entity.
 Link decomposeMessage(const EntityComponentManager &_ecm,
     const msgs::EntityWrench &_msg, math::Vector3d &_force,
-    math::Vector3d &_torque)
+    math::Vector3d &_torque, math::Vector3d &_offset)
 {
   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, torque, offset);
     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, torque, offset);
 
     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, torque, offset);
     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, torque, offset);
   }
 }
 

src/systems/apply_link_wrench/ApplyLinkWrench.hh

@@ -38,6 +38,9 @@ namespace systems
   /// that will receive a wrench. If a model is provided, its canonical link
   /// will receive it. No other entity types are supported.
   ///
+  /// Forces and torques are expressed in world coordinates, and the force
+  /// offset is expressed in the link frame.
+  ///
   /// ## Topics
   ///
   /// * /world/<world_name>/wrench

test/integration/link.cc

@@ -608,6 +608,32 @@ 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, torque, offset);
+
+  wrenchComp = ecm.Component<components::ExternalWorldWrenchCmd>(eLink);
+  EXPECT_NE(nullptr, wrenchComp);
+  wrenchMsg = wrenchComp->Data();
+
+  math::Vector3d posComWorldCoord = linkWorldPose.Rot().RotateVector(offset);
+  expectedTorque = torque + posComWorldCoord.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, -torque, 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(