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
|