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