69 lines
2.2 KiB
Python
69 lines
2.2 KiB
Python
import math
|
||
|
||
|
||
def inverseF(x, y, l1, l2, l3, l4, l5):
|
||
"""
|
||
五连杆机构逆向运动学函数（Python 实现）
|
||
|
||
输入：
|
||
x, y: 末端执行器坐标
|
||
l1~l5: 各杆长度
|
||
|
||
输出：
|
||
theta1, theta2: 两个主动关节角度（弧度）
|
||
"""
|
||
Xc = x
|
||
Yc = y
|
||
|
||
# ===== 左侧链路（l1, l2）=====
|
||
numerator_left = Xc ** 2 + Yc ** 2 - l1 ** 2 - l2 ** 2
|
||
denominator_left = 2 * l1 * l2
|
||
cosfoai_12 = numerator_left / denominator_left
|
||
|
||
if cosfoai_12 > 1 or cosfoai_12 < -1:
|
||
raise ValueError("目标点超出工作空间！左侧无解。")
|
||
foai_12 = 2 * math.pi - math.acos(cosfoai_12)
|
||
|
||
# 求第一个电机角度 foai_01
|
||
numerator_foai01 = l2 * Yc * math.sin(foai_12) + Xc * (l2 * math.cos(foai_12) + l1)
|
||
denominator_foai01 = (l2 * math.cos(foai_12) + l1) ** 2 + (l2 * math.sin(foai_12)) ** 2
|
||
|
||
if denominator_foai01 == 0:
|
||
raise ZeroDivisionError("分母为零，无法计算左侧角度。")
|
||
|
||
cosfoai_01 = numerator_foai01 / denominator_foai01
|
||
if cosfoai_01 > 1 or cosfoai_01 < -1:
|
||
raise ValueError("cosfoai_01 超出 [-1, 1]，左侧无解。")
|
||
foai_01 = math.acos(cosfoai_01)
|
||
|
||
# ===== 右侧链路（l3, l4）=====
|
||
Xc_shifted = Xc - l5
|
||
numerator_right = Xc_shifted ** 2 + Yc ** 2 - l3 ** 2 - l4 ** 2
|
||
denominator_right = 2 * l3 * l4
|
||
cosfoai_34 = numerator_right / denominator_right
|
||
|
||
if cosfoai_34 > 1 or cosfoai_34 < -1:
|
||
raise ValueError("目标点超出工作空间！右侧无解。")
|
||
foai_34 = 2 * math.pi - math.acos(cosfoai_34)
|
||
|
||
A = l5 - Xc
|
||
B = l3 * math.sin(foai_34)
|
||
C = l4 + l3 * math.cos(foai_34)
|
||
|
||
if B == 0 and C == 0:
|
||
raise ZeroDivisionError("B 和 C 均为零，无法计算右侧角度。")
|
||
|
||
try:
|
||
foai_t = math.acos(B / math.sqrt(B ** 2 + C ** 2))
|
||
foai_40 = foai_t - math.asin(A / math.sqrt(B ** 2 + C ** 2))
|
||
except:
|
||
raise ValueError("右侧三角函数计算失败，请检查输入是否合法。")
|
||
|
||
# 转换为角度再转回弧度
|
||
theta1_deg = math.degrees(foai_01)
|
||
theta2_deg = 180 - math.degrees(foai_40)
|
||
|
||
theta1 = math.radians(theta1_deg)
|
||
theta2 = math.radians(theta2_deg)
|
||
|
||
return theta1, theta2 |