抓取规划

import numpy as np
from scipy.optimize import linprog
def fc_forces(n,fric=.5,ne=8):
    if abs(n[0])<.9: t1=np.cross(n,[1,0,0])
    else: t1=np.cross(n,[0,1,0])
    t1=t1/np.linalg.norm(t1); t2=np.cross(n,t1)
    ang=np.arctan(fric); edges=[]
    for i in range(ne):
        th=2*np.pi*i/ne; f=np.cos(ang)*n+np.sin(ang)*(np.cos(th)*t1+np.sin(th)*t2)
        edges.append(f)
    return np.array(edges)
def grasp_wrench(pts,norms,mu=.5):
    cols=[]
    for p,n in zip(pts,norms):
        for f in fc_forces(n,mu):
            cols.append(np.concatenate([f,np.cross(p,f)]))
    return np.array(cols).T
def force_closure(G):
    nc=G.shape[1]; c=np.zeros(nc)
    Aeq=np.vstack([G,np.ones(nc)]); beq=np.zeros(G.shape[0]+1); beq[-1]=1
    r=linprog(c,A_eq=Aeq,b_eq=beq,bounds=[(1e-6,None)]*nc,method='highs')
    return r.success
obj_h=np.array([.05,.03,.02])
p1=np.array([obj_h[0],0,0]); p2=np.array([-obj_h[0],0,0])
n1=np.array([-1,0,0]); n2=np.array([1,0,0])
G1=grasp_wrench([p1,p2],[n1,n2])
print("抓取规划仿真"); print("="*60)
print(f"x方向对向抓取: 力封闭={force_closure(G1)}, σ_min={np.linalg.svd(G1,compute_uv=False)[-1]:.4f}")
p3=np.array([0,obj_h[1],0]); p4=np.array([0,-obj_h[1],0])
G2=grasp_wrench([p3,p4],[np.array([0,-1,0]),np.array([0,1,0])])
print(f"y方向对向抓取: 力封闭={force_closure(G2)}, σ_min={np.linalg.svd(G2,compute_uv=False)[-1]:.4f}")
p5=np.array([obj_h[0],0,0]); p6=np.array([-obj_h[0],obj_h[1]/2,0]); p7=np.array([-obj_h[0],-obj_h[1]/2,0])
G3=grasp_wrench([p5,p6,p7],[np.array([-1,0,0]),np.array([.5,-.866,0]),np.array([.5,.866,0])])
print(f"三指抓取: 力封闭={force_closure(G3)}, σ_min={np.linalg.svd(G3,compute_uv=False)[-1]:.4f}")
print("\n摩擦系数影响:")
for mu in [.1,.3,.5,.7,1.0]:
    G=grasp_wrench([p1,p2],[n1,n2],mu)
    print(f"  μ={mu:.1f}: FC={force_closure(G)}, σ={np.linalg.svd(G,compute_uv=False)[-1]:.4f}")

import numpy as np
np.random.seed(42)
print("感知系统噪声鲁棒性")
for sigma in [0.5,1.0,2.0,5.0]:
    K=np.array([[500,0,320],[0,500,240],[0,0,1]]); n=8
    pts3d=np.random.rand(n,3)*0.2; R=np.eye(3); t=np.array([0,0,1.])
    pc=(R@pts3d.T).T+t; p2d=(K@pc.T).T; p2d=p2d[:,:2]/p2d[:,2:3]
    noisy=p2d+np.random.randn(*p2d.shape)*sigma
    proj_err=np.mean(np.linalg.norm(noisy-p2d,axis=1))
    print(f"  σ={sigma:.1f}px: 2D误差={proj_err:.2f}px, 3D误差≈{proj_err/500*1000:.1f}mm")