122 lines
6.1 KiB
Python
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()
|