奇异性分析

import numpy as np
def jac_2r(q,L1=1.0,L2=0.8):
    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])]])
L1,L2=1.0,0.8; print("奇异性分析"); print("="*55)
print("奇异条件: det(J) = L1*L2*sin(θ₂) = 0")
tests=[(np.pi/4,0,"完全伸展"),(0,np.pi,"完全折叠"),(np.pi/3,np.pi/6,"非奇异"),(np.pi/4,-np.pi/4,"非奇异")]
for t1,t2,desc in tests:
    J=jac_2r([t1,t2]); det=np.linalg.det(J); S=np.linalg.svd(J,compute_uv=False)
    manip=np.sqrt(abs(det)); sing=abs(det)<0.01
    print(f"  {desc} θ₂={np.degrees(t2):.0f}°: det={det:+.4f} σ=({S[0]:.3f},{S[1]:.3f}) μ={manip:.4f} {'⚠️奇异' if sing else '✓'}")
# 可操作度扫描
print("\n可操作度分析:")
mus=[]; t2s=np.linspace(-np.pi,np.pi,36)
for t2 in t2s: J=jac_2r([0,t2]); mus.append(np.sqrt(abs(np.linalg.det(J@J.T))))
print(f"  最大μ={max(mus):.4f}(θ₂=±90°), 最小μ={min(mus):.4f}")
# 奇异规避IK
print("\n奇异规避IK:")
from numpy.linalg import pinv
for wp in [(1.5,0.3),(1.7,0.1),(1.79,0.01)]:
    q=np.array([0.5,0.5])
    for i in range(300):
        x=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])])
        e=np.array(wp)-x; if np.linalg.norm(e)<1e-6: break
        J=jac_2r(q); mu=np.sqrt(abs(np.linalg.det(J@J.T)))
        lam=0.5*(1-mu/0.1)**2 if mu<0.1 else 0.01
        JJT=J@J.T; dq=J.T@np.linalg.solve(JJT+lam**2*np.eye(2),e); q+=dq
    J=jac_2r(q); mu=np.sqrt(abs(np.linalg.det(J@J.T)))
    print(f"  目标({wp[0]:.2f},{wp[1]:.2f}): θ=({np.degrees(q[0]):.1f}°,{np.degrees(q[1]):.1f}°) μ={mu:.4f} iter={i+1}")

import numpy as np
L1,L2=1.,0.8
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])]])
q=np.array([0.01,0.01])
for step in range(20):
    x=L1*np.cos(q[0])+L2*np.cos(q[0]+q[1]); J=jac(q); det=np.linalg.det(J); manip=np.sqrt(abs(det))
    e=np.array([1.8*step/19,0])-np.array([x,L1*np.sin(q[0])+L2*np.sin(q[0]+q[1])])
    lam=0.1 if manip<0.1 else 0.01
    JJT=J@J.T; dq=J.T@np.linalg.solve(JJT+lam**2*np.eye(2),e); q+=dq
    print(f"step{step:2d}: x={x:.3f} det={det:+.4f} μ={manip:.4f} {"⚠️" if abs(det)<0.1 else ""}")