199 lines
9.9 KiB
Python
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)
|