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_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_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/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_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/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