Use pose multiplication instead of subtraction (#1521)

Part of gz-math#60.

Signed-off-by: Steve Peters <[email protected]>

Author: scpeters

src/systems/multicopter_motor_model/MulticopterMotorModel.cc

@@ -626,10 +626,11 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments(
       // is equivalent to use WorldPose().Rot().
       Link parentLink(this->parentLinkEntity);
       const auto parentWorldPose = parentLink.WorldPose(_ecm);
-      // The tansformation from the parent_link to the link_.
+      // The transformation from the parent_link to the link_.
       // Pose poseDifference =
-      //  link_->GetWorldCoGPose() - parent_links.at(0)->GetWorldCoGPose();
-      Pose poseDifference = *worldPose - *parentWorldPose;
+      //  parent_links.at(0)->GetWorldCoGPose().Inverse()
+      //  * link_->GetWorldCoGPose()
+      Pose poseDifference = (*parentWorldPose).Inverse() * (*worldPose);
       Vector3 dragTorque(
           0, 0, -this->turningDirection * thrust * this->momentConstant);
       // Transforming the drag torque into the parent frame to handle

test/integration/logical_camera_system.cc

@@ -181,7 +181,7 @@ TEST_F(LogicalCameraTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LogicalCameraBox))
   EXPECT_EQ(sensor1Pose, gz::msgs::Convert(img1.pose()));
   EXPECT_EQ(1, img1.model().size());
   EXPECT_EQ(boxName, img1.model(0).name());
-  gz::math::Pose3d boxPoseCamera1Frame = boxPose - sensor1Pose;
+  gz::math::Pose3d boxPoseCamera1Frame = sensor1Pose.Inverse() * boxPose;
   EXPECT_EQ(boxPoseCamera1Frame, gz::msgs::Convert(img1.model(0).pose()));
   mutex.unlock();
 
@@ -192,7 +192,7 @@ TEST_F(LogicalCameraTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LogicalCameraBox))
   EXPECT_EQ(sensor2Pose, gz::msgs::Convert(img2.pose()));
   EXPECT_EQ(1, img2.model().size());
   EXPECT_EQ(boxName, img2.model(0).name());
-  gz::math::Pose3d boxPoseCamera2Frame = boxPose - sensor2Pose;
+  gz::math::Pose3d boxPoseCamera2Frame = sensor2Pose.Inverse() * boxPose;
   EXPECT_EQ(boxPoseCamera2Frame, gz::msgs::Convert(img2.model(0).pose()));
   mutex.unlock();
 }