first commit

2D/caculate/fkik.py

@@ -0,0 +1,145 @@
+import math
+import matplotlib.pyplot as plt
+
+# 设置中文字体（防警告）
+plt.rcParams['font.sans-serif'] = ['SimHei', 'Arial Unicode MS', 'DejaVu Sans']
+plt.rcParams['axes.unicode_minus'] = False
+
+class TwoLinkArm:
+    def __init__(self, l1=1.0, l2=1.0):
+        self.l1 = l1
+        self.l2 = l2
+        self.theta1 = 0.0  # 弧度
+        self.theta2 = 0.0  # 弧度
+        self.end_x = l1 + l2  # 初始末端位置
+        self.end_y = 0.0
+        self.joint1 = (l1, 0.0)
+        self.joint2 = (l1 + l2, 0.0)
+
+        self.fig, self.ax = plt.subplots(figsize=(8, 8))
+        plt.subplots_adjust(bottom=0.2)
+        self.ax.set_xlim(-l1 - l2 - 0.5, l1 + l2 + 0.5)
+        self.ax.set_ylim(-l1 - l2 - 0.5, l1 + l2 + 0.5)
+        self.ax.set_aspect('equal')
+        self.ax.grid(True)
+        self.ax.set_title("拖动末端执行器（粉色点）来移动机械臂")
+        self.ax.set_xlabel("X")
+        self.ax.set_ylabel("Y")
+
+        # 绘制机械臂
+        self.line_link1, = self.ax.plot([], [], 'b-', linewidth=4, label=f'连杆1 (L1={l1})')
+        self.line_link2, = self.ax.plot([], [], 'r-', linewidth=4, label=f'连杆2 (L2={l2})')
+        self.joint_point, = self.ax.plot([], [], 'go', markersize=8, label='关节2')
+        self.end_point, = self.ax.plot([], [], 'mo', markersize=8, label='末端执行器')
+        self.base_point, = self.ax.plot(0, 0, 'ko', markersize=10, label='基座')
+        self.angle_text = self.ax.text(
+            -l1 - l2 + 0.2,
+            l1 + l2 - 0.3,
+            '',
+            fontsize=10,
+            bbox=dict(boxstyle="round", facecolor="wheat")
+        )
+
+        self.ax.legend()
+
+        # 拖动相关变量
+        self.dragging = False
+        self.press_xdata = None
+        self.press_ydata = None
+
+        # 连接事件
+        self.cid_press = self.fig.canvas.mpl_connect('button_press_event', self.on_press)
+        self.cid_release = self.fig.canvas.mpl_connect('button_release_event', self.on_release)
+        self.cid_motion = self.fig.canvas.mpl_connect('motion_notify_event', self.on_motion)
+
+        # 初始绘制
+        self.update_plot()
+
+    def fk(self, theta1, theta2):
+        """正向运动学"""
+        x1 = self.l1 * math.cos(theta1)
+        y1 = self.l1 * math.sin(theta1)
+        x2 = x1 + self.l2 * math.cos(theta1 + theta2)
+        y2 = y1 + self.l2 * math.sin(theta1 + theta2)
+        return (x1, y1), (x2, y2)
+
+    def ik(self, x, y, elbow_up=True):
+        """逆向运动学"""
+        d_sq = x*x + y*y
+        cos_theta2_numerator = d_sq - self.l1**2 - self.l2**2
+        denominator = 2 * self.l1 * self.l2
+
+        if abs(cos_theta2_numerator) > abs(denominator):
+            return None  # 不可达
+
+        cos_theta2 = max(-1, min(1, cos_theta2_numerator / denominator))
+        theta2 = math.acos(cos_theta2) if elbow_up else -math.acos(cos_theta2)
+
+        k1 = self.l1 + self.l2 * math.cos(theta2)
+        k2 = self.l2 * math.sin(theta2)
+        theta1 = math.atan2(y, x) - math.atan2(k2, k1)
+
+        return theta1, theta2
+
+    def update_plot(self):
+        """根据当前末端位置计算 IK 并更新图像"""
+        solution = self.ik(self.end_x, self.end_y, elbow_up=True)  # 默认肘上解
+        if solution is not None:
+            self.theta1, self.theta2 = solution
+            (x1, y1), (x2, y2) = self.fk(self.theta1, self.theta2)
+            self.joint1 = (x1, y1)
+            self.joint2 = (x2, y2)
+        else:
+            # 如果不可达，保持上一个有效姿态，但末端显示为红色
+            x2, y2 = self.end_x, self.end_y
+            self.end_point.set_color('red')
+
+        # 更新线条和点
+        self.line_link1.set_data([0, self.joint1[0]], [0, self.joint1[1]])
+        self.line_link2.set_data([self.joint1[0], self.joint2[0]], [self.joint1[1], self.joint2[1]])
+        self.joint_point.set_data(self.joint1[0], self.joint1[1])
+        self.end_point.set_data(self.joint2[0], self.joint2[1])
+        self.end_point.set_color('m')  # 恢复正常颜色
+
+        # 显示角度（现在是弧度）
+        angle_str = f"θ1 = {self.theta1:+.3f} rad\nθ2= {self.theta2:+.3f} rad"
+        self.angle_text.set_text(angle_str)
+
+        self.fig.canvas.draw_idle()
+
+    def on_press(self, event):
+        if event.inaxes != self.ax:
+            return
+        if self.end_point.contains(event)[0]:  # 点中末端点
+            self.dragging = True
+            self.press_xdata = event.xdata
+            self.press_ydata = event.ydata
+        else:
+            self.dragging = False
+
+    def on_release(self, event):
+        self.dragging = False
+
+    def on_motion(self, event):
+        if not self.dragging or event.inaxes != self.ax:
+            return
+        # 更新末端位置
+        dx = event.xdata - self.press_xdata
+        dy = event.ydata - self.press_ydata
+        self.end_x += dx
+        self.end_y += dy
+        self.press_xdata = event.xdata
+        self.press_ydata = event.ydata
+        self.update_plot()
+
+    def show(self):
+        plt.show()
+
+
+# ========================
+# 主程序
+# ========================
+if __name__ == "__main__":
+    print("拖动末端执行器（粉色点）来交互式控制两连杆机械臂！")
+    arm = TwoLinkArm(l1=1.0, l2=1.0)
+    arm.show()

2D/caculate/trajectory.py

@@ -0,0 +1,99 @@
+# trajectory.py
+
+import numpy as np
+
+def circle_trajectory(center=(80, 0), radius=40, num_points=200):
+    """ 圆形轨迹 """
+    angles = np.linspace(0, 2 * np.pi, num_points)
+    x_list = center[0] + radius * np.cos(angles)
+    y_list = center[1] + radius * np.sin(angles)
+    return x_list, y_list
+
+def line_trajectory(start=(40, 0), end=(120, 0), num_points=100):
+    """ 直线轨迹 """
+    t = np.linspace(0, 1, num_points)
+    x_list = start[0] + t * (end[0] - start[0])
+    y_list = start[1] + t * (end[1] - start[1])
+    return x_list, y_list
+
+
+def line_trajectory_fix(start=(40, 0), end=(120, 100), vx=0.1, vy=0.1, num_points=20):
+    """
+    生成带速度分量的匀速斜线轨迹
+    参数：
+        start: 起始点 (x, y)
+        end: 终点 (x, y) —— 仅用于估算运行时间（可选）
+        vx: x方向速度（单位/秒）
+        vy: y方向速度（单位/秒）
+        num_points: 生成的轨迹点数
+    返回：
+        x_list, y_list: 轨迹坐标数组
+    """
+    # 速度大小
+    speed = np.sqrt(vx**2 + vy**2)
+    if speed == 0:
+        raise ValueError("速度不能为零")
+
+    # 估算从 start 到 end 的距离（用于估算总时间）
+    if end is not None:
+        dx = end[0] - start[0]
+        dy = end[1] - start[1]
+        distance = np.sqrt(dx**2 + dy**2)
+        total_time = distance / speed  # 理论到达时间
+        print(total_time)
+    else:
+        total_time = 10.0  # 默认运行10秒
+
+    # 时间序列：从 0 到 total_time，均匀分布 num_points 个点
+    t = np.linspace(0, total_time, num_points)
+
+    # 位置 = 起点 + 速度 × 时间
+    x_list = start[0] + vx * t
+    y_list = start[1] + vy * t
+
+    return x_list, y_list
+
+def ellipse_trajectory(center=(80, 0), rx=50, ry=25, num_points=200):
+    """ 椭圆轨迹 """
+    angles = np.linspace(0, 2 * np.pi, num_points)
+    x_list = center[0] + rx * np.cos(angles)
+    y_list = center[1] + ry * np.sin(angles)
+    return x_list, y_list
+
+def square_trajectory(side=60, num_points=60):
+    """ 正方形轨迹 """
+    x_list, y_list = [], []
+    for i in range(num_points):
+        t = i / num_points
+        if t < 0.25:
+            x = 80 + 60 * t * 4
+            y = 0
+        elif t < 0.5:
+            x = 140
+            y = 0 + 60 * (t - 0.25) * 4
+        elif t < 0.75:
+            x = 140 - 60 * (t - 0.5) * 4
+            y = 60
+        else:
+            x = 80
+            y = 60 - 60 * (t - 0.75) * 4
+        x_list.append(x)
+        y_list.append(y)
+    return x_list, y_list
+
+def triangle_trajectory(base_length=100, height=80, num_points=60):
+    """ 三角形轨迹 """
+    x_list, y_list = [], []
+    points = [(80, 0), (130, 80), (30, 80), (80, 0)]
+    for i in range(num_points):
+        idx = int(i / num_points * 3)
+        t = (i % (num_points // 3)) / (num_points // 3)
+        x = points[idx][0] + t * (points[idx+1][0] - points[idx][0])
+        y = points[idx][1] + t * (points[idx+1][1] - points[idx][1])
+        x_list.append(x)
+        y_list.append(y)
+    return x_list, y_list
+
+def custom_trajectory(custom_x, custom_y):
+    """ 自定义轨迹，输入两个列表即可 """
+    return custom_x, custom_y

2D/main.py

@@ -0,0 +1,106 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.animation import FuncAnimation
+
+# -------------------------
+# 参数
+# -------------------------
+L1 = 100.0   # 连杆1长度
+L2 = 100.0   # 连杆2长度
+
+KP, KD = 60.0, 1.0
+DEBUG_MODE = True  # 没有电机时开启 DEBUG
+
+# -------------------------
+# 逆运动学
+# -------------------------
+def inverse_2link(x, y, L1, L2):
+    D = (x**2 + y**2 - L1**2 - L2**2) / (2 * L1 * L2)
+    if abs(D) > 1:
+        raise ValueError("目标点超出二连杆工作空间")
+    theta2 = np.arctan2(np.sqrt(1 - D**2), D)
+    theta1 = np.arctan2(y, x) - np.arctan2(L2*np.sin(theta2), L1 + L2*np.cos(theta2))
+    return theta1, theta2
+
+# -------------------------
+# 正运动学
+# -------------------------
+def forward_2link(theta1, theta2, L1, L2):
+    x = L1*np.cos(theta1) + L2*np.cos(theta1 + theta2)
+    y = L1*np.sin(theta1) + L2*np.sin(theta1 + theta2)
+    return x, y
+
+# -------------------------
+# 控制两个电机
+# -------------------------
+def control_two_motors_mit(theta1_rad, theta2_rad):
+    if not DEBUG_MODE:
+        # motor_control.controlMIT(motor1, KP, KD, theta1_rad, 0.1, 0.0)
+        # motor_control.controlMIT(motor2, KP, KD, theta2_rad, 0.1, 0.0)
+        pass
+    else:
+        print(f"[DEBUG] 控制 -> θ1={theta1_rad:.2f}rad, θ2={theta2_rad:.2f}rad")
+
+# -------------------------
+# 轨迹生成
+# -------------------------
+def circle_trajectory(r=60, steps=100):
+    angles = np.linspace(0, 2*np.pi, steps)
+    x = r * np.cos(angles) + 100
+    y = r * np.sin(angles) + 50
+    return x, y
+
+def line_trajectory(x0, y0, x1, y1, steps=100):
+    x = np.linspace(x0, x1, steps)
+    y = np.linspace(y0, y1, steps)
+    return x, y
+
+# -------------------------
+# 执行轨迹
+# -------------------------
+def execute_trajectory(x_list, y_list):
+    for x, y in zip(x_list, y_list):
+        try:
+            theta1, theta2 = inverse_2link(x, y, L1, L2)
+            control_two_motors_mit(theta1, theta2)
+            x_fk, y_fk = forward_2link(theta1, theta2, L1, L2)
+            print(f"目标 ({x:.1f},{y:.1f}) -> FK验证 ({x_fk:.1f},{y_fk:.1f})")
+        except ValueError as e:
+            print(f" {e}")
+
+# -------------------------
+# 动画演示
+# -------------------------
+def run_animation(x_list, y_list):
+    fig, ax = plt.subplots()
+    ax.set_xlim(-L1-L2, L1+L2)
+    ax.set_ylim(-L1-L2, L1+L2)
+    line, = ax.plot([], [], 'o-', lw=2)
+
+    def draw_frame(i):
+        x, y = x_list[i], y_list[i]
+        try:
+            theta1, theta2 = inverse_2link(x, y, L1, L2)
+            x1 = L1 * np.cos(theta1)
+            y1 = L1 * np.sin(theta1)
+            line.set_data([0, x1, x], [0, y1, y])
+        except:
+            line.set_data([], [])
+        return line,
+
+    ani = FuncAnimation(fig, draw_frame, frames=len(x_list), interval=100, blit=True)
+    plt.show()
+
+# -------------------------
+# 主程序
+# -------------------------
+if __name__ == "__main__":
+    # 选择轨迹
+    x_list, y_list = circle_trajectory(r=60, steps=100)
+    # x_list, y_list = line_trajectory(50, 50, 150, 100, steps=100)
+
+    # 执行轨迹
+    execute_trajectory(x_list, y_list)
+
+    # 动画演示
+    run_animation(x_list, y_list)

2D/main_diaoyong.py

@@ -0,0 +1,132 @@
+# main1.py
+
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.animation import FuncAnimation
+from caculate.fkik import TwoLinkArm
+
+# -------------------------
+# 参数设置
+# -------------------------
+L1 = 100.0  # 连杆1长度
+L2 = 100.0  # 连杆2长度
+
+KP, KD = 60.0, 1.0
+DEBUG_MODE = True  # 调试模式
+
+# ✅ 创建机械臂实例（关键！）
+arm = TwoLinkArm(l1=L1, l2=L2)
+
+
+# -------------------------
+# 控制两个电机（调试用）
+# -------------------------
+def control_two_motors_mit(theta1_rad, theta2_rad):
+    if not DEBUG_MODE:
+        # 这里可以接入真实电机控制
+        # motor_control.controlMIT(motor1, KP, KD, theta1_rad, 0.1, 0.0)
+        # motor_control.controlMIT(motor2, KP, KD, theta2_rad, 0.1, 0.0)
+        pass
+    else:
+        print(f"[DEBUG] 控制 -> θ1={theta1_rad:+.2f}rad, θ2={theta2_rad:+.2f}rad")
+
+
+# -------------------------
+# 轨迹生成
+# -------------------------
+def circle_trajectory(r=60, steps=100):
+    """生成圆形轨迹"""
+    angles = np.linspace(0, 2 * np.pi, steps)
+    x = r * np.cos(angles) + 100
+    y = r * np.sin(angles) + 50
+    return x, y
+
+
+def line_trajectory(x0, y0, x1, y1, steps=100):
+    """生成直线轨迹"""
+    x = np.linspace(x0, x1, steps)
+    y = np.linspace(y0, y1, steps)
+    return x, y
+
+
+# -------------------------
+# 执行轨迹
+# -------------------------
+def execute_trajectory(x_list, y_list):
+    print("开始执行轨迹...")
+    for i, (x, y) in enumerate(zip(x_list, y_list)):
+        try:
+            theta1, theta2 = arm.ik(x, y)  # ✅ 使用实例调用 ik
+            control_two_motors_mit(theta1, theta2)
+
+            _, (x_fk, y_fk) = arm.fk(theta1, theta2)  # 注意：用 _ 忽略第一个返回值 (x1,y1)
+            print(f"点 {i:3d}: 目标({x:6.1f},{y:6.1f}) -> FK验证({x_fk:6.1f},{y_fk:6.1f})") # ✅ 使用实例调用 fk
+        except ValueError as e:
+            print(f"点 {i:3d}: 跳过不可达点 ({x:.1f}, {y:.1f}) -> {e}")
+    print("轨迹执行完成。\n")
+
+
+# -------------------------
+# 动画演示
+# -------------------------
+def run_animation(x_list, y_list):
+    print("启动动画...")
+    fig, ax = plt.subplots(figsize=(8, 8))
+    ax.set_xlim(-200, 200)
+    ax.set_ylim(-200, 200)
+    ax.set_aspect('equal')
+    ax.grid(True, linestyle='--', alpha=0.6)
+    ax.set_title("2R 机械臂轨迹跟踪动画")
+    ax.set_xlabel("X (mm)")
+    ax.set_ylabel("Y (mm)")
+
+    # 绘图元素
+    line_arm, = ax.plot([], [], 'o-', lw=2, color='blue', label='机械臂')
+    target_point, = ax.plot([], [], 'r*', markersize=10, label='目标点')
+    ax.legend()
+
+    def draw_frame(i):
+        x_target, y_target = x_list[i], y_list[i]
+        try:
+            # ✅ 使用实例 arm 调用 ik
+            theta1, theta2 = arm.ik(x_target, y_target)
+            x1 = L1 * np.cos(theta1)
+            y1 = L1 * np.sin(theta1)
+            (x1, y1), (x_fk, y_fk) = arm.fk(theta1, theta2)  # 实际末端位置
+
+            # 更新机械臂：基座 -> 关节1 -> 末端
+            line_arm.set_data([0, x1, x_fk], [0, y1, y_fk])
+        except Exception as e:
+            # IK失败，只显示目标
+            line_arm.set_data([], [])
+
+        # 始终显示目标点
+        target_point.set_data([x_target], [y_target])
+        return line_arm, target_point
+
+    # 创建动画
+    ani = FuncAnimation(
+        fig, draw_frame,
+        frames=len(x_list),
+        interval=100,
+        blit=True,
+        repeat=True
+    )
+
+    plt.show()
+    return ani  # 防止动画被回收
+
+
+# -------------------------
+# 主程序
+# -------------------------
+if __name__ == "__main__":
+    # 选择轨迹
+    x_list, y_list = circle_trajectory(r=40, steps=100)
+    # x_list, y_list = line_trajectory(50, 50, 150, 100, steps=100)
+
+    # 执行轨迹控制
+    execute_trajectory(x_list, y_list)
+
+    # 播放动画
+    run_animation(x_list, y_list)