9fe9d27865
Consolidate next-stage docs, archive superseded narratives, and preserve frozen document inputs. Retain invalid P2 pairs, partial RootXY training, failed attempts and nonselectable checkpoints as explicit limitations. Include supervised evaluation/training, desktop GPU admission, bounded evidence and atomic-publication monitor repairs with CPU regressions. Ignore local dotenv configuration. Validation: 1403 passed, 6 skipped for contact-cycle and related contracts/CLI tests. Full-suite legacy ABI and registration tests require Python 3.12/Torch 2.10/pxr; no runtime checks relaxed. No new physical run or accepted skill.
118 lines
4.4 KiB
Python
118 lines
4.4 KiB
Python
"""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
|