2DOF平面臂

import numpy as np
print("="*60); print("2DOF平面臂完整系统"); print("="*60)
L1,L2,m1,m2,lc1,lc2,I1,I2,g=1.,.8,5.,3.,.5,.4,.5,.2,9.81
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
def jac(q): return 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])]])
def dyn(q,qd,tau):
    c2,s2=np.cos(q[1]),np.sin(q[1])
    M=np.array([[m1*lc1**2+I1+m2*(L1**2+lc2**2+2*L1*lc2*c2)+I2,m2*(lc2**2+L1*lc2*c2)+I2],[m2*(lc2**2+L1*lc2*c2)+I2,m2*lc2**2+I2]])
    h=m2*L1*lc2*s2; C=np.array([[-h*qd[1],-h*(qd[0]+qd[1])],[h*qd[0],0]])
    G=np.array([(m1*lc1+m2*L1)*g*np.cos(q[0])+m2*lc2*g*np.cos(q[0]+q[1]),m2*lc2*g*np.cos(q[0]+q[1])])
    return np.linalg.solve(M,tau-C@qd-G)
# 轨迹：画圆
center=np.array([1.0,0.5]); R=0.3; T_total=5.0; dt=0.001
q=np.array(ik(*center+np.array([R,0]))); qd=np.zeros(2)
max_pos_err=0; max_vel_err=0; n_steps=int(T_total/dt)
for i in range(n_steps):
    t=i*dt; phi=2*np.pi*t/T_total
    x_des=center+R*np.array([np.cos(phi),np.sin(phi)])
    xd_des=R*2*np.pi/T_total*np.array([-np.sin(phi),np.cos(phi)])
    q_des=np.array(ik(*x_des)); J=jac(q); x=fk(q); xd=J@qd
    tau=500*(q_des-q)+80*(0-qd)
    G=np.array([(m1*lc1+m2*L1)*g*np.cos(q[0])+m2*lc2*g*np.cos(q[0]+q[1]),m2*lc2*g*np.cos(q[0]+q[1])])
    tau+=G
    qdd=dyn(q,qd,tau); qd+=qdd*dt; q+=qd*dt
    pe=np.linalg.norm(x-x_des); max_pos_err=max(max_pos_err,pe)
print(f"圆形轨迹跟踪: T={T_total}s, R={R}m")
print(f"最大位置误差: {max_pos_err*1000:.2f}mm")
# 点到点运动
q_start=np.array([0.,0.]); q_end=np.array([np.pi/2,np.pi/4])
q2=q_start.copy(); qd2=np.zeros(2); T2=2.0
for i in range(int(T2/dt)):
    tau=200*(q_end-q2)+50*(0-qd2)
    G=np.array([(m1*lc1+m2*L1)*g*np.cos(q2[0])+m2*lc2*g*np.cos(q2[0]+q2[1]),m2*lc2*g*np.cos(q2[0]+q2[1])])
    tau+=G; qdd=dyn(q2,qd2,tau); qd2+=qdd*dt; q2+=qd2*dt
print(f"\n点到点运动: 误差={np.degrees(np.linalg.norm(q2-q_end)):.3f}°")

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