206 lines
10 KiB
Python
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()
|