229 lines
12 KiB
Python
229 lines
12 KiB
Python
"""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)
|