fix(llm-plan-executor): harden constrained planning and record Webots QA

src/iiwa_llm_plan_executor/test/test_motion_policy.py

@@ -89,6 +89,42 @@ def test_tcp_tilt_above_ten_degrees_is_rejected():
     )
     assert rejected.ok is False
     assert 'tilt' in rejected.reason
+    assert checker.validate_trajectory(
+        [point(0.0)], [down_pose(10.05)],
+    ).ok
+
+
+def test_an_empty_gripper_may_reorient_to_the_working_posture():
+    checker = policy()
+    up = Transform.from_xyz_rpy(
+        [0.4, 0.0, 1.2], [0.0, 0.0, 0.0], parent='world', child='tcp',
+    )
+    accepted = checker.validate_trajectory(
+        [point(0.0), point(0.0)],
+        [up, down_pose()],
+        start_tcp_pose=up,
+        held_object=None,
+    )
+    assert accepted.ok is True
+    assert accepted.details['reorientation'] is True
+
+    held = checker.validate_trajectory(
+        [point(0.0), point(0.0)],
+        [up, down_pose()],
+        start_tcp_pose=up,
+        held_object='cap',
+    )
+    assert held.ok is False
+    assert 'tilt' in held.reason
+
+    stuck_up = checker.validate_trajectory(
+        [point(0.0), point(0.0)],
+        [up, up],
+        start_tcp_pose=up,
+        held_object=None,
+    )
+    assert stuck_up.ok is False
+    assert 'working posture' in stuck_up.reason
 
 
 def test_stale_joint_state_and_scene_are_rejected_before_motion():

src/iiwa_llm_plan_executor/test/test_node.py

@@ -200,6 +200,21 @@ def test_context_factory_sets_sources_and_inspection_configuration():
     assert context.notify('x', {}) is True
 
 
+def test_future_scene_timestamps_cannot_delay_the_start_gate():
+    from std_msgs.msg import String
+
+    node = fake_node()
+    node._scene_ready_at = None
+    node._latest_scene = None
+    message = String()
+    message.data = json.dumps({'timestamp': 500.0, 'scene_revision': 1})
+    PlannerExecutorNode._on_scene(node, message)
+    assert node._scene_ready_at == 10.0
+    message.data = json.dumps({'timestamp': 5.0, 'scene_revision': 1})
+    PlannerExecutorNode._on_scene(node, message)
+    assert node._scene_ready_at == 5.0
+
+
 def test_robot_freshness_uses_the_joint_limit_age():
     node = fake_node()
     node.limits = SimpleNamespace(joint_state_max_age_sec=1.0)