42 lines
2.4 KiB
Python
42 lines
2.4 KiB
Python
"""Exercise the real left ROS callback without DDS or hardware publishing."""
|
|
import sys,json
|
|
from types import SimpleNamespace
|
|
from build import ROOT,OUTPUT,MODEL
|
|
sys.path.insert(0,str(ROOT/'src/linkerhand-sim/linker_hand_mujoco_ros2'))
|
|
import mujoco
|
|
from sensor_msgs.msg import JointState
|
|
from builtin_interfaces.msg import Time
|
|
from linker_hand_mujoco_ros2 import o30_left_plan_bridge as left
|
|
class Node:
|
|
def __init__(self):self.subs=[];self.messages=[]
|
|
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,t,topic,cb,q):self.subs.append((topic,cb));return cb
|
|
def create_publisher(self,*args):return SimpleNamespace(publish=self.messages.append)
|
|
def create_timer(self,*args):return None
|
|
m=mujoco.MjModel.from_xml_path(str(MODEL/'linker_hand_o30_left/linker_hand_o30_left.xml'));d=mujoco.MjData(m);node=Node()
|
|
b=left.O30LeftPlanBridge(node,m,d,nominal_topic='/unused',collision_project=False,mapping='g20_urdf')
|
|
assert node.subs[0][0]=='/cb_left_hand_control_cmd'
|
|
spec=json.loads((OUTPUT/'commands.json').read_text());order=spec['left_ros_order']
|
|
assert order==list(left.o30j.O30_LEFT_JOINT_ORDER)
|
|
cases=[(x['sdk_command'],x['left_ros_radians']) for x in spec['pinches'].values()]
|
|
cases.append((spec['zero']['sdk_command'],spec['zero']['left_ros_radians']))
|
|
for n,a in zip(order,spec['zero']['left_ros_radians']):assert abs(d.qpos[b.joint_qpos[n]]-a)<1e-10
|
|
for command in [[0.]*20,[255.]*20,[-10.]*20,[300.]*20,list(range(0,200,10))]:
|
|
angles={}
|
|
for n,v in zip(left.o30j.O30_LEFT_SDK_JOINT_ORDER,command):
|
|
lo,hi=m.jnt_range[m.joint(n).id]
|
|
angles[n]=float(lo+(hi-lo)*min(max(v,0),255)/255)
|
|
cases.append((command,[angles[n] for n in order]))
|
|
for command,expected in cases:
|
|
node.subs[0][1](JointState(position=[float(v) for v in command]))
|
|
for _ in range(500):
|
|
with b.lock:b.step_simulation_unlocked()
|
|
for n,a in zip(order,expected):assert abs(d.qpos[b.joint_qpos[n]]-a)<1e-10,(n,a)
|
|
b._publish_state();assert node.messages[-1].name==order
|
|
before=dict(b.goal)
|
|
for invalid in [[0.]*19,[0.]*21,[float('nan')]*20,[float('inf')]*20]:
|
|
node.subs[0][1](JointState(position=invalid))
|
|
assert b.goal==before
|
|
print('PASS: 10 SDK commands (new startup zero, four pinches, endpoints, clipping, mixed order) through left callback; malformed messages rejected.')
|