fix: retain planning feedback and add acceptance probes

2026-09-22 00:47:11 +08:00
parent f9d8feb6f0
commit 91bcd92d6b
12 changed files with 871 additions and 10 deletions
@@ -0,0 +1,59 @@
+import copy
+import sys
+import unittest
+from pathlib import Path
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1] / 'tools'))
+from physics_delivery_probe import verify_delivery
+
+
+class DeliveryPredicateTests(unittest.TestCase):
+    def setUp(self):
+        self.frames = [dict(time=i / 240, body_id=4, destination_id=2,
+                            aabb=[[-.02, -.02, .01], [.02, .02, .05]],
+                            linear_speed=0., angular_speed=0.,
+                            floor_contact=True, held=False) for i in range(121)]
+        self.bounds = [[-.2, -.2, 0.], [.2, .2, .3]]
+
+    def verify(self, frames=None):
+        return verify_delivery(self.frames if frames is None else frames,
+                               self.bounds, 4, 2)
+
+    def test_settled_released_object(self):
+        self.assertTrue(self.verify()['verified'])
+
+    def test_reject_each_unsafe_condition(self):
+        for update in ({'held': True}, {'floor_contact': False},
+                       {'linear_speed': .1}, {'angular_speed': .3},
+                       {'body_id': 8}, {'destination_id': 9},
+                       {'aabb': [[.19, 0., .01], [.23, .04, .05]]},
+                       {'linear_speed': float('nan')}, {'held': None},
+                       {'floor_contact': 'true'}):
+            with self.subTest(update=update):
+                frames = copy.deepcopy(self.frames)
+                frames[-1].update(update)
+                self.assertFalse(self.verify(frames)['verified'])
+
+    def test_missing_or_short_evidence(self):
+        for frames in ([], self.frames[-20:], [{}]):
+            self.assertFalse(self.verify(frames)['verified'])
+
+    def test_time_must_increase_without_sampling_gap(self):
+        for value in (self.frames[-2]['time'], 1., float('nan')):
+            frames = copy.deepcopy(self.frames)
+            frames[-1]['time'] = value
+            self.assertFalse(self.verify(frames)['verified'])
+
+    def test_stability_requires_whole_window(self):
+        frames = copy.deepcopy(self.frames)
+        frames[-40]['held'] = True
+        self.assertFalse(self.verify(frames)['verified'])
+
+    def test_invalid_earlier_timestamp_cannot_be_filtered_out(self):
+        frames = copy.deepcopy(self.frames)
+        frames[-30]['time'] = float('nan')
+        self.assertFalse(self.verify(frames)['verified'])
+
+
+if __name__ == '__main__':
+    unittest.main()