5dof/calculate/ik.py

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