Skip to content

Commit

Permalink
FrameSemantics: improve handling of World frame
Browse files Browse the repository at this point in the history
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>
  • Loading branch information
scpeters committed Jul 16, 2021
1 parent 5261eee commit 12b4cd4
Showing 1 changed file with 10 additions and 2 deletions.
12 changes: 10 additions & 2 deletions include/ignition/physics/detail/FrameSemantics.hh
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down

0 comments on commit 12b4cd4

Please sign in to comment.