SCARA分拣

import numpy as np
print("="*60); print("SCARA分拣仿真"); print("="*60)
L1,L2=0.4,0.3; Lz=0.2
def fk_scara(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]),q[2]])
def ik_scara(x,y,z):
    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 np.array([t1,t2,z])
# 分拣任务：4个物体从A区搬到B区
objects=[(0.5,0.1,0),(0.55,0.15,0),(0.5,0.2,0),(0.55,0.25,0)]
targets=[(-0.3,0.1,0),(-0.25,0.15,0),(-0.3,0.2,0),(-0.25,0.25,0)]
home=np.array([0,0,-0.15])
total_time=0; successes=0
for i,(obj,tgt) in enumerate(zip(objects,targets)):
    q_pick=ik_scara(*obj); q_place=ik_scara(*tgt)
    # 计算时间（简化）
    d1=np.linalg.norm(q_pick[:2]-home[:2]); d2=np.linalg.norm(q_place[:2]-q_pick[:2])
    t_move=d1/0.5+d2/0.5+0.1  # 拾取+放置+夹爪时间
    total_time+=t_move
    # 验证IK
    p_pick=fk_scara(q_pick); p_place=fk_scara(q_place)
    err_p=np.linalg.norm(p_pick[:2]-np.array(obj[:2]))
    err_t=np.linalg.norm(p_place[:2]-np.array(tgt[:2]))
    ok=err_p<0.001 and err_t<0.001
    successes+=ok
    print(f"物体{i+1}: ({obj[0]:.2f},{obj[1]:.2f})→({tgt[0]:.2f},{tgt[1]:.2f}) {'✓' if ok else '✗'}")
print(f"\n分拣结果: {successes}/{len(objects)}成功, 总时间≈{total_time:.1f}s")
print(f"平均周期: {total_time/len(objects):.2f}s/个")

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("  ✅ 满足实时要求")