diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc index 08fb0ad455..fd4c661047 100644 --- a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc +++ b/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 diff --git a/test/integration/logical_camera_system.cc b/test/integration/logical_camera_system.cc index 0c5ed29390..54381541ad 100644 --- a/test/integration/logical_camera_system.cc +++ b/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(); }