first commit

3D/caculate/fkik.py

@@ -0,0 +1,152 @@
+import math
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from matplotlib.widgets import Slider
+
+# 设置中文字体
+plt.rcParams['font.sans-serif'] = ['SimHei', 'Arial Unicode MS', 'DejaVu Sans']
+plt.rcParams['axes.unicode_minus'] = False
+
+class SpatialTwoLinkArm:
+    def __init__(self, l1=1.0, l2=1.0):
+        self.l1 = l1
+        self.l2 = l2
+        self.theta1 = 0.0  # 绕Z轴旋转（偏航，Yaw）
+        self.theta2 = 0.0  # 绕Y轴旋转（俯仰，Pitch）
+
+        # 创建3D图形
+        self.fig = plt.figure(figsize=(10, 8))
+        self.ax = self.fig.add_subplot(111, projection='3d')
+        self.ax.set_xlim(-2, 2)
+        self.ax.set_ylim(-2, 2)
+        self.ax.set_zlim(0, 2)
+        self.ax.set_xlabel("X")
+        self.ax.set_ylabel("Y")
+        self.ax.set_zlabel("Z")
+        self.ax.set_title("空间两自由度机械臂（绕Z轴 + 绕Y轴俯仰）")
+        self.ax.grid(True)
+
+        # 绘制机械臂
+        self.line_link1, = self.ax.plot([], [], [], 'b-', linewidth=4, label='连杆1')
+        self.line_link2, = self.ax.plot([], [], [], 'r-', linewidth=4, label='连杆2')
+        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], [0], 'ko', markersize=10, label='基座')
+
+        self.ax.legend()
+
+        # 添加角度显示文本
+        self.angle_text = self.ax.text2D(
+            0.02, 0.95, '', transform=self.ax.transAxes,
+            fontsize=10, bbox=dict(boxstyle="round", facecolor="wheat")
+        )
+
+        # 添加滑块控制
+        self.slider_ax_theta1 = plt.axes([0.2, 0.1, 0.5, 0.03])
+        self.slider_ax_theta2 = plt.axes([0.2, 0.05, 0.5, 0.03])
+
+        self.slider_theta1 = Slider(self.slider_ax_theta1, 'θ1(绕Z轴)', -np.pi, np.pi, valinit=0, valfmt="%.2f rad")
+        self.slider_theta2 = Slider(self.slider_ax_theta2, 'θ2(俯仰角)', -np.pi/2, np.pi/2, valinit=0, valfmt="%.2f rad")
+
+        self.slider_theta1.on_changed(self.on_slider_change)
+        self.slider_theta2.on_changed(self.on_slider_change)
+
+        # 初始绘制
+        self.update_plot()
+
+    def rotation_matrix_z(self, theta):
+        """绕Z轴旋转矩阵"""
+        return np.array([
+            [np.cos(theta), -np.sin(theta), 0],
+            [np.sin(theta),  np.cos(theta), 0],
+            [0,               0,            1]
+        ])
+
+    def rotation_matrix_y(self, theta):
+        """绕Y轴旋转矩阵"""
+        return np.array([
+            [np.cos(theta),  0, np.sin(theta)],
+            [0,               1,            0],
+            [-np.sin(theta), 0, np.cos(theta)]
+        ])
+
+    def fk(self, theta1, theta2):
+        """正向运动学：返回关节1和末端的位置"""
+        # 关节1位置（绕Z旋转后）
+        joint1 = np.array([self.l1, 0, 0])
+        joint1 = self.rotation_matrix_z(theta1) @ joint1
+
+        # 连杆2的方向：初始沿X轴，先绕Y转θ2，再绕Z转θ1
+        link2_dir = np.array([self.l2, 0, 0])
+        link2_dir = self.rotation_matrix_z(theta1) @ (self.rotation_matrix_y(theta2) @ link2_dir)
+
+        end_effector = joint1 + link2_dir
+
+        return joint1, end_effector
+
+    def ik(self, x, y, z):
+        """逆向运动学：给定末端位置(x,y,z)，求θ1和θ2"""
+        # θ1 由 x,y 决定（绕Z轴）
+        theta1 = math.atan2(y, x)
+
+        # 在局部坐标系中，x' = sqrt(x^2 + y^2), z' = z
+        x_prime = math.sqrt(x*x + y*y)
+        z_prime = z
+
+        # 现在是2D平面问题：连杆1长度l1，连杆2长度l2，目标(x_prime, z_prime)
+        d_sq = x_prime*x_prime + z_prime*z_prime
+        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 = np.clip(cos_theta2_numerator / denominator, -1.0, 1.0)
+        theta2 = math.acos(cos_theta2)
+
+        # 修正方向：确保z方向正确
+        # 注意：这里假设θ2是向上为正（z>0）
+        # 可扩展为elbow_up/down选择
+
+        return theta1, theta2
+
+    def update_plot(self):
+        """更新3D图像"""
+        joint1, end_effector = self.fk(self.theta1, self.theta2)
+
+        # 确保使用numpy数组以避免shape属性问题
+        joint1_np = np.array(joint1)
+        end_effector_np = np.array(end_effector)
+
+        # 更新线条
+        self.line_link1.set_data_3d([0, joint1_np[0]], [0, joint1_np[1]], [0, joint1_np[2]])
+        self.line_link2.set_data_3d([joint1_np[0], end_effector_np[0]], [joint1_np[1], end_effector_np[1]],
+                                    [joint1_np[2], end_effector_np[2]])
+
+        self.joint_point.set_data_3d([joint1_np[0]], [joint1_np[1]], [joint1_np[2]])
+        self.end_point.set_data_3d([end_effector_np[0]], [end_effector_np[1]], [end_effector_np[2]])
+
+        # 更新角度显示
+        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_slider_change(self, val):
+        """滑块变化时更新角度"""
+        self.theta1 = self.slider_theta1.val
+        self.theta2 = self.slider_theta2.val
+        self.update_plot()
+
+    def show(self):
+        plt.show()
+
+
+# ========================
+# 主程序
+# ========================
+if __name__ == "__main__":
+    print("使用滑块控制空间两自由度机械臂！")
+    arm = SpatialTwoLinkArm(l1=1.0, l2=1.0)
+    arm.show()

3D/caculate/trajectory.py

@@ -0,0 +1,146 @@
+# trajectory_3d.py
+
+import numpy as np
+
+
+def circle_trajectory_3d(center=(0, 0, 50), radius=30, axis='xy', num_points=200):
+    """
+    3D 圆形轨迹（可指定平面）
+    参数：
+        center: 圆心 (x, y, z)
+        radius: 半径
+        axis: 'xy', 'yz', 'xz' 平面
+        num_points: 点数
+    返回：
+        x_list, y_list, z_list
+    """
+    angles = np.linspace(0, 2 * np.pi, num_points)
+    cx, cy, cz = center
+
+    if axis == 'xy':
+        x_list = cx + radius * np.cos(angles)
+        y_list = cy + radius * np.sin(angles)
+        z_list = cz + np.zeros(num_points)
+    elif axis == 'yz':
+        x_list = cx + np.zeros(num_points)
+        y_list = cy + radius * np.cos(angles)
+        z_list = cz + radius * np.sin(angles)
+    elif axis == 'xz':
+        x_list = cx + radius * np.cos(angles)
+        y_list = cy + np.zeros(num_points)
+        z_list = cz + radius * np.sin(angles)
+    else:
+        raise ValueError("axis must be 'xy', 'yz', or 'xz'")
+
+    return x_list, y_list, z_list
+
+
+def line_trajectory_3d(start=(40, 0, 0), end=(120, 0, 60), num_points=100):
+    """ 3D 直线轨迹 """
+    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])
+    z_list = start[2] + t * (end[2] - start[2])
+    return x_list, y_list, z_list
+
+
+def line_trajectory_3d_uniform_speed(start=(40, 0, 0), end=(120, 60, 60), vx=0.1, vy=0.1, vz=0.05, num_points=20):
+    """ 匀速 3D 斜线轨迹，按速度分量生成 """
+    speed = np.sqrt(vx ** 2 + vy ** 2 + vz ** 2)
+    if speed == 0:
+        raise ValueError("速度不能为零")
+
+    # 计算距离和总时间
+    dx = end[0] - start[0]
+    dy = end[1] - start[1]
+    dz = end[2] - start[2]
+    distance = np.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
+    total_time = distance / speed
+
+    t = np.linspace(0, total_time, num_points)
+    x_list = start[0] + vx * t
+    y_list = start[1] + vy * t
+    z_list = start[2] + vz * t
+    return x_list, y_list, z_list
+
+
+def ellipse_trajectory_3d(center=(0, 0, 50), rx=50, ry=25, axis='xy', num_points=200):
+    """ 3D 椭圆轨迹 """
+    angles = np.linspace(0, 2 * np.pi, num_points)
+    cx, cy, cz = center
+
+    if axis == 'xy':
+        x_list = cx + rx * np.cos(angles)
+        y_list = cy + ry * np.sin(angles)
+        z_list = cz + np.zeros(num_points)
+    elif axis == 'yz':
+        x_list = cx + np.zeros(num_points)
+        y_list = cy + rx * np.cos(angles)
+        z_list = cz + ry * np.sin(angles)
+    elif axis == 'xz':
+        x_list = cx + rx * np.cos(angles)
+        y_list = cy + np.zeros(num_points)
+        z_list = cz + ry * np.sin(angles)
+    else:
+        raise ValueError("axis must be 'xy', 'yz', or 'xz'")
+
+    return x_list, y_list, z_list
+
+
+def helix_trajectory(center=(0, 0, 0), radius=30, height=100, turns=2, num_points=200):
+    """ 螺旋轨迹（沿 Z 轴上升） """
+    t = np.linspace(0, turns * 2 * np.pi, num_points)
+    z_list = center[2] + height * t / (turns * 2 * np.pi)
+    x_list = center[0] + radius * np.cos(t)
+    y_list = center[1] + radius * np.sin(t)
+    return x_list, y_list, z_list
+
+
+def square_trajectory_3d(side=60, center=(80, 0, 50), axis='xy', num_points=80):
+    """ 3D 正方形轨迹（指定平面） """
+    x_list, y_list, z_list = [], [], []
+    cx, cy, cz = center
+    half = side / 2
+
+    if axis == 'xy':
+        corners = [
+            (cx - half, cy - half, cz),
+            (cx + half, cy - half, cz),
+            (cx + half, cy + half, cz),
+            (cx - half, cy + half, cz),
+            (cx - half, cy - half, cz)
+        ]
+    elif axis == 'yz':
+        corners = [
+            (cx, cy - half, cz - half),
+            (cx, cy + half, cz - half),
+            (cx, cy + half, cz + half),
+            (cx, cy - half, cz + half),
+            (cx, cy - half, cz - half)
+        ]
+    elif axis == 'xz':
+        corners = [
+            (cx - half, cy, cz - half),
+            (cx + half, cy, cz - half),
+            (cx + half, cy, cz + half),
+            (cx - half, cy, cz + half),
+            (cx - half, cy, cz - half)
+        ]
+    else:
+        raise ValueError("axis must be 'xy', 'yz', or 'xz'")
+
+    points_per_side = num_points // 4
+    for i in range(4):
+        x_start, y_start, z_start = corners[i]
+        x_end, y_end, z_end = corners[i + 1]
+        t = np.linspace(0, 1, points_per_side)
+        x_list.extend(x_start + t * (x_end - x_start))
+        y_list.extend(y_start + t * (y_end - y_start))
+        z_list.extend(z_start + t * (z_end - z_start))
+
+    return np.array(x_list), np.array(y_list), np.array(z_list)
+
+
+def custom_trajectory_3d(custom_x, custom_y, custom_z):
+    """ 自定义 3D 轨迹 """
+    return np.array(custom_x), np.array(custom_y), np.array(custom_z)

3D/main.py

@@ -0,0 +1,154 @@
+# trajectory_control.py
+
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+from matplotlib.animation import FuncAnimation
+
+# 导入写好的 fkik 模块
+from caculate.fkik import SpatialTwoLinkArm
+
+# -------------------------
+# 参数设置
+# -------------------------
+L1 = 100.0
+L2 = 100.0
+KP, KD = 60.0, 1.0
+DEBUG_MODE = True
+
+# 创建机械臂实例
+arm = SpatialTwoLinkArm(l1=L1, l2=L2)
+
+# -------------------------
+# 3D 轨迹生成函数
+# -------------------------
+def circle_trajectory_3d(center=(100, 50, 30), radius=50, axis='xy', steps=100):
+    angles = np.linspace(0, 2*np.pi, steps)
+    cx, cy, cz = center
+    if axis == 'xy':
+        x = cx + radius * np.cos(angles)
+        y = cy + radius * np.sin(angles)
+        z = cz + np.zeros(steps)
+    elif axis == 'yz':
+        x = cx + np.zeros(steps)
+        y = cy + radius * np.cos(angles)
+        z = cz + radius * np.sin(angles)
+    elif axis == 'xz':
+        x = cx + radius * np.cos(angles)
+        y = cy + np.zeros(steps)
+        z = cz + radius * np.sin(angles)
+    else:
+        raise ValueError("axis must be 'xy', 'yz', or 'xz'")
+    return x, y, z
+
+def line_trajectory_3d(start=(50, 50, 20), end=(150, 100, 80), steps=100):
+    t = np.linspace(0, 1, steps)
+    x = start[0] + t * (end[0] - start[0])
+    y = start[1] + t * (end[1] - start[1])
+    z = start[2] + t * (end[2] - start[2])
+    return x, y, z
+
+def helix_trajectory_3d(center=(0,0,0), radius=60, height=100, turns=2, steps=200):
+    t = np.linspace(0, turns * 2*np.pi, steps)
+    x = center[0] + radius * np.cos(t)
+    y = center[1] + radius * np.sin(t)
+    z = center[2] + height * t / (turns * 2*np.pi)
+    return x, y, z
+
+# -------------------------
+# 控制电机（调试用）
+# -------------------------
+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={np.degrees(theta1_rad):+.2f}°, θ2={np.degrees(theta2_rad):+.2f}°")
+
+# -------------------------
+# 执行轨迹（调用 IK 和 FK）
+# -------------------------
+def execute_trajectory(x_list, y_list, z_list):
+    for i, (x, y, z) in enumerate(zip(x_list, y_list, z_list)):
+        try:
+            # 👉 调用你 fkik.py 中的 IK
+            solution = arm.ik(x, y, z)
+            if solution is None:
+                print(f"跳过不可达点 ({x:.1f}, {y:.1f}, {z:.1f})")
+                continue
+            theta1, theta2 = solution
+
+            # 发送控制指令
+            control_two_motors_mit(theta1, theta2)
+
+            # 👉 调用 FK 验证
+            _, (x_fk, y_fk, z_fk) = arm.fk(theta1, theta2)
+            error = np.sqrt((x - x_fk)**2 + (y - y_fk)**2 + (z - z_fk)**2)
+            print(f"点 {i}: 目标({x:.1f},{y:.1f},{z:.1f}) -> FK({x_fk:.1f},{y_fk:.1f},{z_fk:.1f}) | 误差={error:.3f}mm")
+
+        except Exception as e:
+            print(f"IK 计算失败: {e}")
+
+# -------------------------
+# 3D 动画演示
+# -------------------------
+def run_animation(x_list, y_list, z_list):
+    fig = plt.figure("Trajectory Animation")
+    ax = fig.add_subplot(111, projection='3d')
+    ax.set_xlim(-L1-L2, L1+L2)
+    ax.set_ylim(-L1-L2, L1+L2)
+    ax.set_zlim(-L1-L2, L1+L2)
+    ax.set_xlabel("X")
+    ax.set_ylabel("Y")
+    ax.set_zlabel("Z")
+    ax.set_title("3D Trajectory Tracking")
+
+    # 机械臂线条
+    line_arm, = ax.plot([], [], [], 'o-', lw=2, color='blue')
+    target_point, = ax.plot([], [], [], 'r*', markersize=10, label='Target')
+    ax.legend()
+
+    def animate(frame):
+        x, y, z = x_list[frame], y_list[frame], z_list[frame]
+        try:
+            sol = arm.ik(x, y, z)
+            if sol is None:
+                return line_arm, target_point
+            theta1, theta2 = sol
+
+            # 更新机械臂位置
+            joint1, end_effector = arm.fk(theta1, theta2)
+            joint1 = np.array(joint1)
+            end_effector = np.array(end_effector)
+
+            # 更新线条
+            line_arm.set_data_3d([0, joint1[0], end_effector[0]],
+                                 [0, joint1[1], end_effector[1]],
+                                 [0, joint1[2], end_effector[2]])
+            target_point.set_data_3d([x], [y], [z])
+        except:
+            pass
+        return line_arm, target_point
+
+    ani = FuncAnimation(fig, animate, frames=len(x_list),
+                        interval=100, blit=False, repeat=True)
+    plt.show()
+
+# -------------------------
+# 主程序
+# -------------------------
+if __name__ == "__main__":
+    print("使用滑块控制空间两自由度机械臂！")
+
+    # 选择一种轨迹
+    x_list, y_list, z_list = circle_trajectory_3d(center=(40, -100, -100), radius=20, axis='xy', steps=100)
+    # x_list, y_list, z_list = line_trajectory_3d(start=(60, 30, 10), end=(140, 90, 70), steps=80)
+    # x_list, y_list, z_list = helix_trajectory_3d(center=(0,0,0), radius=50, height=80, turns=2, steps=200)
+
+    # 执行轨迹（调用 IK 并打印调试信息）
+    execute_trajectory(x_list, y_list, z_list)
+
+    # 播放动画
+    run_animation(x_list, y_list, z_list)