增加电机控制时间延迟

calculate/ik.py

@@ -3,67 +3,94 @@ import math
 
 def inverseF(x, y, l1, l2, l3, l4, l5):
     """
-    五连杆机构逆向运动学函数（Python 实现）
+    五连杆机构逆向运动学函数（优化版）
 
     输入：
         x, y: 末端执行器坐标
-        l1~l5: 各杆长度
+        l1, l2: 左侧连杆长度 (O1→B, B→C)
+        l3, l4: 右侧连杆长度 (C→D, D→O2)
+        l5: 两基座距离 (O1O2)
 
     输出：
-        theta1, theta2: 两个主动关节角度（弧度）
+        theta1, theta2: 两个主动关节角度（弧度，从 x 轴逆时针）
+
+    注意：
+        - 构型为“肘部向上”（通过 2π - acos 保证）
+        - 若目标点不可达，抛出 ValueError
     """
-    Xc = x
-    Yc = y
+    Xc, Yc = x, y
 
-    # ===== 左侧链路（l1, l2）=====
-    numerator_left = Xc ** 2 + Yc ** 2 - l1 ** 2 - l2 ** 2
-    denominator_left = 2 * l1 * l2
-    cosfoai_12 = numerator_left / denominator_left
+    # ---------------- 左侧链路：求解 θ1 (O1 处电机角) ----------------
+    R2_left = Xc * Xc + Yc * Yc  # O1 到末端距离平方
+    cos_phi12 = (R2_left - l1 * l1 - l2 * l2) / (2 * l1 * l2)
 
-    if cosfoai_12 > 1 or cosfoai_12 < -1:
-        raise ValueError("目标点超出工作空间！左侧无解。")
-    foai_12 = 2 * math.pi - math.acos(cosfoai_12)
+    if cos_phi12 < -1 or cos_phi12 > 1:
+        raise ValueError(f"左侧无解：cosφ12 = {cos_phi12:.4f} 超出 [-1,1]")
 
-    # 求第一个电机角度 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
+    # φ12 = ∠B (三角形 O1-B-C 的内角)
+    phi12 = 2 * math.pi - math.acos(cos_phi12)  # 肘部向上构型
 
-    if denominator_foai01 == 0:
-        raise ZeroDivisionError("分母为零，无法计算左侧角度。")
+    # 计算 B 点坐标（从 C 反推）
+    # 向量 BC 的方向角 = atan2(Yc, Xc) + delta_angle
+    # 更稳定的方法：使用向量投影或直接几何
+    cos_phi12_val = math.cos(phi12)
+    sin_phi12_val = math.sin(phi12)
 
-    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)
+    # 使用向量法求 B 点
+    # B = C - l2 * (cos(u2), sin(u2))，但 u2 未知
+    # 改用：B 在以 O1 为圆心 l1 的圆 和 以 C 为圆心 l2 的圆 的交点
+    # 我们已有 φ12，可用三角法
 
-    # ===== 右侧链路（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
+    # 方法：从 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 cosfoai_34 > 1 or cosfoai_34 < -1:
-        raise ValueError("目标点超出工作空间！右侧无解。")
-    foai_34 = 2 * math.pi - math.acos(cosfoai_34)
+    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(foai_34)
-    C = l4 + l3 * math.cos(foai_34)
+    B = l3 * math.sin(phi34)
+    C = l4 + l3 * math.cos(phi34)
 
-    if B == 0 and C == 0:
-        raise ZeroDivisionError("B 和 C 均为零，无法计算右侧角度。")
+    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 / math.sqrt(B ** 2 + C ** 2))
-        foai_40 = foai_t - math.asin(A / math.sqrt(B ** 2 + C ** 2))
-    except:
-        raise ValueError("右侧三角函数计算失败，请检查输入是否合法。")
+        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}")
 
-    # 转换为角度再转回弧度
-    theta1_deg = math.degrees(foai_01)
-    theta2_deg = 180 - math.degrees(foai_40)
+    # 关键：直接输出弧度，不再转角度
+    # 原 C 代码中：theta2 = 180 - foai_40_deg，对应 u4 = pi - foai_40
+    theta2 = math.pi - foai_40
 
-    theta1 = math.radians(theta1_deg)
-    theta2 = math.radians(theta2_deg)
+    # ---------------- 可选：验证解的合理性 ----------------
+    # 可添加工作空间边界检查、关节限位等
 
     return theta1, theta2