cbb/Robot_Arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix+exp: 轮次30——统一认证与执行限位，到达57.4%不变；加密回放仍检出段内碰撞

Browse Source
This commit is contained in:
cbb
2026-10-20 17:26:20 +08:00
parent 9eaff7341f
commit 95da49193f
12 changed files with 2516 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tests/test_eval_cl.py
+216 -1
View File
@@ -9,7 +9,7 @@ from unittest.mock import patch
import torch
from ml.eval.eval_cl import json_finite, run
from ml.eval.diag_collision import replay_first_hits
from ml.eval.diag_collision import replay_dense, replay_first_hits
from ml.sim.closed_loop import TeacherOracle, rollout
@@ -91,6 +91,39 @@ class EvalMetricsTest(unittest.TestCase):
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)
class DlsSafetyTest(unittest.TestCase):
def test_rejected_refinement_retains_safe_candidate(self):
@@ -158,5 +191,187 @@ class DlsSafetyTest(unittest.TestCase):
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())
if __name__ == '__main__':
unittest.main()