Files

206 lines
10 KiB
Python

"""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()<end:
self.sock.settimeout(max(.001,end-time.monotonic()))
ident,n,data=struct.unpack('=IB3x8s',self.sock.recv(16))
if ident==0x28 and n>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()<self.table+.005:
raise RuntimeError(f'整机路径桌面间距不足: {self.m.body(self.m.geom_bodyid[i]).name}, z={above[:,2].min():.4f}')
for c in self.d.contact:
b1,b2=[int(self.m.geom_bodyid[g]) for g in c.geom]
n1,n2=[self.m.body(b).name or '' for b in (b1,b2)]
robot=lambda n:n.startswith(('arm_','left_hand_','right_hand_','base_'))
if c.dist<-.001 and (b1 in self.left or b2 in self.left) and robot(n1) and robot(n2):
raise RuntimeError(f'整机路径自碰撞: {n1}/{n2}')
def segment(self,j0,j1,q0,q1,right):
steps=max(51,int(np.max(abs(np.array(j1)-j0))/.01)+1)
for fraction in np.linspace(0,1,steps):
self.update((1-fraction)*np.array(j0)+fraction*np.array(j1),
{n:(1-fraction)*q0[n]+fraction*q1[n] for n in NAMES},right)
self.check()
def fresh(arm):
n=arm._st['n'];state=arm.read()
if arm._st['n']==n:raise RuntimeError('Stale arm state')
return state
def main():
ap=argparse.ArgumentParser();ap.add_argument('--execute',action='store_true')
args=ap.parse_args();cap=ROOT/'data/left_cup_current'
plan=json.loads((cap/'cup_yaw_plan.json').read_text())['solutions'][0]
g=plan['grasp'];mount=np.array(plan['mount_rotation']);table=plan['geometry']['table_z_m']
hand=Hand();arm=None;tp=None;arm_motion_sent=False
try:
arm=RealArm('left',table_z=table);state=fresh(arm);q0=hand.read()
right_box=[]
tp=roslibpy.Topic(arm.ros,'/right/arm_state','lbot_arm_interfaces/msg/ArmState')
tp.subscribe(lambda m:right_box.__setitem__(slice(None), [m['joint_state']['position']]))
end=time.monotonic()+3
while not right_box and time.monotonic()<end:time.sleep(.02)
if not right_box:raise RuntimeError('No right-arm state for collision checks')
right=np.array(right_box[-1]);model=ModelCheck(mount,table)
model.update(state['joints'],q0,right)
simulated=model.d.xpos[model.m.body('arm_left_L8_Link').id]-model.base
error=float(np.linalg.norm(simulated-state['pos']))
(cap/'execution_preflight.json').write_text(json.dumps({'executed':False,
'model_flange':simulated.tolist(),'controller_flange':state['pos'].tolist(),
'fk_error_m':error,'status':'checking'},indent=2))
if error>.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()<end:
settled=settled+1 if np.max(abs(fresh(arm)['joints']-joints))<.015 else 0
if settled>=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()