106 lines
6.0 KiB
Python
106 lines
6.0 KiB
Python
"""Regression: linear active mapping, optional passive-only JSON and no import IO."""
|
|
import importlib
|
|
import json
|
|
import os
|
|
from pathlib import Path
|
|
import subprocess
|
|
import sys
|
|
import tempfile
|
|
import unittest
|
|
import numpy as np
|
|
import mujoco
|
|
ROOT=Path(__file__).resolve().parents[1]
|
|
sys.path.insert(0,str(ROOT/'src/linkerhand-sim/linker_hand_mujoco_ros2'))
|
|
from linker_hand_mujoco_ros2.utils.runtime_mapping import configure
|
|
ASSETS=ROOT/'src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf'
|
|
CASES=[
|
|
('g20_joints','get_mapping_fns','g20_calibrated','G20/linker_hand_g20_left/linker_hand_g20_left.xml','L20_SDK_TO_URDF','G20_URDF_L_DIR'),
|
|
('g20_joints','get_mapping_fns','g20_urdf','G20/linker_hand_g20_right/linker_hand_g20_right.xml','L20_SDK_TO_URDF','G20_URDF_L_DIR'),
|
|
('o6_joints','get_o6_mapping_fns','g20_urdf','O6/linker_hand_o6_right/linker_hand_o6_right.xml','O6_SDK_TO_URDF','O6_URDF_L_DIR'),
|
|
('o6_left_joints','get_o6_left_mapping_fns','g20_urdf','O6/linker_hand_o6_left/linker_hand_o6_left.xml','O6_LEFT_SDK_TO_URDF','O6_LEFT_URDF_L_DIR'),
|
|
('l6_joints','get_l6_mapping_fns','g20_urdf','L6/linker_hand_l6v31_right/linker_hand_l6v31_right.xml','L6_SDK_TO_URDF','L6_URDF_L_DIR'),
|
|
('l6_left_joints','get_l6_left_mapping_fns','g20_urdf','L6/linker_hand_l6v31_left/linker_hand_l6v31_left.xml','L6_LEFT_SDK_TO_URDF','L6_LEFT_URDF_L_DIR'),
|
|
('o30_right_joints','get_o30_right_mapping_fns','g20_urdf','O30/linker_hand_o30_right/linker_hand_o30_right.xml','O30_RIGHT_SDK_TO_URDF',None),
|
|
('o30_left_joints','get_o30_left_mapping_fns','g20_urdf','O30/linker_hand_o30_left/linker_hand_o30_left.xml','O30_LEFT_SDK_JOINT_ORDER',None),
|
|
]
|
|
|
|
def setup_case(case):
|
|
mod,fn,profile,relative,channels,direction=case
|
|
module=importlib.import_module('linker_hand_mujoco_ros2.utils.'+mod)
|
|
xml=ASSETS/relative;model=mujoco.MjModel.from_xml_path(str(xml));factory=getattr(module,fn)
|
|
configure(model,xml,factory,profile)
|
|
return module,model,xml,factory
|
|
|
|
class Policy(unittest.TestCase):
|
|
def test_active_endpoints_and_intermediate_values_all_sdk_hands(self):
|
|
for case in CASES:
|
|
with self.subTest(case=case[0:3]):
|
|
module,model,xml,factory=setup_case(case)
|
|
channels=getattr(module,case[4]);channels=channels if isinstance(channels,dict) else dict(enumerate(channels))
|
|
direction=getattr(module,case[5]) if case[5] else [1]*20
|
|
convert=module.sdk_to_full_positions if case[0]=='o30_left_joints' else factory(case[2])[0]
|
|
for u in [0,17,64,127.5,128,201,255]:
|
|
pose=convert([u]*(max(channels)+1))
|
|
for i,name in channels.items():
|
|
lo,hi=model.jnt_range[model.joint(name).id];fraction=u/255 if direction[i]>0 else 1-u/255
|
|
self.assertAlmostEqual(pose[name],lo+(hi-lo)*fraction,places=11)
|
|
def test_json_active_curve_changes_cannot_change_active_output(self):
|
|
case=CASES[0];module,model,xml,factory=setup_case(case)
|
|
before=factory(case[2])[0]([80.0]*20)
|
|
data=json.loads(module._G20_CALIB_JSON.with_name('g20_left_G20_LEFT_001_calibration.json').read_text())
|
|
# Even master curves may only affect passive coupling, never active targets.
|
|
for name in module.L20_SDK_TO_URDF.values():
|
|
data['joints'][name]['angle_rad']=[x*.7 for x in data['joints'][name]['angle_rad']]
|
|
with tempfile.TemporaryDirectory() as tmp:
|
|
p=Path(tmp)/'passive.json';p.write_text(json.dumps(data))
|
|
configure(model,xml,factory,case[2],passive_json=p)
|
|
after=factory(case[2])[0]([80.0]*20)
|
|
for name in module.L20_SDK_TO_URDF.values():self.assertEqual(before[name],after[name])
|
|
self.assertTrue(any(abs(before[k]-after[k])>1e-6 for k in module.G20_MIMIC_JOINT_NAMES))
|
|
def test_passive_none_uses_urdf_and_does_not_read_json(self):
|
|
module,model,xml,factory=setup_case(CASES[0])
|
|
configure(model,xml,factory,'g20_calibrated',passive_json='none')
|
|
result=factory('g20_calibrated')[0]([100.0]*20)
|
|
for slave,(master,ratio,offset) in module.G20_URDF_MIMIC_OF.items():
|
|
self.assertAlmostEqual(result[slave],result[master]*ratio+offset)
|
|
def test_missing_optional_passive_file_falls_back(self):
|
|
module,model,xml,factory=setup_case(CASES[0])
|
|
saved=module._G20_CALIB_JSON
|
|
try:
|
|
with tempfile.TemporaryDirectory() as tmp:
|
|
module._G20_CALIB_JSON=Path(tmp)/'absent.json'
|
|
with self.assertWarns(UserWarning):
|
|
info=configure(model,xml,factory,'g20_calibrated')
|
|
self.assertEqual(info['passive'],'URDF mimic')
|
|
self.assertEqual(module.G20_PASSIVE_MAPS,{})
|
|
finally:module._G20_CALIB_JSON=saved
|
|
|
|
def test_live_model_range_replaces_copied_constants(self):
|
|
module,model,xml,factory=setup_case(CASES[2])
|
|
name='rh_index_mcp_pitch';model.jnt_range[model.joint(name).id]=[.2,.8]
|
|
configure(model,xml,factory,'g20_urdf')
|
|
self.assertAlmostEqual(factory('g20_urdf')[0]([127.5]*6)[name],.5)
|
|
def test_o12_rad_endpoints_use_loaded_model(self):
|
|
module=importlib.import_module('linker_hand_mujoco_ros2.utils.o12_joints')
|
|
xml=ASSETS/'O12/linker_hand_o12_right/linker_hand_o12_right.xml';model=mujoco.MjModel.from_xml_path(str(xml))
|
|
configure(model,xml,module.get_o12_mapping_fns,'g20_urdf')
|
|
for name in module.O12_ACTUATED_JOINTS:
|
|
self.assertEqual(module.O12_URDF_LIMITS[name],tuple(model.jnt_range[model.joint(name).id]))
|
|
lo,hi=module.O12_SDK_LIMITS[name]
|
|
vals=[module.sdk_rad_to_urdf_rad(name,x) for x in [lo,(lo+hi)/2,hi]]
|
|
# Existing SDK protocol clipping/sign conventions remain; all values finite.
|
|
self.assertTrue(np.isfinite(vals).all())
|
|
def test_all_bridge_imports_succeed_with_json_reads_forbidden(self):
|
|
code='''import sys,importlib
|
|
sys.path.insert(0,sys.argv[1])
|
|
def audit(event,args):
|
|
if event=='open' and str(args[0]).endswith('.json'):raise RuntimeError('Unexpected JSON read: '+str(args[0]))
|
|
sys.addaudithook(audit)
|
|
importlib.import_module('linker_hand_mujoco_ros2.'+sys.argv[2])
|
|
'''
|
|
for name in ['g20_plan_bridge','o6_plan_bridge','o6_left_plan_bridge','l6_plan_bridge','l6_left_plan_bridge','o12_plan_bridge','o30_right_plan_bridge','o30_left_plan_bridge']:
|
|
r=subprocess.run([sys.executable,'-B','-c',code,str(ROOT/'src/linkerhand-sim/linker_hand_mujoco_ros2'),name],capture_output=True,text=True)
|
|
self.assertEqual(r.returncode,0,r.stderr)
|
|
|
|
if __name__=='__main__':unittest.main()
|