Files
a7l20-hug-sim/scripts/search_cup_poses_offline.py

122 lines
6.1 KiB
Python

"""Offline IK + table-only path diagnostics; never an executable plan.
Uses saved provisional mount orientation. No controller, ROS or CAN imports.
"""
import json
import xml.etree.ElementTree as ET
import numpy as np
import mujoco
from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation as R
from offline_table_guard import ROOT,TableGuard
def hand_cloud(values):
path=ROOT/'assets/l20_left_mj.urdf'
m=mujoco.MjModel.from_xml_path(str(path));d=mujoco.MjData(m)
q=dict(values)
for j in ET.parse(path).getroot().findall('joint'):
mimic=j.find('mimic')
if mimic is not None:q[j.get('name')]=q[mimic.get('joint')]*float(mimic.get('multiplier','1'))+float(mimic.get('offset','0'))
for name,value in q.items():
joint=m.joint(name)
if joint.limited[0] and not joint.range[0]-1e-6<=value<=joint.range[1]+1e-6:
raise ValueError('Hand joint limit violation')
d.qpos[joint.qposadr[0]]=value
mujoco.mj_forward(m,d);points=[]
for i in range(m.ngeom):
if m.geom_type[i]!=mujoco.mjtGeom.mjGEOM_MESH:raise ValueError('Unsupported hand geometry')
mesh=m.geom_dataid[i];off=m.mesh_vertadr[mesh];num=m.mesh_vertnum[mesh]
v=m.mesh_vert[off:off+num]@d.geom_xmat[i].reshape(3,3).T+d.geom_xpos[i]
points.append(v)
return np.concatenate(points)
class PoseGuard(TableGuard):
def fk(self,q):
T=np.eye(4)
for a,(O,axis,v,lo,hi) in zip(q,self.chain):
A=np.eye(4);A[:3,:3]=R.from_rotvec(axis*a).as_matrix();T=T@O@A
return T
def clearance(self,q):
T=np.eye(4);worst=(float('inf'),'')
for i,(a,(O,axis,v,lo,hi)) in enumerate(zip(q,self.chain),1):
if not lo<=a<=hi:raise ValueError('Arm joint limit violation')
A=np.eye(4);A[:3,:3]=R.from_rotvec(axis*a).as_matrix();T=T@O@A
gap=float(((v@T[:3,:3].T+T[:3,3]-self.p)@self.n).min())
if gap<worst[0]:worst=(gap,f'L{i}_Link')
for name,vertices in [('adapter_yaw_envelope',None),('hand_mesh',self.cloud)]:
if vertices is None:
gap=float((T[:3,:3]@self.adapter_center+T[:3,3]-self.p)@self.n)-self.adapter_radius
else:
v=vertices@self.mount.T+self.hand_center
gap=float(((v@T[:3,:3].T+T[:3,3]-self.p)@self.n).min())
if gap<worst[0]:worst=(gap,name)
return worst
def solve(self,pos,rot,seeds):
lo=np.array([j[3] for j in self.chain]);hi=np.array([j[4] for j in self.chain])
def errors(q):
T=self.fk(q)
return np.r_[T[:3,:3]@self.hand_center+T[:3,3]-pos,
.15*R.from_matrix(rot.T@T[:3,:3]).as_rotvec()]
for seed in seeds:
fit=least_squares(errors,np.clip(seed,lo+1e-7,hi-1e-7),bounds=(lo,hi),max_nfev=140,
ftol=1e-8,xtol=1e-8,gtol=1e-8)
e=errors(fit.x)
if np.linalg.norm(e[:3])<.002 and np.linalg.norm(e[3:])/.15<np.deg2rad(1):return fit.x
return None
def main():
cap=ROOT/'data/left_cup_current';saved=json.loads((cap/'cup_yaw_plan.json').read_text())['solutions'][0]
table=json.loads((cap/'camera.json').read_text())['table_estimate']
n=np.array(table['normal']);n=n if n[2]>0 else -n
guard=PoseGuard(n/np.linalg.norm(n),table['point'])
guard.mount=np.array(saved['mount_rotation']);g=saved['grasp']
precloud=hand_cloud(g['hand_pre']);closedcloud=hand_cloud(g['hand_qpos'])
center=np.array(saved['contact_centroid_base']);position=np.array(g['wrist_pos'])
handrot=R.from_quat(np.roll(g['wrist_quat_wxyz'],-1)).as_matrix()
seeds=[np.array(saved['grasp_joints']),np.array(saved['pregrasp_joints'])];rows=[]
for dz in (.02,.04,.06):
for yaw in (0,-30,30,-60,60,-90,90):
turn=R.from_euler('z',yaw,degrees=True).as_matrix()
p=center+turn@(position-center)+[0,0,dz]
rot=turn@handrot@guard.mount.T
approach=turn@np.array(g['approach']);approach/=np.linalg.norm(approach)
pre=p-.10*approach
row=dict(raise_m=dz,yaw_relative_saved_deg=yaw,executable=False,hand_target_m=p.tolist())
q=guard.solve(p,rot,seeds)
if q is None:row['status']='grasp_ik_not_found'
else:
guard.cloud=closedcloud;gap,body=guard.clearance(q)
row.update(grasp_joints=q.tolist(),grasp_gap_m=gap,grasp_limiting_body=body)
qp=None
for distance in (.10,.07,.05,.03):
pre=p-distance*approach
qp=guard.solve(pre,rot,[q]+seeds)
if qp is not None:
row['approach_distance_m']=distance
break
if qp is None:row['status']='pregrasp_ik_not_found'
else:
row['pregrasp_joints']=qp.tolist();guard.cloud=precloud
try:
row['approach_joint_linear_check']=guard.check_segment(qp,q)
row['status']='table_segment_pass' if gap>=guard.margin else 'closed_hand_table_reject'
except (ValueError,RuntimeError) as e:row.update(status='table_segment_reject',reason=str(e))
rows.append(row);print(dz,yaw,row['status'],flush=True)
report=dict(executable=False,hardware_connected=False,candidates=rows,
scope='Local IK with CAD translation; full left-arm meshes, hand preshape meshes and adapter sphere vs saved table. Joint-linear pregrasp-to-grasp only.',
limitations=['Mount rotation is saved provisional reference, not independently verified.',
'No current-to-pregrasp transit, closure sweep, lift, self/other-arm/cup collision or force/contact validation.',
'Raised and yawed contact hypotheses must be checked against actual cup surface.',
'No real controller interpolation/stop validation; model joint limits only.',
'Local IK failure does not prove global unreachability.'])
path=cap/'other_poses_offline_report.json';path.write_text(json.dumps(report,indent=2))
print('REPORT',path,flush=True)
if __name__=='__main__':main()