Files
a7l20-hug-sim/scripts/test_real_assembly.py

199 lines
9.9 KiB
Python

"""Offline regressions for real-CAD assembly; no network, CAN, or motion APIs.
Run: .venv/bin/python scripts/test_real_assembly.py
The built scene must already exist. Only temporary test fixtures are written.
"""
from __future__ import annotations
import ast
import copy
import hashlib
import json
from pathlib import Path
import sys
import tempfile
import unittest
import mujoco
import numpy as np
from scipy.spatial.transform import Rotation
ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(ROOT / "scene"))
import build_real_assembly as assembly
import view_real_assembly as viewer
class RealAssemblyTests(unittest.TestCase):
@classmethod
def setUpClass(cls):
cls.info = json.loads((ROOT / "scene/real_assembly.json").read_text())
cls.model = mujoco.MjModel.from_xml_path(str(ROOT / "scene/real_assembly.xml"))
cls.snapshot = json.loads((ROOT / cls.info["snapshot_file"]).read_text())
cls.mount = json.loads(assembly.MOUNT_FILE.read_text())
def pose(self, name):
data = mujoco.MjData(self.model)
viewer.select_pose(mujoco, self.model, data, name)
return data
def test_snapshot_name_mapping_survives_reversed_order(self):
reversed_snapshot = copy.deepcopy(self.snapshot)
for side in ("left", "right"):
state = reversed_snapshot["states"][side]["joint_state"]
state["name"] = state["name"][::-1]
state["position"] = state["position"][::-1]
names, expected = assembly.snapshot_angles(self.snapshot, side)
actual_names, actual = assembly.snapshot_angles(reversed_snapshot, side)
self.assertEqual(names, actual_names)
np.testing.assert_array_equal(actual, expected)
data = self.pose("snapshot")
for name, angle in zip(names, expected):
self.assertAlmostEqual(float(data.qpos[self.model.joint(name).qposadr[0]]), angle, delta=1e-5)
def test_snapshot_fk_position_and_orientation_both_arms(self):
data = self.pose("snapshot")
for side in ("left", "right"):
with self.subTest(side=side):
pose = self.snapshot["states"][side]["pose"]
expected = [pose["position"][key] for key in "xyz"]
site = data.site(f"{side}_Arm_Tip")
np.testing.assert_allclose(site.xpos, expected, atol=1e-6, rtol=0)
orientation = [pose["orientation"][key] for key in "xyzw"]
relative = Rotation.from_matrix(site.xmat.reshape(3, 3)).inv() * Rotation.from_quat(orientation)
self.assertLess(relative.magnitude(), 1e-5)
def test_only_left_j4_moves_in_elbow90_pose(self):
data = self.pose("elbow90")
expected = np.zeros(self.model.nq)
expected[self.model.joint("L4_JOINT").qposadr[0]] = np.pi / 2
# MjSpec XML export rounds numeric values to six significant digits.
np.testing.assert_allclose(data.qpos, expected, atol=1e-5, rtol=0)
self.assertEqual(np.count_nonzero(data.qpos), 1)
np.testing.assert_array_equal(self.pose("zero").qpos, np.zeros(self.model.nq))
def test_left_pinky_low_45_degree_reference(self):
data = self.pose("elbow90")
rotation = data.body("left_hand_hand_base_link").xmat.reshape(3, 3)
self.assertLess(rotation[2, 0], -.7)
self.assertLess(rotation[2, 1], -.7)
self.assertAlmostEqual(np.degrees(np.arccos(abs(rotation[2, 0]))), 45, places=3)
pinky = data.body("left_hand_pinky_metacarpals").xpos
index = data.body("left_hand_index_metacarpals").xpos
self.assertAlmostEqual(float((pinky - index)[2]), -.04985, places=5)
self.assertEqual(self.mount["hand_local_z_twist_from_original_sim_deg"], 45)
self.assertEqual(self.mount["pinky_side_local"], [0, 1, 0])
def test_mating_faces_touch_in_every_pose(self):
for name in ("snapshot", "zero", "elbow90"):
with self.subTest(pose=name):
data = self.pose(name)
hand = data.body("left_hand_hand_base_link")
adapter = data.body("left_adapter")
l7 = data.body("L7_Link")
l7_rotation = l7.xmat.reshape(3, 3)
np.testing.assert_allclose(data.site("adapter_hand_face").xpos, hand.xpos, atol=1e-12)
np.testing.assert_allclose(l7_rotation.T @ (adapter.xpos - l7.xpos), [-.028, 0, -.0525], atol=1e-12)
np.testing.assert_allclose(l7_rotation.T @ (hand.xpos - l7.xpos), [-.028, 0, -.0885], atol=1e-12)
np.testing.assert_allclose(hand.xmat, adapter.xmat, atol=1e-12)
self.assertAlmostEqual(float(np.linalg.norm(hand.xpos - adapter.xpos)), .036, places=12)
def test_snapshot_missing_duplicate_mismatched_nonfinite_joints_rejected(self):
for mode in ("missing", "duplicate", "length", "nan", "inf"):
with self.subTest(mode=mode):
invalid = copy.deepcopy(self.snapshot)
state = invalid["states"]["left"]["joint_state"]
if mode == "missing":
state["name"].pop()
state["position"].pop()
elif mode == "duplicate":
state["name"][1] = state["name"][0]
elif mode == "length":
state["position"].pop()
else:
state["position"][0] = float(mode)
with self.assertRaises((ValueError, KeyError)):
assembly.snapshot_angles(invalid, "left")
def test_mount_missing_or_nonfinite_inputs_rejected(self):
for field in ("rotation_Arm_Tip_to_hand", "translation_L7_to_hand_m", "translation_Arm_Tip_to_hand_m"):
with self.subTest(field=field):
invalid = copy.deepcopy(self.mount)
del invalid[field]
with self.assertRaises((ValueError, KeyError)):
assembly.validate_mount(invalid)
invalid = copy.deepcopy(self.mount)
value = np.array(invalid[field], dtype=float)
value.flat[0] = np.nan
invalid[field] = value.tolist()
with self.assertRaises(ValueError):
assembly.validate_mount(invalid)
def test_nonfinite_expected_controller_pose_rejected(self):
for mode in ("nan_position", "nan_orientation", "zero_quaternion", "wrong_orientation", "nonunit_quaternion"):
with self.subTest(mode=mode):
invalid = copy.deepcopy(self.snapshot)
pose = invalid["states"]["left"]["pose"]
if mode == "nan_position":
pose["position"]["x"] = float("nan")
elif mode == "nan_orientation":
pose["orientation"]["w"] = float("nan")
elif mode == "zero_quaternion":
pose["orientation"] = dict.fromkeys("xyzw", 0.0)
elif mode == "wrong_orientation":
pose["orientation"] = dict(zip("xyzw", [0., 0., 0., 1.]))
else:
pose["orientation"] = {key: 2 * value for key, value in pose["orientation"].items()}
with tempfile.TemporaryDirectory(prefix="real-assembly-test-") as directory:
path = Path(directory) / "snapshot.json"
path.write_text(json.dumps(invalid))
info = copy.deepcopy(self.info)
info["snapshot_file"] = str(path)
with self.assertRaises(ValueError):
assembly.verify_model(self.model, info)
def test_original_urdf_preserved_by_import(self):
before = assembly.ARM_URDF.read_bytes()
self.assertEqual(hashlib.sha256(before).hexdigest(), "136c6c0781ade1baca92aeeb13136c732008ff1177a1f9f47382688b4c12329d")
assembly.load_arm_spec()
self.assertEqual(assembly.ARM_URDF.read_bytes(), before)
def test_limit_discrepancy_retained_not_clamped(self):
data = self.pose("snapshot")
joint = self.model.joint("R5_JOINT")
self.assertGreater(data.qpos[joint.qposadr[0]], joint.range[1])
self.assertTrue(any(w["joint"] == "R5_JOINT" for w in self.info["snapshot_limit_warnings"]))
def test_viewer_and_builder_no_hardware_imports_or_step_calls(self):
prohibited_modules = {"real_arm", "roslibpy", "can", "socket", "requests", "pyrealsense2", "linkerhand", "g20"}
for path in (ROOT / "scripts/view_real_assembly.py", ROOT / "scene/build_real_assembly.py", ROOT / "scene/build_scene.py"):
with self.subTest(path=str(path)):
tree = ast.parse(path.read_text())
imported = []
called = []
for node in ast.walk(tree):
if isinstance(node, ast.Import):
imported.extend(alias.name.split(".")[0] for alias in node.names)
elif isinstance(node, ast.ImportFrom):
imported.append((node.module or "").split(".")[0])
elif isinstance(node, ast.Call):
if isinstance(node.func, ast.Attribute):
called.append(node.func.attr)
elif isinstance(node.func, ast.Name):
called.append(node.func.id)
self.assertFalse(set(imported) & prohibited_modules)
if path.name != "build_scene.py":
self.assertFalse(set(called) & {"mj_step", "mj_step1", "mj_step2"})
def test_table_reference_is_not_collision_certification(self):
geom = self.model.geom("measured_table_reference_NOT_collision")
self.assertEqual(int(geom.contype[0]), 0)
self.assertEqual(int(geom.conaffinity[0]), 0)
self.assertFalse(self.info["hardware_connected"])
self.assertFalse(self.info["execution_authorized"])
self.assertFalse(self.info["snapshot_is_live"])
if __name__ == "__main__":
unittest.main(verbosity=2)