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 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(