Use pose multiplication instead of addition (#1369)

The ign-math Pose addition operator is going to be deprecated, so use the multiplication operator instead. It works in the opposite order, matching the behavior of coordinate transform multiplication. Signed-off-by: Steve Peters <[email protected]>
gazebosim · Mar 2, 2022 · 6ac12c6 · 6ac12c6
1 parent b04aaed
commit 6ac12c6
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 7 deletions.
diff --git a/src/Util.cc b/src/Util.cc
@@ -81,7 +81,7 @@ math::Pose3d worldPose(const Entity &_entity,
     if (!parentPose)
       break;
     // transform pose
-    pose = pose + parentPose->Data();
+    pose = parentPose->Data() * pose;
     // keep going up the tree
     p = _ecm.Component<components::ParentEntity>(p->Data());
   }

diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc
@@ -1321,7 +1321,7 @@ void RenderUtil::Update()
           trajPose.Rot() = tf.second["actorPose"].Rotation();
         }
 
-        actorVisual->SetLocalPose(trajPose + globalPose);
+        actorVisual->SetLocalPose(globalPose * trajPose);
 
         tf.second.erase("actorPose");
         actorMesh->SetSkeletonLocalTransforms(tf.second);
@@ -3035,7 +3035,7 @@ void RenderUtilPrivate::UpdateAnimation(const std::unordered_map<Entity,
       trajPose.Rot() = poseFrame.Rotation();
     }
 
-    actorVisual->SetLocalPose(trajPose + globalPose);
+    actorVisual->SetLocalPose(globalPose * trajPose);
   }
 }
 

diff --git a/src/systems/optical_tactile_plugin/Visualization.cc b/src/systems/optical_tactile_plugin/Visualization.cc
@@ -209,8 +209,8 @@ void OpticalTactilePluginVisualization::AddNormalForceToMarkerMsgs(
   ignition::math::Pose3f normalForcePoseFromSensor(
     normalForcePositionFromSensor, normalForceOrientationFromSensor);
 
-  ignition::math::Pose3f normalForcePoseFromWorld =
-    (normalForcePoseFromSensor + this->depthCameraOffset) + _sensorWorldPose;
+  ignition::math::Pose3f normalForcePoseFromWorld = _sensorWorldPose *
+    this->depthCameraOffset * normalForcePoseFromSensor;
   normalForcePoseFromWorld.Correct();
 
   // Get the start point of the normal force
@@ -222,8 +222,8 @@ void OpticalTactilePluginVisualization::AddNormalForceToMarkerMsgs(
   normalForcePoseFromSensor.Set(normalForcePositionFromSensor +
     _normalForce * this->forceLength, normalForceOrientationFromSensor);
 
-  normalForcePoseFromWorld =
-    (normalForcePoseFromSensor + this->depthCameraOffset) + _sensorWorldPose;
+  normalForcePoseFromWorld = _sensorWorldPose * this->depthCameraOffset *
+    normalForcePoseFromSensor;
   normalForcePoseFromWorld.Correct();
 
   ignition::math::Vector3f endPointFromWorld =