"""No-Kit geometry and read-only applied-force visualization regressions.""" import sys from pathlib import Path from types import SimpleNamespace import pytest import torch sys.path.insert(0, str(Path(__file__).resolve().parents[1] / "scripts")) import standing_force_visualizer as viz def rotated_z(q): z = torch.zeros_like(q[:, :3]) z[:, 2] = 1 t = 2 * torch.cross(q[:, :3], z, dim=-1) return z + q[:, 3:] * t + torch.cross(q[:, :3], t, dim=-1) @pytest.mark.parametrize("dtype", [torch.float32, torch.float64]) def test_arrow_world_tail_tip_and_xyzw_all_directions(dtype): force = torch.tensor( [ [5, 0, 0], [-5, 0, 0], [0, 5, 0], [0, -5, 0], [0, 0, 10], [0, 0, -10], [2, -3, 4], [0.001, -0.002, -10], [1e-5, 0, 0], ], dtype=dtype, ) origin = torch.arange(len(force) * 3, dtype=dtype).reshape(-1, 3) / 10 before = origin.clone(), force.clone() g = viz.arrow_geometry(origin, force, 0.03) n = len(force) direction = force / force.norm(dim=-1, keepdim=True) assert g["translations"].shape == (2 * n, 3) assert torch.allclose(rotated_z(g["orientations"][:n]), direction, atol=2e-7) assert torch.allclose(g["orientations"].norm(dim=-1), torch.ones(2 * n, dtype=dtype)) assert torch.allclose(g["translations"][:n] - direction * g["scales"][:n, 2:] / 2, origin, atol=1e-6) assert torch.allclose(g["translations"][n:] + direction * g["scales"][n:, 2:] / 2, origin + 0.03 * force, atol=1e-6) assert g["marker_indices"].tolist() == [0] * n + [1] * n assert torch.equal(origin, before[0]) and torch.equal(force, before[1]) def test_zero_force_and_zero_batch_have_no_visible_geometry(): for count in (0, 5): g = viz.arrow_geometry(torch.zeros(count, 3), torch.zeros(count, 3)) assert g["translations"].shape == (0, 3) assert g["orientations"].shape == (0, 4) assert all(torch.isfinite(v).all() for v in g.values()) @pytest.mark.parametrize("scale", [0, -0.01, float("nan"), float("inf")]) def test_bad_scale_rejected_before_kit_import(scale): with pytest.raises(ValueError, match="scale"): viz.DisturbanceForceArrows(scale) with pytest.raises(ValueError, match="scale"): viz.arrow_geometry(torch.zeros(1, 3), torch.ones(1, 3), scale) @pytest.mark.parametrize( "force", [torch.ones(1, 6), torch.ones(2, 3), torch.full((1, 3), float("nan")), torch.ones(1, 3).long()] ) def test_bad_force_rejected(force): with pytest.raises(ValueError): viz.arrow_geometry(torch.zeros(1, 3), force) def test_applied_disturbance_only_and_visibility_lifecycle(monkeypatch): class Markers: def __init__(self): self.visible = True self.frames = [] def set_visibility(self, value): self.visible = value def visualize(self, **kwargs): self.frames.append(kwargs) markers = Markers() monkeypatch.setattr(viz, "_make_markers", lambda: markers) arrows = viz.DisturbanceForceArrows() assert not markers.visible position = torch.tensor([[12.0, -3.0, 0.4], [-8.0, 4.0, 0.5]]) # First environment is pure torque: must not be interpreted as a force. wrench = torch.tensor([[0.0, 0.0, 0.0, 1.0, 2.0, 3.0], [0.0, 0.0, -10.0, 0.0, 0.0, 0.0]]) state = SimpleNamespace( root=lambda: (position,), last_disturbance=wrench, last_guide=torch.ones(2, 6) * 500, mount_wrench=lambda: (_ for _ in ()).throw(AssertionError("Not sensor data")), ) before = position.clone(), wrench.clone(), state.last_guide.clone(), torch.get_rng_state().clone() arrows.update(state) assert markers.visible and len(markers.frames) == 1 g = markers.frames[0] assert len(g["translations"]) == 2 # one shaft + one head, not two arrows assert torch.allclose( g["translations"][1] + rotated_z(g["orientations"])[1] * g["scales"][1, 2] / 2, position[1] + torch.tensor([0.0, 0.0, -0.3]), ) assert all(torch.equal(a, b) for a, b in zip(before, (position, wrench, state.last_guide, torch.get_rng_state()))) wrench.zero_() arrows.update(state) assert not markers.visible and len(markers.frames) == 1 # no stale arrow wrench[0, 0] = 5 arrows.update(state) assert markers.visible and len(markers.frames) == 2 arrows.close() assert not markers.visible