163 lines
7.3 KiB
Python
163 lines
7.3 KiB
Python
|
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_first_hits
|
||
|
|
from ml.sim.closed_loop import TeacherOracle, rollout
|
||
|
|
|
||
|
|
|
||
|
|
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],
|
||
|
|
}.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)
|
||
|
|
|
||
|
|
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)
|
||
|
|
|
||
|
|
|
||
|
|
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)
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
unittest.main()
|