commit 2ada26f1768c31f8b761f40ad638ff0168aad99d
Author: 琉璃月光 <15630071+llyg777@user.noreply.gitee.com>
Date: Mon Sep 22 14:19:00 2025 +0800
first commit
diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 0000000..10b731c
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,5 @@
+# 默认忽略的文件
+/shelf/
+/workspace.xml
+# 基于编辑器的 HTTP 客户端请求
+/httpRequests/
diff --git a/.idea/3dof.iml b/.idea/3dof.iml
new file mode 100644
index 0000000..0c27a7f
--- /dev/null
+++ b/.idea/3dof.iml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml
new file mode 100644
index 0000000..105ce2d
--- /dev/null
+++ b/.idea/inspectionProfiles/profiles_settings.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000..37f5d6d
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,7 @@
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
new file mode 100644
index 0000000..efb6e4a
--- /dev/null
+++ b/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/2D/caculate/__pycache__/fkik.cpython-36.pyc b/2D/caculate/__pycache__/fkik.cpython-36.pyc
new file mode 100644
index 0000000..82d1e5f
Binary files /dev/null and b/2D/caculate/__pycache__/fkik.cpython-36.pyc differ
diff --git a/2D/caculate/fkik.py b/2D/caculate/fkik.py
new file mode 100644
index 0000000..2dc2fc0
--- /dev/null
+++ b/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()
\ No newline at end of file
diff --git a/2D/caculate/test.py b/2D/caculate/test.py
new file mode 100644
index 0000000..e69de29
diff --git a/2D/caculate/trajectory.py b/2D/caculate/trajectory.py
new file mode 100644
index 0000000..cf0b530
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/2D/main.py b/2D/main.py
new file mode 100644
index 0000000..ee764cb
--- /dev/null
+++ b/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)
diff --git a/2D/main_diaoyong.py b/2D/main_diaoyong.py
new file mode 100644
index 0000000..5fd424e
--- /dev/null
+++ b/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)
\ No newline at end of file
diff --git a/3D/caculate/__pycache__/fkik.cpython-36.pyc b/3D/caculate/__pycache__/fkik.cpython-36.pyc
new file mode 100644
index 0000000..d3f18d0
Binary files /dev/null and b/3D/caculate/__pycache__/fkik.cpython-36.pyc differ
diff --git a/3D/caculate/fkik.py b/3D/caculate/fkik.py
new file mode 100644
index 0000000..7888223
--- /dev/null
+++ b/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()
\ No newline at end of file
diff --git a/3D/caculate/test.py b/3D/caculate/test.py
new file mode 100644
index 0000000..e69de29
diff --git a/3D/caculate/trajectory.py b/3D/caculate/trajectory.py
new file mode 100644
index 0000000..dd37a1c
--- /dev/null
+++ b/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)
\ No newline at end of file
diff --git a/3D/main.py b/3D/main.py
new file mode 100644
index 0000000..a5abfc9
--- /dev/null
+++ b/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)
\ No newline at end of file