Orientation Composition Fixes (#241)

* Added test highlighting incorrect orientation behavior.

* Orientation fixes and additional tests.

* Updated `relative to` docs.

* Doc tweaks.

* Fix OrientedPoint mutator docs.

* Fixed docstring formatting.

* Added localAnglesfor test.

* Apply suggestions from code review

Co-authored-by: Daniel Fremont <dfremont@ucsc.edu>

* Added missing test piece.

* Update network format version.

---------

Co-authored-by: Daniel Fremont <dfremont@ucsc.edu>

docs/reference/operators.rst

@@ -93,9 +93,12 @@ The orientation specified by the vector field at the given position
 
 .. _{direction} relative to {direction}:
 
-(*heading* | *vectorField*) relative to (*heading* | *vectorField*)
+(*direction*) relative to (*direction*)
 -------------------------------------------------------------------
-The first heading/vector field, interpreted as an offset relative to the second heading/vector field. For example, :scenic:`-5 deg relative to 90 deg` is simply 85 degrees. If either direction is a vector field, then this operator yields an expression depending on the :prop:`position` property of the object being specified.
+The orientation obtained by starting in the second direction and then rotating according to the first direction. For example, :scenic:`-5 deg relative to 90 deg` is simply 85 degrees. If either direction is a vector field, then this operator yields an expression depending on the :prop:`position` property of the object being specified. Both operator values must be of type `heading`, `Orientation`, or `vectorField`, not tuples, as tuples are by default intepreted as `Vector` objects.
+
+.. note::
+	This operator is not necessarily commutative, for example, when composing two 3D orientations.
 
 
 Vector Operators

src/scenic/core/object_types.py

@@ -823,14 +823,11 @@ def __getattr__(self, attr):
 class OrientedPoint(Point):
     """The Scenic class ``OrientedPoint``.
 
-    The default mutator for `OrientedPoint` adds Gaussian noise to ``yaw`` while
-    leaving ``pitch`` and ``roll`` unchanged, using the three standard deviations
-    (for yaw/pitch/roll respectively) given by the  ``orientationStdDev`` property.
-    It then also applies the mutator for `Point`.
-
     The default mutator for `OrientedPoint` adds Gaussian noise to ``yaw``, ``pitch``
-    and ``roll`` according to ``orientationStdDev``. By default the standard deviations
-    for ``pitch`` and ``roll`` are zero so that, by default, only ``yaw`` is mutated.
+    and ``roll``, using the three standard deviations (for yaw/pitch/roll respectively)
+    given by the  ``orientationStdDev`` property. It then also applies the mutator for `Point`.
+    By default the standard deviations for ``pitch`` and ``roll`` are zero so that, by
+    default, only ``yaw`` is mutated.
 
     Properties:
         yaw (float; dynamic): Yaw of the `OrientedPoint` in radians in the local coordinate system
@@ -865,8 +862,8 @@ class OrientedPoint(Point):
             {"yaw", "pitch", "roll", "parentOrientation"},
             {"dynamic", "final"},
             lambda self: (
-                Orientation.fromEuler(self.yaw, self.pitch, self.roll)
-                * self.parentOrientation
+                self.parentOrientation
+                * Orientation.fromEuler(self.yaw, self.pitch, self.roll)
             ),
         ),
         # Heading is equal to orientation.yaw, which is equal to self.yaw if this OrientedPoint's

src/scenic/core/vectors.py

@@ -324,6 +324,14 @@ def _inverseRotation(self):
 
     # will be converted to a distributionMethod after the class definition
     def __mul__(self, other) -> Orientation:
+        """Apply a rotation to this orientation, yielding a new orientation.
+
+        As we represent orientations as intrinsic rotations, rotation A followed by rotation B is
+        given by the quaternion product A*B, not B*A.
+
+        See https://en.wikipedia.org/wiki/Davenport_chained_rotations#Conversion_to_extrinsic_rotations
+        for more details.
+        """
         if type(other) is not Orientation:
             return NotImplemented
         # Preserve existing orientation objects when possible to help pruning.
@@ -339,15 +347,15 @@ def __add__(self, other) -> Orientation:
             other = Orientation._fromHeading(other)
         elif type(other) is not Orientation:
             return NotImplemented
-        return other * self
+        return self * other
 
     @distributionMethod
     def __radd__(self, other) -> Orientation:
         if isinstance(other, (float, int)):
             other = Orientation._fromHeading(other)
         elif type(other) is not Orientation:
             return NotImplemented
-        return self * other
+        return other * self
 
     def __repr__(self):
         return f"Orientation.fromEuler{tuple(self.eulerAngles)!r}"
@@ -362,7 +370,7 @@ def localAnglesFor(self, orientation) -> typing.Tuple[float, float, float]:
         That is, considering ``self`` as the parent orientation, find the Euler angles
         expressing the given orientation.
         """
-        local = orientation * self.inverse
+        local = self.inverse * orientation
         return local.eulerAngles
 
     @distributionFunction

src/scenic/domains/driving/roads.py

@@ -989,7 +989,7 @@ def _currentFormatVersion(cls):
 
         :meta private:
         """
-        return 32
+        return 33
 
     class DigestMismatchError(Exception):
         """Exception raised when loading a cached map not matching the original file."""

tests/core/test_vectors.py

@@ -1,3 +1,5 @@
+import pytest
+
 from scenic.core.distributions import Options, underlyingFunction
 from scenic.core.lazy_eval import (
     DelayedArgument,
@@ -33,6 +35,23 @@ def test_orientation_equality():
     assert o1 != o3 and not o1.approxEq(o3)
 
 
+def test_orientation_localAnglesFor():
+    parent = Orientation.fromEuler(math.pi / 4, math.pi / 4, 0)
+    target = Orientation.fromEuler(-3 * math.pi / 4, math.pi / 4, math.pi / 4)
+    angles = parent.localAnglesFor(target)
+    assert angles == pytest.approx((-3 * math.pi / 4, math.pi / 2, 0))
+
+    for i in range(100):
+        parent = Orientation.fromEuler(
+            *[random.uniform(-math.pi, math.pi) for _ in range(3)]
+        )
+        target = Orientation.fromEuler(
+            *[random.uniform(-math.pi, math.pi) for _ in range(3)]
+        )
+        local = Orientation.fromEuler(*parent.localAnglesFor(target))
+        assert target.approxEq(parent * local)
+
+
 def test_distribution_method_encapsulation():
     vf = VectorField("Foo", lambda pos: 0)
     pt = vf.followFrom(Vector(0, 0), Options([1, 2]), steps=1)

tests/syntax/test_operators.py

@@ -688,6 +688,21 @@ def test_orientation_relative_to_orientation():
     assert ego.orientation.approxEq(Orientation.fromEuler(math.pi / 2, math.pi / 2, 0))
 
 
+def test_orientation_relative_to_orientation2():
+    ego = sampleEgoFrom(
+        """
+        ego = new Object facing (Orientation.fromEuler(-135 deg, 45 deg, 0)
+            relative to Orientation.fromEuler(90 deg, 0, 0))
+        """
+    )
+    assert ego.yaw == pytest.approx(math.radians(-45))
+    assert ego.pitch == pytest.approx(math.radians(45))
+    assert ego.roll == pytest.approx(0)
+    assert ego.orientation.approxEq(
+        Orientation.fromEuler(math.radians(-45), math.radians(45), 0)
+    )
+
+
 def test_heading_relative_to_orientation():
     ego = sampleEgoFrom(
         """

tests/syntax/test_specifiers.py

@@ -10,6 +10,7 @@
     compileScenic,
     sampleEgo,
     sampleEgoFrom,
+    sampleParamPFrom,
     sampleScene,
     sampleSceneFrom,
 )
@@ -1069,6 +1070,20 @@ def test_facing_vf_3d():
     )
 
 
+def test_facing_equivalence():
+    p = sampleParamPFrom(
+        """
+        a = new OrientedPoint facing (Orientation.fromEuler(-135 deg, 45 deg, 0)
+            relative to Orientation.fromEuler(90 deg, 0, 0))
+
+        b = new OrientedPoint with parentOrientation (90 deg, 0, 0), with yaw -135 deg, with pitch 45 deg, with roll 0
+        param p = (a, b)
+        """
+    )
+    a, b = p
+    assert a.orientation.eulerAngles == pytest.approx(b.orientation.eulerAngles)
+
+
 # Facing Toward/Away From
 def test_facing_toward():
     ego = sampleEgoFrom(
@@ -1111,6 +1126,21 @@ def test_facing_directly_away_from():
     assert ego.roll == 0
 
 
+def test_facing_directly_toward_parent_orientation():
+    ego = sampleEgoFrom(
+        """
+        ego = new Object facing directly toward (1, 1, 2**0.5),
+            with parentOrientation (90 deg, 0, 0)
+    """
+    )
+    assert ego.yaw == pytest.approx(-math.radians(135))
+    assert ego.pitch == pytest.approx(math.radians(45))
+    assert ego.roll == pytest.approx(0)
+    assert ego.orientation.approxEq(
+        Orientation.fromEuler(math.radians(-45), math.radians(45), 0)
+    )
+
+
 # Apparently Facing
 def test_apparently_facing():
     ego = sampleEgoFrom(