40 lines
1.7 KiB
Python
40 lines
1.7 KiB
Python
import numpy as np
|
|
|
|
|
|
def forwardF(u1, u4, omega1, omega4, l1, l2, l3, l4, l5, alpha1, alpha4):
|
|
# 位置分析
|
|
xb = l1 * np.cos(u1)
|
|
yb = l1 * np.sin(u1)
|
|
xd = l5 + l4 * np.cos(u4)
|
|
yd = l4 * np.sin(u4)
|
|
lbd = np.sqrt((xd - xb) ** 2 + (yd - yb) ** 2)
|
|
A0 = 2 * l2 * (xd - xb)
|
|
B0 = 2 * l2 * (yd - yb)
|
|
C0 = l2 ** 2 + lbd ** 2 - l3 ** 2
|
|
u2 = 2 * np.arctan((B0 + np.sqrt(A0 ** 2 + B0 ** 2 - C0 ** 2)) / (A0 + C0))
|
|
xc = xb + l2 * np.cos(u2)
|
|
yc = yb + l2 * np.sin(u2)
|
|
u3 = np.arctan2((yc - yd), (xc - xd)) + np.pi
|
|
|
|
# 速度分析
|
|
A = np.array([[l2 * np.sin(u2), -l3 * np.sin(u3)],
|
|
[l2 * np.cos(u2), -l3 * np.cos(u3)]])
|
|
B = np.array([-l1 * omega1 * np.sin(u1) + l4 * omega4 * np.sin(u4),
|
|
-l1 * omega1 * np.cos(u1) + l4 * omega4 * np.cos(u4)])
|
|
omega = np.linalg.solve(A, B)
|
|
omega2, omega3 = omega[0], omega[1]
|
|
|
|
# 加速度分析
|
|
Aa = np.array([[l2 * np.sin(u2), -l3 * np.sin(u3)],
|
|
[l2 * np.cos(u2), -l3 * np.cos(u3)]])
|
|
Ba = np.array([l3 * np.cos(u3) * omega3 ** 2 - l2 * np.cos(u2) * omega2 ** 2 + l4 * np.cos(
|
|
u4) * omega4 ** 2 + l4 * np.sin(u4) * alpha4 - l1 * np.cos(u1) * omega1 ** 2 - l1 * np.sin(u1) * alpha1,
|
|
-l3 * np.sin(u3) * omega3 ** 2 + l2 * np.sin(u2) * omega2 ** 2 - l4 * np.sin(
|
|
u4) * omega4 ** 2 + l4 * np.cos(u4) * alpha4 + l1 * np.sin(u1) * omega1 ** 2 - l1 * np.cos(
|
|
u1) * alpha1])
|
|
alpha = np.linalg.solve(Aa, Ba)
|
|
|
|
return xc, yc, u2, u3, omega2, omega3, alpha[0], alpha[1]
|
|
|
|
# 示例调用
|
|
# xc, yc, u2, u3, omega2, omega3, alpha2, alpha3 = five(np.pi/6, np.pi/4, 1, 2, 1, 2, 3, 4, 5, 0.1, 0.2) |