Added additional tests to LinkAddWorldForce integration test

Signed-off-by: Henrique-BO <[email protected]>
gazebosim · Jul 6, 2023 · 34101f7 · 34101f7
1 parent 6b3f196
commit 34101f7
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 4 deletions.
diff --git 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
@@ -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(