domino_real: fold perceived roll modulo pi

predicators/envs/pybullet_domino_real.py

@@ -43,6 +43,28 @@ def _base_pose(xyz: Sequence[float], quat_xyzw: Sequence[float]) -> Pose6D:
     return Pose6D((x, y, z), (qx, qy, qz, qw))
 
 
+def _canonical_roll(roll: float) -> float:
+    """Fold a perceived roll into ``[-pi/2, pi/2)``.
+
+    A domino is a box, so turning it 180 degrees about its own width
+    axis leaves it exactly where it was. Both orientations are equally
+    correct descriptions of the same physical domino, and a marker-based
+    pose estimate returns one or the other arbitrarily -- in a single
+    capture, some dominoes come back at roll 0 and others at roll +-pi.
+
+    Roll is therefore only meaningful modulo pi, and folding it is not
+    cosmetic: ``Toppled`` is defined as ``|roll| >= 10 degrees``, so an
+    unfolded ``roll = pi`` makes an upright domino read as knocked over
+    before anything has moved. A task whose goal is ``Toppled(target)``
+    is then already satisfied in its own initial state, and the planner
+    returns an empty plan and reports success.
+
+    Standing (0 or +-pi) folds to ~0. Knocked over (+-pi/2) keeps its
+    magnitude, which is all ``Toppled`` and ``Upright`` read.
+    """
+    return (roll + math.pi / 2) % math.pi - math.pi / 2
+
+
 @dataclass(frozen=True)
 class _PerceivedDomino:
     """One domino as perceived, normalized from either source.
@@ -288,14 +310,16 @@ def _env_angles(self, world: Pose6D,
         table. Anything else (propped diagonally on a neighbour, say)
         loses its pitch when the state is written into PyBullet, so say
         so rather than silently flattening it.
+
+        The roll is folded modulo pi; see :func:`_canonical_roll`.
         """
         roll, pitch, yaw = domino_env_euler(world)
         if abs(pitch) >= DominoComponent.domino_roll_threshold:
             logging.warning(
                 "pybullet_domino_real: domino %s is pitched %.1f deg, which "
                 "the (yaw, roll) domino state cannot represent; dropping the "
                 "pitch", capture_id, math.degrees(pitch))
-        return roll, yaw
+        return _canonical_roll(roll), yaw
 
     @staticmethod
     def _canonical_start_yaw(

tests/envs/test_pybullet_domino_real.py

@@ -29,7 +29,8 @@
 from predicators import utils
 from predicators.envs.pybullet_domino.real_geometry import _REAL_TO_ENV_BODY, \
     Pose6D, domino_env_euler, domino_upright_yaw, pose_base_to_world
-from predicators.envs.pybullet_domino_real import PyBulletDominoRealEnv
+from predicators.envs.pybullet_domino_real import PyBulletDominoRealEnv, \
+    _canonical_roll
 from predicators.pybullet_helpers.real_robot_bridge import \
     gripper_joint_layout_from_robot
 from predicators.structs import GroundAtom
@@ -616,3 +617,79 @@ def test_unrepresentable_pitch_is_reported(env, caplog):
         env.state_from_observation(obs, task.init)
 
     assert "pitched" in caplog.text
+
+
+# -- the 180-degree box symmetry ---------------------------------------------
+# A domino is a box: turning it 180 degrees about its own width axis leaves it
+# exactly where it was, and a marker-based pose estimate returns either
+# representative arbitrarily. Roll is therefore only meaningful modulo pi.
+
+
+@pytest.mark.parametrize("raw_deg, folded_deg", [
+    (0.0, 0.0),
+    (180.0, 0.0),
+    (-180.0, 0.0),
+    (5.0, 5.0),
+    (-5.0, -5.0),
+    (90.0, -90.0),
+    (-90.0, -90.0),
+    (175.0, -5.0),
+])
+def test_canonical_roll_folds_modulo_pi(raw_deg, folded_deg):
+    """Standing folds to ~0; knocked over keeps its magnitude.
+
+    Magnitude is what matters: ``Toppled`` and ``Upright`` are both
+    defined on ``|roll|``, so a fold that flips the sign at +-90 degrees
+    is free, while folding 180 -> 0 is the whole point.
+    """
+    got = _canonical_roll(np.deg2rad(raw_deg))
+    assert np.rad2deg(got) == pytest.approx(folded_deg, abs=1e-9)
+
+
+def test_no_domino_starts_toppled_when_perception_flips_it(tmp_path):
+    """A capture where some dominoes come back 180-degree-flipped must still
+    build a task whose dominoes are all standing.
+
+    This is the regression. Perception legitimately reports a standing
+    domino as ``roll = pi``; unfolded, ``Toppled`` (``|roll| >= 10 deg``)
+    then holds for it in the task's own initial state, so a
+    ``Toppled(target)`` goal is satisfied before anything moves and the
+    planner returns an empty plan and reports success.
+    """
+    flipped = _base_quat(roll=np.pi)
+    scene = _write_scene(tmp_path, [
+        _record(_START_ID, (0.0, 0.0)),
+        _record(11, (0.1, 0.05), quat=flipped),
+        _record(_TARGET_ID, (0.2, 0.0), quat=flipped),
+    ],
+                         name="flipped.json")
+    env = _make_env(scene)
+    comp = env._domino_component  # pylint: disable=protected-access
+    task = env._build_task_from_scene()  # pylint: disable=protected-access
+
+    for slot in range(3):
+        dom = comp.dominos[slot]
+        # pylint: disable=protected-access
+        assert comp._Upright_holds(task.init, [dom]), \
+            f"{dom.name} did not start upright"
+        assert not comp._Toppled_holds(task.init, [dom]), \
+            f"{dom.name} started toppled"
+
+    # And the goal is therefore not already satisfied by the initial state.
+    assert not task.goal.issubset(utils.abstract(task.init, env.predicates))
+
+
+def test_a_flipped_observation_is_still_upright(env):
+    """The same fold applies mid-episode, not just at task construction."""
+    task = env._build_task_from_scene()  # pylint: disable=protected-access
+    comp = env._domino_component  # pylint: disable=protected-access
+    target = comp.dominos[3]
+
+    obs = _StubDominoObservation(
+        [_StubDominoPose(_TARGET_ID, (0.2, 0.0, 0.03), _base_quat(np.pi))])
+    state = env.state_from_observation(obs, task.init)
+
+    assert state.get(target, "roll") == pytest.approx(0.0, abs=1e-9)
+    # pylint: disable=protected-access
+    assert comp._Upright_holds(state, [target])
+    assert not comp._Toppled_holds(state, [target])