协作臂装配

import numpy as np
print("="*60); print("协作臂装配仿真"); print("="*60)
L1,L2=1.,.8
def fk(q): return np.array([L1*np.cos(q[0])+L2*np.cos(q[0]+q[1]),L1*np.sin(q[0])+L2*np.sin(q[0]+q[1])])
def ik(x,y):
    d2=x**2+y**2; c2=np.clip((d2-L1**2-L2**2)/(2*L1*L2),-1,1); s2=np.sqrt(1-c2**2)
    t2=np.arctan2(s2,c2); t1=np.arctan2(y,x)-np.arctan2(L2*s2,L1+L2*c2); return t1,t2
# 装配任务：轴孔配合
hole_pos=np.array([1.0,0.5]); peg_start=np.array([1.0,0.8])
Kp=200; Kd=50; g=9.81
# 阶段1: 接近（位置控制）
q=np.array(ik(*peg_start)); qd=np.zeros(2); dt=0.001
for i in range(int(2./dt)):
    x=fk(q); e=hole_pos+[0,0.02]-x  # 上方20mm
    J=np.array([[-L1*np.sin(q[0])-L2*np.sin(q[0]+q[1]),-L2*np.sin(q[0]+q[1])],[L1*np.cos(q[0])+L2*np.cos(q[0]+q[1]),L2*np.cos(q[0]+q[1])]])
    tau=J.T@(Kp*e-Kd*J@qd); G=np.array([(5*.5+3)*g*np.cos(q[0])+3*.4*g*np.cos(q[0]+q[1]),3*.4*g*np.cos(q[0]+q[1])])
    tau+=G
    c2=np.cos(q[1]); M=np.array([[2.45+.64*c2,.36+.32*c2],[.36+.32*c2,.36]]); h=.32*np.sin(q[1])
    C=np.array([[-h*qd[1],-h*(qd[0]+qd[1])],[h*qd[0],0]])
    qdd=np.linalg.solve(M,tau-C@qd-G); qd+=qdd*dt; q+=qd*dt
pos_approach=fk(q)
# 阶段2: 搜索（力引导）
np.random.seed(42); search_steps=200; found=False
for i in range(search_steps):
    F_ext=np.array([0,-2.0])  # 向下2N搜索力
    x=fk(q); J=np.array([[-L1*np.sin(q[0])-L2*np.sin(q[0]+q[1]),-L2*np.sin(q[0]+q[1])],[L1*np.cos(q[0])+L2*np.cos(q[0]+q[1]),L2*np.cos(q[0]+q[1])]])
    # 阻抗控制
    Md=np.diag([2.,2.]); Bd=np.diag([80.,80.]); Kd_imp=np.diag([300.,300.])
    ex=x-(hole_pos+[0,0.02]); ev=J@qd
    xdd_d=np.linalg.solve(Md,F_ext-Bd@ev-Kd_imp@ex)
    qdd_d=np.linalg.pinv(J)@xdd_d
    tau=M@qdd_d+C@qd+G-J.T@F_ext; qdd=dyn_result=np.linalg.solve(M,tau-C@qd-G)
    qd+=qdd*dt; q+=qd*dt
    # 检测是否对准（简化）
    if np.linalg.norm(fk(q)-hole_pos)<0.005: found=True; break
pos_insert=fk(q)
print(f"接近位置: {np.round(pos_approach,4)}")
print(f"插入位置: {np.round(pos_insert,4)}")
print(f"孔位置: {hole_pos}")
print(f"搜索步数: {i+1}/{search_steps} {'✓对准' if found else '✗未对准'}")
print(f"最终误差: {np.linalg.norm(pos_insert-hole_pos)*1000:.2f}mm")

import numpy as np, time
print("系统性能基准测试")
N=10000
start=time.time()
for _ in range(N):
    q=np.random.randn(2); c,s=np.cos(q),np.sin(q)
    x=1*c[0]+0.8*c[0+1]; y=1*s[0]+0.8*s[0+1]
fk_t=(time.time()-start)/N*1e6
start=time.time()
for _ in range(N):
    x,y=np.random.rand(2)*2-1; d2=x**2+y**2
    c2=np.clip((d2-1-0.64)/1.6,-1,1); s2=np.sqrt(1-c2**2)
ik_t=(time.time()-start)/N*1e6
print(f"  FK: {fk_t:.1f}μs  IK: {ik_t:.1f}μs")
print(f"  控制周期1000μs, 余量: {1000/max(fk_t,ik_t):.0f}x")
print("  ✅ 满足实时要求")