FrameSemantics: improve handling of World frame

Avoid calling FrameDataRelativeToWorld for quantities relative to the world frame but expressed in different coordinates. This prevents an assertion in KinematicsFeatures.cc Signed-off-by: Steven Peters <[email protected]>
gazebosim · Jul 16, 2021 · 12b4cd4 · 12b4cd4
1 parent 5261eee
commit 12b4cd4
Showing 1 changed file with 10 additions and 2 deletions.
diff --git a/include/ignition/physics/detail/FrameSemantics.hh b/include/ignition/physics/detail/FrameSemantics.hh
@@ -57,8 +57,16 @@ namespace ignition
           }
 
           q = _quantity.RelativeToParent();
-          currentCoordinates = _impl.FrameDataRelativeToWorld(
-                _relativeTo).pose.linear();
+          if (_relativeTo.IsWorld())
+          {
+            // The World Frame has all zero fields
+            currentCoordinates = RotationType::Identity();
+          }
+          else
+          {
+            currentCoordinates = _impl.FrameDataRelativeToWorld(
+                  _relativeTo).pose.linear();
+          }
         }
         else
         {