From a0b13baad6371269312b24995ddd56c9491194a5 Mon Sep 17 00:00:00 2001 From: longzhongliang <1493886965@qq.com> Date: Wed, 26 Nov 2025 16:14:56 +0800 Subject: [PATCH 1/7] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E9=80=89=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/robotic_arm_grasping1/README.md | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 src/robotic_arm_grasping1/README.md diff --git a/src/robotic_arm_grasping1/README.md b/src/robotic_arm_grasping1/README.md new file mode 100644 index 0000000000..8bbe162607 --- /dev/null +++ b/src/robotic_arm_grasping1/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 From aae3d3674135db8c695fa316d18029103a7fbc87 Mon Sep 17 00:00:00 2001 From: longzhongliang <1493886965@qq.com> Date: Wed, 26 Nov 2025 16:17:58 +0800 Subject: [PATCH 2/7] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E9=80=89=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/{robotic_arm_grasping1 => robotic_arm_grasping}/README.md | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/{robotic_arm_grasping1 => robotic_arm_grasping}/README.md (100%) diff --git a/src/robotic_arm_grasping1/README.md b/src/robotic_arm_grasping/README.md similarity index 100% rename from src/robotic_arm_grasping1/README.md rename to src/robotic_arm_grasping/README.md From fe38550f657cd11e0408313b479aff2d1be2d234 Mon Sep 17 00:00:00 2001 From: longzhongliang <1493886965@qq.com> Date: Mon, 15 Dec 2025 12:20:51 +0800 Subject: [PATCH 3/7] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E4=BA=86=E6=A8=A1?= =?UTF-8?q?=E5=9E=8B=E6=96=87=E4=BB=B6=E5=92=8C=E6=B5=8B=E8=AF=95=E4=BB=A3?= =?UTF-8?q?=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/robotic_arm_grasping/arm_with_gripper.xml | 212 ++++++++++++++++++ .../\346\265\213\350\257\225.py" | 112 +++++++++ 2 files changed, 324 insertions(+) create mode 100644 src/robotic_arm_grasping/arm_with_gripper.xml create mode 100644 "src/robotic_arm_grasping/\346\265\213\350\257\225.py" 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/\346\265\213\350\257\225.py" "b/src/robotic_arm_grasping/\346\265\213\350\257\225.py" new file mode 100644 index 0000000000..5faa317914 --- /dev/null +++ "b/src/robotic_arm_grasping/\346\265\213\350\257\225.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 From 35763278ee79d066b215850f2b053248227e48c3 Mon Sep 17 00:00:00 2001 From: longzhongliang <1493886965@qq.com> Date: Mon, 15 Dec 2025 12:47:30 +0800 Subject: [PATCH 4/7] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E4=BA=86=E6=A8=A1?= =?UTF-8?q?=E5=9E=8B=E6=96=87=E4=BB=B6=E5=92=8C=E6=B5=8B=E8=AF=95=E4=BB=A3?= =?UTF-8?q?=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../robotic_arm_grasping/test.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename "src/robotic_arm_grasping/\346\265\213\350\257\225.py" => src/robotic_arm_grasping/test.py (100%) diff --git "a/src/robotic_arm_grasping/\346\265\213\350\257\225.py" b/src/robotic_arm_grasping/test.py similarity index 100% rename from "src/robotic_arm_grasping/\346\265\213\350\257\225.py" rename to src/robotic_arm_grasping/test.py From 623c1c23630056b23ae4a7916c216a3fbfed6192 Mon Sep 17 00:00:00 2001 From: longzhongliang <1493886965@qq.com> Date: Wed, 17 Dec 2025 20:22:13 +0800 Subject: [PATCH 5/7] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E4=BA=86=E6=A8=A1?= =?UTF-8?q?=E5=9E=8B=E6=96=87=E4=BB=B6=E5=92=8C=E6=B5=8B=E8=AF=95=E4=BB=A3?= =?UTF-8?q?=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/robotic_arm_grasping/grasp.py | 407 ++++++++++++++++++++++++++++++ 1 file changed, 407 insertions(+) create mode 100644 src/robotic_arm_grasping/grasp.py 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 From 00a47f892547c7256d0943cca4bcf0d3c1e4fdac Mon Sep 17 00:00:00 2001 From: longzhongliang <1493886965@qq.com> Date: Thu, 18 Dec 2025 20:27:42 +0800 Subject: [PATCH 6/7] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E6=8E=A7?= =?UTF-8?q?=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/robotic_arm_grasping/arm_control.py | 156 ++++++++++++++++++++++++ 1 file changed, 156 insertions(+) create mode 100644 src/robotic_arm_grasping/arm_control.py diff --git a/src/robotic_arm_grasping/arm_control.py b/src/robotic_arm_grasping/arm_control.py new file mode 100644 index 0000000000..d2e13ef94f --- /dev/null +++ b/src/robotic_arm_grasping/arm_control.py @@ -0,0 +1,156 @@ +""" +6-DOF 机械臂 + 三指夹爪 +Tkinter 滑杆(硬限位防重置)+ Grab / Release 按钮 +**修正:同时控制 9 个夹爪执行器,equality 同步生效** +""" +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 From 7f146b3f9ed674a04b91bca470550088a56d73c8 Mon Sep 17 00:00:00 2001 From: longzhongliang <1493886965@qq.com> Date: Thu, 18 Dec 2025 20:53:23 +0800 Subject: [PATCH 7/7] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E6=8E=A7?= =?UTF-8?q?=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/robotic_arm_grasping/arm_control.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/robotic_arm_grasping/arm_control.py b/src/robotic_arm_grasping/arm_control.py index d2e13ef94f..a04f41db8c 100644 --- a/src/robotic_arm_grasping/arm_control.py +++ b/src/robotic_arm_grasping/arm_control.py @@ -1,8 +1,3 @@ -""" -6-DOF 机械臂 + 三指夹爪 -Tkinter 滑杆(硬限位防重置)+ Grab / Release 按钮 -**修正:同时控制 9 个夹爪执行器,equality 同步生效** -""" import threading, queue import tkinter as tk import glfw, mujoco as mj