import contextlib import io import json import tempfile import unittest from pathlib import Path from unittest.mock import patch import torch from ml.eval.eval_cl import json_finite, run from ml.eval.diag_collision import replay_certificates, replay_dense, replay_first_hits from ml.sim.closed_loop import TeacherOracle, rollout from ml.common.kin_torch import Kin, seg_seg_dist class EvalMetricsTest(unittest.TestCase): def setUp(self): self.tasks = [ {'q0': [0] * 6, 'T': [0] * 3, 'exec_s': 1, 'travel': 5}, {'q0': [0] * 6, 'T': [0] * 3, 'exec_s': None, 'travel': None}, {'q0': [0] * 6, 'T': [0] * 3, 'exec_s': 2, 'travel': 6}, ] self.per = {k: torch.tensor(v) for k, v in { 'ok': [True, False, True], 'done': [True, False, True], 't_done': [0.5, float('nan'), 2.0], 'col': [1.0, 0.0, 0.0], 'travel': [6.0, 0.0, 8.0], 'brakes': [2.0, 4.0, 6.0], 'start_fail': [False, True, False], 'goal_ratio_2s': [0.3, 0.9, 0.1], 'err': [0.2, 200.0, 0.8], 'dls_it': [4, -1, 20], 'speed_ratio': [1.66, 0.9, 1.2], 'accel_ratio': [0.3, 0.2, 0.5], }.items()} def evaluate(self, tasks, bs=2, **kw): offset = 0 def fake_rollout(model, kin, batch, device, **kwargs): nonlocal offset p = {k: v[offset:offset + len(batch)] for k, v in self.per.items()} offset += len(batch) return {'time_budget': 'mixed'}, p with patch('ml.eval.eval_cl.rollout', side_effect=fake_rollout), contextlib.redirect_stdout(io.StringIO()): return run(None, None, tasks, 'cpu', bs=bs, **kw)[0] def test_global_denominators_and_collision_free_arrival(self): result = self.evaluate(self.tasks) self.assertEqual(result['n_ref'], 2) self.assertAlmostEqual(result['success'], 2 / 3, places=6) self.assertAlmostEqual(result['collision_free_success'], 1 / 3, places=6) self.assertAlmostEqual(result['collision_rate'], 1 / 3, places=6) self.assertEqual(result['time_ok_1p3'], 1.0) self.assertEqual(result['time_mean_s'], 1.25) self.assertAlmostEqual(result['travel_ratio_median'], 1.2, places=6) self.assertAlmostEqual(result['goal_ratio_2s'], 0.3, places=6) self.assertEqual(result['err_fail_med'], 200.0) self.assertEqual(result['brakes_mean'], 4.0) # 极值/分位按逐题值跨批汇总，不能进按 n 加权求均值的批聚合 self.assertAlmostEqual(result['speed_ratio_p50'], 1.2, places=6) self.assertAlmostEqual(result['speed_ratio_p95'], 1.614, places=6) self.assertAlmostEqual(result['speed_ratio_max'], 1.66, places=5) self.assertAlmostEqual(result['over_vmax_rate'], 2 / 3, places=6) # 1.66 与 1.2 都算超 VMAX，0.9 不算 self.assertAlmostEqual(result['accel_ratio_max'], 0.5, places=6) def test_empty_conditional_metrics_and_explicit_limit(self): task = {**self.tasks[0], 'exec_s': None, 'travel': None} result = self.evaluate([task], max_sec=0.04) self.assertIsNone(result['err_fail_med']) self.assertIsNone(result['time_ok_1p3']) self.assertIsNone(result['travel_ratio_median']) self.assertEqual(result['time_budget'], 'fixed 0.040 s') self.assertEqual(result['collision_free_success'], 0.0) def test_json_replaces_nonfinite_without_changing_zero(self): value = {'items': [float('nan'), float('inf'), -float('inf'), 0.0]} text = json.dumps(json_finite(value), allow_nan=False) self.assertEqual(json.loads(text), {'items': [None, None, None, 0.0]}) def test_first_hit_probe_detects_collision(self): class PlaneKin: def __init__(self, **kwargs): self.cont = torch.zeros(6, dtype=torch.bool) self.lo, self.hi = torch.full((6,), -10.0), torch.full((6,), 10.0) def gaps(self, q, cap): return {'gm': q[:, :1].expand(-1, 49).clamp(max=cap)} traj = torch.zeros(1, 3, 6) traj[0, :, 0] = torch.tensor([1.0, 1.0, -1.0]) per = {'col': torch.tensor([1.0]), 'first_hit': torch.tensor([0.04]), 'dls_it': torch.tensor([-1]), 'traj': traj} with tempfile.TemporaryDirectory() as folder: trace = Path(folder) / 'trace.pt' torch.save({'tasks': [self.tasks[0]], 'batches': [({}, per, 1)]}, trace) with patch('ml.eval.diag_collision.Kin', PlaneKin), contextlib.redirect_stdout(io.StringIO()): result = replay_first_hits(trace) self.assertEqual(result['reported_collision_tasks'], 1) self.assertEqual(result['replayed_first_hits32'], 1) self.assertEqual(result['replayed_first_hits64'], 1) self.assertEqual(result['rows'][0]['gap64_mm'], [0.5, 0.5, 0.0, -1.0]) self.assertEqual(result['rows'][0]['dense_gap64_min_mm'], -1.0) def test_dense_probe_detects_between_checkpoints_and_respects_deadline(self): class ThinObstacle: fp = 'probe' def __init__(self, **kwargs): pass def gaps(self, q, cap): return {'gm': ((q[:, :1] - 0.25).abs() - 0.05).clamp(max=cap)} traj = torch.zeros(4, 3, 6) traj[:, :, 0] = torch.tensor([[0, 1, 1], [0, -0.5, -1], [0, 0, 0.25], [0, 0, 0.25]]) per = {'traj': traj, 'col': torch.zeros(4), 'limit': torch.tensor([0.04, 0.04, 0.02, 0.04]), 't_done': torch.tensor([float('nan')] * 3 + [0.02])} with tempfile.TemporaryDirectory() as folder: trace = Path(folder) / 'trace.pt' output = Path(folder) / 'audit.json' torch.save({'kin_fp': 'probe', 'tasks': [{}] * 4, 'batches': [({}, per, 4)]}, trace) with patch('ml.eval.diag_collision.Kin', ThinObstacle), contextlib.redirect_stdout(io.StringIO()): result = replay_dense(trace, [0, 1, 2, 3], 0.01, out=output) with self.assertRaisesRegex(ValueError, 'valid task indices'): replay_dense(trace, [4]) with self.assertRaisesRegex(ValueError, 'positive max_step_deg'): replay_dense(trace, [0], 0.0) self.assertEqual(json.loads(output.read_text())['collision_tasks'], 1) self.assertEqual(result['collision_tasks'], 1) self.assertEqual(result['rows'][0]['collision_segments'], 1) self.assertAlmostEqual(result['rows'][0]['min_gap64_mm'], -0.05) self.assertGreater(result['rows'][1]['min_gap64_mm'], 0) for row in result['rows'][2:]: self.assertGreater(row['min_gap64_mm'], 0) self.assertEqual(row['steps'], 1) def test_certificate_replay_samples_actual_movement_before_deadlines(self): kin = SegmentCertificateTest.ThinObstacle() kin.fp = 'probe' traj = torch.zeros(3, 4, 6) traj[:, :, 0] = torch.tensor([[0, 1, 1, 1], [0, -0.5, -1, -1], [0, 0, 1, 1]]) per = {'traj': traj, 'limit': torch.tensor([0.06, 0.02, 0.02]), 't_done': torch.tensor([float('nan'), 0.02, float('nan')]), 'plans': {'it': torch.arange(3), 'commit': torch.zeros(3, 3, 1, 6), 'certified': torch.ones(3, 3, 1, dtype=torch.bool), 'gaps': torch.full((3, 3, 2), 0.5)}} with tempfile.TemporaryDirectory() as folder: trace, output = Path(folder) / 'trace.pt', Path(folder) / 'audit.json' torch.save({'kin_fp': 'probe', 'tasks': [{}] * 3, 'batches': [({}, per, 3)]}, trace) with patch('ml.eval.diag_collision.Kin', return_value=kin), contextlib.redirect_stdout(io.StringIO()): result = replay_certificates(trace, [0, 1, 2], out=output, repeats=1, extra_segments=[(0, 0)]) with self.assertRaisesRegex(ValueError, 'extra segment'): replay_certificates(trace, [1], repeats=1, extra_segments=[(1, 1)]) self.assertEqual(json.loads(output.read_text())['dense_collision_segments'], 1) self.assertEqual([(r['i'], r['start_it']) for r in result['rows']], [(0, 0), (1, 0)]) self.assertEqual(result['budgets']['32']['safe'], 1) self.assertEqual(result['budgets']['32']['collision'], 1) self.assertEqual(result['accepted_dense_collision_segments'], 0) self.assertTrue(result['batch_single_match']) self.assertEqual(result['timing']['calls'], 2) class DlsSafetyTest(unittest.TestCase): def test_rejected_refinement_retains_safe_candidate(self): class ObstacleKin: def clamp_box(self, q): return q def gaps(self, q, cap): return {'gm': torch.where(q[:, :1] > 0.3, -torch.ones_like(q[:, :1]), torch.ones_like(q[:, :1]))} task = {'q0': [0] * 6, 'T': [2, 0, 0]} traj = torch.zeros(1, 2, 6) traj[0, 1, 0] = 0.1 ref = torch.zeros(1, 1, 6) ref[0, 0, 0] = 1.0 patches = { 'features': lambda *args: torch.zeros(1, 83), 'goal_err': lambda kin, q, T, R: (torch.full((q.shape[0],), 2.0), torch.zeros(q.shape[0])), 'dls_steps': lambda *args, **kwargs: ref, } results = [] with patch.multiple('ml.sim.closed_loop', **patches): for gate in (0, 1): _, per = rollout(None, ObstacleKin(), [task], 'cpu', K=1, safety=True, max_sec=0.02, return_traj=True, use_sub=False, select='first', oracle=TeacherOracle(traj), replan='empty', commit_n=1, safety_partial=1, dls_safety=gate) results.append(per) self.assertEqual(results[0]['col'].item(), 1.0) self.assertEqual(results[0]['dls_it'].item(), 0) self.assertEqual(results[1]['col'].item(), 0.0) self.assertEqual(results[1]['dls_it'].item(), -1) self.assertAlmostEqual(results[1]['traj'][0, 1, 0].item(), 0.1) def test_feedback_is_reduced_only_after_full_rejection(self): class ObstacleKin: def clamp_box(self, q): return q def gaps(self, q, cap): return {'gm': torch.where(q[:, :1] > 0.3, -torch.ones_like(q[:, :1]), torch.ones_like(q[:, :1]))} task = {'q0': [0] * 6, 'T': [10, 0, 0]} traj = torch.zeros(1, 17, 6) traj[0, :, 0] = -0.2 * torch.arange(17) ref = torch.zeros(1, 1, 6) ref[0, 0, 0] = 1.0 patches = { 'features': lambda *args: torch.zeros(1, 83), 'goal_err': lambda kin, q, T, R: (torch.full((q.shape[0],), 10.0), torch.zeros(q.shape[0])), 'dls_steps': lambda *args, **kwargs: ref, } results = [] with patch.multiple('ml.sim.closed_loop', **patches): for gain in (1.0, 0.5): _, per = rollout(None, ObstacleKin(), [task], 'cpu', K=1, safety=True, max_sec=0.04, return_traj=True, use_sub=False, select='first', oracle=TeacherOracle(traj), replan='empty', commit_n=1, safety_partial=1, fb_gain=0.6, fb_block_gain=gain) results.append(per) self.assertEqual(results[0]['brakes'].item(), 2.0) self.assertEqual(results[1]['brakes'].item(), 1.0) self.assertEqual(results[1]['traj'][0, 1, 0].item(), 0.0) self.assertAlmostEqual(results[1]['traj'][0, 2, 0].item(), 0.1) self.assertEqual(results[1]['col'].item(), 0.0) class ClampedPathSafetyTest(unittest.TestCase): def rollout_candidate(self, increments, safety_partial, goal_error=10.0, **kwargs): class ObstacleKin: def clamp_box(self, q): clamped = q.clone() clamped[..., 4] = clamped[..., 4].clamp(max=0.0) return clamped def gaps(self, q, cap): collision = (q[:, :1] > 0.1) & (q[:, 4:5] < 0.2) return {'gm': torch.where(collision, -torch.ones_like(q[:, :1]), torch.ones_like(q[:, :1]))} task = {'q0': [0] * 6, 'T': [goal_error, 0, 0]} commit_n = len(increments) traj = torch.zeros(1, commit_n + 1, 6) traj[0, 1:] = torch.tensor(increments).cumsum(0) patches = { 'features': lambda *args: torch.zeros(1, 83), 'goal_err': lambda kin, q, T, R: (torch.full((q.shape[0],), goal_error), torch.zeros(q.shape[0])), } with patch.multiple('ml.sim.closed_loop', **patches): _, per = rollout(None, ObstacleKin(), [task], 'cpu', K=1, safety=True, max_sec=0.02 * commit_n, return_traj=True, use_sub=False, select='first', oracle=TeacherOracle(traj), replan='empty', commit_n=commit_n, safety_partial=safety_partial, **kwargs) return per def test_full_safety_rejects_collision_after_joint_clamp(self): # Raw endpoint/midpoint are safe, but the J5 upper limit puts the endpoint at (0.2, 0). per = self.rollout_candidate([[0.2, 0, 0, 0, 0.6, 0]], safety_partial=0) self.assertEqual(per['col'].item(), 0.0) torch.testing.assert_close(per['traj'], torch.zeros(1, 2, 6), rtol=0, atol=0) def test_partial_safety_rejects_collision_after_joint_clamp(self): per = self.rollout_candidate([[0.2, 0, 0, 0, 0.6, 0]], safety_partial=1) self.assertEqual(per['col'].item(), 0.0) torch.testing.assert_close(per['traj'], torch.zeros(1, 2, 6), rtol=0, atol=0) def test_partial_safety_keeps_only_clamped_safe_prefix(self): per = self.rollout_candidate([ [0.05, 0, 0, 0, 0.6, 0], [0.2, 0, 0, 0, 0.6, 0], ], safety_partial=1) self.assertAlmostEqual(per['traj'][0, -1, 0].item(), 0.05) self.assertEqual(per['traj'][0, -1, 4].item(), 0.0) self.assertEqual(per['col'].item(), 0.0) expected = torch.zeros(1, 3, 6) expected[0, 1:, 0] = 0.05 torch.testing.assert_close(per['traj'], expected) torch.testing.assert_close(per['plans']['commit'][0, 0, 1], torch.zeros(6), rtol=0, atol=0) def test_near_dls_rejects_clamped_collision_and_keeps_candidate(self): candidate = [[0.05, 0, 0, 0, 0.6, 0]] ref = torch.tensor([[[0.2, 0, 0, 0, 0.6, 0]]]) for safety_partial in (0, 1): with self.subTest(safety_partial=safety_partial): with patch('ml.sim.closed_loop.dls_steps', return_value=ref) as dls: per = self.rollout_candidate(candidate, safety_partial, goal_error=2.0, dls_safety=1) dls.assert_called_once() self.assertEqual(per['col'].item(), 0.0) self.assertEqual(per['dls_it'].item(), -1) self.assertEqual(per['brakes'].item(), 0.0) expected = torch.zeros(1, 2, 6) expected[0, 1, 0] = 0.05 torch.testing.assert_close(per['traj'], expected) torch.testing.assert_close(per['plans']['commit'], torch.tensor(candidate)[None, None]) self.assertTrue(per['plans']['certified'].all().item()) def test_r3_clamped_queue_origin_prefix_and_certificates_match_execution(self): class SafeKin: def clamp_box(self, q): clamped = q.clone() clamped[..., 4] = clamped[..., 4].clamp(max=0.0) return clamped def gaps(self, q, cap): return {'gm': torch.ones_like(q[:, :1])} prefixes = [] def capture_features(kin, q, qp, T, R, pre, r, sub, sf): prefixes.append((pre.clone(), int(r.item()))) return torch.zeros(1, 83) teacher = torch.zeros(1, 25, 6) teacher[0, :, 0] = 0.02 * torch.arange(25) teacher[0, :, 4] = 0.6 * torch.arange(25) task = {'q0': [0] * 6, 'T': [10, 0, 0]} patches = { 'features': capture_features, 'goal_err': lambda kin, q, T, R: (torch.full((q.shape[0],), 10.0), torch.zeros(q.shape[0])), } with patch.multiple('ml.sim.closed_loop', **patches): _, per = rollout(None, SafeKin(), [task], 'cpu', K=1, safety=True, max_sec=0.16, return_traj=True, use_sub=False, select='first', oracle=TeacherOracle(teacher), replan='r3', commit_n=3, safety_partial=1) self.assertEqual([r for _, r in prefixes], [0, 2, 3, 3]) for pre, r in prefixes[1:]: torch.testing.assert_close(pre[..., 4], torch.zeros(1, 3), rtol=0, atol=0) torch.testing.assert_close(pre[:, :r, 0], torch.full((1, r), 0.02)) self.assertEqual(per['col'].item(), 0.0) expected = torch.zeros(1, 9, 6) expected[0, :, 0] = 0.02 * torch.arange(9) torch.testing.assert_close(per['traj'], expected) plans = per['plans'] self.assertEqual(plans['it'].tolist(), [0, 3, 6, 9]) torch.testing.assert_close(plans['commit'][..., 4], torch.full((1, 4, 3), 0.6)) self.assertTrue(plans['certified'].all().item()) self.assertTrue((plans['gaps'] > 0).all().item()) checked = 0 for i, start in enumerate(plans['it'].tolist()): n = min(3, 8 - start) if n <= 0: continue torch.testing.assert_close(plans['qe'][:, i], per['traj'][:, start], rtol=0, atol=0) before = per['traj'][:, start:start + n] after = per['traj'][:, start + 1:start + n + 1] torch.testing.assert_close(plans['points'][:, i, :n], after, rtol=0, atol=0) torch.testing.assert_close(plans['midpoints'][:, i, :n], before + 0.5 * (after - before), rtol=0, atol=0) checked += n self.assertEqual(checked, 8) def rollout_fallback(self, safety_partial, midpoint_collision=False): class ObstacleKin: def clamp_box(self, q): return q def gaps(self, q, cap): collision = q[:, :1] > 0.3 if midpoint_collision: collision |= (q[:, :1] > 0.09) & (q[:, :1] < 0.11) return {'gm': torch.where(collision, -torch.ones_like(q[:, :1]), torch.ones_like(q[:, :1]))} teacher = torch.zeros(1, 4, 6) teacher[0, :, 0] = torch.arange(4) ref = torch.zeros(1, 1, 6) ref[0, 0, 0] = 0.2 task = {'q0': [0] * 6, 'T': [10, 0, 0]} patches = { 'features': lambda *args: torch.zeros(1, 83), 'goal_err': lambda kin, q, T, R: (torch.full((q.shape[0],), 10.0), torch.zeros(q.shape[0])), 'dls_steps': lambda *args, **kwargs: ref, } with patch.multiple('ml.sim.closed_loop', **patches): _, per = rollout(None, ObstacleKin(), [task], 'cpu', K=1, safety=True, max_sec=0.06, return_traj=True, use_sub=False, select='first', oracle=TeacherOracle(teacher), replan='empty', commit_n=3, safety_partial=safety_partial, fb_gain=1.0, fb_fallback=1.0) return per def test_fallback_executes_one_safe_dls_step_then_certified_zeros(self): for safety_partial in (0, 1): with self.subTest(safety_partial=safety_partial): per = self.rollout_fallback(safety_partial) self.assertEqual(per['col'].item(), 0.0) self.assertEqual(per['brakes'].item(), 1.0) expected = torch.zeros(1, 4, 6) expected[0, 1:, 0] = 0.2 torch.testing.assert_close(per['traj'], expected) commit = torch.zeros(1, 1, 3, 6) commit[0, 0, 0, 0] = 0.2 plans = per['plans'] self.assertEqual(plans['it'].tolist(), [0]) torch.testing.assert_close(plans['commit'], commit, rtol=0, atol=0) self.assertTrue(plans['certified'].all().item()) self.assertTrue((plans['gaps'] > 0).all().item()) torch.testing.assert_close(plans['points'][:, 0], expected[:, 1:]) torch.testing.assert_close(plans['midpoints'][:, 0], expected[:, :-1] + 0.5 * (expected[:, 1:] - expected[:, :-1])) def test_fallback_rejects_safe_endpoint_with_colliding_midpoint(self): for safety_partial in (0, 1): with self.subTest(safety_partial=safety_partial): per = self.rollout_fallback(safety_partial, midpoint_collision=True) self.assertEqual(per['col'].item(), 0.0) self.assertEqual(per['brakes'].item(), 1.0) torch.testing.assert_close(per['traj'], torch.zeros(1, 4, 6), rtol=0, atol=0) torch.testing.assert_close(per['plans']['commit'], torch.zeros(1, 1, 3, 6), rtol=0, atol=0) self.assertFalse(per['plans']['certified'].any().item()) class SegmentCertificateTest(unittest.TestCase): class ThinObstacle(Kin): def __init__(self): self.dtype = torch.float64 self.lo, self.hi = torch.full((6,), -5.0), torch.full((6,), 5.0) self.cont = torch.zeros(6, dtype=torch.bool) self.cert_groups = torch.tensor([0]) self.cert_lever = torch.tensor([[1.0, 0, 0, 0, 0, 0]], dtype=self.dtype) def gaps(self, q, cap): return {'gm': ((q[:, :1] - 0.25).abs() - 0.05).clamp(max=cap)} def test_bound_detects_hidden_collision_and_accepts_clear_segment(self): a, b = torch.zeros(3, 6, dtype=torch.float64), torch.zeros(3, 6, dtype=torch.float64) a[:, 0] = torch.tensor([0.0, -1.0, 0.25]) b[:, 0] = torch.tensor([1.0, 0.0, 0.25]) result = self.ThinObstacle().certify_segments(a, b) self.assertEqual(result['safe'].tolist(), [False, True, False]) self.assertEqual(result['collision'].tolist(), [True, False, True]) self.assertEqual(result['queries'].tolist(), [4, 2, 2]) def test_budget_and_depth_exhaustion_do_not_accept_unknown_segment(self): a, b = torch.zeros(1, 6, dtype=torch.float64), torch.zeros(1, 6, dtype=torch.float64) b[:, 0] = 1.0 kin = self.ThinObstacle() for kwargs in ({'max_queries': 2}, {'max_depth': 0}): with self.subTest(**kwargs): result = kin.certify_segments(a, b, **kwargs) self.assertFalse(result['safe'].item()) self.assertTrue(result['unresolved'].item()) self.assertEqual(result['queries'].item(), 2) def test_shape_mismatch_between_endpoints_is_rejected(self): a = torch.zeros(1, 6, dtype=torch.float64) b = torch.zeros(2, 6, dtype=torch.float64) kin = self.ThinObstacle() for method in (kin.certify_segments, kin.certify_uniform): with self.subTest(method=method.__name__): with self.assertRaisesRegex(ValueError, 'identical shape'): method(a, b) def test_budget_exhaustion_scales_with_gap_and_bound(self): class ConstantGap(self.ThinObstacle): def gaps(self, q, cap): return {'gm': torch.full((q.shape[0], 1), 0.1, dtype=q.dtype, device=q.device)} # 界取 1 mm/度：1° 段要证明无碰撞，必须细分到位移界 < 两端间隙之和(0.2 mm)。 kin = ConstantGap() kin.cert_lever = torch.ones(1, 6, dtype=torch.float64) a, b = torch.zeros(1, 6, dtype=torch.float64), torch.zeros(1, 6, dtype=torch.float64) b[:, 0] = 1.0 for budget, safe, queries in ((4, False, 3), (8, False, 5), (9, True, 9)): with self.subTest(budget=budget): result = kin.certify_segments(a, b, max_queries=budget) self.assertEqual(bool(result['safe'].item()), safe) self.assertEqual(int(result['queries'].item()), queries) def test_tensor_budget_cannot_wrap_depth(self): a, b = torch.zeros(1, 6, dtype=torch.float64), torch.zeros(1, 6, dtype=torch.float64) a[:, 0] = b[:, 0] = 0.25 for tensor in (torch.tensor(255, dtype=torch.uint8), torch.tensor(255)): with self.subTest(dtype=tensor.dtype): with self.assertRaisesRegex(ValueError, 'plain ints'): self.ThinObstacle().certify_segments(a, b, max_depth=tensor) def test_batch_matches_per_root_and_ignores_order(self): kin = self.ThinObstacle() a = torch.tensor([[0.0, 0, 0, 0, 0, 0], [0.25, 0, 0, 0, 0, 0], [-1.0, 0, 0, 0, 0, 0]], dtype=torch.float64) b = torch.tensor([[1.0, 0, 0, 0, 0, 0], [0.25, 0, 0, 0, 0, 0], [0.0, 0, 0, 0, 0, 0]], dtype=torch.float64) batch = kin.certify_segments(a, b, max_queries=16) self.assertEqual(batch['collision'].tolist(), [True, True, False]) self.assertEqual(batch['safe'].tolist(), [False, False, True]) for perm in ([0, 1, 2], [2, 1, 0], [1, 0, 2]): index = torch.tensor(perm) moved = kin.certify_segments(a[index], b[index], max_queries=16) for key in ('safe', 'collision', 'unresolved', 'queries'): torch.testing.assert_close(moved[key], batch[key][index], rtol=0, atol=0) def test_empty_batch_and_all_rejected_frontier(self): kin = self.ThinObstacle() empty = kin.certify_segments(torch.zeros(0, 6, dtype=torch.float64), torch.zeros(0, 6, dtype=torch.float64)) self.assertEqual(empty['safe'].shape, (0,)) a = torch.tensor([[0.25, 0, 0, 0, 0, 0], [6.0, 0, 0, 0, 0, 0]], dtype=torch.float64) result = kin.certify_segments(a, a.clone(), max_queries=2) self.assertEqual(result['safe'].tolist(), [False, False]) self.assertEqual(result['collision'].tolist(), [True, False]) self.assertEqual(result['outside_box'].tolist(), [False, True]) def test_uniform_matches_bound_formula_and_needs_no_round_trips(self): class ConstantGap(self.ThinObstacle): def gaps(self, q, cap): return {'gm': torch.full((q.shape[0], 1), 0.1, dtype=q.dtype, device=q.device)} # 界 1 mm/度、间隙 0.1 mm：子段判据 0.2 > 1/n 需要 n ≥ 6，n=5 必须落 unproven。 kin = ConstantGap() kin.cert_lever = torch.ones(1, 6, dtype=torch.float64) a, b = torch.zeros(1, 6, dtype=torch.float64), torch.zeros(1, 6, dtype=torch.float64) b[:, 0] = 1.0 for n, safe in ((5, False), (6, True)): with self.subTest(n=n): result = kin.certify_uniform(a, b, n=n) self.assertEqual(bool(result['safe'].item()), safe) self.assertEqual(result['points'], n + 1) self.assertFalse(bool(result['collision'].item())) self.assertEqual(bool(result['unproven'].item()), not safe) self.assertAlmostEqual(result['cap_required_mm'], 1.0 / n + 2e-4, places=9) def test_uniform_catches_hidden_collision_and_rejects_short_grids(self): a = torch.tensor([[0.0, 0, 0, 0, 0, 0], [-1.0, 0, 0, 0, 0, 0], [6.0, 0, 0, 0, 0, 0]], dtype=torch.float64) b = torch.tensor([[1.0, 0, 0, 0, 0, 0], [0.0, 0, 0, 0, 0, 0], [6.0, 0, 0, 0, 0, 0]], dtype=torch.float64) kin = self.ThinObstacle() short = kin.certify_uniform(a, b, n=2) self.assertEqual(short['collision'].tolist(), [False, False, False]) self.assertEqual(short['unproven'].tolist(), [True, False, False]) self.assertEqual(short['safe'].tolist(), [False, True, False]) for n in (4, 8, 64): with self.subTest(n=n): result = kin.certify_uniform(a, b, n=n) self.assertEqual(result['collision'].tolist(), [True, False, False]) self.assertEqual(result['safe'].tolist(), [False, True, False]) self.assertEqual(result['outside_box'].tolist(), [False, False, True]) def test_uniform_requires_plain_int_count(self): a = torch.zeros(1, 6, dtype=torch.float64) for tensor in (torch.tensor(8, dtype=torch.uint8), torch.tensor(8)): with self.subTest(dtype=tensor.dtype): with self.assertRaisesRegex(ValueError, 'subdivision count'): self.ThinObstacle().certify_uniform(a, a.clone(), n=tensor) def test_nonfinite_and_outside_box_are_rejected(self): a, b = torch.zeros(1, 6, dtype=torch.float64), torch.zeros(1, 6, dtype=torch.float64) kin = self.ThinObstacle() with patch.object(kin, 'gaps', side_effect=lambda q, cap: {'gm': torch.full((q.shape[0], 1), float('nan'))}): result = kin.certify_segments(a, b) self.assertFalse(result['safe'].item()) self.assertTrue(result['unresolved'].item()) for value in (float('inf'), -float('inf')): with self.subTest(value=value): with patch.object(kin, 'gaps', side_effect=lambda q, cap, v=value: {'gm': torch.full((q.shape[0], 1), v)}): result = kin.certify_segments(a, b) self.assertFalse(result['safe'].item()) a[:, 0] = b[:, 0] = 6.0 result = kin.certify_segments(a, b) self.assertFalse(result['safe'].item()) self.assertTrue(result['outside_box'].item()) def test_point_segment_distance_is_symmetric_and_differentiable(self): p = torch.tensor([[0.0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]], dtype=torch.float64, requires_grad=True) q = torch.tensor([[2.0, 0, 0]] * 4, dtype=torch.float64) point = torch.tensor([[1.0, 0, 0], [1, 3, 0], [4, 0, 0], [-1, 0, 0]], dtype=torch.float64) forward = seg_seg_dist(p, q, point, point) reverse = seg_seg_dist(point, point, p, q) torch.testing.assert_close(forward, torch.tensor([0.0, 3, 2, 1], dtype=torch.float64), atol=1e-8, rtol=0) torch.testing.assert_close(forward, reverse, atol=1e-8, rtol=0) forward.sum().backward() self.assertTrue(torch.isfinite(p.grad).all()) torch.testing.assert_close(seg_seg_dist(p.detach(), p.detach(), point, point), (p.detach() - point).norm(dim=-1), atol=1e-8, rtol=0) class SpeedAndMarginTest(unittest.TestCase): """轮次33：反馈叠加后的每轴速度限幅，以及安全层的正余量门限（含 cap 截顶坑）。""" STEP = [1.2, 1.2, 1.8, 1.8, 2.4, 3.6] def rollout_one(self, teacher_step, gap_value, fb=None, **kwargs): class FlatKin: def clamp_box(self, q): return q def gaps(self, q, cap): return {'gm': torch.full((q.shape[0], 49), float(gap_value))} commit_n = 2 traj = torch.zeros(1, commit_n + 1, 6) traj[0, 1:] = torch.tensor(teacher_step)[None].repeat(commit_n, 1).cumsum(0) # teacher_step = 每步各轴的度数增量 patches = { 'features': lambda *args: torch.zeros(1, 83), 'goal_err': lambda kin, q, T, R: (torch.full((q.shape[0],), 10.0), torch.zeros(q.shape[0])), } with patch.multiple('ml.sim.closed_loop', **patches): with patch('ml.sim.closed_loop.dls_steps', return_value=torch.tensor([[[1.0, 0, 0, 0, 0, 0]]])): _, per = rollout(None, FlatKin(), [{'q0': [0] * 6, 'T': [10, 0, 0]}], 'cpu', K=1, safety=True, max_sec=0.02 * commit_n, return_traj=True, use_sub=False, select='first', oracle=TeacherOracle(traj), replan='empty', commit_n=commit_n, **kwargs) return per def test_feedback_over_speed_is_measured_not_assumed(self): # 策略给满 STEP（J1 +1.2°）+ 2.2×1.0° 反馈 = 3.4° > VMAX·DT per = self.rollout_one([1.2, 0, 0, 0, 0, 0], 5.0, fb_gain=2.2) self.assertAlmostEqual(per['traj'][0, 1, 0].item(), 3.4, places=5) self.assertAlmostEqual(per['speed_ratio'][0].item(), 3.4 / 1.2, places=5) def test_inc_cap_limits_actual_step_to_vmax(self): per = self.rollout_one([1.2, 0, 0, 0, 0, 0], 5.0, fb_gain=2.2, inc_cap=1) self.assertAlmostEqual(per['traj'][0, 1, 0].item(), 1.2, places=5) self.assertLessEqual(per['speed_ratio'][0].item(), 1.0 + 1e-6) def test_margin_rejects_near_obstacle_step_that_passes_at_zero(self): moved = self.rollout_one([0.5, 0, 0, 0, 0, 0], 0.3, safety_partial=1) self.assertAlmostEqual(moved['traj'][0, -1, 0].item(), 1.0, places=5) self.assertEqual(moved['brakes'].item(), 0.0) braked = self.rollout_one([0.5, 0, 0, 0, 0, 0], 0.3, gap_margin=0.5, safety_partial=1) self.assertEqual(braked['brakes'].item(), 1.0) torch.testing.assert_close(braked['traj'], torch.zeros(1, 3, 6), rtol=0, atol=0) def test_margin_below_cap_still_accepts_clear_step(self): # 余量 0.5 必须仍能通过 5 mm 间隙：若宽相位 cap 没抬到 margin+0.5，截顶值 0.5 会让门限恒假 per = self.rollout_one([0.5, 0, 0, 0, 0, 0], 5.0, gap_margin=0.5, safety_partial=1) self.assertEqual(per['brakes'].item(), 0.0) self.assertAlmostEqual(per['traj'][0, -1, 0].item(), 1.0, places=5) self.assertTrue((per['plans']['gaps'] > 0.5).all().item()) class FeatureGeometryReuseTest(unittest.TestCase): """#16 第①条：gm_groups 复用调用者已算好的 cum_xf，省一次 FK，但结果必须与原口径逐位一致。""" def test_gm_groups_with_supplied_xf_is_bitwise_identical(self): from ml.common.features import features, GP, GAP_CAP kin = Kin() q = torch.tensor([[-129.9, 46.68, 43.49, -97.46, -85.1, -139.7], [10.0, -20.0, 30.0, -40.0, 50.0, -60.0]]) xf = kin.cum_xf(q) torch.testing.assert_close(kin.gm_groups(q, GP, GAP_CAP, xf=xf), kin.gm_groups(q, GP, GAP_CAP), rtol=0, atol=0) idx = [a * 7 + b for a, b in GP] table = kin.gaps(q, GAP_CAP, xf)['gm'][:, idx] torch.testing.assert_close(kin.gm_groups(q, GP, GAP_CAP), table, rtol=0, atol=0) z = torch.zeros(len(q), 6) f = features(kin, q, z, q[:, :3], torch.zeros(len(q), 9), z[:, None, :].expand(len(q), 3, 6), torch.zeros(len(q)), z, torch.zeros(len(q))) cols = 12 + 3 + 6 + 3 + 6 + 3 + 6 + 3 + 3 + 2 + 18 + 3 + 6 + 1 # 间隙块之前的特征数 = 75 self.assertEqual(cols + 8, 83) torch.testing.assert_close(f[:, cols:cols + 8], table / GAP_CAP, rtol=0, atol=0) class CertBatchEquivalenceTest(unittest.TestCase): """#16 第②条：把 K 条候选的认证并成一次 gaps 调用，选择结果必须与串行循环逐位一致。""" class StubModel: act_scale = 1.0 def __init__(self, act): self.act = act def sample_k(self, feat, K, z, steps=1): B = feat.shape[0] return (self.act.expand(B, K, 16, 6).clone(), torch.zeros(B, device=feat.device), torch.zeros(B, 13, device=feat.device)) def run_both(self, acts, reject_first): class StepKin: def clamp_box(self, q): return q def gaps(self, q, cap): bad = (q[:, 0] > 0.5) if reject_first else torch.zeros(len(q), dtype=torch.bool, device=q.device) return {'gm': torch.where(bad[:, None], -torch.ones(len(q), 49, device=q.device), torch.full((len(q), 49), 1.0, device=q.device))} acts = torch.tensor(acts, dtype=torch.float32) # [K,16,6] 动作（±1 归一化） model = self.StubModel(acts) tasks = [{'q0': [0] * 6, 'T': [10, 0, 0]}] * 2 # B=2：批量索引形状必须在多行下也成立 out = [] for batched in (0, 1): with patch('ml.sim.closed_loop.features', return_value=torch.zeros(len(tasks), 83)), \ patch('ml.sim.closed_loop.goal_err', side_effect=lambda kin, q, T, R: (torch.full((q.shape[0],), 10.0), torch.zeros(q.shape[0]))): _, per = rollout(model, StepKin(), tasks, 'cpu', K=acts.shape[0], safety=True, max_sec=0.04, return_traj=True, use_sub=False, select='first', replan='empty', commit_n=2, safety_partial=1, cert_batch=batched) out.append(per) return out def test_batched_certification_selects_the_same_candidate(self): # 候选 0 第 2 步撞上 q0>0.5 的障碍 ⇒ 只有前缀 1 步；候选 1 全程安全 ⇒ 应被选中 acts = [[[0.2, 0, 0, 0, 0, 0], [0.5, 0, 0, 0, 0, 0]] + [[0, 0, 0, 0, 0, 0]] * 14, [[0.1, 0, 0, 0, 0, 0], [0.1, 0, 0, 0, 0, 0]] + [[0, 0, 0, 0, 0, 0]] * 14] seq, batched = self.run_both(acts, reject_first=True) torch.testing.assert_close(batched['traj'], seq['traj'], rtol=0, atol=0) torch.testing.assert_close(batched['plans']['commit'], seq['plans']['commit'], rtol=0, atol=0) torch.testing.assert_close(batched['plans']['certified'], seq['plans']['certified'], rtol=0, atol=0) self.assertEqual(batched['brakes'].tolist(), seq['brakes'].tolist()) self.assertAlmostEqual(seq['traj'][0, -1, 0].item(), 0.24, places=5) # 候选 1 的两步 0.1×STEP=0.12° def test_batched_certification_keeps_partial_prefix_choice(self): # 所有候选都只走得了第 1 步 ⇒ 两条路径都取第 1 条候选、前缀长度 1 acts = [[[0.2, 0, 0, 0, 0, 0], [0.5, 0, 0, 0, 0, 0]] + [[0, 0, 0, 0, 0, 0]] * 14, [[0.3, 0, 0, 0, 0, 0], [0.6, 0, 0, 0, 0, 0]] + [[0, 0, 0, 0, 0, 0]] * 14] seq, batched = self.run_both(acts, reject_first=True) torch.testing.assert_close(batched['traj'], seq['traj'], rtol=0, atol=0) torch.testing.assert_close(batched['plans']['commit'], seq['plans']['commit'], rtol=0, atol=0) self.assertAlmostEqual(seq['traj'][0, 1, 0].item(), 0.24, places=5) self.assertAlmostEqual(seq['traj'][0, 2, 0].item(), 0.24, places=5) # 第 2 步被拒后停在原地 def test_mismatched_xf_row_count_is_rejected(self): kin = Kin() q1 = torch.zeros(1, 6); q5 = torch.zeros(5, 6) xf5 = kin.cum_xf(q5) for name, fn in (('gaps', lambda: kin.gaps(q1, 40.0, xf5)), ('gm_groups', lambda: kin.gm_groups(q1, [(0, 2)], 40.0, xf=xf5))): with self.subTest(fn=name): with self.assertRaisesRegex(ValueError, '行数'): fn() self.assertEqual(kin.gaps(q5, 40.0, xf5)['gm'].shape, (5, 49)) self.assertEqual(kin.gm_groups(q5, [(0, 2)], 40.0, xf=xf5).shape, (5, 1)) class BackoffTest(unittest.TestCase): """#17：全拒时把首选候选缩步顶上——不许改变「还有安全前缀」的题，但要把站住变成慢爬。""" LIM = 0.5 # 桩里 J1 超过它就是碰撞（一步满速 = VMAX 60°/s × DT 0.02 = 1.2°） class StubModel: act_scale = 1.0 def __init__(self, act): self.act = act def sample_k(self, feat, K, z, steps=1): B = feat.shape[0] return (self.act.expand(B, K, 16, 6).clone(), torch.zeros(B, device=feat.device), torch.zeros(B, 13, device=feat.device)) def run1(self, acts, backoff, cert_batch=1): class StepKin: def clamp_box(self, q): return q def gaps(self, q, cap): bad = q[:, 0] > BackoffTest.LIM return {'gm': torch.where(bad[:, None], -torch.ones(len(q), 49, device=q.device), torch.full((len(q), 49), 1.0, device=q.device))} model = self.StubModel(torch.tensor(acts, dtype=torch.float32)) tasks = [{'q0': [0] * 6, 'T': [10, 0, 0]}] with patch('ml.sim.closed_loop.features', return_value=torch.zeros(1, 83)), \ patch('ml.sim.closed_loop.goal_err', side_effect=lambda kin, q, T, R: (torch.full((q.shape[0],), 10.0), torch.zeros(q.shape[0]))): res, per = rollout(model, StepKin(), tasks, 'cpu', K=len(acts), safety=True, max_sec=0.06, return_traj=True, use_sub=False, select='first', replan='empty', commit_n=2, safety_partial=1, cert_batch=cert_batch, backoff=backoff) return res, per def test_full_rejection_creeps_instead_of_standing_still(self): acts = [[[1.0, 0, 0, 0, 0, 0], [1.0, 0, 0, 0, 0, 0]] + [[0, 0, 0, 0, 0, 0]] * 14] _, p0 = self.run1(acts, 0) self.assertEqual(p0['traj'][0, :, 0].tolist(), [0.0] * 4) # 0.06 s 内规划 2 次，两次都全拒 self.assertEqual(float(p0['brakes'][0]), 2.0) self.assertEqual(float(p0['backoff'][0]), 0.0) _, p1 = self.run1(acts, 1) # 1/2 档 = 0.6° 仍然越界 ⇒ 与不开缩步逐位相同 torch.testing.assert_close(p1['traj'], p0['traj'], rtol=0, atol=0) torch.testing.assert_close(p1['brakes'], p0['brakes'], rtol=0, atol=0) _, p2 = self.run1(acts, 2) # 1/4 档 = 0.3° 安全 ⇒ 先爬一步，再规划时原地缩步也被拒 self.assertEqual([round(v, 5) for v in p2['traj'][0, :, 0].tolist()], [0.0, 0.3, 0.3, 0.3]) self.assertEqual(float(p2['backoff'][0]), 1.0) self.assertEqual(float(p2['brakes'][0]), 1.0) def test_backoff_never_overrides_a_candidate_with_a_safe_prefix(self): # 第 1 步安全（0.24°）、第 2 步越界（0.84°）⇒ 前缀 1 步就够，缩步档不该插手 acts = [[[0.2, 0, 0, 0, 0, 0], [0.5, 0, 0, 0, 0, 0]] + [[0, 0, 0, 0, 0, 0]] * 14] _, p0 = self.run1(acts, 0) _, p2 = self.run1(acts, 2) for key in ('traj', 'brakes', 'backoff'): with self.subTest(key=key): torch.testing.assert_close(p2[key], p0[key], rtol=0, atol=0) for key in ('commit', 'certified', 'points', 'gaps'): with self.subTest(key=key): torch.testing.assert_close(p2['plans'][key], p0['plans'][key], rtol=0, atol=0) self.assertGreater(float(p2['traj'][0, 1, 0]), 0.0) def test_backoff_requires_batched_certification(self): acts = [[[1.0, 0, 0, 0, 0, 0], [1.0, 0, 0, 0, 0, 0]] + [[0, 0, 0, 0, 0, 0]] * 14] with self.assertRaisesRegex(ValueError, 'cert_batch'): self.run1(acts, 1, cert_batch=0) class TravelPenaltySelectTest(unittest.TestCase): """#17b：候选打分加"归一化行程"惩罚（§9.2 要行程≤规划器×1.10，现在 2.4 倍没人管）。 两条候选末态相同（goal_err 打成常数）、累计位移相同，但一条来回抖、一条匀速 ⇒ 只可能靠行程分开。""" class StubModel: act_scale = 1.0 def __init__(self, act): self.act = act def sample_k(self, feat, K, z, steps=1): B = feat.shape[0] return (self.act.expand(B, K, 16, 6).clone(), torch.zeros(B, device=feat.device), torch.zeros(B, 13, device=feat.device)) def run1(self, len_w, select='ep_len'): class SafeKin: def clamp_box(self, q): return q def gaps(self, q, cap): return {'gm': torch.full((len(q), 49), 1.0, device=q.device)} zig = ([[0.9, 0, 0, 0, 0, 0], [-0.45, 0, 0, 0, 0, 0]] * 8) # 16 步，合计 3.6、行程 10.8 smooth = ([[0.225, 0, 0, 0, 0, 0]] * 16) # 合计 3.6、行程 3.6 acts = torch.tensor([zig, smooth], dtype=torch.float32) # [K=2,16,6] tasks = [{'q0': [0] * 6, 'T': [10, 0, 0]}] with patch('ml.sim.closed_loop.features', return_value=torch.zeros(1, 83)), \ patch('ml.sim.closed_loop.goal_err', side_effect=lambda kin, q, T, R: (torch.full((q.shape[0],), 10.0), torch.zeros(q.shape[0]))): _, per = rollout(self.StubModel(acts), SafeKin(), tasks, 'cpu', K=2, safety=True, max_sec=0.06, return_traj=True, use_sub=False, select=select, replan='empty', commit_n=2, safety_partial=1, len_w=len_w) return per def test_penalty_prefers_the_shorter_path(self): p0 = self.run1(0.0) # 不加惩罚：同分 ⇒ 稳定排序取第 0 条（来回抖那条） self.assertAlmostEqual(p0['traj'][0, 1, 0].item(), 0.9 * 1.2, places=5) p1 = self.run1(1.0) # 加惩罚：行程 10.8 vs 3.6 ⇒ 选匀速那条 self.assertAlmostEqual(p1['traj'][0, 1, 0].item(), 0.225 * 1.2, places=5) # 换向率指标要有灵敏度，而且要能算出来：这里 3 个活动步 × 6 轴为分母，抖动条在第 0 轴翻了 2 次 ⇒ 2/18。 # 口径 = 换向的(步,轴)对 / 活动的(步,轴)对；匀速条一条都不翻。 self.assertAlmostEqual(float(p0['reverse_rate'][0]), 2.0 / 18.0, places=5) self.assertLess(float(p1['reverse_rate'][0]), 1e-6) def test_waste_penalty_also_separates_them(self): """换成"白费比例"（1 − 同轴净位移/累计行程）也必须能分开，且罚度按当前误差缩放： 这里 goal_err 恒为 10 ⇒ 抖动条 waste≈0.78、匀速条 0 ⇒ 差 7.8×len_w 的当量。""" p0 = self.run1(0.0, select='ep_waste') self.assertAlmostEqual(p0['traj'][0, 1, 0].item(), 0.9 * 1.2, places=5) p1 = self.run1(1.0, select='ep_waste') self.assertAlmostEqual(p1['traj'][0, 1, 0].item(), 0.225 * 1.2, places=5) def test_zero_weight_ep_waste_is_the_old_endpoint_path(self): p0 = self.run1(0.0, select='ep_waste') p1 = self.run1(0.0, select='endpoint') torch.testing.assert_close(p0['traj'], p1['traj'], rtol=0, atol=0) def test_zero_weight_is_the_old_endpoint_path(self): """len_w=0 且 select='ep_len' 必须与 select='endpoint' 逐位相同（不许悄悄改默认行为）""" class SafeKin: def clamp_box(self, q): return q def gaps(self, q, cap): return {'gm': torch.full((len(q), 49), 1.0, device=q.device)} zig = ([[0.9, 0, 0, 0, 0, 0], [-0.45, 0, 0, 0, 0, 0]] * 8) acts = torch.tensor([zig, zig], dtype=torch.float32) tasks = [{'q0': [0] * 6, 'T': [10, 0, 0]}] out = [] for sel in ('endpoint', 'ep_len'): with patch('ml.sim.closed_loop.features', return_value=torch.zeros(1, 83)), \ patch('ml.sim.closed_loop.goal_err', side_effect=lambda kin, q, T, R: (torch.full((q.shape[0],), 10.0), torch.zeros(q.shape[0]))): _, per = rollout(self.StubModel(acts), SafeKin(), tasks, 'cpu', K=2, safety=True, max_sec=0.06, return_traj=True, use_sub=False, select=sel, replan='empty', commit_n=2, safety_partial=1, len_w=0.0) out.append(per) torch.testing.assert_close(out[0]['traj'], out[1]['traj'], rtol=0, atol=0) if __name__ == '__main__': unittest.main()