From 34101f7d6184f415aa2164a602642d2fb3d0aa34 Mon Sep 17 00:00:00 2001
From: Henrique-BO <henrique.barrosoliveira@usp.br>
Date: Thu, 6 Jul 2023 10:58:12 -0300
Subject: [PATCH] Added additional tests to LinkAddWorldForce integration test

Signed-off-by: Henrique-BO <henrique.barrosoliveira@usp.br>
---
 src/Link.cc              | 18 ++++++++++++++----
 test/integration/link.cc | 27 +++++++++++++++++++++++++++
 2 files changed, 41 insertions(+), 4 deletions(-)

diff --git a/src/Link.cc b/src/Link.cc
index 522fec40b3..34c10be331 100644
--- a/src/Link.cc
+++ b/src/Link.cc
@@ -435,12 +435,12 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm,
 
 //////////////////////////////////////////////////
 void Link::AddWorldWrench(EntityComponentManager &_ecm,
-                                    const math::Vector3d &_force,
-                                    const math::Vector3d &_offset,
-                                    const math::Vector3d &_torque) const
+                          const math::Vector3d &_force,
+                          const math::Vector3d &_offset,
+                          const math::Vector3d &_torque) const
 {
   auto inertial = _ecm.Component<components::Inertial>(this->dataPtr->id);
-  math::Pose3d linkWorldPose = worldPose(this->dataPtr->id, _ecm);
+  auto worldPoseComp = _ecm.Component<components::WorldPose>(this->dataPtr->id);
 
   // Can't apply force if the inertial's pose is not found
   if (!inertial)
@@ -449,6 +449,16 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm,
     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.
diff --git a/test/integration/link.cc b/test/integration/link.cc
index fd04e8bddb..cd45cb89c6 100644
--- a/test/integration/link.cc
+++ b/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(