"""Saved cup plan -> G20 + arm. Default is preflight only; --execute moves hardware.""" import argparse import json import socket import struct import sys import time from pathlib import Path import numpy as np import mujoco import roslibpy from scipy.spatial.transform import Rotation as Rot from real_arm import RealArm from hand_mapper import HandMapper from hand_table_check import check_candidate from mapping import range_to_arc_left_l20, l20_l_min, l20_l_max ROOT=Path(__file__).resolve().parents[1] NAMES=['thumb_cmc_roll','thumb_cmc_yaw','thumb_cmc_pitch','thumb_mcp'] NAMES += [f'{f}_{j}' for f in ('index','middle','ring','pinky') for j in ('mcp_roll','mcp_pitch','pip')] SLOTS=[10,5,0,15,6,1,16,7,2,17,8,3,18,9,4,19] class Hand: def __init__(self): self.sock=socket.socket(socket.PF_CAN,socket.SOCK_RAW,socket.CAN_RAW) self.sock.bind(('can0',)); self.sock.settimeout(.5) def close(self): self.sock.close() def send(self, prop, values=()): data=bytes([prop]+list(values)) self.sock.send(struct.pack('=IB3x8s',0x28,len(data),data.ljust(8,b'\0'))) def query(self, prop): self.sock.setblocking(False) for _ in range(1000): try:self.sock.recv(16) except BlockingIOError:break else:raise RuntimeError('CAN receive backlog') self.sock.settimeout(.5); self.send(prop) end=time.monotonic()+.5 while time.monotonic()1 and data[0]==prop:return list(data[1:n]) raise RuntimeError('G20 feedback timeout') def read(self): rows=[self.query(0x41+i) for i in range(5)] if any(len(x)!=6 for x in rows):raise RuntimeError('Invalid G20 feedback') sdk=[0]*20 for i,row in enumerate(rows): sdk[i]=row[2];sdk[5+i]=row[1] if i==0 else row[0];sdk[15+i]=row[5] sdk[10]=rows[0][0] radians=range_to_arc_left_l20(sdk) return dict(zip(NAMES,[float(radians[i]) for i in SLOTS])) @staticmethod def encode(q): if set(q)!=set(NAMES):raise ValueError('All 16 named joints required') mapper=HandMapper('left'); values=np.array([q[n] for n in NAMES]) radians=mapper.qpos_to_sdk20(values) for i in SLOTS: if not np.isfinite(radians[i]) or not l20_l_min[i]-1e-6<=radians[i]<=l20_l_max[i]+1e-6: raise ValueError(f'G20 target exceeds calibrated SDK range: slot {i}') u=mapper.qpos_to_u8_commands(values) return [[int(u[10]),int(u[5]),int(u[0]),0,0,int(u[15])]]+[ [int(u[5+i]),0,int(u[i]),0,0,int(u[15+i])] for i in range(1,5)] def move(self, q): start=self.read() for fraction in np.linspace(0,1,101): value={n:start[n]+fraction*(q[n]-start[n]) for n in NAMES} for i,frame in enumerate(self.encode(value)):self.send(0x41+i,frame) time.sleep(.025) time.sleep(.5);actual=self.read() if max(abs(actual[n]-q[n]) for n in NAMES)>.10: raise RuntimeError('手指未达到目标(可能接触/阻挡);停止后续阶段,不自动抬升') class ModelCheck: def __init__(self,mount,table): self.m=mujoco.MjModel.from_xml_path(str(ROOT/'scene/a7l20_scene.xml')) self.d=mujoco.MjData(self.m);self.table=table self.m.body_quat[self.m.body('left_hand_hand_base_link').id]=np.roll(Rot.from_matrix(mount).as_quat(),1) self.base=self.m.body('base_base_link').pos.copy() self.left={i for i in range(self.m.nbody) if (self.m.body(i).name or '').startswith(('arm_left_','left_hand_'))} def update(self,joints,hand,right): for side,letter,values in [('left','L',joints),('right','R',right)]: for i,v in enumerate(values):self.d.qpos[self.m.joint(f'arm_{side}_{letter}{i+1}_Joint').qposadr[0]]=v for name,value in hand.items():self.d.qpos[self.m.joint('left_hand_'+name).qposadr[0]]=value for finger in ('thumb','index','middle','ring','pinky'): child='ip' if finger=='thumb' else 'dip'; parent='mcp' if finger=='thumb' else 'pip' self.d.qpos[self.m.joint(f'left_hand_{finger}_{child}').qposadr[0]]=hand[f'{finger}_{parent}']*(1.03 if finger=='thumb' else .89) mujoco.mj_forward(self.m,self.d) def check(self): for i in range(self.m.ngeom): if int(self.m.geom_bodyid[i]) not in self.left:continue if self.m.geom_type[i]!=mujoco.mjtGeom.mjGEOM_MESH:raise RuntimeError('Unsupported robot geometry') mesh=self.m.geom_dataid[i];off=self.m.mesh_vertadr[mesh];num=self.m.mesh_vertnum[mesh] p=self.m.mesh_vert[off:off+num]@self.d.geom_xmat[i].reshape(3,3).T+self.d.geom_xpos[i]-self.base above=p[p[:,0]>.14] if len(above) and above[:,2].min().01: raise RuntimeError(f'仿真/控制器法兰FK差异 {error*1000:.1f}mm 超过10mm;禁止执行,需核对臂模型/工具坐标') R=Rot.from_quat(np.roll(g['wrist_quat_wxyz'],-1)).as_matrix();Rf=Rot.from_matrix(R@mount.T) geometry=check_candidate(g,'left',table,plan['clearance'],margin=.01) pre=np.array(geometry['pregrasp_position']);target=np.array(g['wrist_pos']) for q in (q0,g['hand_pre'],g['hand_qpos']):hand.encode(q) model.segment(state['joints'],state['joints'],q0,g['hand_pre'],right) seed=state['joints'];segments=[] # Every executed MoveJ segment is checked as the same joint interpolation. positions=[('pregrasp',pre)]+[('approach',p) for p in np.linspace(pre,target,11)[1:]] positions += [('lift',p) for p in np.linspace(target,target+[0,0,.05],6)[1:]] for phase,pos in positions: joints=arm.ik(pos,Rf,seed) if joints is None:raise RuntimeError(f'{phase} IK failed') fk=arm.fk(joints) if fk is None or np.linalg.norm(fk-pos)>.005:raise RuntimeError('Controller FK failed') posture=g['hand_qpos'] if phase=='lift' else g['hand_pre'] model.segment(seed,joints,posture,posture,right) segments.append((phase,joints));seed=joints grasp_joints=next(j for phase,j in reversed(segments) if phase=='approach') model.segment(grasp_joints,grasp_joints,g['hand_pre'],g['hand_qpos'],right) print('PREFLIGHT PASS: G20, meshes, arm path, IK/FK; execution=',args.execute,flush=True) (cap/'execution_preflight.json').write_text(json.dumps({'executed':False, 'status':'pass','fk_error_m':error,'segments':len(segments)},indent=2)) if not args.execute:return if np.max(abs(fresh(arm)['joints']-state['joints']))>.02:raise RuntimeError('Arm moved since preflight') hand.move(g['hand_pre']);closed=False for phase,joints in segments: if phase=='lift' and not closed: hand.move(g['hand_qpos']);closed=True # Joint arrival alone does not establish an object is held. if input('确认杯子被握住后输入 lift 执行5cm试抬,其余输入停止: ').strip()!='lift':return arm_motion_sent=True if arm.movej(joints,speed=.10,accel=.10,block=False,verify=True) is not True: raise RuntimeError('MoveJ refused') end=time.monotonic()+30;settled=0 while time.monotonic()=3:break time.sleep(.05) else:raise RuntimeError('Arm arrival timeout') print('5cm试抬完成;请观察杯子是否稳定,未自动松手。') except BaseException as exc: if arm_motion_sent and arm: stopped=arm.emergency(True) print('停止请求结果:',stopped,flush=True) status={'executed':arm_motion_sent,'status':'blocked','reason':str(exc)} (cap/'execution_preflight.json').write_text(json.dumps(status,indent=2,ensure_ascii=False)) raise finally: if tp:tp.unsubscribe() if arm:arm.close() hand.close() if __name__=='__main__':main()