"""ROS-message/bridge regression tests; fake node means no DDS or hardware IO.""" import json from pathlib import Path import sys import tempfile from types import SimpleNamespace import unittest import xml.etree.ElementTree as ET ROOT = Path(__file__).resolve().parents[2] sys.path.insert(0, str(ROOT/'src/linkerhand-sim/linker_hand_mujoco_ros2')) import mujoco import numpy as np from sensor_msgs.msg import JointState from builtin_interfaces.msg import Time import linker_hand_mujoco_ros2.o30_right_plan_bridge as right from linker_hand_mujoco_ros2.utils import o30_right_joints as mapping MODEL_DIR = right.DEFAULT_XML.parent V5_XML = MODEL_DIR/'linker_hand_o30_right_endpoint_v5.xml' class FakeNode: def __init__(self): self.messages = [] self.subscriptions = [] def get_logger(self): return SimpleNamespace(info=lambda *_: None, warn=lambda *_: None) def get_clock(self): return SimpleNamespace(now=lambda: SimpleNamespace(to_msg=lambda: Time())) def create_subscription(self, msg_type, topic, callback, queue): self.subscriptions.append((msg_type, topic, callback, queue)) return callback def create_publisher(self, msg_type, topic, queue): self.state_topic = topic return SimpleNamespace(publish=self.messages.append) def create_timer(self, *args): return None class CalibrationTest(unittest.TestCase): def setUp(self): mapping.configure_o30_right_limits(right.DEFAULT_XML) def tearDown(self): mapping.configure_o30_right_limits(right.DEFAULT_XML) def test_real_jointstate_callback_reaches_reference_poses(self): model = mujoco.MjModel.from_xml_path(str(right.DEFAULT_XML)) data = mujoco.MjData(model) node = FakeNode() bridge = right.g20b.G20PlanBridge( node, model, data, legacy_sdk=True, nominal_topic='/unused', cmd_topic='/cb_right_hand_control_cmd', sim_state_topic='/sim/mujoco/o30/right/joint_state', collision_project=False, mapping='g20_urdf', ) self.assertEqual(node.subscriptions[0][:2], (JointState, '/cb_right_hand_control_cmd')) measured_zero = [0, 0, 255, 197, 110, 28, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] expected_zero = mapping.sdk_range_to_full_o30_right_urdf_positions(measured_zero) self.assertEqual(bridge.goal, expected_zero) for name, expected in expected_zero.items(): self.assertAlmostEqual(data.qpos[bridge.joint_qpos[name]], expected, places=10) samples = json.loads(Path(__file__).with_name('samples_v9.json').read_text())['samples'] for sample in samples: with self.subTest(finger=sample['finger']): message = JointState(position=[float(v) for v in sample['command']]) node.subscriptions[0][2](message) for _ in range(400): with bridge.lock: bridge.step_simulation_unlocked() self.assertLess(bridge.tracking_error()[0], 1e-10) def geom(finger): return next(g for g in range(model.ngeom) if model.geom_group[g] == 3 and model.geom_bodyid[g] == model.body(finger+'_distal').id) gap = mujoco.mj_geomDistance(model, data, geom('thumb'), geom(sample['finger']), .2, np.zeros(6)) self.assertLess(abs(gap), .0002) bridge._publish_state() state = node.messages[-1] self.assertEqual(len(state.name), 20) self.assertEqual(len(state.position), 20) for name, value in zip(state.name, state.position): self.assertAlmostEqual(value, bridge.goal[name], places=10) def test_changing_xml_updates_travel_and_restores_stock_mapping(self): calibrated = mapping.sdk_range_to_full_o30_right_urdf_positions([255.]*20) mapping.configure_o30_right_limits(MODEL_DIR/'linker_hand_o30_right_stock.xml') stock = mapping.sdk_range_to_full_o30_right_urdf_positions([255.]*20) self.assertGreater(abs(calibrated['thumb_cmc_roll']-stock['thumb_cmc_roll']), .1) self.assertAlmostEqual(stock['thumb_cmc_roll'], .6311) mapping.configure_o30_right_limits(right.DEFAULT_XML) self.assertEqual(calibrated, mapping.sdk_range_to_full_o30_right_urdf_positions([255.]*20)) for value, endpoint in [(-10., 0.), (300., 255.)]: self.assertEqual(mapping.sdk_range_to_full_o30_right_urdf_positions([value]*20), mapping.sdk_range_to_full_o30_right_urdf_positions([endpoint]*20)) def test_invalid_model_fails_without_partially_changing_mapping(self): old = dict(mapping.O30_RIGHT_URDF_LIMITS) tree = ET.parse(right.DEFAULT_XML) joints = tree.getroot().findall('.//worldbody//joint') joints[0].set('range', '0 99') joints[-1].set('range', 'nan 1') with tempfile.TemporaryDirectory() as folder: path = Path(folder)/'invalid.xml' tree.write(path) with self.assertRaises(ValueError): mapping.configure_o30_right_limits(path) self.assertEqual(old, mapping.O30_RIGHT_URDF_LIMITS) def test_historical_active_knots_are_ignored_under_linear_policy(self): mapping.configure_o30_right_limits(V5_XML) self.assertEqual(mapping.O30_RIGHT_SDK_KNOTS, {}) for value in (0., 61., 128.5, 166., 255.): pose = mapping.sdk_range_to_full_o30_right_urdf_positions([value]*20) for name, (lo, hi) in mapping.O30_RIGHT_URDF_LIMITS.items(): self.assertAlmostEqual(pose[name], lo+(hi-lo)*value/255, places=11) def test_endpoint_messages_reach_provisional_right_angles(self): mapping.configure_o30_right_limits(V5_XML) parameters = json.loads((MODEL_DIR/'o30_right_pinky_calibration.json').read_text())['parameter_vector'] config = json.loads(Path(__file__).with_name('endpoint_constraints.json').read_text()) model = mujoco.MjModel.from_xml_path(str(V5_XML)) data = mujoco.MjData(model) node = FakeNode() bridge = right.g20b.G20PlanBridge(node, model, data, legacy_sdk=True, nominal_topic='/unused', cmd_topic='/cb_right_hand_control_cmd', sim_state_topic='/sim/mujoco/o30/right/joint_state', collision_project=False, mapping='g20_urdf') for row in config['constraints']: with self.subTest(joint=row['joint']): cmd = list(mapping.O30_RIGHT_OPEN_CMD) for joint in row['active']: cmd[mapping.O30_RIGHT_SDK_TO_URDF.index(joint)] = 255 node.subscriptions[0][2](JointState(position=[float(v) for v in cmd])) for _ in range(500): with bridge.lock: bridge.step_simulation_unlocked() i = mapping.O30_RIGHT_SDK_TO_URDF.index(row['joint']) physical = data.qpos[bridge.joint_qpos[row['joint']]]+parameters[i] self.assertAlmostEqual(float(np.rad2deg(physical)), 90., places=7) def test_obsolete_knot_metadata_cannot_affect_linear_mapping(self): mapping.configure_o30_right_limits(V5_XML) old_limits = dict(mapping.O30_RIGHT_URDF_LIMITS) old_knots = dict(mapping.O30_RIGHT_SDK_KNOTS) cases = ['0 0 166 1.3 165 1.6', '0 0 166 1.3 255 0.2', '0 0 166 nan 255 1.6', '0 0 255 999', '0 0 255'] with tempfile.TemporaryDirectory() as folder: path = Path(folder)/'invalid.xml' for value in cases: with self.subTest(data=value): tree = ET.parse(V5_XML) tree.getroot().find('./custom/numeric').set('data', value) tree.write(path) mapping.configure_o30_right_limits(path) self.assertEqual(old_limits, mapping.O30_RIGHT_URDF_LIMITS) self.assertEqual(old_knots, mapping.O30_RIGHT_SDK_KNOTS) def test_linear_model_clears_piecewise_mapping_and_uses_only_urdf_limits(self): mapping.configure_o30_right_limits(V5_XML) self.assertEqual(mapping.O30_RIGHT_SDK_KNOTS, {}) mapping.configure_o30_right_limits(right.DEFAULT_XML) self.assertEqual(mapping.O30_RIGHT_SDK_KNOTS, {}) root = ET.parse(MODEL_DIR/'linkerhand_o30_right_zero_v10_calibrated.urdf').getroot() self.assertIsNone(root.find('o30_sdk_mapping')) for sdk in [-1.,0.,61.,79.,107.,128.5,140.,150.,166.,192.,255.,256.]: values = mapping.sdk_range_to_full_o30_right_urdf_positions([sdk]*20) for name in mapping.O30_RIGHT_SDK_TO_URDF: limit = root.find(f"joint[@name='{name}']/limit") low,high = float(limit.get('lower')),float(limit.get('upper')) expected = low+(high-low)*min(max(sdk,0),255)/255 self.assertAlmostEqual(values[name], expected, places=10) def test_two_user_messages_reach_new_endpoints(self): commands = [ [172,0,169,169,162,170,0,0,255,0,0,0,0,0,0,0,0,0,0,0], [172,0,169,169,162,170,0,0,0,0,0,0,0,0,0,255,0,0,0,0], ] parameters = json.loads((MODEL_DIR/'o30_right_two_joint_calibration.json').read_text())['parameter_vector'] model = mujoco.MjModel.from_xml_path(str(right.DEFAULT_XML)) data = mujoco.MjData(model); node = FakeNode() bridge = right.g20b.G20PlanBridge(node, model, data, legacy_sdk=True, nominal_topic='/unused', cmd_topic='/cb_right_hand_control_cmd', sim_state_topic='/sim/mujoco/o30/right/joint_state', collision_project=False, mapping='g20_urdf') for command,index in zip(commands,[8,15]): node.subscriptions[0][2](JointState(position=[float(x) for x in command])) for _ in range(500): with bridge.lock: bridge.step_simulation_unlocked() name = mapping.O30_RIGHT_SDK_TO_URDF[index] angle = data.qpos[bridge.joint_qpos[name]]+parameters[index] self.assertAlmostEqual(float(np.rad2deg(angle)),90.,places=7) self.assertLess(bridge.tracking_error()[0],1e-10) def test_v9_changes_only_middle_pip_upper(self): before = ET.parse(MODEL_DIR/'linkerhand_o30_right_pinky_root_calibrated.urdf').getroot() after = ET.parse(MODEL_DIR/'linkerhand_o30_right_middle_calibrated.urdf').getroot() path = "joint[@name='middle_pip']/limit" self.assertGreater(float(after.find(path).get('upper')), float(before.find(path).get('upper'))) after.find(path).set('upper', before.find(path).get('upper')) self.assertEqual(ET.canonicalize(ET.tostring(before, encoding='unicode'), strip_text=True), ET.canonicalize(ET.tostring(after, encoding='unicode'), strip_text=True)) def test_v8_changes_only_pinky_root_upper(self): before = ET.parse(MODEL_DIR/'linkerhand_o30_right_two_joint_calibrated.urdf').getroot() after = ET.parse(MODEL_DIR/'linkerhand_o30_right_pinky_root_calibrated.urdf').getroot() path = "joint[@name='pinky_mcp_pitch']/limit" self.assertGreater(float(after.find(path).get('upper')), float(before.find(path).get('upper'))) after.find(path).set('upper', before.find(path).get('upper')) self.assertEqual(ET.canonicalize(ET.tostring(before, encoding='unicode'), strip_text=True), ET.canonicalize(ET.tostring(after, encoding='unicode'), strip_text=True)) def test_other_eighteen_joint_definitions_are_identical_to_v6(self): before = ET.parse(MODEL_DIR/'linkerhand_o30_right_linear_calibrated.urdf').getroot() after = ET.parse(MODEL_DIR/'linkerhand_o30_right_two_joint_calibrated.urdf').getroot() for name in mapping.O30_RIGHT_SDK_TO_URDF: if name in ['middle_mcp_pitch','thumb_ip']: continue a = before.find(f"joint[@name='{name}']"); b = after.find(f"joint[@name='{name}']") self.assertEqual(a.attrib,b.attrib) self.assertEqual([(e.tag,e.attrib) for e in a],[(e.tag,e.attrib) for e in b]) if __name__ == '__main__': unittest.main(verbosity=2)