From 12b4cd442362efe76912558471d1f0d7e03d338e Mon Sep 17 00:00:00 2001
From: Steven Peters <scpeters@openrobotics.org>
Date: Thu, 15 Jul 2021 17:29:08 -0700
Subject: [PATCH] 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 <scpeters@openrobotics.org>
---
 include/ignition/physics/detail/FrameSemantics.hh | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/include/ignition/physics/detail/FrameSemantics.hh b/include/ignition/physics/detail/FrameSemantics.hh
index e44397600..079618485 100644
--- 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
         {