diff --git a/src/robotic_arm_grasping/README.md b/src/robotic_arm_grasping/README.md new file mode 100644 index 0000000000..8bbe162607 --- /dev/null +++ b/src/robotic_arm_grasping/README.md @@ -0,0 +1,25 @@ +# Robotic Arm Grasping + +## 项目简介 +本项目使用 MuJoCo 物理引擎实现机械臂抓取系统的仿真与优化。核心目标是利用 MuJoCo 强大的动力学建模能力,精准模拟机械臂、目标物体的物理交互(如夹持力、摩擦力、碰撞响应),完成从目标感知、抓取规划到动作执行的全流程闭环,并通过算法迭代提升抓取稳定性与成功率。 + +## 环境配置 +### 第一步:安装 MuJoCo 引擎 +参考 [MuJoCo 官方文档](https://mujoco.org/book/installation.html) 完成安装: +1. 下载 MuJoCo 2.3.7+ 压缩包,解压至对应路径: + - Linux/Mac:`~/.mujoco/mujoco-2.3.7` + - Windows:`C:\Users\YourName\.mujoco\mujoco-2.3.7` +2. 配置环境变量(可选,用于命令行直接调用 MuJoCo 引擎) + +### 第二步:安装项目依赖 +```bash +# 克隆仓库 +git clone https://github.com/your-username/mujoco-robotic-grasping.git +cd mujoco-robotic-grasping + +# 安装 Python 依赖 +pip install -r requirements.txt + +参考资料 +MuJoCo 官方文档:核心 API 说明、模型导入规范、物理参数配置指南 +MuJoCo Python 接口教程:Python 调用示例、仿真控制细节**** \ No newline at end of file diff --git a/src/robotic_arm_grasping/arm_control.py b/src/robotic_arm_grasping/arm_control.py new file mode 100644 index 0000000000..a04f41db8c --- /dev/null +++ b/src/robotic_arm_grasping/arm_control.py @@ -0,0 +1,151 @@ +import threading, queue +import tkinter as tk +import glfw, mujoco as mj +import numpy as np + +XML_PATH = 'arm_with_gripper.xml' + +CTRL_QUEUE = queue.Queue() # 6 关节指令 +GRIP_QUEUE = queue.Queue() # 抓取状态 True=闭合 False=张开 + +# ---------- Tkinter 面板 ---------- +def tk_panel(joint_names, joint_ranges): + root = tk.Tk() + root.title('Arm Panel + Grab') + + # 6 个关节滑杆 + scales, vars = [], [tk.DoubleVar() for _ in joint_names] + def send_joint(*_): + ctrl = np.clip([v.get() for v in vars], + [r[0] for r in joint_ranges], + [r[1] for r in joint_ranges]) + CTRL_QUEUE.put(ctrl) + + for i, (name, rng) in enumerate(zip(joint_names, joint_ranges)): + tk.Label(root, text=name).grid(row=i, column=0, sticky='e') + s = tk.Scale(root, from_=rng[0], to=rng[1], resolution=0.01, + orient=tk.HORIZONTAL, length=300, variable=vars[i], + command=send_joint) + s.set((rng[0] + rng[1]) * 0.5) + s.grid(row=i, column=1) + scales.append(s) + + # Grab / Release 按钮 + def grab(): + GRIP_QUEUE.put(True) + def release(): + GRIP_QUEUE.put(False) + + tk.Button(root, text='Grab', command=grab, bg='green', fg='white').grid(row=6, column=0, pady=10) + tk.Button(root, text='Release', command=release, bg='red', fg='white').grid(row=6, column=1, pady=10) + + root.mainloop() + +# ---------- MuJoCo 渲染线程 ---------- +def mujoco_thread(joint_ids, joint_ranges, grip_act_ids): + model = mj.MjModel.from_xml_path(XML_PATH) + data = mj.MjData(model) + + # 初始值 + ctrl_jnt = np.array([(r[0] + r[1]) * 0.5 for r in joint_ranges]) + ctrl_grip_open = np.array([0.05, 1.5, 1.2] * 3) # 3 指张开(对应 slide/hinge1/hinge2) + data.ctrl[joint_ids] = ctrl_jnt + data.ctrl[grip_act_ids] = ctrl_grip_open + CTRL_QUEUE.put(ctrl_jnt) + GRIP_QUEUE.put(False) + + if not glfw.init(): + raise RuntimeError('glfw init failed') + win = glfw.create_window(1200, 900, 'Arm Panel + Grab (Fixed)', None, None) + if not win: + glfw.terminate(); raise RuntimeError('glfw window failed') + glfw.make_context_current(win) + glfw.swap_interval(1) + + cam = mj.MjvCamera() + cam.azimuth, cam.elevation = 135, -20 + cam.distance = 1.8 + cam.lookat[:] = [0, 0, 0.3] + opt = mj.MjvOption() + mj.mjv_defaultOption(opt) + scene = mj.MjvScene(model, maxgeom=5000) + con = mj.MjrContext(model, mj.mjtFontScale.mjFONTSCALE_150) + + # 添加状态保持变量 + last_ctrl_jnt = ctrl_jnt.copy() + last_grab_state = False + + # 添加标志位,用于跟踪是否已经处理过初始队列消息 + processed_initial_ctrl = False + processed_initial_grip = False + + while not glfw.window_should_close(win): + glfw.poll_events() + width, height = glfw.get_framebuffer_size(win) + viewport = mj.MjrRect(0, 0, width, height) + + # 1. 关节指令 + try: + ctrl_jnt = CTRL_QUEUE.get_nowait() + # 只有在处理过初始消息后才更新状态 + if processed_initial_ctrl: + ctrl_jnt = np.clip(ctrl_jnt, + [r[0] for r in joint_ranges], + [r[1] for r in joint_ranges]) + data.ctrl[joint_ids] = ctrl_jnt + last_ctrl_jnt = ctrl_jnt.copy() # 保存最后一次控制指令 + else: + processed_initial_ctrl = True + except queue.Empty: + # 如果没有新的控制指令,保持上次的状态 + data.ctrl[joint_ids] = last_ctrl_jnt + + # 2. 抓取指令(9 个执行器一起写,保证 equality 同步) + try: + grab = GRIP_QUEUE.get_nowait() + # 只有在处理过初始消息后才更新状态 + if processed_initial_grip: + if grab: # 闭合 + data.ctrl[grip_act_ids] = [-0.02, -0.2, 0.0] * 3 + else: # 张开 + data.ctrl[grip_act_ids] = [0.05, 1.5, 1.2] * 3 + last_grab_state = grab # 保存最后一次抓取状态 + else: + processed_initial_grip = True + except queue.Empty: + # 如果没有新的抓取指令,保持上次的状态 + if last_grab_state: # 闭合状态 + data.ctrl[grip_act_ids] = [-0.02, -0.2, 0.0] * 3 + else: # 张开状态 + data.ctrl[grip_act_ids] = [0.05, 1.5, 1.2] * 3 + + mj.mj_step(model, data) + mj.mjv_updateScene(model, data, opt, None, cam, mj.mjtCatBit.mjCAT_ALL, scene) + mj.mjr_render(viewport, scene, con) + glfw.swap_buffers(win) + + glfw.terminate() + +# ---------- 主入口 ---------- +def main(): + model = mj.MjModel.from_xml_path(XML_PATH) + + # 6 个 arm 执行器 + jnt_act = ['motor_joint1', 'motor_joint2', 'motor_joint3', + 'motor_joint4', 'motor_joint5', 'motor_joint6'] + # 9 个夹爪执行器(3 指 × 3 自由度) + grip_act = ['motor_finger1_slide', 'motor_finger1_hinge1', 'motor_finger1_hinge2', + 'motor_finger2_slide', 'motor_finger2_hinge1', 'motor_finger2_hinge2', + 'motor_finger3_slide', 'motor_finger3_hinge1', 'motor_finger3_hinge2'] + + joint_ids = [model.actuator(n).id for n in jnt_act] + grip_ids = [model.actuator(n).id for n in grip_act] + + # 用关节真实 range 做硬限位 + joint_ranges = [model.jnt_range[model.joint(j.replace('motor_', '')).id] for j in jnt_act] + + threading.Thread(target=tk_panel, args=(jnt_act, joint_ranges), daemon=True).start() + mujoco_thread(joint_ids, joint_ranges, grip_ids) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/robotic_arm_grasping/arm_path_display.py b/src/robotic_arm_grasping/arm_path_display.py new file mode 100644 index 0000000000..cbe740552f --- /dev/null +++ b/src/robotic_arm_grasping/arm_path_display.py @@ -0,0 +1,264 @@ +import mujoco +import mujoco.viewer +import time +import numpy as np +import matplotlib +matplotlib.use('TkAgg') # 使用TkAgg后端以避免兼容性问题 +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D + + +class ArmController: + def __init__(self, model_path): + # 加载模型 + self.model = mujoco.MjModel.from_xml_path(model_path) + self.data = mujoco.MjData(self.model) + + # 获取关节ID + self.joint_names = [f"joint{i}" for i in range(1, 7)] # joint1 到 joint6 + self.joint_ids = [mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name) + for name in self.joint_names] + + # 获取夹爪关节ID + self.gripper_names = [ + "finger1_slide", "finger1_hinge1", "finger1_hinge2", + "finger2_slide", "finger2_hinge1", "finger2_hinge2", + "finger3_slide", "finger3_hinge1", "finger3_hinge2" + ] + self.gripper_ids = [mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name) + for name in self.gripper_names] + + # 获取夹爪末端执行器位置 + self.gripper_site_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "grip_center") + + # 获取测试物体 + self.object_names = ["test_sphere", "test_box", "test_cylinder", "test_capsule", "test_ellipsoid"] + self.object_ids = [mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, name) + for name in self.object_names] + + def move_to_target(self, target_pos, duration=5.0): + """ + 移动到目标位置(笛卡尔空间) + """ + # 获取当前末端执行器位置 + current_pos = self.data.site_xpos[self.gripper_site_id].copy() + + # 生成线性轨迹 + steps = int(duration / self.model.opt.timestep) + trajectory = np.linspace(current_pos, target_pos, steps) + + return trajectory + + def generate_joint_trajectory(self, target_angles, duration=5.0): + """ + 生成关节空间轨迹 + """ + # 获取当前关节角度 + current_angles = self.data.qpos[:6].copy() + + # 生成线性轨迹 + steps = int(duration / self.model.opt.timestep) + trajectory = np.linspace(current_angles, target_angles, steps) + + return trajectory + + def set_gripper(self, open_close=0.0): + """ + 控制夹爪开合 + open_close: -1.0 (完全张开) 到 1.0 (完全闭合) + """ + # 夹爪范围映射 + slide_range = self.model.jnt_range[self.gripper_ids[0]] # 所有滑动关节使用相同范围 + hinge1_range = self.model.jnt_range[self.gripper_ids[1]] # 所有第一铰链关节使用相同范围 + hinge2_range = self.model.jnt_range[self.gripper_ids[2]] # 所有第二铰链关节使用相同范围 + + # 计算目标位置 + slide_target = slide_range[0] + (open_close + 1.0) / 2.0 * (slide_range[1] - slide_range[0]) + hinge1_target = hinge1_range[0] + (open_close + 1.0) / 2.0 * (hinge1_range[1] - hinge1_range[0]) + hinge2_target = hinge2_range[0] + (open_close + 1.0) / 2.0 * (hinge2_range[1] - hinge2_range[0]) + + # 设置所有夹爪关节 + for i in range(3): # 3个手指 + self.data.ctrl[self.gripper_ids[i*3]] = slide_target # 滑动关节 + self.data.ctrl[self.gripper_ids[i*3+1]] = hinge1_target # 第一铰链关节 + self.data.ctrl[self.gripper_ids[i*3+2]] = hinge2_target # 第二铰链关节 + + def plan_and_save_path(self): + """ + 规划路径并保存用于后续显示 + """ + # 记录一系列目标点来模拟路径规划 + waypoints = [ + np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), # 初始位置 + np.array([0.5, 0.0, -0.5, 0.0, 0.5, 0.0]), # 中间位置1 + np.array([0.3, 0.5, -0.8, 0.2, 0.8, 0.3]), # 中间位置2 + np.array([0.0, 0.8, -0.5, 0.0, 0.5, 0.0]), # 目标位置 + ] + + # 生成轨迹点 + full_trajectory = [] + for i in range(len(waypoints) - 1): + steps = 50 + for j in range(steps): + ratio = j / (steps - 1) + point = waypoints[i] * (1 - ratio) + waypoints[i + 1] * ratio + full_trajectory.append(point) + + full_trajectory = np.array(full_trajectory) + return full_trajectory + + def show_path_plot(self, trajectory): + """ + 显示路径规划图表 + """ + # 创建3D图表显示关节空间轨迹 + fig = plt.figure(figsize=(12, 5)) + + # 显示关节角度变化 + ax1 = fig.add_subplot(121) + for i in range(6): + ax1.plot(trajectory[:, i], label=f'关节 {i+1}') + ax1.set_title('关节空间轨迹') + ax1.set_xlabel('时间步') + ax1.set_ylabel('关节角度 (弧度)') + ax1.legend() + ax1.grid(True) + + # 显示3D工作空间轨迹(简化模拟) + ax2 = fig.add_subplot(122, projection='3d') + + # 将关节角度转换为工作空间位置(简化处理) + x_coords = np.sin(trajectory[:, 0]) * np.cos(trajectory[:, 1]) + y_coords = np.sin(trajectory[:, 0]) * np.sin(trajectory[:, 1]) + z_coords = np.cos(trajectory[:, 0]) + + ax2.plot(x_coords, y_coords, z_coords, 'b-', linewidth=2, label='末端执行器轨迹') + ax2.scatter(x_coords[0], y_coords[0], z_coords[0], color='green', s=100, label='起始点') + ax2.scatter(x_coords[-1], y_coords[-1], z_coords[-1], color='red', s=100, label='终点') + ax2.set_title('工作空间轨迹') + ax2.set_xlabel('X') + ax2.set_ylabel('Y') + ax2.set_zlabel('Z') + ax2.legend() + + plt.tight_layout() + plt.show() + + print("路径规划图表已显示,关闭图表窗口以继续...") + + def run_demo(self): + """ + 运行完整演示:先显示路径规划图表,然后进行物理仿真 + """ + # 1. 规划路径 + print("正在规划路径...") + trajectory = self.plan_and_save_path() + + # 2. 显示路径规划图表 + print("显示路径规划结果...") + self.show_path_plot(trajectory) + + # 3. 运行物理仿真 + print("启动物理仿真...") + self.run_physics_simulation() + + def run_physics_simulation(self): + """ + 运行物理仿真演示 + """ + # 创建可视化环境 + with mujoco.viewer.launch_passive(self.model, self.data) as viewer: + viewer.cam.azimuth = 180 + viewer.cam.elevation = -30 + viewer.cam.distance = 2.0 + viewer.cam.lookat[:] = [0, 0, 0.2] + + print("开始演示序列...") + + try: + # 1. 初始位置 + print("1. 移动到初始位置") + initial_angles = np.array([0.0, -0.5, 0.0, -0.5, 0.0, 0.0]) + joint_trajectory = self.generate_joint_trajectory(initial_angles, duration=2.0) + for target in joint_trajectory: + self.data.ctrl[:6] = target + self.set_gripper(-1.0) # 张开夹爪 + mujoco.mj_step(self.model, self.data) + viewer.sync() + time.sleep(self.model.opt.timestep) + + time.sleep(1) + + # 2. 移动到球体位置 + print("2. 移动到球体位置") + sphere_pos = self.data.body_xpos[self.object_ids[0]].copy() + target_pos = sphere_pos + np.array([0, 0, 0.1]) # 球体上方 + + # 简单的逆运动学(这里用直接关节控制代替) + target_angles = np.array([0.5, 0.0, -0.8, 0.0, 0.8, 0.0]) + joint_trajectory = self.generate_joint_trajectory(target_angles, duration=3.0) + for target in joint_trajectory: + self.data.ctrl[:6] = target + self.set_gripper(-1.0) # 保持张开 + mujoco.mj_step(self.model, self.data) + viewer.sync() + time.sleep(self.model.opt.timestep) + + time.sleep(1) + + # 3. 下降到抓取位置 + print("3. 下降到抓取位置") + target_angles = np.array([0.5, 0.3, -1.0, 0.0, 1.0, 0.0]) + joint_trajectory = self.generate_joint_trajectory(target_angles, duration=2.0) + for target in joint_trajectory: + self.data.ctrl[:6] = target + mujoco.mj_step(self.model, self.data) + viewer.sync() + time.sleep(self.model.opt.timestep) + + time.sleep(1) + + # 4. 闭合夹爪抓取 + print("4. 闭合夹爪抓取") + for i in range(100): + self.set_gripper(1.0) # 闭合夹爪 + mujoco.mj_step(self.model, self.data) + viewer.sync() + time.sleep(self.model.opt.timestep) + + time.sleep(1) + + # 5. 抬起物体 + print("5. 抬起物体") + target_angles = np.array([0.5, -0.5, -0.5, 0.0, 0.5, 0.0]) + joint_trajectory = self.generate_joint_trajectory(target_angles, duration=3.0) + for target in joint_trajectory: + self.data.ctrl[:6] = target + self.set_gripper(1.0) # 保持闭合 + mujoco.mj_step(self.model, self.data) + viewer.sync() + time.sleep(self.model.opt.timestep) + + print("演示完成!") + + # 保持显示几秒钟 + for _ in range(500): + self.set_gripper(1.0) + mujoco.mj_step(self.model, self.data) + viewer.sync() + time.sleep(self.model.opt.timestep) + + except KeyboardInterrupt: + print("用户中断了演示") + + +def main(): + # 创建控制器实例 + controller = ArmController("model/robotic_arm/arm_with_gripper.xml") + + # 运行演示 + controller.run_demo() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/robotic_arm_grasping/arm_sensor_fusion_visualization_sys.py b/src/robotic_arm_grasping/arm_sensor_fusion_visualization_sys.py new file mode 100644 index 0000000000..ab32c0db85 --- /dev/null +++ b/src/robotic_arm_grasping/arm_sensor_fusion_visualization_sys.py @@ -0,0 +1,733 @@ +""" +import numpy as np +import matplotlib.pyplot as plt +from scipy import signal +from scipy.stats import norm +import warnings +warnings.filterwarnings('ignore') + +# ============================================================================ +# 1. 传感器数据生成器 +# ============================================================================ + +class SensorSimulator: + """传感器数据模拟器""" + + def __init__(self, duration=5.0, fs=500): + self.duration = duration + self.fs = fs + self.dt = 1.0 / fs + self.time = np.arange(0, duration, self.dt) + self.n_samples = len(self.time) + + # 噪声参数 + self.noise_levels = { + 'joint_pos': 0.02, + 'joint_vel': 0.05, + 'force': 0.3, + 'accel': 0.1, + 'gyro': 0.02, + } + + def generate_true_trajectory(self): + """生成真实轨迹""" + t = self.time + + # 6个关节的真实轨迹 + true_joints = np.zeros((self.n_samples, 6)) + + # 不同频率的正弦运动 + freqs = [0.5, 0.8, 1.2, 1.5, 2.0, 2.5] + amplitudes = [0.5, 0.4, 0.3, 0.2, 0.15, 0.1] + + for i in range(6): + true_joints[:, i] = amplitudes[i] * np.sin(2 * np.pi * freqs[i] * t + i * 0.5) + + return true_joints + + def add_sensor_noise(self, clean_signal, sensor_type): + """添加传感器噪声""" + noise_std = self.noise_levels.get(sensor_type, 0.1) + noise = noise_std * np.random.randn(*clean_signal.shape) + bias = 0.01 * np.random.randn() # 小偏置 + return clean_signal + noise + bias + + def simulate_sensors(self, true_joints): + """模拟多个传感器""" + n_samples = self.n_samples + + # 计算真实速度(通过差分) + true_vel = np.gradient(true_joints, axis=0) / self.dt + + # 模拟传感器数据 + sensors = {} + + # 1. 关节位置传感器 + sensors['joint_pos'] = self.add_sensor_noise(true_joints, 'joint_pos') + + # 2. 关节速度传感器 + sensors['joint_vel'] = self.add_sensor_noise(true_vel, 'joint_vel') + + # 3. 力传感器(模拟抓取力) + force = np.zeros((n_samples, 3)) + for i in range(3): + base_force = 2.0 + 0.5 * np.sin(2 * np.pi * 0.3 * self.time) + force[:, i] = base_force + 0.1 * np.random.randn(n_samples) + sensors['force'] = force + + # 4. IMU传感器 + # 加速度计 + accel = np.zeros((n_samples, 3)) + accel[:, 2] = -9.81 # 重力 + accel += 0.1 * np.random.randn(n_samples, 3) + sensors['accel'] = self.add_sensor_noise(accel, 'accel') + + # 陀螺仪 + gyro = np.zeros((n_samples, 3)) + gyro[:, 0] = 0.5 * np.sin(2 * np.pi * 0.2 * self.time) + sensors['gyro'] = self.add_sensor_noise(gyro, 'gyro') + + return sensors, true_joints, true_vel + +# ============================================================================ +# 2. 滤波与融合算法 +# ============================================================================ + +class KalmanFilter: + """简化版卡尔曼滤波器""" + + def __init__(self, n_states, n_measurements): + self.n_states = n_states + self.n_measurements = n_measurements + + # 初始化 + self.x = np.zeros(n_states) # 状态 + self.P = np.eye(n_states) # 协方差 + self.F = np.eye(n_states) # 状态转移矩阵 + self.H = np.eye(n_measurements, n_states) # 测量矩阵 + self.Q = np.eye(n_states) * 0.01 # 过程噪声 + self.R = np.eye(n_measurements) * 0.1 # 测量噪声 + + # 历史记录 + self.x_history = [] + self.P_history = [] + + def predict(self): + """预测步骤""" + self.x = self.F @ self.x + self.P = self.F @ self.P @ self.F.T + self.Q + return self.x.copy() + + def update(self, z): + """更新步骤""" + # 计算卡尔曼增益 + S = self.H @ self.P @ self.H.T + self.R + K = self.P @ self.H.T @ np.linalg.inv(S) + + # 更新状态 + y = z - self.H @ self.x + self.x = self.x + K @ y + + # 更新协方差 + I = np.eye(self.n_states) + self.P = (I - K @ self.H) @ self.P + + # 保存历史 + self.x_history.append(self.x.copy()) + self.P_history.append(self.P.copy()) + + return self.x.copy() + + def run(self, measurements): + """运行滤波器""" + estimates = [] + for z in measurements: + self.predict() + x_est = self.update(z) + estimates.append(x_est) + return np.array(estimates) + +def butter_lowpass_filter(data, cutoff_freq, fs, order=4): + """巴特沃斯低通滤波器""" + nyquist = 0.5 * fs + normal_cutoff = cutoff_freq / nyquist + b, a = signal.butter(order, normal_cutoff, btype='low', analog=False) + y = signal.filtfilt(b, a, data, axis=0) + return y + +# ============================================================================ +# 3. 可视化系统 +# ============================================================================ + +class SensorFusionVisualizer: + """传感器融合可视化器""" + + def __init__(self): + plt.style.use('default') + self.colors = ['#2E86AB', '#A23B72', '#F18F01', '#C73E1D', '#3B1F2B', '#6A8D73', '#FF6B6B', '#4ECDC4'] + + def plot_sensor_comparison(self, time, sensors, true_data, filtered_data, sensor_name): + """绘制传感器数据对比""" + fig, axes = plt.subplots(2, 2, figsize=(12, 8)) + + # 1. 原始传感器数据 + ax1 = axes[0, 0] + if sensor_name in sensors: + sensor_data = sensors[sensor_name] + if sensor_data.ndim > 1: + for i in range(min(3, sensor_data.shape[1])): + ax1.plot(time, sensor_data[:, i], + label=f'{sensor_name}[{i}]', alpha=0.7) + else: + ax1.plot(time, sensor_data, label=sensor_name, alpha=0.7) + + ax1.set_xlabel('Time (s)') + ax1.set_ylabel('Sensor Value') + ax1.set_title(f'Raw {sensor_name} Data') + ax1.legend(loc='best', fontsize=8) + ax1.grid(True, alpha=0.3) + + # 2. 滤波前后对比 + ax2 = axes[0, 1] + if sensor_name in sensors and sensor_name in filtered_data: + raw = sensors[sensor_name] + filtered = filtered_data[sensor_name] + + if raw.ndim == 1: + ax2.plot(time, raw, 'b-', alpha=0.5, label='Raw', linewidth=1) + ax2.plot(time, filtered, 'r-', label='Filtered', linewidth=1.5) + else: + ax2.plot(time, raw[:, 0], 'b-', alpha=0.5, label='Raw[0]', linewidth=1) + ax2.plot(time, filtered[:, 0], 'r-', label='Filtered[0]', linewidth=1.5) + + ax2.set_xlabel('Time (s)') + ax2.set_ylabel('Value') + ax2.set_title('Filter Comparison') + ax2.legend(loc='best', fontsize=8) + ax2.grid(True, alpha=0.3) + + # 3. 频谱分析 + ax3 = axes[1, 0] + if sensor_name in sensors: + data = sensors[sensor_name] + if data.ndim == 1: + signal_data = data + else: + signal_data = data[:, 0] if data.shape[1] > 0 else np.mean(data, axis=1) + + # 计算功率谱 + if len(time) > 1: + fs = 1.0 / (time[1] - time[0]) + else: + fs = 1000 + + n_segments = min(256, len(signal_data)) + if n_segments > 10: + f, Pxx = signal.welch(signal_data, fs, nperseg=n_segments) + ax3.semilogy(f, Pxx, 'g-', linewidth=1.5) + ax3.set_xlabel('Frequency (Hz)') + ax3.set_ylabel('Power Spectral Density') + ax3.set_title('Frequency Spectrum') + ax3.grid(True, alpha=0.3) + + # 4. 残差分析 + ax4 = axes[1, 1] + if sensor_name in sensors and sensor_name in filtered_data: + raw = sensors[sensor_name] + filtered = filtered_data[sensor_name] + + if raw.ndim == 1: + residuals = raw - filtered + else: + residuals = raw[:, 0] - filtered[:, 0] + + ax4.plot(time, residuals, 'r-', alpha=0.7, linewidth=1) + ax4.axhline(y=0, color='k', linestyle='--', alpha=0.5) + + # 添加3σ边界 + if len(residuals) > 10: + std = np.std(residuals) + ax4.axhline(y=3*std, color='b', linestyle=':', alpha=0.5) + ax4.axhline(y=-3*std, color='b', linestyle=':', alpha=0.5) + ax4.fill_between(time, -3*std, 3*std, alpha=0.1, color='blue') + + ax4.text(0.02, 0.98, f'Std: {std:.3f}', + transform=ax4.transAxes, fontsize=9, + verticalalignment='top', + bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.5)) + + ax4.set_xlabel('Time (s)') + ax4.set_ylabel('Residual') + ax4.set_title('Filter Residuals') + ax4.grid(True, alpha=0.3) + + plt.suptitle(f'{sensor_name} Sensor Analysis', fontsize=14, fontweight='bold') + plt.tight_layout() + return fig + + def plot_state_estimation(self, time, true_states, estimates, state_names=None): + """绘制状态估计结果""" + if true_states is None or estimates is None: + print("警告: 没有状态估计数据") + return None + + if state_names is None: + state_names = [f'State {i+1}' for i in range(true_states.shape[1])] + + n_states = min(4, true_states.shape[1]) # 最多显示4个状态 + + fig, axes = plt.subplots(n_states, 2, figsize=(14, 3*n_states)) + if n_states == 1: + axes = axes.reshape(1, -1) + + for i in range(n_states): + # 左侧:真实值 vs 估计值 + ax1 = axes[i, 0] + + min_len = min(len(time), len(true_states), len(estimates)) + t_plot = time[:min_len] + true_plot = true_states[:min_len, i] + est_plot = estimates[:min_len, i] + + ax1.plot(t_plot, true_plot, 'g-', linewidth=2, label='True') + ax1.plot(t_plot, est_plot, 'r--', linewidth=2, label='Estimated') + + # 计算并显示误差 + error = true_plot - est_plot + rmse = np.sqrt(np.mean(error**2)) + mae = np.mean(np.abs(error)) + + ax1.text(0.02, 0.98, f'RMSE: {rmse:.4f}\nMAE: {mae:.4f}', + transform=ax1.transAxes, fontsize=9, + verticalalignment='top', + bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.5)) + + ax1.set_xlabel('Time (s)') + ax1.set_ylabel('Value') + ax1.set_title(f'{state_names[i]} Estimation') + ax1.legend(loc='best') + ax1.grid(True, alpha=0.3) + + # 右侧:误差分布 + ax2 = axes[i, 1] + + ax2.hist(error, bins=30, alpha=0.7, color='skyblue', edgecolor='black') + ax2.axvline(x=0, color='k', linestyle='--', alpha=0.5) + + # 添加正态分布拟合 + if len(error) > 10: + mu, sigma = norm.fit(error) + x = np.linspace(min(error), max(error), 100) + y = norm.pdf(x, mu, sigma) * len(error) * (max(error)-min(error))/30 + ax2.plot(x, y, 'r-', linewidth=2, label=f'Normal fit\nμ={mu:.3f}, σ={sigma:.3f}') + + ax2.set_xlabel('Error') + ax2.set_ylabel('Frequency') + ax2.set_title(f'{state_names[i]} Error Distribution') + ax2.legend(loc='best') + ax2.grid(True, alpha=0.3) + + plt.suptitle('State Estimation Results', fontsize=14, fontweight='bold') + plt.tight_layout() + return fig + + def plot_sensor_fusion_summary(self, time, sensors, filtered_data, true_states, estimates): + """绘制传感器融合总结""" + fig = plt.figure(figsize=(16, 10)) + + # 1. 所有传感器原始数据 + ax1 = plt.subplot(3, 3, 1) + sensor_keys = list(sensors.keys())[:3] # 只显示前3个传感器 + + for i, key in enumerate(sensor_keys): + data = sensors[key] + if data.ndim == 1: + ax1.plot(time, data, alpha=0.5, linewidth=1, label=key) + else: + ax1.plot(time, data[:, 0], alpha=0.5, linewidth=1, label=f'{key}[0]') + + ax1.set_xlabel('Time (s)') + ax1.set_ylabel('Value') + ax1.set_title('Raw Sensor Data') + ax1.legend(loc='best', fontsize=7) + ax1.grid(True, alpha=0.3) + + # 2. 所有传感器滤波后数据 + ax2 = plt.subplot(3, 3, 2) + for i, key in enumerate(sensor_keys): + if key in filtered_data: + data = filtered_data[key] + if data.ndim == 1: + ax2.plot(time, data, alpha=0.7, linewidth=1.5, label=key) + else: + ax2.plot(time, data[:, 0], alpha=0.7, linewidth=1.5, label=f'{key}[0]') + + ax2.set_xlabel('Time (s)') + ax2.set_ylabel('Value') + ax2.set_title('Filtered Sensor Data') + ax2.legend(loc='best', fontsize=7) + ax2.grid(True, alpha=0.3) + + # 3. 状态估计对比(第一个状态) + ax3 = plt.subplot(3, 3, 3) + if true_states is not None and estimates is not None: + min_len = min(len(time), len(true_states), len(estimates)) + if min_len > 0: + ax3.plot(time[:min_len], true_states[:min_len, 0], 'g-', linewidth=2, label='True') + ax3.plot(time[:min_len], estimates[:min_len, 0], 'r--', linewidth=2, label='Estimated') + + # 计算误差 + error = true_states[:min_len, 0] - estimates[:min_len, 0] + rmse = np.sqrt(np.mean(error**2)) + + ax3.text(0.02, 0.98, f'RMSE: {rmse:.4f}', + transform=ax3.transAxes, fontsize=9, + verticalalignment='top', + bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.5)) + + ax3.set_xlabel('Time (s)') + ax3.set_ylabel('State Value') + ax3.set_title('State 1: True vs Estimated') + ax3.legend(loc='best') + ax3.grid(True, alpha=0.3) + + # 4. 传感器噪声分析 + ax4 = plt.subplot(3, 3, 4) + noise_levels = [] + sensor_labels = [] + + for key in sensor_keys[:5]: + if key in sensors: + data = sensors[key] + if data.ndim == 1: + noise = np.std(data) + else: + noise = np.std(data, axis=0).mean() + noise_levels.append(noise) + sensor_labels.append(key) + + if noise_levels: + bars = ax4.bar(range(len(noise_levels)), noise_levels, alpha=0.7, color='lightcoral') + ax4.set_xticks(range(len(noise_levels))) + ax4.set_xticklabels(sensor_labels, rotation=45, fontsize=8) + ax4.set_ylabel('Noise Std') + ax4.set_title('Sensor Noise Levels') + ax4.grid(True, alpha=0.3, axis='y') + + # 5. 滤波效果指标 + ax5 = plt.subplot(3, 3, 5) + improvement_ratios = [] + + for key in sensor_keys[:3]: + if key in sensors and key in filtered_data: + raw = sensors[key] + filtered = filtered_data[key] + + if raw.ndim == 1: + raw_std = np.std(raw) + filt_std = np.std(filtered) + else: + raw_std = np.std(raw, axis=0).mean() + filt_std = np.std(filtered, axis=0).mean() + + if raw_std > 0: + improvement = (raw_std - filt_std) / raw_std * 100 + improvement_ratios.append(improvement) + + if improvement_ratios: + bars = ax5.bar(range(len(improvement_ratios)), improvement_ratios, alpha=0.7, color='lightgreen') + ax5.set_xticks(range(len(improvement_ratios))) + ax5.set_xticklabels(sensor_labels[:len(improvement_ratios)], rotation=45, fontsize=8) + ax5.set_ylabel('Noise Reduction (%)') + ax5.set_title('Filter Performance') + ax5.axhline(y=0, color='k', linestyle='-', alpha=0.3) + ax5.grid(True, alpha=0.3, axis='y') + + for bar, imp in zip(bars, improvement_ratios): + height = bar.get_height() + ax5.text(bar.get_x() + bar.get_width()/2., height, + f'{imp:.1f}%', ha='center', va='bottom', fontsize=8) + + # 6. 频谱对比 + ax6 = plt.subplot(3, 3, 6) + if sensor_keys and sensor_keys[0] in sensors and sensor_keys[0] in filtered_data: + key = sensor_keys[0] + raw = sensors[key] + filtered = filtered_data[key] + + if raw.ndim == 1: + signal_raw = raw + signal_filt = filtered + else: + signal_raw = raw[:, 0] if raw.shape[1] > 0 else np.mean(raw, axis=1) + signal_filt = filtered[:, 0] if filtered.shape[1] > 0 else np.mean(filtered, axis=1) + + if len(time) > 1: + fs = 1.0 / (time[1] - time[0]) + else: + fs = 1000 + + n_segments = min(256, len(signal_raw)) + if n_segments > 10: + f_raw, Pxx_raw = signal.welch(signal_raw, fs, nperseg=n_segments) + f_filt, Pxx_filt = signal.welch(signal_filt, fs, nperseg=n_segments) + + ax6.semilogy(f_raw, Pxx_raw, 'b-', alpha=0.5, label='Raw', linewidth=1) + ax6.semilogy(f_filt, Pxx_filt, 'r-', label='Filtered', linewidth=1.5) + + ax6.set_xlabel('Frequency (Hz)') + ax6.set_ylabel('Power') + ax6.set_title('Frequency Spectrum Comparison') + ax6.legend(loc='best') + ax6.grid(True, alpha=0.3) + + # 7. 误差相关性 + ax7 = plt.subplot(3, 3, 7) + if true_states is not None and estimates is not None and true_states.shape[1] >= 2: + min_len = min(len(true_states), len(estimates)) + if min_len > 0: + errors = true_states[:min_len] - estimates[:min_len] + + if errors.shape[1] >= 2: + ax7.scatter(errors[:, 0], errors[:, 1], alpha=0.5, s=10) + ax7.axhline(y=0, color='k', linestyle='--', alpha=0.3) + ax7.axvline(x=0, color='k', linestyle='--', alpha=0.3) + + # 计算相关系数 + corr = np.corrcoef(errors[:, 0], errors[:, 1])[0, 1] + ax7.text(0.02, 0.98, f'Correlation: {corr:.3f}', + transform=ax7.transAxes, fontsize=9, + verticalalignment='top', + bbox=dict(boxstyle='round', facecolor='wheat', alpha=0.5)) + + ax7.set_xlabel('Error State 1') + ax7.set_ylabel('Error State 2') + ax7.set_title('Error Correlation') + ax7.grid(True, alpha=0.3) + + # 8. 估计误差随时间变化 + ax8 = plt.subplot(3, 3, 8) + if true_states is not None and estimates is not None: + min_len = min(len(time), len(true_states), len(estimates)) + if min_len > 0: + errors = true_states[:min_len, 0] - estimates[:min_len, 0] + + ax8.plot(time[:min_len], errors, 'r-', alpha=0.7, linewidth=1) + ax8.axhline(y=0, color='k', linestyle='--', alpha=0.5) + + # 添加滑动平均 + if len(errors) > 50: + window = min(50, len(errors)) + smooth_errors = np.convolve(errors, np.ones(window)/window, mode='valid') + ax8.plot(time[window-1:min_len], smooth_errors, 'b-', linewidth=1.5, label=f'{window}-point avg') + + ax8.set_xlabel('Time (s)') + ax8.set_ylabel('Error') + ax8.set_title('Estimation Error Over Time') + ax8.legend(loc='best', fontsize=8) + ax8.grid(True, alpha=0.3) + + # 9. 协方差演化 + ax9 = plt.subplot(3, 3, 9) + # 模拟协方差演化 + if len(time) > 0: + t_short = time[:100] if len(time) > 100 else time + cov_evolution = 0.1 * np.exp(-0.1 * t_short) + 0.01 # 指数衰减 + + # 修复:正确的颜色和线型指定方式 + ax9.plot(t_short, cov_evolution, color='purple', linestyle='-', linewidth=2) + ax9.set_xlabel('Time (s)') + ax9.set_ylabel('Covariance') + ax9.set_title('State Covariance Evolution') + ax9.grid(True, alpha=0.3) + + plt.suptitle('Sensor Fusion System Summary', fontsize=16, fontweight='bold') + plt.tight_layout() + return fig + +# ============================================================================ +# 4. 主程序 +# ============================================================================ + +def main(): + """主函数""" + print("=" * 60) + print("传感器融合与状态估计可视化系统") + print("=" * 60) + + # 1. 生成数据 + print("\n1. 生成传感器数据...") + simulator = SensorSimulator(duration=5.0, fs=500) + sensors, true_states, true_vel = simulator.simulate_sensors(simulator.generate_true_trajectory()) + + print(f" 时间点数: {len(simulator.time)}") + print(f" 传感器: {list(sensors.keys())}") + print(f" 真实状态形状: {true_states.shape}") + + # 2. 滤波处理 + print("\n2. 应用滤波算法...") + filtered_data = {} + + for sensor_name, data in sensors.items(): + if data.ndim == 1: + filtered_data[sensor_name] = butter_lowpass_filter(data, cutoff_freq=50, fs=simulator.fs) + else: + # 对每个维度单独滤波 + filtered_dims = [] + for dim in range(data.shape[1]): + filtered_dim = butter_lowpass_filter(data[:, dim], cutoff_freq=50, fs=simulator.fs) + filtered_dims.append(filtered_dim) + filtered_data[sensor_name] = np.column_stack(filtered_dims) + + print(" 滤波完成!") + + # 3. 状态估计(卡尔曼滤波) + print("\n3. 运行卡尔曼滤波器...") + + # 使用关节位置进行状态估计 + if 'joint_pos' in filtered_data: + measurements = filtered_data['joint_pos'] + + # 初始化卡尔曼滤波器 + n_states = 6 + n_measurements = 6 + kf = KalmanFilter(n_states, n_measurements) + + # 设置状态转移矩阵(简单模型) + dt = simulator.dt + for i in range(n_states): + kf.F[i, i] = 1.0 # 保持位置不变 + + # 运行滤波器 + estimates = kf.run(measurements) + print(f" 状态估计完成: {estimates.shape}") + else: + estimates = None + print(" 警告: 没有关节位置数据") + + # 4. 创建可视化 + print("\n4. 创建可视化图表...") + visualizer = SensorFusionVisualizer() + + # 4.1 传感器对比图 + print(" 创建传感器对比图...") + sensor_names_to_plot = ['joint_pos', 'joint_vel', 'accel'] + for sensor_name in sensor_names_to_plot[:2]: # 只创建前2个 + if sensor_name in sensors: + fig = visualizer.plot_sensor_comparison( + simulator.time, sensors, true_states, + filtered_data, sensor_name + ) + fig.savefig(f'sensor_{sensor_name}_analysis.png', dpi=120, bbox_inches='tight') + plt.close(fig) + print(f" ✓ 保存: sensor_{sensor_name}_analysis.png") + + # 4.2 状态估计图 + if estimates is not None: + print(" 创建状态估计图...") + fig = visualizer.plot_state_estimation( + simulator.time, true_states, estimates, + state_names=[f'Joint {i+1}' for i in range(6)] + ) + if fig is not None: + fig.savefig('state_estimation_results.png', dpi=120, bbox_inches='tight') + plt.close(fig) + print(" ✓ 保存: state_estimation_results.png") + + # 4.3 融合总结图 + print(" 创建融合总结图...") + fig = visualizer.plot_sensor_fusion_summary( + simulator.time, sensors, filtered_data, + true_states, estimates + ) + fig.savefig('sensor_fusion_summary.png', dpi=120, bbox_inches='tight') + plt.close(fig) + print(" ✓ 保存: sensor_fusion_summary.png") + + # 5. 性能评估 + print("\n5. 系统性能评估...") + + if estimates is not None: + # 计算误差指标 + min_len = min(len(true_states), len(estimates)) + errors = true_states[:min_len] - estimates[:min_len] + + rmse_per_state = np.sqrt(np.mean(errors**2, axis=0)) + mae_per_state = np.mean(np.abs(errors), axis=0) + + print(" 状态估计误差:") + print(" " + "-" * 40) + print(" State | RMSE | MAE ") + print(" " + "-" * 40) + + for i in range(min(6, len(rmse_per_state))): + print(f" {i+1} | {rmse_per_state[i]:.6f} | {mae_per_state[i]:.6f}") + + print(" " + "-" * 40) + print(f" 平均 | {np.mean(rmse_per_state):.6f} | {np.mean(mae_per_state):.6f}") + + # 计算滤波效果 + if 'joint_pos' in sensors and 'joint_pos' in filtered_data: + raw_std = np.std(sensors['joint_pos'], axis=0).mean() + filt_std = np.std(filtered_data['joint_pos'], axis=0).mean() + noise_reduction = (raw_std - filt_std) / raw_std * 100 + + print(f"\n 滤波效果:") + print(f" - 原始数据标准差: {raw_std:.4f}") + print(f" - 滤波后标准差: {filt_std:.4f}") + print(f" - 噪声降低: {noise_reduction:.1f}%") + + print("\n" + "=" * 60) + print("所有任务完成!") + print("=" * 60) + + # 6. 显示一个示例图 + print("\n6. 显示示例图表...") + if estimates is not None: + # 创建简单的性能总结图 + fig, axes = plt.subplots(1, 2, figsize=(12, 4)) + + # 左侧:RMSE对比 + ax1 = axes[0] + states = range(1, min(7, len(rmse_per_state)+1)) + bars = ax1.bar(states, rmse_per_state[:len(states)], alpha=0.7, color='lightcoral') + ax1.set_xlabel('Joint Number') + ax1.set_ylabel('RMSE (rad)') + ax1.set_title('Estimation Error per Joint') + ax1.grid(True, alpha=0.3, axis='y') + + for bar, rmse_val in zip(bars, rmse_per_state[:len(states)]): + height = bar.get_height() + ax1.text(bar.get_x() + bar.get_width()/2., height, + f'{rmse_val:.4f}', ha='center', va='bottom', fontsize=9) + + # 右侧:时间序列示例 + ax2 = axes[1] + if len(simulator.time) > 0 and len(true_states) > 0 and len(estimates) > 0: + min_len = min(len(simulator.time), len(true_states), len(estimates)) + ax2.plot(simulator.time[:min_len], true_states[:min_len, 0], + 'g-', linewidth=2, label='True (Joint 1)') + ax2.plot(simulator.time[:min_len], estimates[:min_len, 0], + 'r--', linewidth=2, label='Estimated') + ax2.set_xlabel('Time (s)') + ax2.set_ylabel('Joint Angle (rad)') + ax2.set_title('Joint 1: True vs Estimated') + ax2.legend(loc='best') + ax2.grid(True, alpha=0.3) + + plt.suptitle('Sensor Fusion System Performance', fontsize=14, fontweight='bold') + plt.tight_layout() + plt.savefig('performance_summary.png', dpi=120, bbox_inches='tight') + plt.show() + + print("\n生成的文件:") + print(" 1. sensor_joint_pos_analysis.png") + print(" 2. sensor_joint_vel_analysis.png") + print(" 3. state_estimation_results.png") + print(" 4. sensor_fusion_summary.png") + print(" 5. performance_summary.png") + else: + print("警告: 无法生成性能总结图,估计数据为空") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/robotic_arm_grasping/arm_training_visualization.py b/src/robotic_arm_grasping/arm_training_visualization.py new file mode 100644 index 0000000000..f9c2c6d0d9 --- /dev/null +++ b/src/robotic_arm_grasping/arm_training_visualization.py @@ -0,0 +1,823 @@ +import numpy as np +import mujoco +import matplotlib +# 强制使用TkAgg后端,避免与MuJoCo冲突 +matplotlib.use('TkAgg') +import matplotlib.pyplot as plt +import time +import os +from collections import deque +import random +from datetime import datetime +import sys + +print("=" * 60) +print("机械臂训练可视化系统 - 修复版") +print("Python版本:", sys.version) +print("MuJoCo版本:", mujoco.__version__) +print("=" * 60) + +# ============================ 1. 完全修复的仿真环境 ============================ + +class RobustArmEnv: + """鲁棒的机械臂环境""" + + def __init__(self, model_path=None, render=False): + # 设置默认模型路径 + if model_path is None: + # 尝试多个可能的模型路径 + possible_paths = [ + "arm_with_gripper.xml", + "./arm_with_gripper.xml", + "../arm_with_gripper.xml" + ] + for path in possible_paths: + if os.path.exists(path): + model_path = path + break + + if model_path is None or not os.path.exists(model_path): + print("错误: 找不到模型文件!") + print("请将 'arm_with_gripper.xml' 放在以下位置之一:") + for path in possible_paths: + print(f" - {path}") + sys.exit(1) + + print(f"加载模型: {model_path}") + + try: + self.model = mujoco.MjModel.from_xml_path(model_path) + self.data = mujoco.MjData(self.model) + print("✓ 模型加载成功") + except Exception as e: + print(f"✗ 加载模型失败: {e}") + sys.exit(1) + + # 渲染设置 + self.render_enabled = False # 初始不渲染 + self.viewer = None + + # 状态维度 + self.state_dim = 18 # 简化的状态维度 + self.action_dim = 6 # 6个关节控制 + + # 训练参数 + self.max_steps = 100 + + # 数据记录 + self.states_history = [] + self.actions_history = [] + + # 执行器映射 + self.actuator_map = {} + self._setup_actuators() + + print(f"状态维度: {self.state_dim}, 动作维度: {self.action_dim}") + + def _setup_actuators(self): + """设置执行器映射""" + print("设置执行器映射...") + for i in range(1, 7): + motor_name = f"motor_joint{i}" + try: + actuator_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, motor_name) + if actuator_id != -1: + self.actuator_map[i-1] = actuator_id + print(f" ✓ {motor_name} -> 执行器 {actuator_id}") + else: + self.actuator_map[i-1] = i-1 + print(f" ⚠ {motor_name} 未找到,使用索引 {i-1}") + except: + self.actuator_map[i-1] = i-1 + + print("执行器映射完成") + + def _get_safe_state(self): + """安全地获取状态""" + state = [] + + try: + # 1. 关节位置 (6个关节) + for i in range(min(6, self.model.nq)): + state.append(float(self.data.qpos[i])) + + # 如果不足6个,用0填充 + for i in range(len(state), 6): + state.append(0.0) + + # 2. 关节速度 (6个关节) + for i in range(min(6, self.model.nv)): + state.append(float(self.data.qvel[i])) + + for i in range(len(state)-6, 6): + state.append(0.0) + + # 3. 末端位置 (估计) + # 使用简单估计:假设机械臂完全伸展时末端在 [0.5, 0, 0.5] + ee_pos = [0.5, 0, 0.5] + state.extend(ee_pos) + + # 4. 目标位置 (固定) + target_pos = [0.4, 0.2, 0.1] # test_sphere的位置 + state.extend(target_pos) + + return np.array(state, dtype=np.float32) + + except Exception as e: + print(f"获取状态时出错: {e}") + # 返回默认状态 + return np.zeros(self.state_dim, dtype=np.float32) + + def reset(self): + """重置环境""" + try: + mujoco.mj_resetData(self.model, self.data) + + # 设置合理的初始位置 + initial_positions = [0.0, 0.0, -0.5, 0.0, 1.0, 0.0] + for i in range(min(6, len(initial_positions))): + self.data.qpos[i] = initial_positions[i] + + # 向前模拟 + mujoco.mj_forward(self.model, self.data) + + # 清空历史 + self.states_history = [] + self.actions_history = [] + + # 获取初始状态 + state = self._get_safe_state() + self.states_history.append(state) + + return state + + except Exception as e: + print(f"重置环境失败: {e}") + return np.zeros(self.state_dim, dtype=np.float32) + + def step(self, action): + """执行一步""" + try: + # 确保动作在有效范围内 + action = np.clip(action, -1.0, 1.0) * 2.0 # 缩放到 [-2, 2] + + # 应用动作 + for i in range(min(6, len(action))): + if i in self.actuator_map: + self.data.ctrl[self.actuator_map[i]] = action[i] + + # 向前模拟 + mujoco.mj_step(self.model, self.data) + + # 如果启用渲染且viewer存在,则同步 + if self.render_enabled and self.viewer is not None: + try: + self.viewer.sync() + time.sleep(0.001) # 小延迟避免过快 + except: + pass + + # 获取新状态 + state = self._get_safe_state() + + # 计算奖励(简化版) + ee_pos = state[12:15] + target_pos = state[15:18] + distance = np.linalg.norm(ee_pos - target_pos) + + # 奖励函数 + reward = -distance * 10.0 # 距离惩罚 + + if distance < 0.1: + reward += 5.0 + if distance < 0.05: + reward += 10.0 + if distance < 0.02: + reward += 50.0 + + # 动作平滑惩罚 + reward -= 0.01 * np.sum(np.square(action)) + + # 检查是否结束 + done = len(self.states_history) >= self.max_steps or distance < 0.02 + + # 记录 + self.states_history.append(state) + self.actions_history.append(action) + + return state, reward, done, {"distance": distance} + + except Exception as e: + print(f"执行步骤失败: {e}") + state = self._get_safe_state() + return state, -10.0, True, {"distance": 10.0} + + def start_viewer(self): + """启动viewer""" + if self.viewer is None: + try: + from mujoco import viewer + print("启动MuJoCo viewer...") + self.viewer = viewer.launch_passive(self.model, self.data) + self.render_enabled = True + print("✓ Viewer启动成功") + except Exception as e: + print(f"✗ 启动viewer失败: {e}") + self.viewer = None + self.render_enabled = False + + def close(self): + """关闭环境""" + if self.viewer is not None: + try: + self.viewer.close() + except: + pass + self.viewer = None + self.render_enabled = False + +# ============================ 2. 简化的RL代理 ============================ + +class SimpleAgent: + """简化的代理""" + + def __init__(self, state_dim, action_dim): + self.state_dim = state_dim + self.action_dim = action_dim + + # 策略参数 + self.epsilon = 1.0 + self.epsilon_min = 0.1 + self.epsilon_decay = 0.995 + + # 简单线性策略 + self.weights = np.random.randn(state_dim, action_dim) * 0.01 + self.bias = np.zeros(action_dim) + + # 经验回放 + self.memory = deque(maxlen=2000) + self.batch_size = 32 + + # 学习参数 + self.lr = 0.001 + self.gamma = 0.95 + + # 记录 + self.losses = [] + self.q_values = [] + + def act(self, state, training=True): + """选择动作""" + if training and np.random.rand() < self.epsilon: + # 随机动作 + action = np.random.uniform(-1, 1, self.action_dim) + return np.clip(action, -1, 1) + else: + # 策略网络 + q_vals = np.dot(state, self.weights) + self.bias + action = np.tanh(q_vals) # 限制在[-1, 1] + + # 记录Q值 + self.q_values.append(np.max(q_vals)) + + # 衰减探索率 + if training: + self.epsilon = max(self.epsilon_min, self.epsilon * self.epsilon_decay) + + return action + + def remember(self, state, action, reward, next_state, done): + """记住经验""" + self.memory.append((state, action, reward, next_state, done)) + + def replay(self): + """经验回放""" + if len(self.memory) < self.batch_size: + return 0.0 + + batch = random.sample(self.memory, self.batch_size) + loss = 0.0 + + for state, action, reward, next_state, done in batch: + # 当前Q值 + current_q = np.dot(state, self.weights) + self.bias + + # 目标Q值 + if done: + target = reward + else: + next_q = np.dot(next_state, self.weights) + self.bias + target = reward + self.gamma * np.max(next_q) + + # 找到最大Q值的动作索引 + action_idx = np.argmax(action) + + # 计算损失 + td_error = target - current_q[action_idx] + loss += td_error ** 2 + + # 更新权重 + self.weights[:, action_idx] += self.lr * td_error * state + self.bias[action_idx] += self.lr * td_error + + avg_loss = loss / self.batch_size + self.losses.append(avg_loss) + + return avg_loss + + def save(self, filename): + """保存模型""" + np.savez(filename, + weights=self.weights, + bias=self.bias, + epsilon=self.epsilon) + print(f"模型已保存: {filename}") + + def load(self, filename): + """加载模型""" + if os.path.exists(filename): + data = np.load(filename) + self.weights = data['weights'] + self.bias = data['bias'] + self.epsilon = data['epsilon'] + print(f"模型已加载: {filename}") + return True + else: + print(f"模型文件不存在: {filename}") + return False + +# ============================ 3. 修复的图表系统 ============================ + +class FixedChartSystem: + """修复的图表系统""" + + def __init__(self): + print("初始化图表系统...") + + # 创建图形窗口 + plt.close('all') # 关闭所有现有图形 + + # 设置图形大小 + self.fig = plt.figure(figsize=(14, 8)) + + # 创建子图 + self.ax1 = plt.subplot(2, 3, 1) # 奖励 + self.ax2 = plt.subplot(2, 3, 2) # 距离 + self.ax3 = plt.subplot(2, 3, 3) # 步数 + self.ax4 = plt.subplot(2, 3, 4) # 动作分布 + self.ax5 = plt.subplot(2, 3, 5) # 轨迹 + self.ax6 = plt.subplot(2, 3, 6) # 统计信息 + + # 数据存储 + self.episode_rewards = [] + self.episode_distances = [] + self.episode_lengths = [] + self.loss_history = [] + self.q_history = [] + + # 初始化图表 + self._init_charts() + + # 显示图形 + plt.ion() + plt.show(block=False) + plt.pause(0.1) # 给图形时间初始化 + + print("✓ 图表系统初始化完成") + + def _init_charts(self): + """初始化图表""" + # 奖励图表 + self.ax1.set_title('Episode Rewards') + self.ax1.set_xlabel('Episode') + self.ax1.set_ylabel('Reward') + self.ax1.grid(True, alpha=0.3) + self.reward_line, = self.ax1.plot([], [], 'b-', linewidth=2) + + # 距离图表 + self.ax2.set_title('Final Distance') + self.ax2.set_xlabel('Episode') + self.ax2.set_ylabel('Distance') + self.ax2.grid(True, alpha=0.3) + self.distance_line, = self.ax2.plot([], [], 'r-', linewidth=2) + self.ax2.axhline(y=0.02, color='g', linestyle='--', alpha=0.5, label='Success') + self.ax2.legend() + + # 步数图表 + self.ax3.set_title('Episode Length') + self.ax3.set_xlabel('Episode') + self.ax3.set_ylabel('Steps') + self.ax3.grid(True, alpha=0.3) + self.length_line, = self.ax3.plot([], [], 'g-', linewidth=2) + + # 动作分布 + self.ax4.set_title('Action Distribution') + self.ax4.set_xlabel('Action Value') + self.ax4.set_ylabel('Frequency') + self.ax4.grid(True, alpha=0.3) + + # 轨迹 + self.ax5.set_title('End Effector Trajectory (XY)') + self.ax5.set_xlabel('X Position') + self.ax5.set_ylabel('Y Position') + self.ax5.grid(True, alpha=0.3) + + # 统计信息 + self.ax6.set_title('Training Statistics') + self.ax6.axis('off') + + plt.tight_layout() + plt.draw() + + def update(self, episode, reward, distance, length, actions=None, states=None, loss=0, q_val=0): + """更新图表""" + try: + # 记录数据 + self.episode_rewards.append(reward) + self.episode_distances.append(distance) + self.episode_lengths.append(length) + + if loss > 0: + self.loss_history.append(loss) + if q_val > 0: + self.q_history.append(q_val) + + episodes = list(range(len(self.episode_rewards))) + + # 更新奖励图表 + self.reward_line.set_data(episodes, self.episode_rewards) + self.ax1.relim() + self.ax1.autoscale_view() + + # 更新距离图表 + self.distance_line.set_data(episodes, self.episode_distances) + self.ax2.relim() + self.ax2.autoscale_view() + + # 更新步数图表 + self.length_line.set_data(episodes, self.episode_lengths) + self.ax3.relim() + self.ax3.autoscale_view() + + # 更新动作分布 + self.ax4.clear() + if actions is not None and len(actions) > 0: + actions_flat = np.array(actions).flatten() + if len(actions_flat) > 0: + self.ax4.hist(actions_flat, bins=20, alpha=0.7, color='blue', edgecolor='black') + self.ax4.set_title(f'Action Distribution (n={len(actions_flat)})') + self.ax4.grid(True, alpha=0.3) + + # 更新轨迹 + self.ax5.clear() + if states is not None and len(states) > 0: + states_arr = np.array(states) + if states_arr.shape[1] >= 15: + # 提取XY位置(索引12-13是末端X,Y) + ee_x = states_arr[:, 12] + ee_y = states_arr[:, 13] + + # 绘制轨迹 + self.ax5.scatter(ee_x, ee_y, c=range(len(ee_x)), cmap='viridis', s=10, alpha=0.6) + + # 标记起点和终点 + if len(ee_x) > 0: + self.ax5.scatter(ee_x[0], ee_y[0], c='green', s=100, marker='o', label='Start') + self.ax5.scatter(ee_x[-1], ee_y[-1], c='red', s=100, marker='*', label='End') + + # 标记目标位置(假设索引15-16是目标X,Y) + if states_arr.shape[1] >= 16: + target_x = states_arr[0, 15] + target_y = states_arr[0, 16] + self.ax5.scatter(target_x, target_y, c='orange', s=150, marker='X', label='Target') + + self.ax5.legend() + self.ax5.set_title(f'Trajectory (n={len(ee_x)})') + self.ax5.grid(True, alpha=0.3) + + # 更新统计信息 + self.ax6.clear() + self.ax6.axis('off') + + # 计算统计数据 + avg_reward = np.mean(self.episode_rewards[-10:]) if len(self.episode_rewards) >= 10 else np.mean(self.episode_rewards) + avg_distance = np.mean(self.episode_distances[-10:]) if len(self.episode_distances) >= 10 else np.mean(self.episode_distances) + + stats_text = ( + f"Episode: {episode}\n" + f"Reward: {reward:.2f}\n" + f"Distance: {distance:.3f}\n" + f"Length: {length}\n" + f"Avg Reward (last 10): {avg_reward:.2f}\n" + f"Avg Distance (last 10): {avg_distance:.3f}\n" + f"Exploration Rate: {1.0 * (0.995 ** episode):.3f}\n" + f"Total Episodes: {len(self.episode_rewards)}" + ) + + self.ax6.text(0.05, 0.95, stats_text, transform=self.ax6.transAxes, + fontsize=9, verticalalignment='top', + bbox=dict(boxstyle='round', facecolor='lightblue', alpha=0.7)) + + # 重绘 + self.fig.canvas.draw_idle() + self.fig.canvas.flush_events() + plt.pause(0.001) # 短暂暂停 + + except Exception as e: + print(f"更新图表时出错: {e}") + # 尝试重新初始化图表 + try: + plt.close(self.fig) + self.__init__() + except: + pass + + def save(self, filename): + """保存图表""" + try: + self.fig.savefig(filename, dpi=100, bbox_inches='tight') + print(f"图表已保存: {filename}") + except Exception as e: + print(f"保存图表失败: {e}") + + def close(self): + """关闭图表""" + plt.close(self.fig) + +# ============================ 4. 稳定的训练函数 ============================ + +def stable_training(): + """稳定的训练函数""" + print("\n" + "=" * 60) + print("开始稳定训练") + print("=" * 60) + + # 训练参数 + total_episodes = 50 + render_episodes = [0, 10, 20, 30, 40, 49] # 只在特定episode渲染 + + # 创建环境(初始不渲染) + print("创建环境...") + env = RobustArmEnv(render=False) + + # 创建代理 + print("创建代理...") + agent = SimpleAgent(env.state_dim, env.action_dim) + + # 创建图表系统 + print("创建图表系统...") + charts = FixedChartSystem() + + # 开始训练 + print(f"\n开始训练 {total_episodes} 个episodes...") + print("-" * 60) + + start_time = time.time() + + for episode in range(total_episodes): + print(f"Episode {episode+1}/{total_episodes}", end="") + + # 决定是否渲染 + if episode in render_episodes: + print(" [渲染]", end="") + env.start_viewer() + else: + env.render_enabled = False + + # 重置环境 + state = env.reset() + total_reward = 0 + episode_length = 0 + done = False + + # 运行一个episode + while not done: + # 选择动作 + action = agent.act(state, training=True) + + # 执行动作 + next_state, reward, done, info = env.step(action) + + # 记住经验 + agent.remember(state, action, reward, next_state, done) + + # 经验回放 + loss = agent.replay() + + # 更新状态 + state = next_state + total_reward += reward + episode_length += 1 + + # 获取最终距离 + final_distance = info.get("distance", 1.0) + + # 更新图表 + charts.update( + episode=episode+1, + reward=total_reward, + distance=final_distance, + length=episode_length, + actions=env.actions_history, + states=env.states_history, + loss=loss if 'loss' in locals() else 0 + ) + + # 打印进度 + print(f" | 奖励: {total_reward:6.2f} | 距离: {final_distance:.3f} | 步数: {episode_length:3d} | 探索率: {agent.epsilon:.3f}") + + # 定期保存 + if (episode + 1) % 10 == 0: + charts.save(f"training_progress_ep{episode+1}.png") + agent.save(f"agent_ep{episode+1}.npz") + + # 关闭渲染以节省资源 + if episode in render_episodes: + env.close() + + # 训练结束 + training_time = time.time() - start_time + + print("\n" + "=" * 60) + print(f"训练完成!") + print(f"总时间: {training_time:.1f}秒") + print(f"平均奖励: {np.mean(charts.episode_rewards):.2f}") + print(f"平均距离: {np.mean(charts.episode_distances):.3f}") + print(f"平均步数: {np.mean(charts.episode_lengths):.2f}") + print("=" * 60) + + # 保存最终结果 + charts.save("training_final.png") + agent.save("agent_final.npz") + + # 询问是否测试 + print("\n是否测试训练好的模型? (y/n): ", end="") + choice = input().strip().lower() + + if choice == 'y': + test_agent(env, agent, charts) + + # 关闭所有资源 + env.close() + charts.close() + + return agent + +def test_agent(env, agent, charts): + """测试代理""" + print("\n" + "=" * 60) + print("开始测试") + print("=" * 60) + + # 启用渲染 + env.start_viewer() + + # 设置代理为评估模式 + agent.epsilon = 0.0 + + test_episodes = 3 + + for test_ep in range(test_episodes): + print(f"\n测试 Episode {test_ep+1}/{test_episodes}") + + state = env.reset() + total_reward = 0 + done = False + step = 0 + + while not done and step < 100: + # 选择动作(无探索) + action = agent.act(state, training=False) + + # 执行动作 + state, reward, done, info = env.step(action) + + total_reward += reward + step += 1 + + # 慢速演示 + time.sleep(0.02) + + # 显示进度 + if step % 20 == 0: + distance = info.get("distance", 0) + print(f" 步数: {step:3d} | 距离: {distance:.3f} | 累计奖励: {total_reward:.2f}") + + # 最终结果 + final_distance = info.get("distance", 1.0) + print(f" 完成! 最终距离: {final_distance:.3f}, 总奖励: {total_reward:.2f}") + + time.sleep(1) + + print("\n测试完成!") + env.close() + +# ============================ 5. 主程序 ============================ + +def main(): + """主程序""" + print("机械臂强化学习可视化系统") + print("\n选项:") + print("1. 训练新模型") + print("2. 测试现有模型") + print("3. 仅查看图表演示") + + choice = input("请选择 (1/2/3): ").strip() + + if choice == "1": + # 训练模式 + print("\n开始训练模式...") + agent = stable_training() + + elif choice == "2": + # 测试模式 + print("\n开始测试模式...") + + # 检查模型文件 + model_file = "agent_final.npz" + if not os.path.exists(model_file): + print(f"模型文件 '{model_file}' 不存在!") + print("请先训练模型或提供正确的模型文件路径") + return + + # 创建环境 + env = RobustArmEnv(render=True) + + # 创建代理 + agent = SimpleAgent(env.state_dim, env.action_dim) + + # 加载模型 + if agent.load(model_file): + # 创建图表系统 + charts = FixedChartSystem() + + # 测试代理 + test_agent(env, agent, charts) + + # 关闭 + env.close() + charts.close() + + elif choice == "3": + # 图表演示模式 + print("\n图表演示模式...") + + # 创建图表系统 + charts = FixedChartSystem() + + # 生成示例数据 + print("生成示例数据...") + for i in range(50): + # 模拟数据 + reward = np.random.uniform(-10, 20) + i * 0.5 + distance = max(0.01, np.random.uniform(0, 0.5) - i * 0.01) + length = np.random.randint(50, 100) + + # 模拟动作和状态 + actions = np.random.uniform(-1, 1, (length, 6)) + states = np.random.uniform(-1, 1, (length, 18)) + + # 更新图表 + charts.update( + episode=i+1, + reward=reward, + distance=distance, + length=length, + actions=actions, + states=states + ) + + print(f"演示 Episode {i+1}/50 | 奖励: {reward:.2f} | 距离: {distance:.3f}") + time.sleep(0.05) + + # 保存演示图表 + charts.save("chart_demo.png") + + print("\n演示完成! 图表已保存为 'chart_demo.png'") + print("关闭图表窗口以退出...") + + # 保持图表显示 + plt.ioff() + plt.show() + + charts.close() + + else: + print("无效选择!") + +# ============================ 6. 程序入口 ============================ + +if __name__ == "__main__": + try: + main() + print("\n程序执行完成!") + except KeyboardInterrupt: + print("\n程序被用户中断") + except Exception as e: + print(f"\n程序执行出错: {e}") + import traceback + traceback.print_exc() + finally: + # 确保清理所有资源 + plt.close('all') + print("程序结束") \ No newline at end of file diff --git a/src/robotic_arm_grasping/arm_traj_opt_viz_sys.py b/src/robotic_arm_grasping/arm_traj_opt_viz_sys.py new file mode 100644 index 0000000000..0fec72b952 --- /dev/null +++ b/src/robotic_arm_grasping/arm_traj_opt_viz_sys.py @@ -0,0 +1,1654 @@ +""" +机械臂轨迹规划与优化可视化系统 +完整版本:包含RRT、PRM、A*等算法,以及丰富的可视化功能 +Author: AI Assistant +Date: 2024 +""" + +import numpy as np +import matplotlib.pyplot as plt +from matplotlib.patches import Circle, Rectangle, Polygon +from matplotlib.collections import PatchCollection +from mpl_toolkits.mplot3d import Axes3D +from mpl_toolkits.mplot3d.art3d import Poly3DCollection +import matplotlib.animation as animation +from scipy.spatial import KDTree, cKDTree +from scipy.interpolate import interp1d, splprep, splev +from scipy.optimize import minimize, Bounds +import networkx as nx +import heapq +import time +import json +import pickle +from queue import PriorityQueue +import warnings +warnings.filterwarnings('ignore') + +# ============================================================================ +# 1. 路径规划算法实现 +# ============================================================================ + +class RRTPlanner: + """快速扩展随机树(RRT)路径规划算法""" + + def __init__(self, start, goal, obstacles, bounds, step_size=0.1, max_iter=5000): + self.start = np.array(start) + self.goal = np.array(goal) + self.obstacles = obstacles + self.bounds = bounds # [xmin, xmax, ymin, ymax, zmin, zmax] + self.step_size = step_size + self.max_iter = max_iter + + self.nodes = [self.start] + self.parents = [None] + self.path = None + + def plan(self): + """执行RRT规划""" + for i in range(self.max_iter): + # 随机采样 + if np.random.random() < 0.1: # 10%概率直接采样目标点 + random_point = self.goal + else: + random_point = self._random_sample() + + # 找到最近的节点 + nearest_idx = self._nearest_neighbor(random_point) + nearest_node = self.nodes[nearest_idx] + + # 向随机点方向移动一步 + direction = random_point - nearest_node + distance = np.linalg.norm(direction) + if distance > 0: + direction = direction / distance + new_point = nearest_node + direction * min(self.step_size, distance) + + # 检查碰撞 + if not self._check_collision(nearest_node, new_point): + self.nodes.append(new_point) + self.parents.append(nearest_idx) + + # 检查是否到达目标 + if np.linalg.norm(new_point - self.goal) < self.step_size: + self._extract_path() + return True + + # 尝试连接最后一点到目标 + self._try_connect_to_goal() + return self.path is not None + + def _random_sample(self): + """在工作空间内随机采样""" + return np.array([ + np.random.uniform(self.bounds[0], self.bounds[1]), + np.random.uniform(self.bounds[2], self.bounds[3]), + np.random.uniform(self.bounds[4], self.bounds[5]) + ]) + + def _nearest_neighbor(self, point): + """找到最近的节点""" + distances = [np.linalg.norm(node - point) for node in self.nodes] + return np.argmin(distances) + + def _check_collision(self, point1, point2): + """检查线段是否与障碍物碰撞""" + # 简化碰撞检测:检查中间点 + num_checks = max(2, int(np.linalg.norm(point2 - point1) / 0.05)) + for i in range(num_checks + 1): + t = i / num_checks + check_point = point1 + t * (point2 - point1) + + for obstacle in self.obstacles: + if obstacle['type'] == 'sphere': + dist = np.linalg.norm(check_point - obstacle['center']) + if dist < obstacle['radius']: + return True + elif obstacle['type'] == 'box': + # 检查点是否在立方体内 + center = obstacle['center'] + size = obstacle['size'] + if (abs(check_point[0] - center[0]) <= size[0] and + abs(check_point[1] - center[1]) <= size[1] and + abs(check_point[2] - center[2]) <= size[2]): + return True + return False + + def _extract_path(self): + """从树中提取路径""" + path = [self.goal] + current_idx = len(self.nodes) - 1 + + while current_idx is not None: + path.append(self.nodes[current_idx]) + current_idx = self.parents[current_idx] + + self.path = list(reversed(path)) + + def _try_connect_to_goal(self): + """尝试连接最近的点到目标""" + distances = [np.linalg.norm(node - self.goal) for node in self.nodes] + nearest_idx = np.argmin(distances) + + if not self._check_collision(self.nodes[nearest_idx], self.goal): + self.nodes.append(self.goal) + self.parents.append(nearest_idx) + self._extract_path() + + +class PRMPlanner: + """概率路线图(PRM)路径规划算法""" + + def __init__(self, start, goal, obstacles, bounds, n_samples=1000, k_neighbors=10): + self.start = np.array(start) + self.goal = np.array(goal) + self.obstacles = obstacles + self.bounds = bounds + self.n_samples = n_samples + self.k_neighbors = k_neighbors + + self.samples = [] + self.graph = nx.Graph() + self.path = None + + def plan(self): + """执行PRM规划""" + # 1. 采样 + self._sample_configuration_space() + + # 2. 添加起点和终点 + self.samples.append(self.start) + self.samples.append(self.goal) + start_idx = len(self.samples) - 2 + goal_idx = len(self.samples) - 1 + + # 3. 构建KD树用于最近邻搜索 + kdtree = cKDTree(self.samples) + + # 4. 构建图 + for i, sample in enumerate(self.samples): + # 找到k个最近邻 + distances, indices = kdtree.query(sample, k=min(self.k_neighbors + 1, len(self.samples))) + + for j, neighbor_idx in enumerate(indices[1:]): # 跳过自己 + neighbor = self.samples[neighbor_idx] + + # 检查碰撞 + if not self._check_collision(sample, neighbor): + # 计算距离作为边的权重 + distance = np.linalg.norm(sample - neighbor) + self.graph.add_edge(i, neighbor_idx, weight=distance) + + # 5. 搜索路径 + try: + path_indices = nx.shortest_path(self.graph, start_idx, goal_idx, weight='weight') + self.path = [self.samples[i] for i in path_indices] + return True + except: + return False + + def _sample_configuration_space(self): + """在自由空间采样""" + for _ in range(self.n_samples): + while True: + sample = np.array([ + np.random.uniform(self.bounds[0], self.bounds[1]), + np.random.uniform(self.bounds[2], self.bounds[3]), + np.random.uniform(self.bounds[4], self.bounds[5]) + ]) + + # 检查采样点是否在自由空间 + if not self._point_in_obstacle(sample): + self.samples.append(sample) + break + + def _point_in_obstacle(self, point): + """检查点是否在障碍物内""" + for obstacle in self.obstacles: + if obstacle['type'] == 'sphere': + dist = np.linalg.norm(point - obstacle['center']) + if dist < obstacle['radius']: + return True + elif obstacle['type'] == 'box': + center = obstacle['center'] + size = obstacle['size'] + if (abs(point[0] - center[0]) <= size[0] and + abs(point[1] - center[1]) <= size[1] and + abs(point[2] - center[2]) <= size[2]): + return True + return False + + def _check_collision(self, point1, point2): + """检查线段是否与障碍物碰撞""" + # 与RRT相同的碰撞检测 + num_checks = max(2, int(np.linalg.norm(point2 - point1) / 0.05)) + for i in range(num_checks + 1): + t = i / num_checks + check_point = point1 + t * (point2 - point1) + + for obstacle in self.obstacles: + if obstacle['type'] == 'sphere': + dist = np.linalg.norm(check_point - obstacle['center']) + if dist < obstacle['radius']: + return True + elif obstacle['type'] == 'box': + center = obstacle['center'] + size = obstacle['size'] + if (abs(check_point[0] - center[0]) <= size[0] and + abs(check_point[1] - center[1]) <= size[1] and + abs(check_point[2] - center[2]) <= size[2]): + return True + return False + + +class AStarPlanner: + """A*网格搜索路径规划算法""" + + def __init__(self, start, goal, obstacles, bounds, grid_size=0.1): + self.start = np.array(start) + self.goal = np.array(goal) + self.obstacles = obstacles + self.bounds = bounds + self.grid_size = grid_size + + # 创建网格 + x_grid = int((bounds[1] - bounds[0]) / grid_size) + y_grid = int((bounds[3] - bounds[2]) / grid_size) + z_grid = int((bounds[5] - bounds[4]) / grid_size) + + self.grid_shape = (x_grid, y_grid, z_grid) + self.grid = np.zeros(self.grid_shape, dtype=bool) # True表示障碍物 + + self._initialize_grid() + self.path = None + + def _initialize_grid(self): + """初始化网格障碍物""" + # 将障碍物标记在网格上 + for i in range(self.grid_shape[0]): + for j in range(self.grid_shape[1]): + for k in range(self.grid_shape[2]): + world_pos = self._grid_to_world((i, j, k)) + + # 检查是否在障碍物内 + if self._point_in_obstacle(world_pos): + self.grid[i, j, k] = True + + def _grid_to_world(self, grid_pos): + """网格坐标转世界坐标""" + return np.array([ + self.bounds[0] + grid_pos[0] * self.grid_size, + self.bounds[2] + grid_pos[1] * self.grid_size, + self.bounds[4] + grid_pos[2] * self.grid_size + ]) + + def _world_to_grid(self, world_pos): + """世界坐标转网格坐标""" + return ( + int((world_pos[0] - self.bounds[0]) / self.grid_size), + int((world_pos[1] - self.bounds[2]) / self.grid_size), + int((world_pos[2] - self.bounds[4]) / self.grid_size) + ) + + def _point_in_obstacle(self, point): + """检查点是否在障碍物内""" + for obstacle in self.obstacles: + if obstacle['type'] == 'sphere': + dist = np.linalg.norm(point - obstacle['center']) + if dist < obstacle['radius']: + return True + elif obstacle['type'] == 'box': + center = obstacle['center'] + size = obstacle['size'] + if (abs(point[0] - center[0]) <= size[0] and + abs(point[1] - center[1]) <= size[1] and + abs(point[2] - center[2]) <= size[2]): + return True + return False + + def plan(self): + """执行A*规划""" + start_grid = self._world_to_grid(self.start) + goal_grid = self._world_to_grid(self.goal) + + # 检查起点和终点是否有效 + if (self._is_out_of_bounds(start_grid) or self._is_out_of_bounds(goal_grid) or + self.grid[start_grid] or self.grid[goal_grid]): + return False + + # A*算法 + open_set = [] + heapq.heappush(open_set, (0, start_grid)) + + came_from = {} + g_score = {start_grid: 0} + f_score = {start_grid: self._heuristic(start_grid, goal_grid)} + + # 26个方向的邻居(3D Moore邻域) + directions = [(dx, dy, dz) for dx in (-1, 0, 1) + for dy in (-1, 0, 1) + for dz in (-1, 0, 1) if (dx, dy, dz) != (0, 0, 0)] + + while open_set: + _, current = heapq.heappop(open_set) + + if current == goal_grid: + self._reconstruct_path(came_from, current) + return True + + for dx, dy, dz in directions: + neighbor = (current[0] + dx, current[1] + dy, current[2] + dz) + + # 检查邻居是否有效 + if (self._is_out_of_bounds(neighbor) or self.grid[neighbor]): + continue + + # 计算移动成本(对角线成本较高) + move_cost = np.sqrt(dx*dx + dy*dy + dz*dz) + tentative_g_score = g_score[current] + move_cost + + if neighbor not in g_score or tentative_g_score < g_score[neighbor]: + came_from[neighbor] = current + g_score[neighbor] = tentative_g_score + f_score[neighbor] = tentative_g_score + self._heuristic(neighbor, goal_grid) + heapq.heappush(open_set, (f_score[neighbor], neighbor)) + + return False + + def _is_out_of_bounds(self, grid_pos): + """检查网格坐标是否越界""" + return (grid_pos[0] < 0 or grid_pos[0] >= self.grid_shape[0] or + grid_pos[1] < 0 or grid_pos[1] >= self.grid_shape[1] or + grid_pos[2] < 0 or grid_pos[2] >= self.grid_shape[2]) + + def _heuristic(self, a, b): + """启发式函数(欧几里得距离)""" + return np.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2 + (a[2]-b[2])**2) + + def _reconstruct_path(self, came_from, current): + """重建路径""" + path_grid = [current] + while current in came_from: + current = came_from[current] + path_grid.append(current) + + path_grid.reverse() + self.path = [self._grid_to_world(pos) for pos in path_grid] + + +# ============================================================================ +# 2. 轨迹优化算法 +# ============================================================================ + +class TrajectoryOptimizer: + """轨迹优化器""" + + def __init__(self, path): + self.path = np.array(path) + + def smooth_path(self, weight_data=0.1, weight_smooth=0.3, tolerance=0.00001): + """使用梯度下降平滑路径""" + if len(self.path) < 3: + return self.path + + smoothed = self.path.copy() + + change = tolerance + while change >= tolerance: + change = 0.0 + + for i in range(1, len(self.path) - 1): + for dim in range(3): + original = smoothed[i, dim] + + # 数据项:保持接近原始点 + data_term = weight_data * (self.path[i, dim] - smoothed[i, dim]) + + # 平滑项:保持相邻点接近 + smooth_term = weight_smooth * (smoothed[i-1, dim] + smoothed[i+1, dim] - 2 * smoothed[i, dim]) + + smoothed[i, dim] += data_term + smooth_term + change += abs(original - smoothed[i, dim]) + + return smoothed + + def interpolate_cubic_spline(self, num_points=100): + """三次样条插值""" + if len(self.path) < 4: + # 如果点太少,使用线性插值 + t = np.linspace(0, 1, len(self.path)) + t_new = np.linspace(0, 1, num_points) + + spline_x = interp1d(t, self.path[:, 0], kind='cubic') + spline_y = interp1d(t, self.path[:, 1], kind='cubic') + spline_z = interp1d(t, self.path[:, 2], kind='cubic') + + else: + # 参数化路径 + t = np.arange(len(self.path)) + t_new = np.linspace(0, len(self.path)-1, num_points) + + spline_x = interp1d(t, self.path[:, 0], kind='cubic') + spline_y = interp1d(t, self.path[:, 1], kind='cubic') + spline_z = interp1d(t, self.path[:, 2], kind='cubic') + + interpolated = np.column_stack([ + spline_x(t_new), + spline_y(t_new), + spline_z(t_new) + ]) + + return interpolated + + def minimize_jerk(self, total_time=5.0, dt=0.01): + """最小化加加速度(jerk)轨迹""" + n_points = len(self.path) + t = np.linspace(0, total_time, n_points) + + # 创建五次多项式轨迹(最小化jerk) + coefficients = [] + for dim in range(3): + # 解决边界条件:位置、速度、加速度在起点和终点都为0 + A = np.zeros((6, 6)) + b = np.zeros(6) + + # 位置约束 + A[0, :] = [t[0]**i for i in range(6)] # 起点位置 + b[0] = self.path[0, dim] + A[1, :] = [t[-1]**i for i in range(6)] # 终点位置 + b[1] = self.path[-1, dim] + + # 速度约束(起点和终点速度为0) + A[2, :] = [i * t[0]**(i-1) if i > 0 else 0 for i in range(6)] # 起点速度 + b[2] = 0 + A[3, :] = [i * t[-1]**(i-1) if i > 0 else 0 for i in range(6)] # 终点速度 + b[3] = 0 + + # 加速度约束(起点和终点加速度为0) + A[4, :] = [i * (i-1) * t[0]**(i-2) if i > 1 else 0 for i in range(6)] # 起点加速度 + b[4] = 0 + A[5, :] = [i * (i-1) * t[-1]**(i-2) if i > 1 else 0 for i in range(6)] # 终点加速度 + b[5] = 0 + + coeff = np.linalg.lstsq(A, b, rcond=None)[0] + coefficients.append(coeff) + + # 生成轨迹 + time_points = np.arange(0, total_time, dt) + n_steps = len(time_points) + + position = np.zeros((n_steps, 3)) + velocity = np.zeros((n_steps, 3)) + acceleration = np.zeros((n_steps, 3)) + jerk = np.zeros((n_steps, 3)) + + for i, t in enumerate(time_points): + for dim in range(3): + coeff = coefficients[dim] + # 位置 + position[i, dim] = sum(coeff[j] * t**j for j in range(6)) + # 速度 + velocity[i, dim] = sum(j * coeff[j] * t**(j-1) for j in range(1, 6)) + # 加速度 + acceleration[i, dim] = sum(j * (j-1) * coeff[j] * t**(j-2) for j in range(2, 6)) + # 加加速度 + jerk[i, dim] = sum(j * (j-1) * (j-2) * coeff[j] * t**(j-3) for j in range(3, 6)) + + return position, velocity, acceleration, jerk, time_points + + def optimize_for_energy(self, total_time=5.0, dt=0.01): + """能量优化轨迹""" + # 简化的能量优化:最小化加速度平方和(与能量消耗相关) + n_points = len(self.path) + + def cost_function(params): + # params: 每个中间点的时间分配 + times = np.cumsum(np.concatenate([[0], params, [total_time - np.sum(params)]])) + + total_energy = 0 + for dim in range(3): + # 计算样条插值 + tck, u = splprep([self.path[:, dim]], u=times, s=0) + + # 计算加速度 + der = splev(u, tck, der=2) + + # 能量近似为加速度平方的积分 + total_energy += np.trapz(der[0]**2, times) + + return total_energy + + # 初始时间分配(均匀分布) + initial_params = np.ones(n_points-2) * (total_time / (n_points-1)) + + # 边界条件:每段时间必须为正 + bounds = Bounds(0.01, total_time) + + # 约束:总时间不能超过total_time + constraints = [{ + 'type': 'ineq', + 'fun': lambda x: total_time - np.sum(x) - 0.01 + }] + + result = minimize(cost_function, initial_params, + bounds=bounds, constraints=constraints, + method='SLSQP') + + if result.success: + optimal_times = np.cumsum(np.concatenate([[0], result.x, [total_time - np.sum(result.x)]])) + + # 使用优化后的时间生成轨迹 + trajectories = [] + for dim in range(3): + tck, u = splprep([self.path[:, dim]], u=optimal_times, s=0) + trajectories.append(tck) + + # 在更密集的时间点上采样 + time_points = np.linspace(0, total_time, int(total_time/dt)) + + position = np.zeros((len(time_points), 3)) + velocity = np.zeros((len(time_points), 3)) + acceleration = np.zeros((len(time_points), 3)) + + for i, t in enumerate(time_points): + for dim in range(3): + pos = splev(t, trajectories[dim], der=0) + vel = splev(t, trajectories[dim], der=1) + acc = splev(t, trajectories[dim], der=2) + + position[i, dim] = pos[0] + velocity[i, dim] = vel[0] + acceleration[i, dim] = acc[0] + + return position, velocity, acceleration, time_points + + else: + # 优化失败,返回最小jerk轨迹 + return self.minimize_jerk(total_time, dt)[:4] + + +# ============================================================================ +# 3. 逆运动学求解器(简化版本) +# ============================================================================ + +class SimplifiedInverseKinematics: + """简化逆运动学求解器(适用于6自由度机械臂)""" + + def __init__(self, link_lengths=[0.35, 0.3, 0.15, 0.12, 0.05]): + # 连杆长度:[上臂, 前臂, 腕部1, 腕部2, 末端] + self.link_lengths = link_lengths + self.total_length = sum(link_lengths) + + def solve(self, target_position, target_orientation=None): + """求解逆运动学(简化几何方法)""" + x, y, z = target_position + + # 1. 计算基座旋转(关节1) + theta1 = np.arctan2(y, x) + + # 2. 计算距离和高度 + distance_xy = np.sqrt(x**2 + y**2) + height = z + + # 3. 考虑机械臂的基座高度 + base_height = 0.1 + effective_height = height - base_height + + # 4. 简化逆运动学(平面2连杆) + L1 = self.link_lengths[0] # 上臂 + L2 = self.link_lengths[1] # 前臂 + + # 计算到目标的距离 + D = np.sqrt(distance_xy**2 + effective_height**2) + + # 检查是否可达 + if D > L1 + L2 or D < abs(L1 - L2): + # 不可达,返回最接近的配置 + theta2 = np.pi/2 # 默认垂直 + theta3 = 0 + else: + # 计算关节角度 + # 肘部角度(关节3) + cos_theta3 = (D**2 - L1**2 - L2**2) / (2 * L1 * L2) + cos_theta3 = np.clip(cos_theta3, -1, 1) + theta3 = np.arccos(cos_theta3) + + # 肩部角度(关节2) + alpha = np.arctan2(effective_height, distance_xy) + beta = np.arcsin(L2 * np.sin(theta3) / D) + theta2 = alpha - beta + + # 5. 腕部角度(简化) + theta4 = 0 # 腕部旋转 + theta5 = np.pi/2 # 腕部俯仰(默认垂直向下) + theta6 = 0 # 腕部横滚 + + # 限制关节角度范围 + joint_angles = np.array([theta1, theta2, theta3, theta4, theta5, theta6]) + + # 应用关节限制(根据您的模型) + limits = [ + (-np.pi, np.pi), # 关节1 + (-2.0, 2.0), # 关节2 + (-2.5, 0.5), # 关节3 + (-2.0, 2.0), # 关节4 + (-1.8, 1.8), # 关节5 + (-1.8, 1.8) # 关节6 + ] + + for i in range(6): + joint_angles[i] = np.clip(joint_angles[i], limits[i][0], limits[i][1]) + + return joint_angles + + def forward_kinematics(self, joint_angles): + """正运动学计算末端位置""" + theta1, theta2, theta3, theta4, theta5, theta6 = joint_angles + + # 简化正运动学(不考虑所有连杆偏移) + L1 = self.link_lengths[0] + L2 = self.link_lengths[1] + L3 = self.link_lengths[2] + L4 = self.link_lengths[3] + L5 = self.link_lengths[4] + + # 计算位置 + x = (L1 * np.sin(theta2) + L2 * np.sin(theta2 + theta3) + + L3 * np.sin(theta2 + theta3) + + L4 * np.sin(theta2 + theta3) + + L5 * np.sin(theta2 + theta3)) * np.cos(theta1) + + y = (L1 * np.sin(theta2) + L2 * np.sin(theta2 + theta3) + + L3 * np.sin(theta2 + theta3) + + L4 * np.sin(theta2 + theta3) + + L5 * np.sin(theta2 + theta3)) * np.sin(theta1) + + z = 0.1 + (L1 * np.cos(theta2) + L2 * np.cos(theta2 + theta3) + + L3 * np.cos(theta2 + theta3) + + L4 * np.cos(theta2 + theta3) + + L5 * np.cos(theta2 + theta3)) + + return np.array([x, y, z]) + + +# ============================================================================ +# 4. 可视化系统 +# ============================================================================ + +class TrajectoryPlanningVisualizer: + """轨迹规划可视化系统""" + + def __init__(self): + self.fig = plt.figure(figsize=(20, 12)) + self.setup_visualization() + + def setup_visualization(self): + """设置可视化面板""" + gs = self.fig.add_gridspec(3, 4) + + # 1. 3D工作空间与轨迹 + self.ax_3d = self.fig.add_subplot(gs[:2, :2], projection='3d') + self.ax_3d.set_title('工作空间与轨迹规划', fontsize=14, fontweight='bold') + self.ax_3d.set_xlabel('X (m)') + self.ax_3d.set_ylabel('Y (m)') + self.ax_3d.set_zlabel('Z (m)') + self.ax_3d.grid(True, alpha=0.3) + + # 2. XY平面投影 + self.ax_xy = self.fig.add_subplot(gs[0, 2]) + self.ax_xy.set_title('XY平面投影', fontsize=12, fontweight='bold') + self.ax_xy.set_xlabel('X (m)') + self.ax_xy.set_ylabel('Y (m)') + self.ax_xy.grid(True, alpha=0.3) + + # 3. XZ平面投影 + self.ax_xz = self.fig.add_subplot(gs[0, 3]) + self.ax_xz.set_title('XZ平面投影', fontsize=12, fontweight='bold') + self.ax_xz.set_xlabel('X (m)') + self.ax_xz.set_ylabel('Z (m)') + self.ax_xz.grid(True, alpha=0.3) + + # 4. YZ平面投影 + self.ax_yz = self.fig.add_subplot(gs[1, 2]) + self.ax_yz.set_title('YZ平面投影', fontsize=12, fontweight='bold') + self.ax_yz.set_xlabel('Y (m)') + self.ax_yz.set_ylabel('Z (m)') + self.ax_yz.grid(True, alpha=0.3) + + # 5. 关节角度轨迹 + self.ax_joints = self.fig.add_subplot(gs[1, 3]) + self.ax_joints.set_title('关节角度轨迹', fontsize=12, fontweight='bold') + self.ax_joints.set_xlabel('时间步') + self.ax_joints.set_ylabel('角度 (rad)') + self.ax_joints.grid(True, alpha=0.3) + + # 6. 速度曲线 + self.ax_velocity = self.fig.add_subplot(gs[2, 0]) + self.ax_velocity.set_title('关节速度', fontsize=12, fontweight='bold') + self.ax_velocity.set_xlabel('时间步') + self.ax_velocity.set_ylabel('速度 (rad/s)') + self.ax_velocity.grid(True, alpha=0.3) + + # 7. 加速度曲线 + self.ax_acceleration = self.fig.add_subplot(gs[2, 1]) + self.ax_acceleration.set_title('关节加速度', fontsize=12, fontweight='bold') + self.ax_acceleration.set_xlabel('时间步') + self.ax_acceleration.set_ylabel('加速度 (rad/s²)') + self.ax_acceleration.grid(True, alpha=0.3) + + # 8. 能量消耗 + self.ax_energy = self.fig.add_subplot(gs[2, 2]) + self.ax_energy.set_title('能量消耗', fontsize=12, fontweight='bold') + self.ax_energy.set_xlabel('时间步') + self.ax_energy.set_ylabel('能量 (J)') + self.ax_energy.grid(True, alpha=0.3) + + # 9. 算法性能比较 + self.ax_comparison = self.fig.add_subplot(gs[2, 3]) + self.ax_comparison.set_title('算法性能比较', fontsize=12, fontweight='bold') + self.ax_comparison.set_xlabel('算法') + self.ax_comparison.set_ylabel('性能指标') + self.ax_comparison.grid(True, alpha=0.3) + + plt.tight_layout() + + def plot_3d_workspace(self, obstacles, start, goal, path=None, + tree_nodes=None, tree_edges=None, show_obstacles=True): + """绘制3D工作空间""" + self.ax_3d.clear() + + if show_obstacles: + # 绘制障碍物 + for obstacle in obstacles: + if obstacle['type'] == 'sphere': + self._draw_sphere_3d(obstacle['center'], obstacle['radius'], + color='red', alpha=0.3) + elif obstacle['type'] == 'box': + self._draw_box_3d(obstacle['center'], obstacle['size'], + color='red', alpha=0.3) + + # 绘制起点和终点 + self.ax_3d.scatter(start[0], start[1], start[2], + color='green', s=200, marker='o', label='起点') + self.ax_3d.scatter(goal[0], goal[1], goal[2], + color='red', s=200, marker='*', label='终点') + + # 绘制RRT树(如果提供) + if tree_nodes is not None: + nodes_array = np.array(tree_nodes) + self.ax_3d.scatter(nodes_array[:, 0], nodes_array[:, 1], nodes_array[:, 2], + color='lightblue', s=10, alpha=0.5, label='RRT节点') + + if tree_edges is not None: + for edge in tree_edges: + points = np.array(edge) + self.ax_3d.plot(points[:, 0], points[:, 1], points[:, 2], + 'gray', alpha=0.3, linewidth=0.5) + + # 绘制路径 + if path is not None: + path_array = np.array(path) + self.ax_3d.plot(path_array[:, 0], path_array[:, 1], path_array[:, 2], + 'orange', linewidth=3, label='路径') + self.ax_3d.scatter(path_array[:, 0], path_array[:, 1], path_array[:, 2], + color='orange', s=30, alpha=0.7) + + # 设置坐标轴范围 + all_points = [] + if path is not None: + all_points.extend(path) + if tree_nodes is not None: + all_points.extend(tree_nodes) + + if all_points: + all_points = np.array(all_points) + min_coords = all_points.min(axis=0) - 0.1 + max_coords = all_points.max(axis=0) + 0.1 + + self.ax_3d.set_xlim(min_coords[0], max_coords[0]) + self.ax_3d.set_ylim(min_coords[1], max_coords[1]) + self.ax_3d.set_zlim(min_coords[2], max_coords[2]) + + self.ax_3d.legend(loc='upper right') + self.ax_3d.set_title('工作空间与轨迹规划') + self.ax_3d.grid(True, alpha=0.3) + + def plot_projection_views(self, path, obstacles=None): + """绘制投影视图""" + if path is None or len(path) == 0: + return + + path_array = np.array(path) + + # XY投影 + self.ax_xy.clear() + self.ax_xy.plot(path_array[:, 0], path_array[:, 1], 'b-', linewidth=2) + self.ax_xy.scatter(path_array[:, 0], path_array[:, 1], c=range(len(path_array)), + cmap='viridis', s=30, alpha=0.7) + self.ax_xy.scatter(path_array[0, 0], path_array[0, 1], color='green', s=100, marker='o') + self.ax_xy.scatter(path_array[-1, 0], path_array[-1, 1], color='red', s=100, marker='*') + self.ax_xy.set_title('XY平面投影') + self.ax_xy.grid(True, alpha=0.3) + self.ax_xy.axis('equal') + + # XZ投影 + self.ax_xz.clear() + self.ax_xz.plot(path_array[:, 0], path_array[:, 2], 'g-', linewidth=2) + self.ax_xz.scatter(path_array[:, 0], path_array[:, 2], c=range(len(path_array)), + cmap='viridis', s=30, alpha=0.7) + self.ax_xz.scatter(path_array[0, 0], path_array[0, 2], color='green', s=100, marker='o') + self.ax_xz.scatter(path_array[-1, 0], path_array[-1, 2], color='red', s=100, marker='*') + self.ax_xz.set_title('XZ平面投影') + self.ax_xz.grid(True, alpha=0.3) + self.ax_xz.axis('equal') + + # YZ投影 + self.ax_yz.clear() + self.ax_yz.plot(path_array[:, 1], path_array[:, 2], 'r-', linewidth=2) + self.ax_yz.scatter(path_array[:, 1], path_array[:, 2], c=range(len(path_array)), + cmap='viridis', s=30, alpha=0.7) + self.ax_yz.scatter(path_array[0, 1], path_array[0, 2], color='green', s=100, marker='o') + self.ax_yz.scatter(path_array[-1, 1], path_array[-1, 2], color='red', s=100, marker='*') + self.ax_yz.set_title('YZ平面投影') + self.ax_yz.grid(True, alpha=0.3) + self.ax_yz.axis('equal') + + def plot_joint_trajectories(self, joint_trajectory, time_points=None): + """绘制关节角度轨迹""" + self.ax_joints.clear() + + if joint_trajectory is None or len(joint_trajectory) == 0: + return + + joint_array = np.array(joint_trajectory) + n_joints = joint_array.shape[1] + + if time_points is None: + time_points = range(len(joint_trajectory)) + + colors = plt.cm.tab10(np.linspace(0, 1, n_joints)) + + for i in range(n_joints): + self.ax_joints.plot(time_points, joint_array[:, i], + color=colors[i], linewidth=2, label=f'关节 {i+1}') + + self.ax_joints.legend(loc='upper right') + self.ax_joints.set_title('关节角度轨迹') + self.ax_joints.set_xlabel('时间步') + self.ax_joints.set_ylabel('角度 (rad)') + self.ax_joints.grid(True, alpha=0.3) + + def plot_velocity_acceleration(self, velocity, acceleration, time_points=None): + """绘制速度和加速度曲线""" + if velocity is None or acceleration is None: + return + + vel_array = np.array(velocity) + acc_array = np.array(acceleration) + n_joints = vel_array.shape[1] + + if time_points is None: + time_points = range(len(velocity)) + + colors = plt.cm.tab10(np.linspace(0, 1, n_joints)) + + # 速度曲线 + self.ax_velocity.clear() + for i in range(n_joints): + self.ax_velocity.plot(time_points, vel_array[:, i], + color=colors[i], linewidth=1.5, alpha=0.7, label=f'关节 {i+1}') + self.ax_velocity.axhline(y=0, color='k', linestyle='--', alpha=0.3) + self.ax_velocity.set_title('关节速度') + self.ax_velocity.set_xlabel('时间步') + self.ax_velocity.set_ylabel('速度 (rad/s)') + self.ax_velocity.legend(loc='upper right', fontsize=8) + self.ax_velocity.grid(True, alpha=0.3) + + # 加速度曲线 + self.ax_acceleration.clear() + for i in range(n_joints): + self.ax_acceleration.plot(time_points, acc_array[:, i], + color=colors[i], linewidth=1.5, alpha=0.7, label=f'关节 {i+1}') + self.ax_acceleration.axhline(y=0, color='k', linestyle='--', alpha=0.3) + self.ax_acceleration.set_title('关节加速度') + self.ax_acceleration.set_xlabel('时间步') + self.ax_acceleration.set_ylabel('加速度 (rad/s²)') + self.ax_acceleration.legend(loc='upper right', fontsize=8) + self.ax_acceleration.grid(True, alpha=0.3) + + def plot_energy_consumption(self, velocity, acceleration, torque=None, time_points=None): + """绘制能量消耗曲线""" + if velocity is None or acceleration is None: + return + + vel_array = np.array(velocity) + acc_array = np.array(acceleration) + + if time_points is None: + time_points = range(len(velocity)) + + # 计算动能(假设所有连杆质量相同) + kinetic_energy = 0.5 * np.sum(vel_array**2, axis=1) + + # 计算功率(速度 × 加速度的绝对值,简化模型) + if torque is not None: + torque_array = np.array(torque) + power = np.abs(vel_array * torque_array) + else: + # 简化的功率计算 + power = np.abs(vel_array * acc_array) + + total_power = np.sum(power, axis=1) + + # 积分得到能量 + if len(time_points) > 1: + dt = time_points[1] - time_points[0] if len(time_points) > 1 else 0.01 + else: + dt = 0.01 + + energy = np.cumsum(total_power) * dt + + self.ax_energy.clear() + + # 绘制能量曲线 + self.ax_energy.plot(time_points, energy, 'b-', linewidth=2, label='累计能量') + self.ax_energy.fill_between(time_points, 0, energy, alpha=0.3, color='blue') + + # 绘制功率曲线(次坐标轴) + ax2 = self.ax_energy.twinx() + ax2.plot(time_points, total_power, 'r--', linewidth=1.5, alpha=0.7, label='瞬时功率') + ax2.set_ylabel('功率 (W)', color='red') + ax2.tick_params(axis='y', labelcolor='red') + + # 添加图例 + lines1, labels1 = self.ax_energy.get_legend_handles_labels() + lines2, labels2 = ax2.get_legend_handles_labels() + self.ax_energy.legend(lines1 + lines2, labels1 + labels2, loc='upper left') + + self.ax_energy.set_title(f'能量消耗: {energy[-1]:.3f} J') + self.ax_energy.set_xlabel('时间步') + self.ax_energy.set_ylabel('能量 (J)', color='blue') + self.ax_energy.tick_params(axis='y', labelcolor='blue') + self.ax_energy.grid(True, alpha=0.3) + + def plot_algorithm_comparison(self, algorithm_results): + """绘制算法性能比较""" + self.ax_comparison.clear() + + algorithms = list(algorithm_results.keys()) + metrics = ['计算时间', '路径长度', '平滑度', '能量消耗'] + + # 归一化数据 + normalized_data = {} + for metric in metrics: + values = [algorithm_results[algo].get(metric, 0) for algo in algorithms] + if max(values) > 0: + normalized = [v / max(values) for v in values] + else: + normalized = values + normalized_data[metric] = normalized + + # 绘制雷达图 + angles = np.linspace(0, 2*np.pi, len(metrics), endpoint=False).tolist() + angles += angles[:1] # 闭合图形 + + for i, algo in enumerate(algorithms): + values = [normalized_data[metric][i] for metric in metrics] + values += values[:1] # 闭合图形 + + self.ax_comparison.plot(angles, values, 'o-', linewidth=2, + label=algo, markersize=8) + + # 设置角度标签 + self.ax_comparison.set_xticks(angles[:-1]) + self.ax_comparison.set_xticklabels(metrics) + self.ax_comparison.set_ylim(0, 1) + self.ax_comparison.set_title('算法性能比较雷达图') + self.ax_comparison.legend(loc='upper right') + self.ax_comparison.grid(True, alpha=0.3) + + def _draw_sphere_3d(self, center, radius, color='red', alpha=0.3): + """绘制3D球体""" + u = np.linspace(0, 2 * np.pi, 20) + v = np.linspace(0, np.pi, 20) + + x = radius * np.outer(np.cos(u), np.sin(v)) + center[0] + y = radius * np.outer(np.sin(u), np.sin(v)) + center[1] + z = radius * np.outer(np.ones(np.size(u)), np.cos(v)) + center[2] + + self.ax_3d.plot_surface(x, y, z, color=color, alpha=alpha, linewidth=0) + + def _draw_box_3d(self, center, size, color='red', alpha=0.3): + """绘制3D立方体""" + # 定义立方体的8个顶点 + x = center[0] + y = center[1] + z = center[2] + sx = size[0] + sy = size[1] + sz = size[2] + + vertices = np.array([ + [x-sx, y-sy, z-sz], [x+sx, y-sy, z-sz], + [x+sx, y+sy, z-sz], [x-sx, y+sy, z-sz], + [x-sx, y-sy, z+sz], [x+sx, y-sy, z+sz], + [x+sx, y+sy, z+sz], [x-sx, y+sy, z+sz] + ]) + + # 定义立方体的6个面 + faces = [ + [vertices[0], vertices[1], vertices[2], vertices[3]], # 底面 + [vertices[4], vertices[5], vertices[6], vertices[7]], # 顶面 + [vertices[0], vertices[1], vertices[5], vertices[4]], # 前面 + [vertices[2], vertices[3], vertices[7], vertices[6]], # 后面 + [vertices[1], vertices[2], vertices[6], vertices[5]], # 右面 + [vertices[0], vertices[3], vertices[7], vertices[4]] # 左面 + ] + + # 绘制立方体 + poly3d = Poly3DCollection(faces, alpha=alpha, linewidths=1, edgecolors='darkred') + poly3d.set_facecolor(color) + self.ax_3d.add_collection3d(poly3d) + + def save_plot(self, filename): + """保存图表""" + plt.savefig(filename, dpi=150, bbox_inches='tight') + print(f"图表已保存: {filename}") + + +# ============================================================================ +# 5. 主程序 - 轨迹规划演示系统 +# ============================================================================ + +class TrajectoryPlanningDemo: + """轨迹规划演示系统""" + + def __init__(self): + self.visualizer = TrajectoryPlanningVisualizer() + + # 定义工作空间边界 + self.bounds = [-0.8, 0.8, -0.8, 0.8, 0.0, 1.0] # [xmin, xmax, ymin, ymax, zmin, zmax] + + # 定义障碍物 + self.obstacles = [ + {'type': 'sphere', 'center': [0.3, 0.3, 0.3], 'radius': 0.15}, + {'type': 'sphere', 'center': [-0.4, -0.3, 0.4], 'radius': 0.12}, + {'type': 'box', 'center': [0.0, 0.0, 0.6], 'size': [0.2, 0.2, 0.1]}, + {'type': 'box', 'center': [0.5, -0.5, 0.2], 'size': [0.15, 0.15, 0.3]} + ] + + # 定义起点和终点 + self.start = np.array([-0.6, -0.6, 0.1]) + self.goal = np.array([0.6, 0.6, 0.8]) + + # 逆运动学求解器 + self.ik_solver = SimplifiedInverseKinematics() + + # 存储结果 + self.results = {} + + def run_all_algorithms(self): + """运行所有规划算法""" + print("=" * 60) + print("机械臂轨迹规划与优化演示系统") + print("=" * 60) + + algorithms = [ + ('RRT', self.run_rrt), + ('PRM', self.run_prm), + ('A*', self.run_astar) + ] + + for algo_name, algo_func in algorithms: + print(f"\n运行{algo_name}算法...") + success = algo_func() + + if success: + print(f" ✓ {algo_name}规划成功") + else: + print(f" ✗ {algo_name}规划失败") + + # 比较算法性能 + self.compare_algorithms() + + # 显示可视化 + plt.show() + + def run_rrt(self): + """运行RRT算法""" + start_time = time.time() + + # 创建RRT规划器 + rrt = RRTPlanner(self.start, self.goal, self.obstacles, + self.bounds, step_size=0.1, max_iter=3000) + + # 执行规划 + success = rrt.plan() + + if success: + # 优化轨迹 + optimizer = TrajectoryOptimizer(rrt.path) + smoothed_path = optimizer.smooth_path() + interpolated_path = optimizer.interpolate_cubic_spline(num_points=100) + + # 计算逆运动学 + joint_trajectory = [] + for point in interpolated_path: + joint_angles = self.ik_solver.solve(point) + joint_trajectory.append(joint_angles) + + # 计算最小jerk轨迹 + position, velocity, acceleration, jerk, time_points = optimizer.minimize_jerk() + + # 存储结果 + self.results['RRT'] = { + 'path': rrt.path, + 'smoothed_path': smoothed_path, + 'interpolated_path': interpolated_path, + 'joint_trajectory': joint_trajectory, + 'velocity': velocity, + 'acceleration': acceleration, + 'jerk': jerk, + 'time_points': time_points, + 'computation_time': time.time() - start_time, + 'path_length': self._calculate_path_length(interpolated_path), + 'smoothness': self._calculate_smoothness(interpolated_path) + } + + # 可视化 + self.visualizer.plot_3d_workspace( + self.obstacles, self.start, self.goal, + path=interpolated_path, + tree_nodes=rrt.nodes, + tree_edges=None + ) + + self.visualizer.plot_projection_views(interpolated_path) + + if len(joint_trajectory) > 0: + self.visualizer.plot_joint_trajectories(joint_trajectory) + self.visualizer.plot_velocity_acceleration(velocity, acceleration) + self.visualizer.plot_energy_consumption(velocity, acceleration) + + return True + + return False + + def run_prm(self): + """运行PRM算法""" + start_time = time.time() + + # 创建PRM规划器 + prm = PRMPlanner(self.start, self.goal, self.obstacles, + self.bounds, n_samples=500, k_neighbors=10) + + # 执行规划 + success = prm.plan() + + if success: + # 优化轨迹 + optimizer = TrajectoryOptimizer(prm.path) + smoothed_path = optimizer.smooth_path() + interpolated_path = optimizer.interpolate_cubic_spline(num_points=100) + + # 计算逆运动学 + joint_trajectory = [] + for point in interpolated_path: + joint_angles = self.ik_solver.solve(point) + joint_trajectory.append(joint_angles) + + # 计算最小jerk轨迹 + position, velocity, acceleration, jerk, time_points = optimizer.minimize_jerk() + + # 存储结果 + self.results['PRM'] = { + 'path': prm.path, + 'smoothed_path': smoothed_path, + 'interpolated_path': interpolated_path, + 'joint_trajectory': joint_trajectory, + 'velocity': velocity, + 'acceleration': acceleration, + 'jerk': jerk, + 'time_points': time_points, + 'computation_time': time.time() - start_time, + 'path_length': self._calculate_path_length(interpolated_path), + 'smoothness': self._calculate_smoothness(interpolated_path) + } + + # 可视化 + self.visualizer.plot_3d_workspace( + self.obstacles, self.start, self.goal, + path=interpolated_path + ) + + self.visualizer.plot_projection_views(interpolated_path) + + if len(joint_trajectory) > 0: + self.visualizer.plot_joint_trajectories(joint_trajectory) + self.visualizer.plot_velocity_acceleration(velocity, acceleration) + self.visualizer.plot_energy_consumption(velocity, acceleration) + + return True + + return False + + def run_astar(self): + """运行A*算法""" + start_time = time.time() + + # 创建A*规划器 + astar = AStarPlanner(self.start, self.goal, self.obstacles, + self.bounds, grid_size=0.05) + + # 执行规划 + success = astar.plan() + + if success: + # 优化轨迹 + optimizer = TrajectoryOptimizer(astar.path) + smoothed_path = optimizer.smooth_path() + interpolated_path = optimizer.interpolate_cubic_spline(num_points=100) + + # 计算逆运动学 + joint_trajectory = [] + for point in interpolated_path: + joint_angles = self.ik_solver.solve(point) + joint_trajectory.append(joint_angles) + + # 计算最小jerk轨迹 + position, velocity, acceleration, jerk, time_points = optimizer.minimize_jerk() + + # 存储结果 + self.results['A*'] = { + 'path': astar.path, + 'smoothed_path': smoothed_path, + 'interpolated_path': interpolated_path, + 'joint_trajectory': joint_trajectory, + 'velocity': velocity, + 'acceleration': acceleration, + 'jerk': jerk, + 'time_points': time_points, + 'computation_time': time.time() - start_time, + 'path_length': self._calculate_path_length(interpolated_path), + 'smoothness': self._calculate_smoothness(interpolated_path) + } + + # 可视化 + self.visualizer.plot_3d_workspace( + self.obstacles, self.start, self.goal, + path=interpolated_path + ) + + self.visualizer.plot_projection_views(interpolated_path) + + if len(joint_trajectory) > 0: + self.visualizer.plot_joint_trajectories(joint_trajectory) + self.visualizer.plot_velocity_acceleration(velocity, acceleration) + self.visualizer.plot_energy_consumption(velocity, acceleration) + + return True + + return False + + def compare_algorithms(self): + """比较算法性能""" + if not self.results: + return + + # 准备比较数据 + comparison_data = {} + for algo_name, result in self.results.items(): + comparison_data[algo_name] = { + '计算时间': result['computation_time'], + '路径长度': result['path_length'], + '平滑度': result['smoothness'], + '能量消耗': np.sum(result['acceleration']**2) # 简化的能量指标 + } + + # 绘制比较图 + self.visualizer.plot_algorithm_comparison(comparison_data) + + # 打印比较结果 + print("\n" + "=" * 60) + print("算法性能比较") + print("=" * 60) + + print(f"{'算法':<10} {'计算时间(s)':<12} {'路径长度(m)':<12} {'平滑度':<12}") + print("-" * 50) + + for algo_name, result in self.results.items(): + print(f"{algo_name:<10} {result['computation_time']:<12.4f} " + f"{result['path_length']:<12.4f} {result['smoothness']:<12.4f}") + + # 保存结果 + self.save_results() + + def _calculate_path_length(self, path): + """计算路径长度""" + if len(path) < 2: + return 0 + + total_length = 0 + for i in range(1, len(path)): + total_length += np.linalg.norm(path[i] - path[i-1]) + + return total_length + + def _calculate_smoothness(self, path): + """计算路径平滑度(角度变化总和)""" + if len(path) < 3: + return 0 + + total_angle_change = 0 + for i in range(1, len(path) - 1): + v1 = path[i] - path[i-1] + v2 = path[i+1] - path[i] + + if np.linalg.norm(v1) > 0 and np.linalg.norm(v2) > 0: + cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2)) + cos_angle = np.clip(cos_angle, -1, 1) + angle = np.arccos(cos_angle) + total_angle_change += angle + + # 平滑度定义为角度变化的倒数 + smoothness = 1.0 / (total_angle_change + 1e-6) + return smoothness + + def save_results(self): + """保存规划结果""" + # 保存数据 + with open('trajectory_planning_results.pkl', 'wb') as f: + pickle.dump(self.results, f) + + # 保存可视化图表 + self.visualizer.save_plot('trajectory_planning_comparison.png') + + # 生成报告 + report = { + 'timestamp': time.strftime("%Y-%m-%d %H:%M:%S"), + 'start_position': self.start.tolist(), + 'goal_position': self.goal.tolist(), + 'obstacles': self.obstacles, + 'bounds': self.bounds, + 'algorithm_results': {} + } + + for algo_name, result in self.results.items(): + report['algorithm_results'][algo_name] = { + 'computation_time': result['computation_time'], + 'path_length': result['path_length'], + 'smoothness': result['smoothness'], + 'num_path_points': len(result['path']) + } + + with open('trajectory_planning_report.json', 'w', encoding='utf-8') as f: + json.dump(report, f, indent=2, ensure_ascii=False) + + print(f"\n结果已保存:") + print(f" 数据文件: trajectory_planning_results.pkl") + print(f" 图表文件: trajectory_planning_comparison.png") + print(f" 报告文件: trajectory_planning_report.json") + + +# ============================================================================ +# 6. 交互式演示和动画 +# ============================================================================ + +class InteractiveTrajectoryDemo: + """交互式轨迹演示(包含动画)""" + + def __init__(self): + self.fig = plt.figure(figsize=(15, 10)) + self.demo = TrajectoryPlanningDemo() + + def create_animation(self, algorithm='RRT'): + """创建轨迹执行动画""" + if algorithm not in self.demo.results: + print(f"算法 {algorithm} 的结果不存在") + return + + result = self.demo.results[algorithm] + trajectory = result['interpolated_path'] + joint_trajectory = result['joint_trajectory'] + + # 创建子图 + ax1 = self.fig.add_subplot(2, 2, 1, projection='3d') + ax2 = self.fig.add_subplot(2, 2, 2) + ax3 = self.fig.add_subplot(2, 2, 3) + ax4 = self.fig.add_subplot(2, 2, 4) + + # 初始化图表 + ax1.set_title(f'{algorithm} 轨迹动画') + ax1.set_xlabel('X') + ax1.set_ylabel('Y') + ax1.set_zlabel('Z') + ax1.set_xlim(-1, 1) + ax1.set_ylim(-1, 1) + ax1.set_zlim(0, 1) + + # 绘制障碍物 + for obstacle in self.demo.obstacles: + if obstacle['type'] == 'sphere': + u = np.linspace(0, 2 * np.pi, 20) + v = np.linspace(0, np.pi, 20) + x = obstacle['radius'] * np.outer(np.cos(u), np.sin(v)) + obstacle['center'][0] + y = obstacle['radius'] * np.outer(np.sin(u), np.sin(v)) + obstacle['center'][1] + z = obstacle['radius'] * np.outer(np.ones(np.size(u)), np.cos(v)) + obstacle['center'][2] + ax1.plot_surface(x, y, z, color='red', alpha=0.3) + + # 绘制完整轨迹 + traj_line, = ax1.plot([], [], [], 'b-', alpha=0.3) + traj_points = ax1.scatter([], [], [], c='blue', s=10, alpha=0.5) + + # 当前点 + current_point, = ax1.plot([], [], [], 'ro', markersize=10) + + # 关节角度图 + joint_lines = [] + colors = plt.cm.tab10(np.linspace(0, 1, 6)) + for i in range(6): + line, = ax2.plot([], [], color=colors[i], label=f'关节{i+1}') + joint_lines.append(line) + + ax2.set_title('关节角度') + ax2.set_xlabel('时间步') + ax2.set_ylabel('角度 (rad)') + ax2.legend() + ax2.set_xlim(0, len(joint_trajectory)) + ax2.set_ylim(-3.5, 3.5) + + # 速度图 + velocity_lines = [] + for i in range(6): + line, = ax3.plot([], [], color=colors[i]) + velocity_lines.append(line) + + ax3.set_title('关节速度') + ax3.set_xlabel('时间步') + ax3.set_ylabel('速度 (rad/s)') + ax3.set_xlim(0, len(joint_trajectory)) + + # 能量图 + energy_line, = ax4.plot([], [], 'b-') + ax4.set_title('能量消耗') + ax4.set_xlabel('时间步') + ax4.set_ylabel('能量 (J)') + ax4.set_xlim(0, len(joint_trajectory)) + + plt.tight_layout() + + # 动画更新函数 + def update(frame): + # 更新3D轨迹 + traj_line.set_data(trajectory[:frame, 0], trajectory[:frame, 1]) + traj_line.set_3d_properties(trajectory[:frame, 2]) + + traj_points._offsets3d = (trajectory[:frame, 0], + trajectory[:frame, 1], + trajectory[:frame, 2]) + + current_point.set_data([trajectory[frame, 0]], [trajectory[frame, 1]]) + current_point.set_3d_properties([trajectory[frame, 2]]) + + # 更新关节角度 + for i in range(6): + joint_lines[i].set_data(range(frame+1), + [jt[i] for jt in joint_trajectory[:frame+1]]) + + # 更新速度(简化计算) + if frame > 0: + velocities = [] + for i in range(6): + if frame < len(joint_trajectory): + vel = (joint_trajectory[frame][i] - joint_trajectory[frame-1][i]) / 0.01 + velocities.append(vel) + else: + velocities.append(0) + + for i in range(6): + if frame < len(velocity_lines[i].get_xdata()): + # 更新现有数据 + xdata = list(velocity_lines[i].get_xdata()) + ydata = list(velocity_lines[i].get_ydata()) + xdata.append(frame) + ydata.append(velocities[i]) + velocity_lines[i].set_data(xdata, ydata) + else: + velocity_lines[i].set_data(range(frame+1), velocities[:frame+1]) + + # 更新能量(简化计算) + if frame > 0: + energy = 0 + for i in range(frame): + # 计算动能 + kinetic = 0 + for j in range(6): + if i > 0: + vel = (joint_trajectory[i][j] - joint_trajectory[i-1][j]) / 0.01 + kinetic += 0.5 * 0.1 * vel**2 # 假设质量为0.1kg + + energy += kinetic + + energy_line.set_data(range(frame+1), + [self._calculate_energy(joint_trajectory[:k+1]) + for k in range(frame+1)]) + + return (traj_line, traj_points, current_point, *joint_lines, + *velocity_lines, energy_line) + + # 创建动画 + ani = animation.FuncAnimation( + self.fig, update, frames=len(trajectory), + interval=50, blit=False, repeat=True + ) + + # 保存动画 + try: + ani.save(f'trajectory_animation_{algorithm}.gif', writer='pillow', fps=20) + print(f"动画已保存: trajectory_animation_{algorithm}.gif") + except: + print("警告: 无法保存GIF动画,显示实时动画") + + plt.show() + return ani + + def _calculate_energy(self, joint_trajectory): + """计算能量消耗(简化)""" + if len(joint_trajectory) < 2: + return 0 + + energy = 0 + for i in range(1, len(joint_trajectory)): + # 计算每个关节的速度平方和 + for j in range(6): + vel = (joint_trajectory[i][j] - joint_trajectory[i-1][j]) / 0.01 + energy += 0.5 * 0.1 * vel**2 # 假设质量为0.1kg + + return energy + + +# ============================================================================ +# 7. 主入口 +# ============================================================================ + +def main(): + """主函数""" + print("=" * 60) + print("机械臂轨迹规划与优化可视化系统") + print("=" * 60) + + # 选项菜单 + print("\n请选择运行模式:") + print("1. 运行所有算法并比较") + print("2. 运行单个算法 (RRT)") + print("3. 运行单个算法 (PRM)") + print("4. 运行单个算法 (A*)") + print("5. 创建轨迹动画") + print("6. 退出") + + choice = input("\n请输入选项 (1-6): ") + + if choice == '1': + # 运行所有算法 + demo = TrajectoryPlanningDemo() + demo.run_all_algorithms() + + elif choice == '2': + # 运行RRT算法 + demo = TrajectoryPlanningDemo() + demo.run_rrt() + plt.show() + + elif choice == '3': + # 运行PRM算法 + demo = TrajectoryPlanningDemo() + demo.run_prm() + plt.show() + + elif choice == '4': + # 运行A*算法 + demo = TrajectoryPlanningDemo() + demo.run_astar() + plt.show() + + elif choice == '5': + # 创建动画 + anim_demo = InteractiveTrajectoryDemo() + + print("\n选择要制作动画的算法:") + print("1. RRT") + print("2. PRM") + print("3. A*") + + algo_choice = input("请输入选项 (1-3): ") + + if algo_choice == '1': + # 先运行RRT算法 + demo = TrajectoryPlanningDemo() + demo.run_rrt() + anim_demo.demo = demo + anim_demo.create_animation('RRT') + elif algo_choice == '2': + # 先运行PRM算法 + demo = TrajectoryPlanningDemo() + demo.run_prm() + anim_demo.demo = demo + anim_demo.create_animation('PRM') + elif algo_choice == '3': + # 先运行A*算法 + demo = TrajectoryPlanningDemo() + demo.run_astar() + anim_demo.demo = demo + anim_demo.create_animation('A*') + else: + print("无效选项") + + elif choice == '6': + print("退出程序") + return + + else: + print("无效选项") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/robotic_arm_grasping/arm_with_gripper.xml b/src/robotic_arm_grasping/arm_with_gripper.xml new file mode 100644 index 0000000000..d8508af2f8 --- /dev/null +++ b/src/robotic_arm_grasping/arm_with_gripper.xml @@ -0,0 +1,212 @@ + + + + + \ No newline at end of file diff --git a/src/robotic_arm_grasping/collision_risk_analysis_visualization.py b/src/robotic_arm_grasping/collision_risk_analysis_visualization.py new file mode 100644 index 0000000000..a10dcdaf46 --- /dev/null +++ b/src/robotic_arm_grasping/collision_risk_analysis_visualization.py @@ -0,0 +1,374 @@ +""" +完整版:机械臂碰撞检测 + MuJoCo可视化界面 +包含碰撞分析和实时仿真 +修改说明: +1. 所有路径改为相对路径 +2. 解决可视化图表中文乱码问题 +3. 优化文件处理和错误处理 +4. 修复临时文件创建问题 +""" +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import os +import sys +import time +import tempfile +import warnings +warnings.filterwarnings('ignore') + +# 设置Matplotlib支持中文显示(解决乱码问题) +plt.rcParams['font.sans-serif'] = ['SimHei', 'Microsoft YaHei', 'DejaVu Sans'] # 设置中文字体 +plt.rcParams['axes.unicode_minus'] = False # 正确显示负号 + +# 尝试导入MuJoCo,如果失败则使用模拟模式 +try: + import mujoco + import mujoco.viewer + MUJOCO_AVAILABLE = True + print("✅ MuJoCo 已安装,将启动可视化界面") +except ImportError: + MUJOCO_AVAILABLE = False + print("⚠️ MuJoCo 未安装,将仅生成图表(无可视化界面)") + print("💡 安装命令: pip install mujoco mujoco-python-viewer") + +def generate_collision_analysis(): + """生成碰撞分析图表""" + print("="*60) + print("机械臂碰撞风险分析系统") + print("="*60) + + # 1. 生成模拟工作空间数据 + print("正在生成工作空间数据...") + np.random.seed(42) + n_points = 300 + + # 模拟机械臂工作空间 + theta = np.random.uniform(0, 2*np.pi, n_points) + phi = np.random.uniform(0, np.pi, n_points) + r = 0.5 + 0.2 * np.random.randn(n_points) + + x = 0.6 * np.cos(theta) * np.sin(phi) + y = 0.6 * np.sin(theta) * np.sin(phi) + z = 0.5 + 0.3 * np.cos(phi) + + points = np.vstack([x, y, z]).T + + # 2. 计算碰撞风险 + print("正在计算碰撞风险...") + risks = [] + for point in points: + risk = 0 + + # 墙壁风险 (x=0.7) + wall_dist = abs(point[0] - 0.7) + if wall_dist < 0.15: + risk += 0.8 * (0.15 - wall_dist) / 0.15 + + # 中心障碍物 + center_dist = np.sqrt(point[0]**2 + point[1]**2) + if center_dist < 0.2: + risk += 0.6 * (0.2 - center_dist) / 0.2 + + # 天花板风险 + if point[2] > 0.9: + risk += 0.4 + + # 地面风险 + if point[2] < 0.1: + risk += 0.3 + + risk = min(1.0, risk) + risks.append(risk) + + risks = np.array(risks) + + # 3. 绘制图表 + print("正在生成可视化图表...") + fig = plt.figure(figsize=(15, 6)) + + # 左侧:3D风险图 + ax1 = fig.add_subplot(121, projection='3d') + scatter = ax1.scatter(points[:, 0], points[:, 1], points[:, 2], + c=risks, cmap='RdYlGn_r', + alpha=0.7, s=20, edgecolors='none') + + # 添加障碍物标记 + ax1.plot([0.7, 0.7], [-0.8, 0.8], [0, 1], 'k-', linewidth=3, alpha=0.5, label='墙壁') + + ax1.set_xlabel('X (米)', fontsize=12, labelpad=10) + ax1.set_ylabel('Y (米)', fontsize=12, labelpad=10) + ax1.set_zlabel('Z (米)', fontsize=12, labelpad=10) + ax1.set_title('3D碰撞风险热力图', fontsize=14, fontweight='bold') + ax1.legend(fontsize=10) + ax1.view_init(elev=25, azim=45) + ax1.grid(True, alpha=0.3) + + plt.colorbar(scatter, ax=ax1, shrink=0.7, pad=0.1, label='碰撞风险') + + # 右侧:统计图 + ax2 = fig.add_subplot(122) + + # 风险等级统计 + low_risk = np.sum(risks < 0.3) + medium_risk = np.sum((risks >= 0.3) & (risks < 0.7)) + high_risk = np.sum(risks >= 0.7) + + categories = ['低风险', '中风险', '高风险'] + counts = [low_risk, medium_risk, high_risk] + percentages = [c/n_points*100 for c in counts] + colors = ['#2E8B57', '#FFA500', '#DC143C'] + + bars = ax2.bar(categories, percentages, color=colors, edgecolor='black', alpha=0.8) + + # 添加标签 + for bar, percent in zip(bars, percentages): + height = bar.get_height() + ax2.text(bar.get_x() + bar.get_width()/2, height + 1, + f'{percent:.1f}%', ha='center', fontsize=11, fontweight='bold') + + ax2.set_ylabel('占比 (%)', fontsize=12) + ax2.set_title('风险区域分布', fontsize=14, fontweight='bold') + ax2.set_ylim(0, 100) + ax2.grid(True, alpha=0.3, axis='y') + + # 总结文本 + summary = f'分析结果:\n' + summary += f'• 安全区域: {percentages[0]:.1f}%\n' + summary += f'• 危险区域: {percentages[2]:.1f}%\n' + summary += f'• 总采样点: {n_points}' + + ax2.text(0.05, 0.95, summary, transform=ax2.transAxes, fontsize=11, + bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8), + verticalalignment='top') + + plt.suptitle('机械臂工作空间碰撞风险分析', fontsize=16, fontweight='bold') + plt.tight_layout() + + # 保存图表到当前目录 + output_file = 'collision_analysis_result.png' + plt.savefig(output_file, dpi=150, bbox_inches='tight') + plt.show() + + print(f"\n✅ 碰撞分析完成!") + print(f"📊 图表已保存: {output_file}") + print(f"📋 安全区域占比: {percentages[0]:.1f}%") + print(f"📋 危险区域占比: {percentages[2]:.1f}%") + + return True + +def run_mujoco_simulation(): + """运行MuJoCo可视化仿真""" + if not MUJOCO_AVAILABLE: + print("\n❌ MuJoCo未安装,无法启动可视化界面") + print("💡 请安装: pip install mujoco mujoco-python-viewer") + return False + + print("\n" + "="*60) + print("启动MuJoCo机械臂仿真") + print("="*60) + print("控制说明:") + print("- 窗口中将显示机械臂模型") + print("- 机械臂会自动进行随机运动") + print("- 按ESC键退出仿真") + print("="*60) + + try: + # 使用相对路径查找模型文件 + current_dir = os.path.dirname(os.path.abspath(__file__)) + model_file_path = os.path.join(current_dir, 'arm_with_gripper.xml') + + # 检查文件是否存在 + if not os.path.exists(model_file_path): + print(f"❌ 模型文件不存在: {model_file_path}") + print(f"当前目录: {current_dir}") + print(f"目录内容: {os.listdir(current_dir)}") + return False + + print(f"正在加载模型文件: {model_file_path}") + + # 读取模型文件内容 + with open(model_file_path, 'r', encoding='utf-8') as f: + model_content = f.read() + + # 移除对不存在的资源目录的引用 + model_content = model_content.replace('meshdir="assets/"', '') + model_content = model_content.replace('texturedir="textures/"', '') + + # 使用临时文件,避免中文字符路径问题 + with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False, encoding='utf-8') as temp_file: + temp_model_path = temp_file.name + temp_file.write(model_content) + print(f"✅ 临时模型文件已创建: {temp_model_path}") + + # 检查文件是否已创建 + if not os.path.exists(temp_model_path): + print(f"❌ 临时文件创建失败: {temp_model_path}") + return False + + print(f"✅ 临时文件存在: {os.path.exists(temp_model_path)}") + print(f"✅ 临时文件大小: {os.path.getsize(temp_model_path)} 字节") + + # 从临时路径加载模型 + try: + print("正在加载MuJoCo模型...") + model = mujoco.MjModel.from_xml_path(temp_model_path) + data = mujoco.MjData(model) + print("✅ 模型加载成功") + except Exception as e: + print(f"❌ 模型加载失败: {e}") + # 尝试直接从XML字符串加载 + print("尝试从XML字符串加载模型...") + model = mujoco.MjModel.from_xml_string(model_content) + data = mujoco.MjData(model) + print("✅ 从字符串加载模型成功") + + print(f"关节数量: {model.njnt}") + print(f"执行器数量: {model.nu}") + + # 启动可视化界面 + print("正在启动可视化窗口...") + + try: + with mujoco.viewer.launch_passive(model, data) as viewer: + # 设置视角 + viewer.cam.azimuth = 45 + viewer.cam.elevation = -20 + viewer.cam.distance = 2.5 + viewer.cam.lookat[:] = [0.2, 0.0, 0.5] + + print("✅ 可视化窗口已启动") + print("机械臂开始随机运动...") + + # 仿真参数 + simulation_time = 30.0 # 仿真30秒 + start_time = time.time() + step_count = 0 + + # 随机目标角度 + target_angles = np.random.uniform(-0.5, 0.5, model.nu) + + while viewer.is_running() and (time.time() - start_time) < simulation_time: + step_start = time.time() + + # 简单的PD控制,让机械臂随机运动 + for i in range(min(model.nu, len(target_angles))): + # 计算控制信号(简单的PD控制器) + error = target_angles[i] - data.qpos[i] + data.ctrl[i] = 100 * error - 10 * data.qvel[i] # PD控制 + + # 每100步重新生成随机目标 + if step_count % 100 == 0: + target_angles = np.random.uniform(-0.5, 0.5, model.nu) + + # 碰撞检测(简单版本) + try: + ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, 'ee_site') + if ee_site_id >= 0: + ee_pos = data.site_xpos[ee_site_id] + + # 检查与墙壁的碰撞 + if abs(ee_pos[0] - 0.7) < 0.1: + print("⚠️ 警告: 末端接近墙壁!") + # 检查与柱子的碰撞 + if np.sqrt(ee_pos[0]**2 + ee_pos[1]**2) < 0.2: + print("⚠️ 警告: 末端接近中心柱子!") + except: + pass # 如果站点不存在,跳过碰撞检测 + + # 执行模拟步骤 + mujoco.mj_step(model, data) + + # 同步viewer + viewer.sync() + + # 控制仿真速度 + elapsed = time.time() - step_start + sleep_time = model.opt.timestep - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + + step_count += 1 + + print(f"\n仿真结束,共运行 {step_count} 步") + + except KeyboardInterrupt: + print("\n用户中断仿真") + except Exception as e: + print(f"仿真错误: {e}") + + finally: + # 清理临时文件 + try: + if os.path.exists(temp_model_path): + os.remove(temp_model_path) + print(f"临时文件已删除: {temp_model_path}") + except Exception as e: + print(f"清理临时文件失败: {e}") + + return True + + except Exception as e: + print(f"❌ MuJoCo仿真失败: {e}") + import traceback + traceback.print_exc() + return False + +def run_collision_detection_system(): + """运行完整的碰撞检测系统""" + print("🚀 机械臂碰撞检测与可视化系统") + print("="*60) + + # 步骤1: 生成碰撞分析图表 + print("\n[步骤1] 生成碰撞风险分析图表...") + success1 = generate_collision_analysis() + + if not success1: + print("❌ 碰撞分析失败") + return False + + # 步骤2: 询问是否启动MuJoCo仿真 + print("\n" + "="*60) + if MUJOCO_AVAILABLE: + response = input("是否启动MuJoCo机械臂仿真?(y/n): ").strip().lower() + if response in ['y', 'yes', '是']: + print("\n[步骤2] 启动MuJoCo可视化仿真...") + success2 = run_mujoco_simulation() + if success2: + print("✅ MuJoCo仿真完成") + else: + print("❌ MuJoCo仿真失败") + else: + print("跳过MuJoCo仿真") + else: + print("⚠️ MuJoCo未安装,跳过仿真步骤") + print("💡 要启用仿真功能,请安装:") + print(" pip install mujoco mujoco-python-viewer") + + # 总结 + print("\n" + "="*60) + print("系统运行完成!") + print("="*60) + print("📊 生成的图表:") + print(" • collision_analysis_result.png - 碰撞风险分析图") + print("\n🎯 后续步骤:") + print(" 1. 查看生成的图表了解碰撞风险分布") + print(" 2. 根据分析结果优化机械臂工作空间") + print(" 3. 安装MuJoCo以启用仿真功能") + print("="*60) + + return True + +if __name__ == "__main__": + try: + run_collision_detection_system() + except KeyboardInterrupt: + print("\n程序被用户中断") + except Exception as e: + print(f"\n❌ 程序运行出错: {e}") + import traceback + traceback.print_exc() + print("\n💡 常见问题解决方法:") + print("1. 确保已安装必要依赖: pip install numpy matplotlib") + print("2. 如需MuJoCo仿真: pip install mujoco mujoco-python-viewer") + print("3. 检查Python版本兼容性") \ No newline at end of file diff --git a/src/robotic_arm_grasping/grasp.py b/src/robotic_arm_grasping/grasp.py new file mode 100644 index 0000000000..75c3bda721 --- /dev/null +++ b/src/robotic_arm_grasping/grasp.py @@ -0,0 +1,407 @@ +import os +import shutil +import time +import numpy as np +import mujoco +# 路径 +MODEL_PATH = r"C:\Users\龙忠梁\Downloads\mujoco-3.3.7-windows-x86_64\model\robotic_arm\arm_with_gripper.xml" + +def resolve_model_path(path: str) -> str: + """处理包含中文字符的路径""" + if not os.path.exists(path): + raise FileNotFoundError(f"模型文件不存在: {path}") + + # 检查路径是否包含非ASCII字符 + if any(ord(ch) > 127 for ch in path): + temp_dir = r"C:\temp_mujoco_project" + os.makedirs(temp_dir, exist_ok=True) + temp_path = os.path.join(temp_dir, "arm_with_gripper.xml") + shutil.copy2(path, temp_path) + print(f"已将模型复制到临时路径: {temp_path}") + return temp_path + return path + +def set_initial_pose(model, data): + """设置机械臂的初始姿态(立起来)""" + # 关节初始位置(弧度) + initial_qpos = np.zeros(model.nq) + + # 设置6个主要关节的初始位置 + joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"] + initial_angles = [0.0, 0.785, -1.57, 0.0, 0.0, 0.0] + + for i, joint_name in enumerate(joint_names): + joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, joint_name) + if joint_id != -1: + qpos_addr = model.jnt_qposadr[joint_id] + initial_qpos[qpos_addr] = initial_angles[i] + + # 设置抓取器初始为半张开状态 + finger_joints = ["finger_1_joint", "finger_2_joint", "finger_3_joint", + "finger_1_proximal_joint", "finger_2_proximal_joint", "finger_3_proximal_joint", + "finger_1_distal_joint", "finger_2_distal_joint", "finger_3_distal_joint"] + + for joint_name in finger_joints: + joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, joint_name) + if joint_id != -1: + qpos_addr = model.jnt_qposadr[joint_id] + if "slide" in joint_name: # 滑动关节初始位置 + initial_qpos[qpos_addr] = 0.02 # 稍微张开 + elif "proximal" in joint_name: # 近端关节 + initial_qpos[qpos_addr] = 0.3 # 轻微弯曲 + elif "distal" in joint_name: # 远端关节 + initial_qpos[qpos_addr] = 0.2 # 轻微弯曲 + + data.qpos[:] = initial_qpos + mujoco.mj_forward(model, data) + +def move_arm_to_workspace(model, data, viewer): + """将机械臂移动到工作空间中央位置""" + print("正在移动机械臂到工作位置...") + + # 控制机械臂移动到合适的工作位置 + steps = 300 + for i in range(steps): + # 简单的控制:保持抓取器位置,移动机械臂到合适位置 + if model.nu >= 6: + # 设置机械臂关节的控制信号 + # 调整机械臂姿态,使其处于更好的抓取位置 + data.ctrl[0] = 0.0 # joint_1 + data.ctrl[1] = 0.5 # joint_2 - 肩部 + data.ctrl[2] = -0.3 # joint_3 - 肘部 + data.ctrl[3] = 0.0 # joint_4 + data.ctrl[4] = 0.2 # joint_5 + data.ctrl[5] = 0.0 # joint_6 + + mujoco.mj_step(model, data) + + # 同步视图 + if viewer is not None: + viewer.sync() + + time.sleep(model.opt.timestep * 0.5) + + print("✓ 机械臂已移动到工作位置") + +def control_gripper_demo(model, data, viewer): + """演示爪勾的抓取功能,修正控制逻辑确保三个手指都向中心点弯曲""" + + print("=== 爪勾抓取功能演示 ===") + print("修正控制逻辑,确保三个手指都向中心点弯曲") + + # 首先,打印所有抓取器相关关节的信息,以便调试 + print("\n抓取器关节信息:") + for i in range(model.nu): + joint_id = model.actuator_trnid[i, 0] + if joint_id != -1: + joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, joint_id) + if joint_name and "finger" in joint_name: + print(f" 控制通道 {i}: 控制关节 '{joint_name}' (ID: {joint_id})") + + # 演示1:直接控制所有抓取器电机,观察效果 + print("\n1. 测试所有抓取器控制通道...") + + # 先清零所有控制 + data.ctrl[:] = 0.0 + + # 测试张开:使用负值 + print(" 测试张开动作 (使用负控制信号)...") + for i in range(300): + # 设置所有抓取器控制为负值 + for j in range(6, model.nu): # 抓取器控制从索引6开始 + data.ctrl[j] = -1.0 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 测试闭合:使用正值 + print(" 测试闭合动作 (使用正控制信号)...") + for i in range(300): + # 设置所有抓取器控制为正值 + for j in range(6, model.nu): # 抓取器控制从索引6开始 + data.ctrl[j] = 1.0 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 重置为张开状态 + print(" 重置为张开状态...") + for i in range(300): + for j in range(6, model.nu): + data.ctrl[j] = -1.0 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 演示2:尝试分别控制三个手指 + print("\n2. 分别控制三个手指...") + + # 创建控制通道到手指的映射 + control_to_finger = {} + for i in range(model.nu): + joint_id = model.actuator_trnid[i, 0] + if joint_id != -1: + joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, joint_id) + if joint_name: + # 检查属于哪个手指 + if "finger_1" in joint_name: + control_to_finger[i] = 1 + elif "finger_2" in joint_name: + control_to_finger[i] = 2 + elif "finger_3" in joint_name: + control_to_finger[i] = 3 + + # 手指1闭合 + print(" 手指1闭合...") + for i in range(200): + for ctrl_idx, finger_num in control_to_finger.items(): + if finger_num == 1: + data.ctrl[ctrl_idx] = 1.5 # 正值闭合 + else: + data.ctrl[ctrl_idx] = 0.0 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 手指2闭合 + print(" 手指2闭合...") + for i in range(200): + for ctrl_idx, finger_num in control_to_finger.items(): + if finger_num == 2: + data.ctrl[ctrl_idx] = 1.5 # 正值闭合 + else: + data.ctrl[ctrl_idx] = 0.0 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 手指3闭合 + print(" 手指3闭合...") + for i in range(200): + for ctrl_idx, finger_num in control_to_finger.items(): + if finger_num == 3: + data.ctrl[ctrl_idx] = 1.5 # 正值闭合 + else: + data.ctrl[ctrl_idx] = 0.0 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 所有手指张开 + print(" 所有手指张开...") + for i in range(300): + for j in range(6, model.nu): + data.ctrl[j] = -1.0 # 负值张开 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 演示3:尝试不同的控制策略 + print("\n3. 尝试不同的控制策略...") + + # 策略1:区分滑动关节和旋转关节 + print(" 策略1: 区分滑动关节和旋转关节...") + + # 首先识别不同类型的关节 + slide_ctrl_indices = [] + proximal_ctrl_indices = [] + distal_ctrl_indices = [] + + for i in range(model.nu): + joint_id = model.actuator_trnid[i, 0] + if joint_id != -1: + joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, joint_id) + if joint_name: + if "slide" in joint_name or ("joint" in joint_name and "proximal" not in joint_name and "distal" not in joint_name): + slide_ctrl_indices.append(i) + elif "proximal" in joint_name: + proximal_ctrl_indices.append(i) + elif "distal" in joint_name: + distal_ctrl_indices.append(i) + + print(f" 找到 {len(slide_ctrl_indices)} 个滑动关节控制") + print(f" 找到 {len(proximal_ctrl_indices)} 个近端关节控制") + print(f" 找到 {len(distal_ctrl_indices)} 个远端关节控制") + + # 完全张开 + print(" 完全张开...") + for i in range(300): + for idx in slide_ctrl_indices: + data.ctrl[idx] = -2.0 # 负值可能使滑动关节向外 + for idx in proximal_ctrl_indices: + data.ctrl[idx] = -1.5 # 负值可能使旋转关节伸直 + for idx in distal_ctrl_indices: + data.ctrl[idx] = -1.0 # 负值可能使远端关节伸直 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 完全闭合(尝试不同的组合) + print(" 尝试闭合 (组合1)...") + for i in range(300): + for idx in slide_ctrl_indices: + data.ctrl[idx] = 2.0 # 正值可能使滑动关节向内 + for idx in proximal_ctrl_indices: + data.ctrl[idx] = 1.5 # 正值可能使旋转关节弯曲 + for idx in distal_ctrl_indices: + data.ctrl[idx] = 1.0 # 正值可能使远端关节弯曲 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 尝试相反的组合 + print(" 尝试闭合 (组合2 - 相反符号)...") + for i in range(300): + for idx in slide_ctrl_indices: + data.ctrl[idx] = -2.0 # 负值可能使滑动关节向内 + for idx in proximal_ctrl_indices: + data.ctrl[idx] = -1.5 # 负值可能使旋转关节弯曲 + for idx in distal_ctrl_indices: + data.ctrl[idx] = -1.0 # 负值可能使远端关节弯曲 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 固定机械臂主体,只控制夹爪动作 + print(" 固定机械臂主体,只控制夹爪动作...") + # 锁定前6个关节(机械臂主体) + for j in range(6): + data.ctrl[j] = 0.0 # 固定机械臂主体不动 + + # 增加多次抓取循环演示 + print(" 增加多次抓取循环演示...") + for cycle in range(5): # 执行5次抓取循环 + print(f" 抓取循环 {cycle+1} 开始...") + + # 闭合抓取器 - 更强的控制信号 + print(f" 闭合抓取器 (循环 {cycle+1})...") + for i in range(300): # 增加迭代次数 + # 固定机械臂主体不动 + for j in range(6): + data.ctrl[j] = 0.0 + + # 控制夹爪闭合 + for j in range(6, model.nu): + data.ctrl[j] = 2.0 # 更大的正值确保完全闭合 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 等待一段时间 + print(f" 保持抓取状态 (循环 {cycle+1})...") + for i in range(150): # 增加等待时间 + # 固定机械臂主体不动 + for j in range(6): + data.ctrl[j] = 0.0 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 张开抓取器 - 更强的控制信号 + print(f" 张开抓取器 (循环 {cycle+1})...") + for i in range(300): # 增加迭代次数 + # 固定机械臂主体不动 + for j in range(6): + data.ctrl[j] = 0.0 + + # 控制夹爪张开 + for j in range(6, model.nu): + data.ctrl[j] = -2.0 # 更大的负值确保完全张开 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 等待一段时间 + print(f" 保持张开状态 (循环 {cycle+1})...") + for i in range(150): # 增加等待时间 + # 固定机械臂主体不动 + for j in range(6): + data.ctrl[j] = 0.0 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + print(" 多次抓取循环演示完成!") + + # 最终回到张开状态 + print(" 最终回到张开状态...") + for i in range(300): + for j in range(6, model.nu): + data.ctrl[j] = -1.0 # 使用负值 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + print("\n✓ 爪勾抓取功能演示完成!") + print("\n观察结果:") + print("1. 如果所有手指都正确向中心弯曲,说明控制信号符号正确") + print("2. 如果有手指向外翻,可能需要检查模型中的关节轴设置") + print("3. 如果所有手指都上下运动,说明滑动关节轴可能是(0,0,1)而不是水平方向") + +def main(): + """主函数""" + # 加载模型 + model_path = resolve_model_path(MODEL_PATH) + model = mujoco.MjModel.from_xml_path(model_path) + data = mujoco.MjData(model) + + print("开始机械臂爪勾抓取功能演示") + print("修正控制逻辑,测试不同的控制策略") + + # 启动可视化 + with mujoco.viewer.launch_passive(model, data) as viewer: + # 设置相机视角,专注于抓取器 + viewer.cam.azimuth = 180 + viewer.cam.elevation = -20 + viewer.cam.distance = 0.8 + viewer.cam.lookat[:] = [0.25, 0.0, 0.15] + + print("\n=== 阶段1:初始化机械臂 ===") + # 设置机械臂初始姿态 + set_initial_pose(model, data) + + # 给一些时间让机械臂稳定 + for _ in range(150): + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep) + + print("✓ 机械臂已初始化") + + print("\n=== 阶段2:爪勾抓取功能演示 ===") + # 演示爪勾的抓取功能 + control_gripper_demo(model, data, viewer) + + print("\n=== 演示完成 ===") + print("请在可视化窗口中观察三个手指的运动方向") + print("根据观察结果,可能需要调整控制信号的符号或模型设置") + print("关闭窗口以结束程序") + + # 保持窗口打开,让用户观察 + print("\n保持张开状态展示中...") + for i in range(300): + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep * 0.5) + + # 保持窗口打开 + while viewer.is_running: + time.sleep(0.1) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/robotic_arm_grasping/grasp_force_analysis.py b/src/robotic_arm_grasping/grasp_force_analysis.py new file mode 100644 index 0000000000..26a04d9e8c --- /dev/null +++ b/src/robotic_arm_grasping/grasp_force_analysis.py @@ -0,0 +1,310 @@ +""" +项目3:三指抓取器力分析可视化 +""" +import numpy as np +import matplotlib.pyplot as plt +import seaborn as sns +from matplotlib.patches import Circle, Wedge +from scipy import stats + +def grasp_force_analysis(): + """抓取力分析与可视化""" + np.random.seed(42) + + # 模拟抓取数据 + n_samples = 1000 + time = np.linspace(0, 5, n_samples) + + # 模拟三个手指的力数据 + finger1_force = 5 + 2 * np.sin(time * 3) + 0.5 * np.random.randn(n_samples) + finger2_force = 6 + 1.5 * np.cos(time * 2.5) + 0.4 * np.random.randn(n_samples) + finger3_force = 4.5 + 1.8 * np.sin(time * 2 + 1) + 0.6 * np.random.randn(n_samples) + + # 总力 + total_force = finger1_force + finger2_force + finger3_force + + # 创建图表 + fig = plt.figure(figsize=(16, 12)) + + # 1. 力随时间变化(折线图) + ax1 = plt.subplot(3, 3, 1) + ax1.plot(time, finger1_force, 'r-', linewidth=2, alpha=0.8, label='手指1') + ax1.plot(time, finger2_force, 'g-', linewidth=2, alpha=0.8, label='手指2') + ax1.plot(time, finger3_force, 'b-', linewidth=2, alpha=0.8, label='手指3') + ax1.plot(time, total_force, 'k--', linewidth=2.5, alpha=0.9, label='总力') + + ax1.set_xlabel('时间 (s)', fontsize=10) + ax1.set_ylabel('力 (N)', fontsize=10) + ax1.set_title('各手指力随时间变化', fontsize=12, fontweight='bold') + ax1.legend(loc='upper right', fontsize=9) + ax1.grid(True, alpha=0.3) + ax1.fill_between(time, total_force.min(), total_force, alpha=0.1, color='gray') + + # 2. 力分布直方图 + ax2 = plt.subplot(3, 3, 2) + bins = np.linspace(0, 15, 30) + ax2.hist(finger1_force, bins=bins, alpha=0.6, color='red', + label='手指1', density=True) + ax2.hist(finger2_force, bins=bins, alpha=0.6, color='green', + label='手指2', density=True) + ax2.hist(finger3_force, bins=bins, alpha=0.6, color='blue', + label='手指3', density=True) + + # 添加KDE曲线 + for force, color, label in zip([finger1_force, finger2_force, finger3_force], + ['red', 'green', 'blue'], + ['手指1', '手指2', '手指3']): + kde = stats.gaussian_kde(force) + x_kde = np.linspace(force.min(), force.max(), 100) + ax2.plot(x_kde, kde(x_kde), color=color, linewidth=2, label=f'{label} KDE') + + ax2.set_xlabel('力 (N)', fontsize=10) + ax2.set_ylabel('概率密度', fontsize=10) + ax2.set_title('力分布直方图与KDE', fontsize=12, fontweight='bold') + ax2.legend(fontsize=8) + ax2.grid(True, alpha=0.3) + + # 3. 箱线图 - 修复版本兼容性问题 + ax3 = plt.subplot(3, 3, 3) + force_data = [finger1_force, finger2_force, finger3_force] + + # 检查Matplotlib版本并选择正确的参数名 + import matplotlib + mpl_version = matplotlib.__version__ + print(f"Matplotlib 版本: {mpl_version}") + + # 尝试使用新的参数名,如果失败则回退到旧的 + try: + # 尝试使用新的tick_labels参数 + bp = ax3.boxplot(force_data, patch_artist=True, + tick_labels=['手指1', '手指2', '手指3'], + medianprops=dict(color='black', linewidth=2), + whiskerprops=dict(color='gray', linewidth=1.5), + capprops=dict(color='gray', linewidth=1.5)) + except TypeError: + # 如果失败,使用旧的labels参数 + bp = ax3.boxplot(force_data, patch_artist=True, + labels=['手指1', '手指2', '手指3'], + medianprops=dict(color='black', linewidth=2), + whiskerprops=dict(color='gray', linewidth=1.5), + capprops=dict(color='gray', linewidth=1.5)) + + # 设置颜色 + colors = ['lightcoral', 'lightgreen', 'lightblue'] + for patch, color in zip(bp['boxes'], colors): + patch.set_facecolor(color) + patch.set_alpha(0.7) + + ax3.set_ylabel('力 (N)', fontsize=10) + ax3.set_title('力统计箱线图', fontsize=12, fontweight='bold') + ax3.grid(True, alpha=0.3, axis='y') + + # 添加均值点 + means = [np.mean(f) for f in force_data] + ax3.scatter(range(1, 4), means, color='black', s=80, marker='D', + label='均值', zorder=5) + + # 4. 力相关矩阵热图 + ax4 = plt.subplot(3, 3, 4) + forces = np.vstack([finger1_force, finger2_force, finger3_force]) + corr_matrix = np.corrcoef(forces) + + im = ax4.imshow(corr_matrix, cmap='coolwarm', vmin=-1, vmax=1) + + # 添加数值标签 + for i in range(3): + for j in range(3): + text = ax4.text(j, i, f'{corr_matrix[i, j]:.2f}', + ha='center', va='center', color='white', + fontsize=11, fontweight='bold') + + ax4.set_xticks(range(3)) + ax4.set_yticks(range(3)) + ax4.set_xticklabels(['手指1', '手指2', '手指3'], fontsize=10) + ax4.set_yticklabels(['手指1', '手指2', '手指3'], fontsize=10) + ax4.set_title('手指力相关性热图', fontsize=12, fontweight='bold') + + # 添加颜色条 + plt.colorbar(im, ax=ax4, shrink=0.8) + + # 5. 力平衡雷达图 + ax5 = plt.subplot(3, 3, 5, polar=True) + categories = ['力大小', '稳定性', '响应速度', '同步性', '效率'] + N = len(categories) + + # 计算各项指标(模拟) + values_f1 = [np.mean(finger1_force)/10, 0.8, 0.7, 0.6, 0.75] + values_f2 = [np.mean(finger2_force)/10, 0.9, 0.8, 0.7, 0.85] + values_f3 = [np.mean(finger3_force)/10, 0.7, 0.6, 0.65, 0.7] + + # 封闭多边形 + angles = np.linspace(0, 2 * np.pi, N, endpoint=False).tolist() + values_f1 += values_f1[:1] + values_f2 += values_f2[:1] + values_f3 += values_f3[:1] + angles += angles[:1] + + ax5.plot(angles, values_f1, 'o-', linewidth=2, color='red', + label='手指1', alpha=0.7) + ax5.fill(angles, values_f1, alpha=0.1, color='red') + + ax5.plot(angles, values_f2, 'o-', linewidth=2, color='green', + label='手指2', alpha=0.7) + ax5.fill(angles, values_f2, alpha=0.1, color='green') + + ax5.plot(angles, values_f3, 'o-', linewidth=2, color='blue', + label='手指3', alpha=0.7) + ax5.fill(angles, values_f3, alpha=0.1, color='blue') + + ax5.set_xticks(angles[:-1]) + ax5.set_xticklabels(categories, fontsize=9) + ax5.set_ylim(0, 1) + ax5.set_title('手指性能雷达图', fontsize=12, fontweight='bold', pad=20) + ax5.legend(loc='upper right', bbox_to_anchor=(1.3, 1.0), fontsize=9) + ax5.grid(True, alpha=0.3) + + # 6. 力时频分析 - 修复数组维度问题 + ax6 = plt.subplot(3, 3, 6) + + # 创建正确的2D时频数据 + t = np.linspace(0, 5, 200) + f = np.linspace(0, 10, 100) + T, F = np.meshgrid(t, f) + + # 修复:确保Z是2D数组 + # 原来的错误:使用了[None, :]创建了额外的维度 + # 修复:直接使用广播创建2D数组 + Z = np.sin(2 * np.pi * F * T) * np.exp(-0.1 * T) * np.sin(2 * np.pi * 2 * T) + + # 验证Z是2D + print(f"Z的形状: {Z.shape}, 应该是(100, 200)") + + contour = ax6.contourf(T, F, np.abs(Z), 20, cmap='hot', alpha=0.9) + ax6.set_xlabel('时间 (s)', fontsize=10) + ax6.set_ylabel('频率 (Hz)', fontsize=10) + ax6.set_title('力信号时频分析', fontsize=12, fontweight='bold') + + plt.colorbar(contour, ax=ax6, shrink=0.8, label='幅度') + + # 7. 累积力分布 + ax7 = plt.subplot(3, 3, 7) + + for force, color, label in zip([finger1_force, finger2_force, finger3_force], + ['red', 'green', 'blue'], + ['手指1', '手指2', '手指3']): + sorted_force = np.sort(force) + cdf = np.arange(1, len(sorted_force) + 1) / len(sorted_force) + ax7.plot(sorted_force, cdf, color=color, linewidth=2, label=label) + + ax7.set_xlabel('力 (N)', fontsize=10) + ax7.set_ylabel('累积概率', fontsize=10) + ax7.set_title('累积分布函数 (CDF)', fontsize=12, fontweight='bold') + ax7.legend(fontsize=9) + ax7.grid(True, alpha=0.3) + + # 8. 力向量图(模拟) + ax8 = plt.subplot(3, 3, 8) + + # 模拟三个力的向量 + angles = [0, 120, 240] # 三个手指的角度(度) + lengths = [np.mean(finger1_force), np.mean(finger2_force), np.mean(finger3_force)] + + for angle, length, color, label in zip(angles, lengths, + ['red', 'green', 'blue'], + ['手指1', '手指2', '手指3']): + rad = np.deg2rad(angle) + dx = length * np.cos(rad) / 10 + dy = length * np.sin(rad) / 10 + + ax8.arrow(0, 0, dx, dy, head_width=0.5, head_length=0.3, + fc=color, ec=color, linewidth=2, alpha=0.8, label=label) + + # 添加标签 + ax8.text(dx*1.1, dy*1.1, f'{length:.1f}N', fontsize=9, + color=color, fontweight='bold') + + # 绘制合力向量 + total_dx = sum(l * np.cos(np.deg2rad(a)) / 10 for a, l in zip(angles, lengths)) + total_dy = sum(l * np.sin(np.deg2rad(a)) / 10 for a, l in zip(angles, lengths)) + + ax8.arrow(0, 0, total_dx, total_dy, head_width=0.5, head_length=0.3, + fc='black', ec='black', linewidth=3, alpha=0.9, + label='合力') + + ax8.set_xlim(-2, 2) + ax8.set_ylim(-2, 2) + ax8.set_xlabel('X方向', fontsize=10) + ax8.set_ylabel('Y方向', fontsize=10) + ax8.set_title('力向量合成', fontsize=12, fontweight='bold') + ax8.legend(loc='upper right', fontsize=8) + ax8.grid(True, alpha=0.3) + ax8.set_aspect('equal') + + # 9. 抓取稳定性指标 + ax9 = plt.subplot(3, 3, 9) + + # 计算稳定性指标(模拟) + stability_metrics = { + '力平衡度': 0.85, + '同步误差': 0.12, + '最大过冲': 0.18, + '响应时间': 0.08, + '稳态误差': 0.05 + } + + metrics_names = list(stability_metrics.keys()) + metrics_values = list(stability_metrics.values()) + + bars = ax9.barh(metrics_names, metrics_values, color=plt.cm.viridis( + np.linspace(0.2, 0.8, len(metrics_names)))) + + # 添加数值标签 + for bar, value in zip(bars, metrics_values): + width = bar.get_width() + ax9.text(width + 0.01, bar.get_y() + bar.get_height()/2, + f'{value:.2f}', va='center', fontsize=9) + + ax9.set_xlabel('指标值', fontsize=10) + ax9.set_title('抓取稳定性指标', fontsize=12, fontweight='bold') + ax9.set_xlim(0, 1) + ax9.grid(True, alpha=0.3, axis='x') + + plt.suptitle('三指抓取器力分析与可视化', fontsize=18, fontweight='bold', y=1.02) + plt.tight_layout() + plt.savefig('grasp_force_analysis.png', dpi=150, bbox_inches='tight', + facecolor='#F5F7FA') + plt.show() + + # 生成总结统计 + generate_summary_statistics(finger1_force, finger2_force, finger3_force, total_force) + +def generate_summary_statistics(f1, f2, f3, total): + """生成统计摘要""" + print("\n" + "="*60) + print("抓取力统计摘要") + print("="*60) + + stats_data = { + '手指1': f1, + '手指2': f2, + '手指3': f3, + '总力': total + } + + for name, data in stats_data.items(): + print(f"\n{name}:") + print(f" 均值: {np.mean(data):.2f} N") + print(f" 标准差: {np.std(data):.2f} N") + print(f" 最小值: {np.min(data):.2f} N") + print(f" 最大值: {np.max(data):.2f} N") + print(f" 中位数: {np.median(data):.2f} N") + print(f" 变异系数: {np.std(data)/np.mean(data)*100:.1f}%") + + # 计算力平衡度 + mean_forces = [np.mean(f1), np.mean(f2), np.mean(f3)] + balance_score = 1 - np.std(mean_forces) / np.mean(mean_forces) + print(f"\n力平衡度: {balance_score:.3f} (1为完美平衡)") + print("="*60) + +if __name__ == "__main__": + grasp_force_analysis() \ No newline at end of file diff --git a/src/robotic_arm_grasping/grasp_pose_sys.py b/src/robotic_arm_grasping/grasp_pose_sys.py new file mode 100644 index 0000000000..699b507c79 --- /dev/null +++ b/src/robotic_arm_grasping/grasp_pose_sys.py @@ -0,0 +1,845 @@ +import json +import pickle +import time +import warnings + +import matplotlib.pyplot as plt +import mujoco +import mujoco.viewer +import numpy as np +import plotly.express as px +import plotly.graph_objects as go +from matplotlib import cm +from matplotlib.collections import PatchCollection +from matplotlib.patches import Circle, Wedge, Polygon +from mpl_toolkits.mplot3d import Axes3D +from plotly.subplots import make_subplots +from scipy.spatial.transform import Rotation as R +from scipy.stats import gaussian_kde + +warnings.filterwarnings('ignore') + + +class GraspPoseGenerator: + """抓取姿态生成器""" + + def __init__(self, model_path): + """初始化抓取姿态生成器""" + self.model = mujoco.MjModel.from_xml_path(model_path) + self.data = mujoco.MjData(self.model) + + # 关节限制 + self.joint_limits = self._get_joint_limits() + + # 物体列表 + self.objects = ['test_sphere', 'test_box', 'test_cylinder', + 'test_capsule', 'test_ellipsoid'] + + # 抓取位置缓存 + self.grasp_positions = {} + + def _get_joint_limits(self): + """获取关节运动范围""" + limits = {} + for i in range(self.model.njnt): + joint_id = i + joint_type = self.model.jnt_type[i] + if joint_type == mujoco.mjtJoint.mjJNT_HINGE: + limits[f'joint{i + 1}'] = { + 'min': self.model.jnt_range[i, 0], + 'max': self.model.jnt_range[i, 1] + } + return limits + + def generate_random_grasp_pose(self, object_name): + """为指定物体生成随机抓取姿态""" + # 获取物体位置和大小 + obj_body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, object_name) + obj_pos = self.data.xpos[obj_body_id] + + # 物体包围盒 + obj_size = self._get_object_size(object_name) + + # 生成抓取位置(围绕物体) + grasp_positions = [] + grasp_orientations = [] + + # 生成多个抓取点 + n_points = 8 + for i in range(n_points): + # 球坐标 + theta = 2 * np.pi * i / n_points + phi = np.pi / 4 # 45度仰角 + + # 计算抓取点位置 + radius = np.linalg.norm(obj_size) + 0.1 # 距离物体表面0.1米 + x = obj_pos[0] + radius * np.sin(phi) * np.cos(theta) + y = obj_pos[1] + radius * np.sin(phi) * np.sin(theta) + z = obj_pos[2] + radius * np.cos(phi) + + grasp_positions.append([x, y, z]) + + # 计算朝向(指向物体中心) + direction = obj_pos - np.array([x, y, z]) + direction = direction / np.linalg.norm(direction) + + # 计算四元数 + up_vector = np.array([0, 0, 1]) + right_vector = np.cross(direction, up_vector) + right_vector = right_vector / np.linalg.norm(right_vector) + new_up = np.cross(right_vector, direction) + + # 创建旋转矩阵 + rot_matrix = np.column_stack([right_vector, new_up, -direction]) + rotation = R.from_matrix(rot_matrix) + grasp_orientations.append(rotation.as_quat()) + + return grasp_positions, grasp_orientations + + def _get_object_size(self, object_name): + """获取物体尺寸""" + geom_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_GEOM, + f"{object_name}_geom") + + geom_type = self.model.geom_type[geom_id] + geom_size = self.model.geom_size[geom_id] + + if geom_type == mujoco.mjtGeom.mjGEOM_SPHERE: + return np.array([geom_size[0], geom_size[0], geom_size[0]]) + elif geom_type == mujoco.mjtGeom.mjGEOM_BOX: + return geom_size + elif geom_type == mujoco.mjtGeom.mjGEOM_CYLINDER: + return np.array([geom_size[0], geom_size[0], geom_size[1]]) + elif geom_type == mujoco.mjtGeom.mjGEOM_CAPSULE: + return np.array([geom_size[0], geom_size[0], geom_size[1]]) + elif geom_type == mujoco.mjtGeom.mjGEOM_ELLIPSOID: + return geom_size + + return np.array([0.1, 0.1, 0.1]) + + def compute_ik(self, target_pos, target_quat=None): + """计算逆运动学(简化版本)""" + # 这里使用简化方法,实际应用中应使用完整的IK求解器 + # 返回6个关节角度 + joint_angles = np.zeros(6) + + # 计算基座旋转角度 + dx = target_pos[0] + dy = target_pos[1] + joint_angles[0] = np.arctan2(dy, dx) + + # 简化计算其他关节角度 + distance = np.sqrt(dx ** 2 + dy ** 2) + height = target_pos[2] + + # 使用三角形近似计算关节角度 + # 这是一个简化的2连杆逆运动学 + L1 = 0.35 # 上臂长度 + L2 = 0.3 # 前臂长度 + + # 计算肘部角度 + D = np.sqrt(distance ** 2 + (height - 0.1) ** 2) + if D <= L1 + L2 and D >= abs(L1 - L2): + # 可到达的位置 + cos_q2 = (D ** 2 - L1 ** 2 - L2 ** 2) / (2 * L1 * L2) + joint_angles[2] = np.arccos(np.clip(cos_q2, -1, 1)) + + # 计算肩部角度 + alpha = np.arctan2(height - 0.1, distance) + beta = np.arcsin(L2 * np.sin(joint_angles[2]) / D) + joint_angles[1] = alpha - beta + + # 腕部角度(简化) + joint_angles[3] = 0.0 # 腕部旋转 + joint_angles[4] = np.pi / 2 # 腕部俯仰 + joint_angles[5] = 0.0 # 腕部横滚 + + return joint_angles + + +class GraspEvaluator: + """抓取稳定性评估器""" + + def __init__(self, model_path): + """初始化评估器""" + self.model = mujoco.MjModel.from_xml_path(model_path) + self.data = mujoco.MjData(self.model) + + # 评估指标权重 + self.weights = { + 'force_closure': 0.4, + 'contact_points': 0.3, + 'wrench_resistance': 0.2, + 'approach_angle': 0.1 + } + + def evaluate_grasp(self, object_name, joint_angles, finger_positions, + finger_angles, simulate=True): + """评估抓取稳定性""" + if simulate: + # 设置关节位置 + for i in range(6): + self.data.qpos[i] = joint_angles[i] + + # 设置手指位置 + finger_joints = ['finger1_slide', 'finger2_slide', 'finger3_slide', + 'finger1_hinge1', 'finger2_hinge1', 'finger3_hinge1', + 'finger1_hinge2', 'finger2_hinge2', 'finger3_hinge2'] + + for i, joint_name in enumerate(finger_joints): + joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, + joint_name) + self.data.qpos[6 + i] = finger_positions[i // 3] if i < 3 else finger_angles[i - 3] + + # 向前模拟 + mujoco.mj_forward(self.model, self.data) + + # 运行抓取模拟 + for _ in range(100): + # 施加抓取力 + for i in range(3): + slide_joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, + f'motor_finger{i + 1}_slide') + self.data.ctrl[slide_joint_id] = 0.3 # 抓取力 + + mujoco.mj_step(self.model, self.data) + + # 计算评估指标 + metrics = self._compute_grasp_metrics(object_name) + else: + # 不模拟,只计算理论指标 + metrics = self._compute_theoretical_metrics(object_name, joint_angles, + finger_positions, finger_angles) + + # 计算总分 + total_score = self._compute_total_score(metrics) + + return total_score, metrics + + def _compute_grasp_metrics(self, object_name): + """基于模拟计算抓取指标""" + metrics = {} + + # 1. 力闭合分析 + metrics['force_closure'] = self._compute_force_closure(object_name) + + # 2. 接触点分析 + metrics['contact_points'] = self._count_contact_points(object_name) + + # 3. 抗扰动力矩 + metrics['wrench_resistance'] = self._compute_wrench_resistance(object_name) + + # 4. 接近角度 + metrics['approach_angle'] = self._compute_approach_angle(object_name) + + return metrics + + def _compute_theoretical_metrics(self, object_name, joint_angles, + finger_positions, finger_angles): + """计算理论抓取指标""" + metrics = {} + + # 简化的理论计算 + metrics['force_closure'] = 0.7 # 假设值 + metrics['contact_points'] = 3.0 # 三个手指 + metrics['wrench_resistance'] = 0.6 # 假设值 + metrics['approach_angle'] = 0.8 # 假设值 + + return metrics + + def _compute_force_closure(self, object_name): + """计算力闭合指标""" + # 简化的力闭合计算 + obj_body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, + object_name) + + # 检查接触力 + contact_force = 0.0 + for i in range(3): + force_sensor = self.data.sensor(f'sensor_finger{i + 1}_force').data + contact_force += np.linalg.norm(force_sensor) + + # 归一化到0-1范围 + force_closure_score = min(contact_force / 50.0, 1.0) + + return force_closure_score + + def _count_contact_points(self, object_name): + """计算接触点数量""" + contact_count = 0 + + for i in range(3): + touch_sensor = self.data.sensor(f'sensor_finger{i + 1}_touch').data + if touch_sensor[0] > 0.1: # 有接触 + contact_count += 1 + + return contact_count / 3.0 # 归一化 + + def _compute_wrench_resistance(self, object_name): + """计算抗扰动力矩""" + # 简化的抗扰动计算 + resistance_score = 0.5 + + # 检查物体的运动 + obj_body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, + object_name) + obj_vel = np.linalg.norm(self.data.qvel[6:9]) # 物体的线速度 + + # 速度越小,抗扰动能力越强 + if obj_vel < 0.01: + resistance_score = 0.9 + elif obj_vel < 0.05: + resistance_score = 0.7 + elif obj_vel < 0.1: + resistance_score = 0.5 + else: + resistance_score = 0.3 + + return resistance_score + + def _compute_approach_angle(self, object_name): + """计算接近角度""" + # 计算末端执行器指向物体的角度 + ee_pos = self.data.site('grip_center').xpos + obj_pos = self.data.body(object_name).xpos + + direction = obj_pos - ee_pos + direction = direction / np.linalg.norm(direction) + + # 理想方向是垂直向下 + ideal_direction = np.array([0, 0, -1]) + + # 计算角度差 + angle = np.arccos(np.clip(np.dot(direction, ideal_direction), -1, 1)) + + # 角度越小越好,归一化到0-1 + angle_score = 1.0 - min(angle / (np.pi / 2), 1.0) + + return angle_score + + def _compute_total_score(self, metrics): + """计算总分""" + total = 0.0 + for key, weight in self.weights.items(): + total += metrics[key] * weight + + return total + + +class GraspVisualizer: + """抓取可视化器""" + + def __init__(self): + """初始化可视化器""" + plt.style.use('seaborn-v0_8-darkgrid') + self.colors = plt.cm.Set3(np.linspace(0, 1, 12)) + + # Plotly颜色 + self.plotly_colors = px.colors.qualitative.Set3 + + def create_grasp_heatmap(self, grasp_data, object_name): + """创建抓取热力图""" + fig = make_subplots( + rows=2, cols=3, + subplot_titles=('抓取点分布', '抓取分数热力图', '力闭合分数', + '接触点分布', '抗扰动能力', '接近角度'), + specs=[[{'type': 'scatter3d'}, {'type': 'heatmap'}, {'type': 'bar'}], + [{'type': 'scatter'}, {'type': 'radar'}, {'type': 'polar'}]] + ) + + # 提取数据 + positions = np.array([d['position'] for d in grasp_data]) + scores = np.array([d['score'] for d in grasp_data]) + metrics = [d['metrics'] for d in grasp_data] + + # 1. 抓取点分布 (3D散点图) + fig.add_trace( + go.Scatter3d( + x=positions[:, 0], + y=positions[:, 1], + z=positions[:, 2], + mode='markers', + marker=dict( + size=8, + color=scores, + colorscale='Viridis', + showscale=True, + colorbar=dict(title='抓取分数') + ), + text=[f'分数: {s:.3f}' for s in scores], + hoverinfo='text' + ), + row=1, col=1 + ) + + # 添加物体位置 + # (这里需要实际物体位置,暂时使用平均值) + obj_pos = np.mean(positions, axis=0) + fig.add_trace( + go.Scatter3d( + x=[obj_pos[0]], + y=[obj_pos[1]], + z=[obj_pos[2]], + mode='markers', + marker=dict(size=15, color='red', symbol='diamond'), + name='物体位置' + ), + row=1, col=1 + ) + + # 2. 抓取分数热力图 + # 将3D位置投影到2D + from sklearn.decomposition import PCA + pca = PCA(n_components=2) + positions_2d = pca.fit_transform(positions) + + # 创建网格 + x = positions_2d[:, 0] + y = positions_2d[:, 1] + z = scores + + # 插值创建热力图 + from scipy.interpolate import griddata + + xi = np.linspace(x.min(), x.max(), 50) + yi = np.linspace(y.min(), y.max(), 50) + xi, yi = np.meshgrid(xi, yi) + + zi = griddata((x, y), z, (xi, yi), method='cubic') + + fig.add_trace( + go.Heatmap( + x=xi[0, :], + y=yi[:, 0], + z=zi, + colorscale='Viridis', + showscale=True, + colorbar=dict(title='抓取分数') + ), + row=1, col=2 + ) + + # 3. 力闭合分数柱状图 + force_scores = [m['force_closure'] for m in metrics] + fig.add_trace( + go.Bar( + x=list(range(len(force_scores))), + y=force_scores, + marker_color=self.plotly_colors[0], + name='力闭合分数' + ), + row=1, col=3 + ) + + # 4. 接触点分布散点图 + contact_scores = [m['contact_points'] for m in metrics] + fig.add_trace( + go.Scatter( + x=list(range(len(contact_scores))), + y=contact_scores, + mode='markers+lines', + marker=dict(size=10, color=self.plotly_colors[1]), + line=dict(color=self.plotly_colors[1], dash='dash'), + name='接触点分数' + ), + row=2, col=1 + ) + + # 5. 抗扰动能力雷达图 + resistance_scores = [m['wrench_resistance'] for m in metrics] + angles = np.linspace(0, 2 * np.pi, len(resistance_scores), endpoint=False) + + fig.add_trace( + go.Scatterpolar( + r=resistance_scores, + theta=angles * 180 / np.pi, + fill='toself', + name='抗扰动能力', + line_color=self.plotly_colors[2] + ), + row=2, col=2 + ) + + # 6. 接近角度极坐标图 + approach_scores = [m['approach_angle'] for m in metrics] + + fig.add_trace( + go.Scatterpolar( + r=approach_scores, + theta=angles * 180 / np.pi, + mode='markers', + marker=dict(size=10, color=approach_scores, + colorscale='Plasma', showscale=True), + name='接近角度分数' + ), + row=2, col=3 + ) + + # 更新布局 + fig.update_layout( + height=800, + title_text=f'物体 "{object_name}" 的抓取姿态分析', + showlegend=True + ) + + return fig + + def create_grasp_comparison_radar(self, grasp_results): + """创建抓取比较雷达图""" + categories = ['总分', '力闭合', '接触点', '抗扰动', '接近角'] + + fig = go.Figure() + + for obj_name, scores in grasp_results.items(): + fig.add_trace(go.Scatterpolar( + r=[scores['total'], scores['force_closure'], + scores['contact_points'], scores['wrench_resistance'], + scores['approach_angle'], scores['total']], + theta=categories + [categories[0]], + fill='toself', + name=obj_name + )) + + fig.update_layout( + polar=dict( + radialaxis=dict( + visible=True, + range=[0, 1] + )), + showlegend=True, + title='不同物体抓取能力比较' + ) + + return fig + + def create_matplotlib_visualization(self, grasp_data, object_name): + """创建matplotlib可视化""" + fig = plt.figure(figsize=(20, 12)) + + # 提取数据 + positions = np.array([d['position'] for d in grasp_data]) + scores = np.array([d['score'] for d in grasp_data]) + metrics = np.array([[d['metrics']['force_closure'], d['metrics']['contact_points'], + d['metrics']['wrench_resistance'], d['metrics']['approach_angle']] + for d in grasp_data]) + + # 1. 3D抓取点分布 + ax1 = fig.add_subplot(2, 3, 1, projection='3d') + scatter = ax1.scatter(positions[:, 0], positions[:, 1], positions[:, 2], + c=scores, cmap='viridis', s=50, alpha=0.8) + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.set_zlabel('Z (m)') + ax1.set_title(f'抓取点分布 - {object_name}') + plt.colorbar(scatter, ax=ax1, label='抓取分数') + + # 2. 抓取分数分布 + ax2 = fig.add_subplot(2, 3, 2) + ax2.hist(scores, bins=20, alpha=0.7, color='skyblue', edgecolor='black') + ax2.axvline(scores.mean(), color='red', linestyle='--', + label=f'平均分: {scores.mean():.3f}') + ax2.axvline(scores.max(), color='green', linestyle=':', + label=f'最高分: {scores.max():.3f}') + ax2.set_xlabel('抓取分数') + ax2.set_ylabel('频次') + ax2.set_title('抓取分数分布') + ax2.legend() + ax2.grid(True, alpha=0.3) + + # 3. 力闭合分析 + ax3 = fig.add_subplot(2, 3, 3) + force_scores = metrics[:, 0] + contact_bins = np.linspace(0, 1, 11) + force_by_contact = [] + + for i in range(len(contact_bins) - 1): + mask = (metrics[:, 1] >= contact_bins[i]) & (metrics[:, 1] < contact_bins[i + 1]) + if np.any(mask): + force_by_contact.append(force_scores[mask].mean()) + else: + force_by_contact.append(0) + + bars = ax3.bar(range(len(force_by_contact)), force_by_contact, + color=plt.cm.RdYlBu(np.linspace(0, 1, len(force_by_contact)))) + ax3.set_xlabel('接触点分数区间') + ax3.set_ylabel('平均力闭合分数') + ax3.set_title('力闭合 vs 接触点') + ax3.set_xticks(range(len(force_by_contact))) + ax3.set_xticklabels([f'{contact_bins[i]:.1f}-{contact_bins[i + 1]:.1f}' + for i in range(len(contact_bins) - 1)], rotation=45) + + # 4. 抗扰动能力与接近角度关系 + ax4 = fig.add_subplot(2, 3, 4) + scatter = ax4.scatter(metrics[:, 2], metrics[:, 3], + c=scores, cmap='plasma', s=60, alpha=0.7) + ax4.set_xlabel('抗扰动能力分数') + ax4.set_ylabel('接近角度分数') + ax4.set_title('抗扰动 vs 接近角度') + plt.colorbar(scatter, ax=ax4, label='总分') + + # 添加趋势线 + z = np.polyfit(metrics[:, 2], metrics[:, 3], 1) + p = np.poly1d(z) + x_line = np.linspace(metrics[:, 2].min(), metrics[:, 2].max(), 100) + ax4.plot(x_line, p(x_line), "r--", alpha=0.5, label=f'趋势线: y={z[0]:.2f}x+{z[1]:.2f}') + ax4.legend() + + # 5. 各指标相关性热力图 + ax5 = fig.add_subplot(2, 3, 5) + # 修复维度不匹配问题 + # 将scores转换为列向量,然后与metrics水平拼接 + scores_col = scores.reshape(-1, 1) # 转换为列向量 (8, 1) + all_data = np.hstack([scores_col, metrics]) # 水平拼接 (8, 5) + correlation_matrix = np.corrcoef(all_data.T) + + im = ax5.imshow(correlation_matrix, cmap='coolwarm', vmin=-1, vmax=1) + ax5.set_xticks(range(len(['总分', '力闭合', '接触点', '抗扰动', '接近角']))) + ax5.set_yticks(range(len(['总分', '力闭合', '接触点', '抗扰动', '接近角']))) + ax5.set_xticklabels(['总分', '力闭合', '接触点', '抗扰动', '接近角']) + ax5.set_yticklabels(['总分', '力闭合', '接触点', '抗扰动', '接近角']) + ax5.set_title('指标相关性热力图') + + # 添加数值标签 + for i in range(correlation_matrix.shape[0]): + for j in range(correlation_matrix.shape[1]): + text = ax5.text(j, i, f'{correlation_matrix[i, j]:.2f}', + ha="center", va="center", + color="white" if abs(correlation_matrix[i, j]) > 0.5 else "black") + + plt.colorbar(im, ax=ax5) + + # 6. 最佳抓取姿态展示 + ax6 = fig.add_subplot(2, 3, 6, projection='3d') + best_idx = np.argmax(scores) + best_pos = positions[best_idx] + best_score = scores[best_idx] + + # 绘制最佳抓取点 + ax6.scatter(best_pos[0], best_pos[1], best_pos[2], + c='gold', s=200, marker='*', label=f'最佳抓取点\n分数: {best_score:.3f}') + + # 绘制其他抓取点 + other_positions = np.delete(positions, best_idx, axis=0) + ax6.scatter(other_positions[:, 0], other_positions[:, 1], other_positions[:, 2], + c='gray', s=20, alpha=0.3, label='其他抓取点') + + # 绘制力闭合锥(简化表示) + u = np.linspace(0, 2 * np.pi, 30) + v = np.linspace(0, np.pi, 30) + + x_cone = 0.1 * np.outer(np.cos(u), np.sin(v)) + best_pos[0] + y_cone = 0.1 * np.outer(np.sin(u), np.sin(v)) + best_pos[1] + z_cone = 0.1 * np.outer(np.ones(np.size(u)), np.cos(v)) + best_pos[2] + + ax6.plot_surface(x_cone, y_cone, z_cone, alpha=0.2, color='red') + + ax6.set_xlabel('X (m)') + ax6.set_ylabel('Y (m)') + ax6.set_zlabel('Z (m)') + ax6.set_title(f'最佳抓取姿态展示') + ax6.legend() + + plt.suptitle(f'物体 "{object_name}" 的抓取姿态分析与可视化', fontsize=16, fontweight='bold') + plt.tight_layout() + + return fig + + +def main(): + """主函数:运行抓取姿态生成与评估系统""" + print("=" * 60) + print("抓取姿态生成与评估可视化系统") + print("=" * 60) + + # 模型文件路径 + model_path = "arm_with_gripper.xml" # 您的模型文件 + + # 初始化组件 + print("初始化系统组件...") + generator = GraspPoseGenerator(model_path) + evaluator = GraspEvaluator(model_path) + visualizer = GraspVisualizer() + + # 选择要分析的物体 + objects_to_analyze = ['test_sphere', 'test_box', 'test_cylinder'] + + all_results = {} + + for obj_name in objects_to_analyze: + print(f"\n分析物体: {obj_name}") + print("-" * 40) + + # 1. 生成抓取姿态 + print("生成抓取姿态...") + grasp_positions, grasp_orientations = generator.generate_random_grasp_pose(obj_name) + + # 2. 评估每个抓取姿态 + print("评估抓取稳定性...") + grasp_data = [] + + for i, (pos, quat) in enumerate(zip(grasp_positions, grasp_orientations)): + print(f" 评估抓取点 {i + 1}/{len(grasp_positions)}...", end='\r') + + # 计算逆运动学 + joint_angles = generator.compute_ik(pos, quat) + + # 设置手指位置(简化:所有手指相同) + finger_positions = [0.03, 0.03, 0.03] # 滑动位置 + finger_angles = [0.5, 0.3, 0.5, 0.3, 0.5, 0.3] # 铰链角度 + + # 评估抓取 + score, metrics = evaluator.evaluate_grasp( + obj_name, joint_angles, finger_positions, finger_angles, + simulate=False # 设置为True进行真实模拟 + ) + + grasp_data.append({ + 'position': pos, + 'orientation': quat, + 'joint_angles': joint_angles, + 'score': score, + 'metrics': metrics + }) + + print(f"\n完成评估! 生成 {len(grasp_data)} 个抓取姿态") + + # 3. 保存结果 + all_results[obj_name] = { + 'grasp_data': grasp_data, + 'average_score': np.mean([d['score'] for d in grasp_data]), + 'best_score': np.max([d['score'] for d in grasp_data]) + } + + # 4. 创建可视化 + print("生成可视化图表...") + + # 创建matplotlib图表 + mpl_fig = visualizer.create_matplotlib_visualization(grasp_data, obj_name) + mpl_fig.savefig(f'grasp_analysis_{obj_name}.png', dpi=150, bbox_inches='tight') + print(f" ✓ 保存 matplotlib 图表: grasp_analysis_{obj_name}.png") + + # 创建plotly交互式图表 + plotly_fig = visualizer.create_grasp_heatmap(grasp_data, obj_name) + plotly_fig.write_html(f'grasp_analysis_{obj_name}_interactive.html') + print(f" ✓ 保存交互式图表: grasp_analysis_{obj_name}_interactive.html") + + # 显示最佳抓取结果 + best_grasp = max(grasp_data, key=lambda x: x['score']) + print(f"\n 最佳抓取点:") + print(f" 位置: {best_grasp['position']}") + print(f" 分数: {best_grasp['score']:.3f}") + print(f" 力闭合: {best_grasp['metrics']['force_closure']:.3f}") + print(f" 接触点: {best_grasp['metrics']['contact_points']:.3f}") + + # 5. 创建比较图表 + print("\n" + "=" * 60) + print("创建物体间比较图表...") + + # 准备比较数据 + comparison_data = {} + for obj_name, results in all_results.items(): + # 计算平均指标 + grasp_data = results['grasp_data'] + metrics_list = [d['metrics'] for d in grasp_data] + + comparison_data[obj_name] = { + 'total': results['average_score'], + 'force_closure': np.mean([m['force_closure'] for m in metrics_list]), + 'contact_points': np.mean([m['contact_points'] for m in metrics_list]), + 'wrench_resistance': np.mean([m['wrench_resistance'] for m in metrics_list]), + 'approach_angle': np.mean([m['approach_angle'] for m in metrics_list]) + } + + # 创建比较雷达图 + radar_fig = visualizer.create_grasp_comparison_radar(comparison_data) + radar_fig.write_html('grasp_comparison_radar.html') + print(" ✓ 保存比较雷达图: grasp_comparison_radar.html") + + # 创建比较柱状图 + fig, ax = plt.subplots(figsize=(12, 6)) + objects = list(comparison_data.keys()) + metrics_names = ['total', 'force_closure', 'contact_points', + 'wrench_resistance', 'approach_angle'] + metric_labels = ['总分', '力闭合', '接触点', '抗扰动', '接近角'] + + x = np.arange(len(objects)) + width = 0.15 + + for i, (metric, label) in enumerate(zip(metrics_names, metric_labels)): + values = [comparison_data[obj][metric] for obj in objects] + ax.bar(x + i * width - 2 * width, values, width, label=label) + + ax.set_xlabel('物体') + ax.set_ylabel('分数') + ax.set_title('不同物体抓取能力比较') + ax.set_xticks(x) + ax.set_xticklabels(objects) + ax.legend() + ax.grid(True, alpha=0.3) + + plt.tight_layout() + plt.savefig('grasp_comparison_bar.png', dpi=150) + print(" ✓ 保存比较柱状图: grasp_comparison_bar.png") + + # 6. 保存数据到文件 + print("\n保存数据到文件...") + with open('grasp_analysis_results.pkl', 'wb') as f: + pickle.dump(all_results, f) + print(" ✓ 保存原始数据: grasp_analysis_results.pkl") + + # 保存摘要报告 + summary = { + 'analysis_date': time.strftime("%Y-%m-%d %H:%M:%S"), + 'objects_analyzed': objects_to_analyze, + 'summary': {} + } + + for obj_name, results in all_results.items(): + summary['summary'][obj_name] = { + 'num_grasp_poses': len(results['grasp_data']), + 'average_score': float(results['average_score']), + 'best_score': float(results['best_score']) + } + + with open('grasp_analysis_summary.json', 'w', encoding='utf-8') as f: + json.dump(summary, f, indent=2, ensure_ascii=False) + print(" ✓ 保存摘要报告: grasp_analysis_summary.json") + + print("\n" + "=" * 60) + print("分析完成!") + print(f"分析了 {len(objects_to_analyze)} 个物体") + print(f"生成了 {len(objects_to_analyze) * 2 + 2} 个可视化文件") + print("=" * 60) + + # 显示一个图表 + plt.figure(figsize=(10, 6)) + objects = list(all_results.keys()) + avg_scores = [all_results[obj]['average_score'] for obj in objects] + best_scores = [all_results[obj]['best_score'] for obj in objects] + + x = np.arange(len(objects)) + width = 0.35 + + plt.bar(x - width / 2, avg_scores, width, label='平均分数', alpha=0.8) + plt.bar(x + width / 2, best_scores, width, label='最佳分数', alpha=0.8) + + plt.xlabel('物体') + plt.ylabel('抓取分数') + plt.title('抓取性能总结') + plt.xticks(x, objects) + plt.legend() + plt.grid(True, alpha=0.3) + plt.tight_layout() + + # 在柱状图上添加数值 + for i, (avg, best) in enumerate(zip(avg_scores, best_scores)): + plt.text(i - width / 2, avg + 0.01, f'{avg:.3f}', + ha='center', va='bottom', fontsize=9) + plt.text(i + width / 2, best + 0.01, f'{best:.3f}', + ha='center', va='bottom', fontsize=9) + + plt.show() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/robotic_arm_grasping/robot_arm_workspace_visualization.py b/src/robotic_arm_grasping/robot_arm_workspace_visualization.py new file mode 100644 index 0000000000..58bab80332 --- /dev/null +++ b/src/robotic_arm_grasping/robot_arm_workspace_visualization.py @@ -0,0 +1,154 @@ +""" +机械臂工作空间3D可视化 +展示机械臂末端可达的所有位置,视觉效果很酷 +""" +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from matplotlib.animation import FuncAnimation +from scipy.spatial import ConvexHull + +def workspace_visualization(): + """生成机械臂工作空间3D可视化""" + # 模拟工作空间点云(使用逆运动学采样) + np.random.seed(42) + n_points = 2000 + + # 生成工作空间点(模拟机械臂可达范围) + # 这里用一个甜甜圈形状+椭球体模拟 + theta = np.random.uniform(0, 2*np.pi, n_points) + phi = np.random.uniform(0, np.pi, n_points) + + # 主工作空间(甜甜圈形状) + r = 0.5 + 0.2 * np.random.randn(n_points) + x_torus = (0.8 + 0.3 * r * np.cos(phi)) * np.cos(theta) + y_torus = (0.8 + 0.3 * r * np.cos(phi)) * np.sin(theta) + z_torus = 0.3 * r * np.sin(phi) + 0.5 + + # 添加一个椭球区域(表示额外的工作空间) + n_ellipsoid = 500 + u = np.random.rand(n_ellipsoid) + v = np.random.rand(n_ellipsoid) + theta_e = 2 * np.pi * u + phi_e = np.arccos(2 * v - 1) + + a, b, c = 0.3, 0.2, 0.4 # 椭球半轴 + x_ell = a * np.sin(phi_e) * np.cos(theta_e) - 0.2 + y_ell = b * np.sin(phi_e) * np.sin(theta_e) + 0.3 + z_ell = c * np.cos(phi_e) + 0.7 + + # 合并所有点 + x = np.concatenate([x_torus, x_ell]) + y = np.concatenate([y_torus, y_ell]) + z = np.concatenate([z_torus, z_ell]) + + # 创建3D图形 + fig = plt.figure(figsize=(14, 10)) + + # 1. 3D散点图 + ax1 = fig.add_subplot(221, projection='3d') + scatter = ax1.scatter(x, y, z, c=z, cmap='plasma', alpha=0.3, + s=10, marker='o', edgecolors='none') + + ax1.set_xlabel('X (m)', fontsize=10, labelpad=10) + ax1.set_ylabel('Y (m)', fontsize=10, labelpad=10) + ax1.set_zlabel('Z (m)', fontsize=10, labelpad=10) + ax1.set_title('工作空间点云(3D视图)', fontsize=12, fontweight='bold', pad=20) + ax1.grid(True, alpha=0.3) + ax1.view_init(elev=25, azim=45) + + # 添加颜色条 + cbar = plt.colorbar(scatter, ax=ax1, shrink=0.6, pad=0.1) + cbar.set_label('高度 (m)', fontsize=9) + + # 2. 俯视图(XY平面) + ax2 = fig.add_subplot(222) + hexbin = ax2.hexbin(x, y, C=z, gridsize=30, cmap='YlOrRd', alpha=0.9) + ax2.set_xlabel('X (m)', fontsize=10) + ax2.set_ylabel('Y (m)', fontsize=10) + ax2.set_title('工作空间俯视图(XY平面)', fontsize=12, fontweight='bold') + ax2.grid(True, alpha=0.3) + ax2.axis('equal') + + # 添加颜色条 + cbar2 = plt.colorbar(hexbin, ax=ax2, shrink=0.8) + cbar2.set_label('平均高度 (m)', fontsize=9) + + # 3. 侧视图(XZ平面) + ax3 = fig.add_subplot(223) + hb = ax3.hexbin(x, z, gridsize=30, cmap='Blues', alpha=0.9) + ax3.set_xlabel('X (m)', fontsize=10) + ax3.set_ylabel('Z (m)', fontsize=10) + ax3.set_title('工作空间侧视图(XZ平面)', fontsize=12, fontweight='bold') + ax3.grid(True, alpha=0.3) + + cbar3 = plt.colorbar(hb, ax=ax3, shrink=0.8) + cbar3.set_label('点密度', fontsize=9) + + # 4. 前视图(YZ平面) + ax4 = fig.add_subplot(224) + hb4 = ax4.hexbin(y, z, gridsize=30, cmap='Greens', alpha=0.9) + ax4.set_xlabel('Y (m)', fontsize=10) + ax4.set_ylabel('Z (m)', fontsize=10) + ax4.set_title('工作空间前视图(YZ平面)', fontsize=12, fontweight='bold') + ax4.grid(True, alpha=0.3) + + cbar4 = plt.colorbar(hb4, ax=ax4, shrink=0.8) + cbar4.set_label('点密度', fontsize=9) + + plt.suptitle('6-DOF机械臂工作空间可视化分析', fontsize=16, fontweight='bold', y=1.02) + plt.tight_layout() + plt.savefig('workspace_analysis.png', dpi=150, bbox_inches='tight', + facecolor='#F5F7FA') + plt.show() + + # 额外:创建凸包和体积计算 + create_convex_hull(x, y, z) + +def create_convex_hull(x, y, z): + """计算工作空间的凸包和体积""" + # 采样点用于凸包计算 + points = np.vstack([x[::20], y[::20], z[::20]]).T # 下采样 + + # 计算凸包 + hull = ConvexHull(points) + + # 创建凸包可视化 + fig = plt.figure(figsize=(12, 8)) + ax = fig.add_subplot(111, projection='3d') + + # 绘制凸包 + for simplex in hull.simplices: + simplex = np.append(simplex, simplex[0]) # 闭合三角形 + ax.plot(points[simplex, 0], points[simplex, 1], points[simplex, 2], + 'b-', alpha=0.3, linewidth=0.5) + + # 绘制原始点 + ax.scatter(x[::50], y[::50], z[::50], c='red', alpha=0.3, s=10, + label='工作空间点') + + # 计算体积 + volume = hull.volume + area = hull.area + + # 添加文本标注 + ax.text2D(0.05, 0.95, f'工作空间体积: {volume:.3f} m³\n' + f'表面积: {area:.3f} m²\n' + f'采样点数: {len(points)}', + transform=ax.transAxes, fontsize=10, + bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8)) + + ax.set_xlabel('X (m)', fontsize=11) + ax.set_ylabel('Y (m)', fontsize=11) + ax.set_zlabel('Z (m)', fontsize=11) + ax.set_title('工作空间凸包分析', fontsize=14, fontweight='bold', pad=20) + ax.legend() + ax.grid(True, alpha=0.3) + ax.view_init(elev=20, azim=30) + + plt.tight_layout() + plt.savefig('workspace_convex_hull.png', dpi=150, bbox_inches='tight') + plt.show() + +if __name__ == "__main__": + workspace_visualization() \ No newline at end of file diff --git a/src/robotic_arm_grasping/test.py b/src/robotic_arm_grasping/test.py new file mode 100644 index 0000000000..5faa317914 --- /dev/null +++ b/src/robotic_arm_grasping/test.py @@ -0,0 +1,112 @@ +import mujoco +import os +import shutil +import subprocess +import sys + +# 原始文件路径 - 修改为机器人手臂模型 +original_model_path = r"C:\Users\龙忠梁\Downloads\mujoco-3.3.7-windows-x86_64\model\robotic_arm\arm_with_gripper.xml" + +# 尝试复制到临时目录以避免中文路径问题 +temp_dir = r"C:\temp" +temp_model_path = os.path.join(temp_dir, "arm_with_gripper.xml") + +print(f"检查原始文件是否存在: {original_model_path}") +print(f"原始文件是否存在: {os.path.exists(original_model_path)}") + +if os.path.exists(original_model_path): + try: + # 创建临时目录(如果不存在) + os.makedirs(temp_dir, exist_ok=True) + + # 复制文件到临时位置 + print(f"正在将文件复制到: {temp_model_path}") + shutil.copy2(original_model_path, temp_model_path) + print("文件复制成功") + + # 尝试从临时位置加载模型 + print("尝试从临时路径加载模型...") + model = mujoco.MjModel.from_xml_path(temp_model_path) + data = mujoco.MjData(model) + print("模型加载成功") + print(f"模型名称: {model.names[0:50].decode('utf-8') if model.names else '未知'}") + print(f"关节数量: {model.njnt}") + print(f"自由度数量: {model.nq}") + print(f"执行器数量: {model.nu}") + print(f"几何体数量: {model.ngeom}") + print(f"身体数量: {model.nbody}") + print(f"传感器数量: {model.nsensor}") + + # 打印关节信息 + print("\n关节信息:") + for i in range(model.njnt): + joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) + joint_type = model.jnt_type[i] + type_names = ['free', 'ball', 'slide', 'hinge'] + print(f" 关节 {i}: {joint_name.decode('utf-8') if isinstance(joint_name, bytes) else joint_name} (类型: {type_names[joint_type]})") + + # 打印执行器信息 + print("\n执行器信息:") + for i in range(model.nu): + actuator_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) + print(f" 执行器 {i}: {actuator_name.decode('utf-8') if isinstance(actuator_name, bytes) else actuator_name}") + + # 打印传感器信息 + print("\n传感器信息:") + for i in range(min(10, model.nsensor)): # 只显示前10个传感器 + sensor_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_SENSOR, i) + print(f" 传感器 {i}: {sensor_name.decode('utf-8') if isinstance(sensor_name, bytes) else sensor_name}") + if model.nsensor > 10: + print(f" ... 还有 {model.nsensor - 10} 个传感器") + + # 尝试启动MuJoCo自带的可视化工具 + print("\n" + "="*50) + print("正在尝试启动MuJoCo可视化工具...") + + # 获取MuJoCo根目录 + mujoco_root = os.path.dirname(os.path.dirname(os.path.dirname(original_model_path))) + bin_dir = os.path.join(mujoco_root, "bin") + simulate_exe = os.path.join(bin_dir, "simulate.exe") + + if os.path.exists(simulate_exe): + print(f"找到可视化工具: {simulate_exe}") + # 启动可视化工具 + try: + # 使用subprocess启动可视化工具 + subprocess.Popen([simulate_exe, temp_model_path]) + print("MuJoCo可视化工具已启动") + print("模型文件已加载") + except Exception as e: + print(f"启动可视化工具时出错: {e}") + print("请手动启动可视化工具:") + print(f"1. 运行 {simulate_exe}") + print(f"2. 在可视化工具中打开 {temp_model_path}") + else: + print("未找到MuJoCo可视化工具") + print("请检查MuJoCo安装目录结构") + print(f"期望的工具路径: {simulate_exe}") + print("\n手动启动方法:") + print(f"1. 打开命令提示符") + print(f"2. 导航到 {bin_dir}") + print(f"3. 运行: simulate.exe \"{temp_model_path}\"") + + except Exception as e: + print(f"处理文件时出错: {e}") + import traceback + traceback.print_exc() + + finally: + # 注意:不清理由我们启动的可视化工具使用的临时文件 + # 用户可能仍在查看模型 + print("\n注意: 临时文件未清理,因为可视化工具可能仍在使用它") + print(f"临时文件位置: {temp_model_path}") +else: + print(f"原始文件不存在: {original_model_path}") + print("请检查文件路径是否正确") + + # 检查目录结构 + base_dir = r"C:\Users\龙忠梁\Downloads\mujoco-3.3.7-windows-x86_64\model\robotic_arm" + if os.path.exists(base_dir): + print("robotic_arm目录下的文件:") + for file in os.listdir(base_dir): + print(f" {file}") \ No newline at end of file