96 lines
3.4 KiB
Python
96 lines
3.4 KiB
Python
import math
|
||
|
||
|
||
def inverseF(x, y, l1, l2, l3, l4, l5):
|
||
"""
|
||
五连杆机构逆向运动学函数（优化版）
|
||
|
||
输入：
|
||
x, y: 末端执行器坐标
|
||
l1, l2: 左侧连杆长度 (O1→B, B→C)
|
||
l3, l4: 右侧连杆长度 (C→D, D→O2)
|
||
l5: 两基座距离 (O1O2)
|
||
|
||
输出：
|
||
theta1, theta2: 两个主动关节角度（弧度，从 x 轴逆时针）
|
||
|
||
注意：
|
||
- 构型为“肘部向上”（通过 2π - acos 保证）
|
||
- 若目标点不可达，抛出 ValueError
|
||
"""
|
||
Xc, Yc = x, y
|
||
|
||
# ---------------- 左侧链路：求解 θ1 (O1 处电机角) ----------------
|
||
R2_left = Xc * Xc + Yc * Yc # O1 到末端距离平方
|
||
cos_phi12 = (R2_left - l1 * l1 - l2 * l2) / (2 * l1 * l2)
|
||
|
||
if cos_phi12 < -1 or cos_phi12 > 1:
|
||
raise ValueError(f"左侧无解：cosφ12 = {cos_phi12:.4f} 超出 [-1,1]")
|
||
|
||
# φ12 = ∠B (三角形 O1-B-C 的内角)
|
||
phi12 = 2 * math.pi - math.acos(cos_phi12) # 肘部向上构型
|
||
|
||
# 计算 B 点坐标（从 C 反推）
|
||
# 向量 BC 的方向角 = atan2(Yc, Xc) + delta_angle
|
||
# 更稳定的方法：使用向量投影或直接几何
|
||
cos_phi12_val = math.cos(phi12)
|
||
sin_phi12_val = math.sin(phi12)
|
||
|
||
# 使用向量法求 B 点
|
||
# B = C - l2 * (cos(u2), sin(u2))，但 u2 未知
|
||
# 改用：B 在以 O1 为圆心 l1 的圆 和 以 C 为圆心 l2 的圆 的交点
|
||
# 我们已有 φ12，可用三角法
|
||
|
||
# 方法：从 O1 出发，B 的方向角 foai_01
|
||
numerator = l2 * Yc * sin_phi12_val + Xc * (l2 * cos_phi12_val + l1)
|
||
denominator = (l2 * cos_phi12_val + l1) ** 2 + (l2 * sin_phi12_val) ** 2
|
||
|
||
if abs(denominator) < 1e-12:
|
||
raise ZeroDivisionError("左侧分母接近零，机构处于奇异位形。")
|
||
|
||
cos_theta1 = numerator / denominator
|
||
if cos_theta1 < -1 or cos_theta1 > 1:
|
||
raise ValueError(f"cosθ1 = {cos_theta1:.4f} 超出 [-1,1]，左侧无解。")
|
||
|
||
theta1 = math.acos(cos_theta1)
|
||
# 根据 Yc 符号判断象限（保持上凸构型）
|
||
if Yc < 0:
|
||
theta1 = 2 * math.pi - theta1 # 或根据机构限制决定
|
||
|
||
# ---------------- 右侧链路：求解 θ2 (O2 处电机角) ----------------
|
||
Xc_rel = Xc - l5 # C 点相对于 O2 的 x 坐标
|
||
R2_right = Xc_rel * Xc_rel + Yc * Yc # O2 到末端距离平方
|
||
cos_phi34 = (R2_right - l3 * l3 - l4 * l4) / (2 * l3 * l4)
|
||
|
||
if cos_phi34 < -1 or cos_phi34 > 1:
|
||
raise ValueError(f"右侧无解：cosφ34 = {cos_phi34:.4f} 超出 [-1,1]")
|
||
|
||
phi34 = 2 * math.pi - math.acos(cos_phi34) # 肘部向上
|
||
|
||
# 计算辅助量
|
||
A = l5 - Xc
|
||
B = l3 * math.sin(phi34)
|
||
C = l4 + l3 * math.cos(phi34)
|
||
|
||
if abs(B) < 1e-12 and abs(C) < 1e-12:
|
||
raise ValueError("右侧分母为零，机构处于奇异位形。")
|
||
|
||
sqrt_BC = math.sqrt(B * B + C * C)
|
||
|
||
try:
|
||
foai_t = math.acos(B / sqrt_BC)
|
||
inner_asin = A / sqrt_BC
|
||
if inner_asin < -1 or inner_asin > 1:
|
||
raise ValueError("asin 参数越界")
|
||
foai_40 = foai_t - math.asin(inner_asin)
|
||
except (ValueError, ZeroDivisionError) as e:
|
||
raise ValueError(f"右侧三角计算失败: {e}")
|
||
|
||
# 关键：直接输出弧度，不再转角度
|
||
# 原 C 代码中：theta2 = 180 - foai_40_deg，对应 u4 = pi - foai_40
|
||
theta2 = math.pi - foai_40
|
||
|
||
# ---------------- 可选：验证解的合理性 ----------------
|
||
# 可添加工作空间边界检查、关节限位等
|
||
|
||
return theta1, theta2 |