From 4d4864d55c464366e511f81565ae445c5469b40f Mon Sep 17 00:00:00 2001
From: longzhongliang <1493886965@qq.com>
Date: Mon, 22 Dec 2025 11:10:17 +0800
Subject: [PATCH 1/3] =?UTF-8?q?=E7=A2=B0=E6=92=9E=E5=88=86=E6=9E=90?=
 =?UTF-8?q?=E5=8F=AF=E8=A7=86=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../collision_risk_analysis_visualization.py  | 374 ++++++++++++++++++
 src/robotic_arm_grasping/test.py              |   6 +-
 2 files changed, 377 insertions(+), 3 deletions(-)
 create mode 100644 src/robotic_arm_grasping/collision_risk_analysis_visualization.py

diff --git a/src/robotic_arm_grasping/collision_risk_analysis_visualization.py b/src/robotic_arm_grasping/collision_risk_analysis_visualization.py
new file mode 100644
index 0000000000..a10dcdaf46
--- /dev/null
+++ b/src/robotic_arm_grasping/collision_risk_analysis_visualization.py
@@ -0,0 +1,374 @@
+"""
+完整版：机械臂碰撞检测 + MuJoCo可视化界面
+包含碰撞分析和实时仿真
+修改说明：
+1. 所有路径改为相对路径
+2. 解决可视化图表中文乱码问题
+3. 优化文件处理和错误处理
+4. 修复临时文件创建问题
+"""
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+import os
+import sys
+import time
+import tempfile
+import warnings
+warnings.filterwarnings('ignore')
+
+# 设置Matplotlib支持中文显示（解决乱码问题）
+plt.rcParams['font.sans-serif'] = ['SimHei', 'Microsoft YaHei', 'DejaVu Sans']  # 设置中文字体
+plt.rcParams['axes.unicode_minus'] = False  # 正确显示负号
+
+# 尝试导入MuJoCo，如果失败则使用模拟模式
+try:
+    import mujoco
+    import mujoco.viewer
+    MUJOCO_AVAILABLE = True
+    print("✅ MuJoCo 已安装，将启动可视化界面")
+except ImportError:
+    MUJOCO_AVAILABLE = False
+    print("⚠️  MuJoCo 未安装，将仅生成图表（无可视化界面）")
+    print("💡 安装命令: pip install mujoco mujoco-python-viewer")
+
+def generate_collision_analysis():
+    """生成碰撞分析图表"""
+    print("="*60)
+    print("机械臂碰撞风险分析系统")
+    print("="*60)
+
+    # 1. 生成模拟工作空间数据
+    print("正在生成工作空间数据...")
+    np.random.seed(42)
+    n_points = 300
+
+    # 模拟机械臂工作空间
+    theta = np.random.uniform(0, 2*np.pi, n_points)
+    phi = np.random.uniform(0, np.pi, n_points)
+    r = 0.5 + 0.2 * np.random.randn(n_points)
+
+    x = 0.6 * np.cos(theta) * np.sin(phi)
+    y = 0.6 * np.sin(theta) * np.sin(phi)
+    z = 0.5 + 0.3 * np.cos(phi)
+
+    points = np.vstack([x, y, z]).T
+
+    # 2. 计算碰撞风险
+    print("正在计算碰撞风险...")
+    risks = []
+    for point in points:
+        risk = 0
+
+        # 墙壁风险 (x=0.7)
+        wall_dist = abs(point[0] - 0.7)
+        if wall_dist < 0.15:
+            risk += 0.8 * (0.15 - wall_dist) / 0.15
+
+        # 中心障碍物
+        center_dist = np.sqrt(point[0]**2 + point[1]**2)
+        if center_dist < 0.2:
+            risk += 0.6 * (0.2 - center_dist) / 0.2
+
+        # 天花板风险
+        if point[2] > 0.9:
+            risk += 0.4
+
+        # 地面风险
+        if point[2] < 0.1:
+            risk += 0.3
+
+        risk = min(1.0, risk)
+        risks.append(risk)
+
+    risks = np.array(risks)
+
+    # 3. 绘制图表
+    print("正在生成可视化图表...")
+    fig = plt.figure(figsize=(15, 6))
+
+    # 左侧：3D风险图
+    ax1 = fig.add_subplot(121, projection='3d')
+    scatter = ax1.scatter(points[:, 0], points[:, 1], points[:, 2],
+                         c=risks, cmap='RdYlGn_r',
+                         alpha=0.7, s=20, edgecolors='none')
+
+    # 添加障碍物标记
+    ax1.plot([0.7, 0.7], [-0.8, 0.8], [0, 1], 'k-', linewidth=3, alpha=0.5, label='墙壁')
+
+    ax1.set_xlabel('X (米)', fontsize=12, labelpad=10)
+    ax1.set_ylabel('Y (米)', fontsize=12, labelpad=10)
+    ax1.set_zlabel('Z (米)', fontsize=12, labelpad=10)
+    ax1.set_title('3D碰撞风险热力图', fontsize=14, fontweight='bold')
+    ax1.legend(fontsize=10)
+    ax1.view_init(elev=25, azim=45)
+    ax1.grid(True, alpha=0.3)
+
+    plt.colorbar(scatter, ax=ax1, shrink=0.7, pad=0.1, label='碰撞风险')
+
+    # 右侧：统计图
+    ax2 = fig.add_subplot(122)
+
+    # 风险等级统计
+    low_risk = np.sum(risks < 0.3)
+    medium_risk = np.sum((risks >= 0.3) & (risks < 0.7))
+    high_risk = np.sum(risks >= 0.7)
+
+    categories = ['低风险', '中风险', '高风险']
+    counts = [low_risk, medium_risk, high_risk]
+    percentages = [c/n_points*100 for c in counts]
+    colors = ['#2E8B57', '#FFA500', '#DC143C']
+
+    bars = ax2.bar(categories, percentages, color=colors, edgecolor='black', alpha=0.8)
+
+    # 添加标签
+    for bar, percent in zip(bars, percentages):
+        height = bar.get_height()
+        ax2.text(bar.get_x() + bar.get_width()/2, height + 1,
+                f'{percent:.1f}%', ha='center', fontsize=11, fontweight='bold')
+
+    ax2.set_ylabel('占比 (%)', fontsize=12)
+    ax2.set_title('风险区域分布', fontsize=14, fontweight='bold')
+    ax2.set_ylim(0, 100)
+    ax2.grid(True, alpha=0.3, axis='y')
+
+    # 总结文本
+    summary = f'分析结果:\n'
+    summary += f'• 安全区域: {percentages[0]:.1f}%\n'
+    summary += f'• 危险区域: {percentages[2]:.1f}%\n'
+    summary += f'• 总采样点: {n_points}'
+
+    ax2.text(0.05, 0.95, summary, transform=ax2.transAxes, fontsize=11,
+            bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8),
+            verticalalignment='top')
+
+    plt.suptitle('机械臂工作空间碰撞风险分析', fontsize=16, fontweight='bold')
+    plt.tight_layout()
+
+    # 保存图表到当前目录
+    output_file = 'collision_analysis_result.png'
+    plt.savefig(output_file, dpi=150, bbox_inches='tight')
+    plt.show()
+
+    print(f"\n✅ 碰撞分析完成！")
+    print(f"📊 图表已保存: {output_file}")
+    print(f"📋 安全区域占比: {percentages[0]:.1f}%")
+    print(f"📋 危险区域占比: {percentages[2]:.1f}%")
+
+    return True
+
+def run_mujoco_simulation():
+    """运行MuJoCo可视化仿真"""
+    if not MUJOCO_AVAILABLE:
+        print("\n❌ MuJoCo未安装，无法启动可视化界面")
+        print("💡 请安装: pip install mujoco mujoco-python-viewer")
+        return False
+
+    print("\n" + "="*60)
+    print("启动MuJoCo机械臂仿真")
+    print("="*60)
+    print("控制说明:")
+    print("- 窗口中将显示机械臂模型")
+    print("- 机械臂会自动进行随机运动")
+    print("- 按ESC键退出仿真")
+    print("="*60)
+
+    try:
+        # 使用相对路径查找模型文件
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        model_file_path = os.path.join(current_dir, 'arm_with_gripper.xml')
+
+        # 检查文件是否存在
+        if not os.path.exists(model_file_path):
+            print(f"❌ 模型文件不存在: {model_file_path}")
+            print(f"当前目录: {current_dir}")
+            print(f"目录内容: {os.listdir(current_dir)}")
+            return False
+
+        print(f"正在加载模型文件: {model_file_path}")
+
+        # 读取模型文件内容
+        with open(model_file_path, 'r', encoding='utf-8') as f:
+            model_content = f.read()
+
+        # 移除对不存在的资源目录的引用
+        model_content = model_content.replace('meshdir="assets/"', '')
+        model_content = model_content.replace('texturedir="textures/"', '')
+
+        # 使用临时文件，避免中文字符路径问题
+        with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False, encoding='utf-8') as temp_file:
+            temp_model_path = temp_file.name
+            temp_file.write(model_content)
+            print(f"✅ 临时模型文件已创建: {temp_model_path}")
+
+        # 检查文件是否已创建
+        if not os.path.exists(temp_model_path):
+            print(f"❌ 临时文件创建失败: {temp_model_path}")
+            return False
+
+        print(f"✅ 临时文件存在: {os.path.exists(temp_model_path)}")
+        print(f"✅ 临时文件大小: {os.path.getsize(temp_model_path)} 字节")
+
+        # 从临时路径加载模型
+        try:
+            print("正在加载MuJoCo模型...")
+            model = mujoco.MjModel.from_xml_path(temp_model_path)
+            data = mujoco.MjData(model)
+            print("✅ 模型加载成功")
+        except Exception as e:
+            print(f"❌ 模型加载失败: {e}")
+            # 尝试直接从XML字符串加载
+            print("尝试从XML字符串加载模型...")
+            model = mujoco.MjModel.from_xml_string(model_content)
+            data = mujoco.MjData(model)
+            print("✅ 从字符串加载模型成功")
+
+        print(f"关节数量: {model.njnt}")
+        print(f"执行器数量: {model.nu}")
+
+        # 启动可视化界面
+        print("正在启动可视化窗口...")
+
+        try:
+            with mujoco.viewer.launch_passive(model, data) as viewer:
+                # 设置视角
+                viewer.cam.azimuth = 45
+                viewer.cam.elevation = -20
+                viewer.cam.distance = 2.5
+                viewer.cam.lookat[:] = [0.2, 0.0, 0.5]
+
+                print("✅ 可视化窗口已启动")
+                print("机械臂开始随机运动...")
+
+                # 仿真参数
+                simulation_time = 30.0  # 仿真30秒
+                start_time = time.time()
+                step_count = 0
+
+                # 随机目标角度
+                target_angles = np.random.uniform(-0.5, 0.5, model.nu)
+
+                while viewer.is_running() and (time.time() - start_time) < simulation_time:
+                    step_start = time.time()
+
+                    # 简单的PD控制，让机械臂随机运动
+                    for i in range(min(model.nu, len(target_angles))):
+                        # 计算控制信号（简单的PD控制器）
+                        error = target_angles[i] - data.qpos[i]
+                        data.ctrl[i] = 100 * error - 10 * data.qvel[i]  # PD控制
+
+                    # 每100步重新生成随机目标
+                    if step_count % 100 == 0:
+                        target_angles = np.random.uniform(-0.5, 0.5, model.nu)
+
+                    # 碰撞检测（简单版本）
+                    try:
+                        ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, 'ee_site')
+                        if ee_site_id >= 0:
+                            ee_pos = data.site_xpos[ee_site_id]
+
+                            # 检查与墙壁的碰撞
+                            if abs(ee_pos[0] - 0.7) < 0.1:
+                                print("⚠️  警告: 末端接近墙壁!")
+                            # 检查与柱子的碰撞
+                            if np.sqrt(ee_pos[0]**2 + ee_pos[1]**2) < 0.2:
+                                print("⚠️  警告: 末端接近中心柱子!")
+                    except:
+                        pass  # 如果站点不存在，跳过碰撞检测
+
+                    # 执行模拟步骤
+                    mujoco.mj_step(model, data)
+
+                    # 同步viewer
+                    viewer.sync()
+
+                    # 控制仿真速度
+                    elapsed = time.time() - step_start
+                    sleep_time = model.opt.timestep - elapsed
+                    if sleep_time > 0:
+                        time.sleep(sleep_time)
+
+                    step_count += 1
+
+                print(f"\n仿真结束，共运行 {step_count} 步")
+
+        except KeyboardInterrupt:
+            print("\n用户中断仿真")
+        except Exception as e:
+            print(f"仿真错误: {e}")
+
+        finally:
+            # 清理临时文件
+            try:
+                if os.path.exists(temp_model_path):
+                    os.remove(temp_model_path)
+                    print(f"临时文件已删除: {temp_model_path}")
+            except Exception as e:
+                print(f"清理临时文件失败: {e}")
+
+        return True
+
+    except Exception as e:
+        print(f"❌ MuJoCo仿真失败: {e}")
+        import traceback
+        traceback.print_exc()
+        return False
+
+def run_collision_detection_system():
+    """运行完整的碰撞检测系统"""
+    print("🚀 机械臂碰撞检测与可视化系统")
+    print("="*60)
+
+    # 步骤1: 生成碰撞分析图表
+    print("\n[步骤1] 生成碰撞风险分析图表...")
+    success1 = generate_collision_analysis()
+
+    if not success1:
+        print("❌ 碰撞分析失败")
+        return False
+
+    # 步骤2: 询问是否启动MuJoCo仿真
+    print("\n" + "="*60)
+    if MUJOCO_AVAILABLE:
+        response = input("是否启动MuJoCo机械臂仿真？(y/n): ").strip().lower()
+        if response in ['y', 'yes', '是']:
+            print("\n[步骤2] 启动MuJoCo可视化仿真...")
+            success2 = run_mujoco_simulation()
+            if success2:
+                print("✅ MuJoCo仿真完成")
+            else:
+                print("❌ MuJoCo仿真失败")
+        else:
+            print("跳过MuJoCo仿真")
+    else:
+        print("⚠️  MuJoCo未安装，跳过仿真步骤")
+        print("💡 要启用仿真功能，请安装:")
+        print("   pip install mujoco mujoco-python-viewer")
+
+    # 总结
+    print("\n" + "="*60)
+    print("系统运行完成！")
+    print("="*60)
+    print("📊 生成的图表:")
+    print("  • collision_analysis_result.png - 碰撞风险分析图")
+    print("\n🎯 后续步骤:")
+    print("  1. 查看生成的图表了解碰撞风险分布")
+    print("  2. 根据分析结果优化机械臂工作空间")
+    print("  3. 安装MuJoCo以启用仿真功能")
+    print("="*60)
+
+    return True
+
+if __name__ == "__main__":
+    try:
+        run_collision_detection_system()
+    except KeyboardInterrupt:
+        print("\n程序被用户中断")
+    except Exception as e:
+        print(f"\n❌ 程序运行出错: {e}")
+        import traceback
+        traceback.print_exc()
+        print("\n💡 常见问题解决方法:")
+        print("1. 确保已安装必要依赖: pip install numpy matplotlib")
+        print("2. 如需MuJoCo仿真: pip install mujoco mujoco-python-viewer")
+        print("3. 检查Python版本兼容性")
\ No newline at end of file
diff --git a/src/robotic_arm_grasping/test.py b/src/robotic_arm_grasping/test.py
index 5faa317914..bc2ca1e0ce 100644
--- a/src/robotic_arm_grasping/test.py
+++ b/src/robotic_arm_grasping/test.py
@@ -5,10 +5,10 @@
 import sys
 
 # 原始文件路径 - 修改为机器人手臂模型
-original_model_path = r"C:\Users\龙忠梁\Downloads\mujoco-3.3.7-windows-x86_64\model\robotic_arm\arm_with_gripper.xml"
+original_model_path = os.path.join(os.path.dirname(__file__), "arm_with_gripper.xml")
 
 # 尝试复制到临时目录以避免中文路径问题
-temp_dir = r"C:\temp"
+temp_dir = os.path.join(os.path.dirname(__file__), "temp")
 temp_model_path = os.path.join(temp_dir, "arm_with_gripper.xml")
 
 print(f"检查原始文件是否存在: {original_model_path}")
@@ -105,7 +105,7 @@
     print("请检查文件路径是否正确")
     
     # 检查目录结构
-    base_dir = r"C:\Users\龙忠梁\Downloads\mujoco-3.3.7-windows-x86_64\model\robotic_arm"
+    base_dir = os.path.dirname(__file__)
     if os.path.exists(base_dir):
         print("robotic_arm目录下的文件:")
         for file in os.listdir(base_dir):

From aa454e02ebaa597c6368afd9291fe59291713229 Mon Sep 17 00:00:00 2001
From: longzhongliang <1493886965@qq.com>
Date: Mon, 22 Dec 2025 11:14:59 +0800
Subject: [PATCH 2/3] =?UTF-8?q?=E7=A2=B0=E6=92=9E=E5=88=86=E6=9E=90?=
 =?UTF-8?q?=E5=8F=AF=E8=A7=86=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../collision_risk_analysis_visualization.py             | 9 ---------
 1 file changed, 9 deletions(-)

diff --git a/src/robotic_arm_grasping/collision_risk_analysis_visualization.py b/src/robotic_arm_grasping/collision_risk_analysis_visualization.py
index a10dcdaf46..b6e31e578a 100644
--- a/src/robotic_arm_grasping/collision_risk_analysis_visualization.py
+++ b/src/robotic_arm_grasping/collision_risk_analysis_visualization.py
@@ -1,12 +1,3 @@
-"""
-完整版：机械臂碰撞检测 + MuJoCo可视化界面
-包含碰撞分析和实时仿真
-修改说明：
-1. 所有路径改为相对路径
-2. 解决可视化图表中文乱码问题
-3. 优化文件处理和错误处理
-4. 修复临时文件创建问题
-"""
 import numpy as np
 import matplotlib.pyplot as plt
 from mpl_toolkits.mplot3d import Axes3D

From febebbf6faafceca77630618a6fcc174a21bb6fb Mon Sep 17 00:00:00 2001
From: longzhongliang <1493886965@qq.com>
Date: Wed, 24 Dec 2025 20:07:32 +0800
Subject: [PATCH 3/3] =?UTF-8?q?=E7=A2=B0=E6=92=9E=E5=88=86=E6=9E=90?=
 =?UTF-8?q?=E5=8F=AF=E8=A7=86=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/robotic_arm_grasping/arm_with_gripper.xml | 399 ++++++++++--------
 1 file changed, 233 insertions(+), 166 deletions(-)

diff --git a/src/robotic_arm_grasping/arm_with_gripper.xml b/src/robotic_arm_grasping/arm_with_gripper.xml
index d8508af2f8..0004125889 100644
--- a/src/robotic_arm_grasping/arm_with_gripper.xml
+++ b/src/robotic_arm_grasping/arm_with_gripper.xml
@@ -1,130 +1,154 @@
-<!-- 6-DOF Robotic Arm with Optimized 3-Finger Gripper for MuJoCo -->
-<mujoco model="robotic_arm_3_finger_gripper">
-  <compiler angle="radian" inertiafromgeom="true"/>
+<!-- 优化的6-DOF机械臂与三指抓取器模型 -->
+<mujoco model="optimized_robotic_arm_3_finger_gripper">
+  <compiler angle="radian" inertiafromgeom="true" meshdir="assets/" texturedir="textures/"/>
 
-  <option timestep="0.002" gravity="0 0 -9.81" iterations="50" solver="Newton"/>
+  <option timestep="0.001" gravity="0 0 -9.81" iterations="100" solver="Newton" cone="elliptic"/>
+
+  <size njmax="500" nconmax="100"/>
+
+  <visual>
+    <global offwidth="1920" offheight="1080"/>
+    <quality shadowsize="2048"/>
+    <headlight ambient="0.4 0.4 0.4" diffuse="0.8 0.8 0.8" specular="0.1 0.1 0.1"/>
+  </visual>
 
   <default>
-    <joint damping="2.0" armature="0.2"/>
-    <geom friction="1.0 0.5 0.01" rgba="0.7 0.7 0.7 1"/>
-    <motor ctrlrange="-1 1" ctrllimited="true"/>
+    <!-- 机械臂关节默认设置 -->
+    <joint armature="0.1" damping="1.5" limited="true" solimplimit="0 0.99 0.001" solreflimit="0.001 0.5"/>
+
+    <!-- 几何体默认设置 -->
+    <geom condim="6" friction="1.0 0.5 0.001" solmix="1.0" solref="0.02 1" margin="0.001" gap="0"/>
+
+    <!-- 电机默认设置 -->
+    <motor ctrllimited="true" ctrlrange="-2 2" forcelimited="true" forcerange="-100 100"/>
+
+    <!-- 站点默认设置 -->
+    <site size="0.005" rgba="1 0 0 0.8" type="sphere"/>
   </default>
 
   <asset>
-    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="512"/>
-    <texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 .2 .3" width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
-    <texture name="texgeom" type="cube" builtin="flat" mark="cross" width="128" height="128" rgb1="0.4 0.4 0.8" rgb2="0.4 0.4 0.8" markrgb="1 1 1"/>
-    <material name="matplane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true"/>
-    <material name="matgeom" texture="texgeom" texuniform="true" rgba="0.4 0.4 0.8 1"/>
-    <material name="gripper_mat" rgba="0.3 0.3 0.8 1"/>
-    <material name="finger_mat" rgba="0.2 0.5 0.8 1"/>
+    <!-- 天空盒和纹理 -->
+    <texture type="skybox" builtin="gradient" rgb1="0.4 0.6 0.8" rgb2="0.1 0.2 0.3" width="1024" height="1024"/>
+    <texture name="grid" type="2d" builtin="checker" rgb1="0.2 0.3 0.4" rgb2="0.1 0.15 0.2" width="300" height="300" mark="edge" markrgb="0.8 0.8 0.8"/>
+    <texture name="metal" type="cube" builtin="flat" mark="cross" width="256" height="256" rgb1="0.5 0.5 0.5" rgb2="0.7 0.7 0.7" markrgb="1 1 1"/>
+
+    <!-- 材质 -->
+    <material name="grid_mat" texture="grid" texrepeat="20 20" texuniform="true" reflectance="0.2"/>
+    <material name="metal_mat" texture="metal" texuniform="true" reflectance="0.5" specular="1" shininess="0.5"/>
+    <material name="arm_mat" rgba="0.6 0.6 0.7 1" specular="0.5" shininess="0.3"/>
+    <material name="gripper_mat" rgba="0.3 0.3 0.8 1" specular="0.8" shininess="0.5"/>
+    <material name="finger_mat" rgba="0.2 0.5 0.8 1" specular="0.5" shininess="0.4"/>
+
+    <!-- 测试物体材质 -->
+    <material name="red_mat" rgba="1 0.2 0.2 1"/>
+    <material name="green_mat" rgba="0.2 0.8 0.2 1"/>
+    <material name="blue_mat" rgba="0.2 0.5 1 1"/>
+    <material name="yellow_mat" rgba="1 0.8 0.2 1"/>
+    <material name="purple_mat" rgba="0.7 0.2 1 1"/>
   </asset>
 
   <worldbody>
-    <!-- 场景设置 -->
-    <geom name="floor" pos="0 0 -0.05" size="2 2 0.05" type="plane" material="matplane" condim="3"/>
-    <light directional="true" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="0 0 3.0" dir="0 0 -1"/>
-    <light directional="false" diffuse=".6 .6 .6" specular="0.2 0.2 0.2" pos="2 2 3" dir="-1 -1 -1"/>
-
-    <!-- 目标物体 - 多种形状用于测试 -->
-    <body name="target_cube" pos="0.2 0.1 0.05">
-      <freejoint name="cube_floating"/>
-      <geom name="cube_geom" type="box" size="0.04 0.04 0.04" rgba="0.2 0.8 0.2 1" mass="0.2"/>
-      <site name="cube_site" pos="0 0 0" size="0.01"/>
-    </body>
+    <!-- 地面 -->
+    <geom name="ground" type="plane" pos="0 0 0" size="3 3 0.1" material="grid_mat" condim="3"/>
 
-    <body name="target_sphere" pos="0.2 -0.1 0.05">
-      <freejoint name="sphere_floating"/>
-      <geom name="sphere_geom" type="sphere" size="0.035" rgba="0.8 0.2 0.2 1" mass="0.15"/>
-      <site name="sphere_site" pos="0 0 0" size="0.01"/>
-    </body>
+    <!-- 环境灯光 -->
+    <light directional="true" diffuse="0.9 0.9 0.9" specular="0.3 0.3 0.3" pos="0 0 4" dir="0 0 -1"/>
+    <light directional="true" diffuse="0.5 0.5 0.6" specular="0.1 0.1 0.1" pos="3 3 2" dir="-1 -1 -1"/>
+    <light directional="true" diffuse="0.4 0.4 0.5" specular="0.05 0.05 0.05" pos="-3 -3 2" dir="1 1 -1"/>
 
     <!-- 机械臂基座 -->
-    <body name="base_link" pos="0 0 0">
-      <geom name="base_geom" type="cylinder" size="0.12 0.06" rgba="0.5 0.5 0.5 1" mass="2.0"/>
-
-      <!-- 关节1: 基座旋转 -->
-      <body name="shoulder_link" pos="0 0 0.12">
-        <joint name="joint_1" type="hinge" axis="0 0 1" range="-3.14 3.14" damping="2.0"/>
-        <geom name="shoulder_geom" type="cylinder" size="0.09 0.07" rgba="0.6 0.6 0.6 1" mass="1.5"/>
-
-        <!-- 关节2: 肩部 -->
-        <body name="upper_arm_link" pos="0 0 0.14">
-          <joint name="joint_2" type="hinge" axis="0 1 0" range="-1.57 1.57" damping="2.0"/>
-          <geom name="upper_arm_geom" type="capsule" size="0.07" fromto="0 0 0 0 0 0.3" rgba="0.7 0.7 0.7 1" mass="1.0"/>
-
-          <!-- 关节3: 肘部 -->
-          <body name="forearm_link" pos="0 0 0.3">
-            <joint name="joint_3" type="hinge" axis="0 1 0" range="-1.57 1.57" damping="1.5"/>
-            <geom name="forearm_geom" type="capsule" size="0.06" fromto="0 0 0 0 0 0.28" rgba="0.7 0.7 0.7 1" mass="0.8"/>
-
-            <!-- 关节4: 腕部1 -->
-            <body name="wrist_1_link" pos="0 0 0.28">
-              <joint name="joint_4" type="hinge" axis="0 0 1" range="-2.0 2.0" damping="1.0"/>
-              <geom name="wrist_1_geom" type="capsule" size="0.05" fromto="0 0 0 0 0 0.12" rgba="0.8 0.8 0.8 1" mass="0.3"/>
-
-              <!-- 关节5: 腕部2 -->
-              <body name="wrist_2_link" pos="0 0 0.12">
-                <joint name="joint_5" type="hinge" axis="0 1 0" range="-1.57 1.57" damping="1.0"/>
-                <geom name="wrist_2_geom" type="capsule" size="0.05" fromto="0 0 0 0.1 0 0" rgba="0.8 0.8 0.8 1" mass="0.2"/>
-
-                <!-- 关节6: 腕部3 (末端执行器连接) -->
-                <body name="wrist_3_link" pos="0.1 0 0">
-                  <joint name="joint_6" type="hinge" axis="1 0 0" range="-1.57 1.57" damping="0.5"/>
-                  <geom name="wrist_3_geom" type="cylinder" size="0.04 0.02" rgba="0.8 0.8 0.8 1" mass="0.1"/>
-
-                  <!-- 优化的三指抓取器设计 - 符合机械结构 -->
-                  <body name="gripper_base" pos="0.04 0 0">
-                    <geom name="gripper_base_geom" type="cylinder" size="0.04 0.025" rgba="0.3 0.3 0.3 1" mass="0.1"/>
-                    <site name="grip_center" pos="0 0 0" size="0.005" rgba="1 0 0 1"/>
-
-                    <!-- 手指1 (0度方向) - 改进的机械结构 -->
-                    <body name="finger_1_base" pos="0.03 0 0">
-                      <joint name="finger_1_joint" type="slide" axis="0 0 1" range="0 0.06" damping="0.5"/>
-                      <geom name="finger_1_base_geom" type="box" size="0.015 0.008 0.03" material="finger_mat" mass="0.03"/>
-
-                      <body name="finger_1_proximal" pos="0 0 0.03">
-                        <joint name="finger_1_proximal_joint" type="hinge" axis="0 1 0" range="-0.3 1.2" damping="0.3"/>
-                        <geom name="finger_1_proximal_geom" type="box" size="0.012 0.006 0.025" material="finger_mat" mass="0.02"/>
-
-                        <body name="finger_1_distal" pos="0 0 0.025">
-                          <joint name="finger_1_distal_joint" type="hinge" axis="0 1 0" range="0 1.0" damping="0.2"/>
-                          <geom name="finger_1_distal_geom" type="box" size="0.01 0.005 0.02" material="finger_mat" mass="0.015"/>
-                          <site name="finger_1_contact" pos="0 0 0.02" size="0.005" rgba="1 0 0 0.5"/>
+    <body name="base" pos="0 0 0.05">
+      <geom name="base_geom" type="cylinder" size="0.15 0.08" material="metal_mat" mass="5.0"/>
+      <site name="base_site" pos="0 0 0" size="0.01"/>
+
+      <!-- 关节1 - 基座旋转 -->
+      <body name="shoulder" pos="0 0 0.1">
+        <joint name="joint1" type="hinge" pos="0 0 0" axis="0 0 1" range="-3.1416 3.1416" damping="2.0" armature="0.2"/>
+        <geom name="shoulder_geom" type="cylinder" size="0.1 0.08" material="arm_mat" mass="2.0"/>
+        <site name="shoulder_site" pos="0 0 0" size="0.008"/>
+
+        <!-- 关节2 - 肩部俯仰 -->
+        <body name="upper_arm" pos="0 0 0.09">
+          <joint name="joint2" type="hinge" pos="0 0 0" axis="0 1 0" range="-2.0 2.0" damping="1.8" armature="0.15"/>
+          <geom name="upper_arm_geom" type="capsule" fromto="0 0 0 0 0 0.35" size="0.075" material="arm_mat" mass="1.5"/>
+          <site name="upper_arm_site" pos="0 0 0.175" size="0.008"/>
+
+          <!-- 关节3 - 肘部 -->
+          <body name="forearm" pos="0 0 0.35">
+            <joint name="joint3" type="hinge" pos="0 0 0" axis="0 1 0" range="-2.5 0.5" damping="1.5" armature="0.1"/>
+            <geom name="forearm_geom" type="capsule" fromto="0 0 0 0 0 0.3" size="0.065" material="arm_mat" mass="1.2"/>
+            <site name="forearm_site" pos="0 0 0.15" size="0.008"/>
+
+            <!-- 关节4 - 腕部1 -->
+            <body name="wrist1" pos="0 0 0.3">
+              <joint name="joint4" type="hinge" pos="0 0 0" axis="0 0 1" range="-2.0 2.0" damping="1.0" armature="0.05"/>
+              <geom name="wrist1_geom" type="capsule" fromto="0 0 0 0 0 0.15" size="0.055" material="arm_mat" mass="0.5"/>
+              <site name="wrist1_site" pos="0 0 0.075" size="0.008"/>
+
+              <!-- 关节5 - 腕部2 -->
+              <body name="wrist2" pos="0 0 0.15">
+                <joint name="joint5" type="hinge" pos="0 0 0" axis="0 1 0" range="-1.8 1.8" damping="0.8" armature="0.04"/>
+                <geom name="wrist2_geom" type="capsule" fromto="0 0 0 0.12 0 0" size="0.045" material="arm_mat" mass="0.3"/>
+                <site name="wrist2_site" pos="0.06 0 0" size="0.008"/>
+
+                <!-- 关节6 - 腕部3（末端执行器连接） -->
+                <body name="wrist3" pos="0.12 0 0">
+                  <joint name="joint6" type="hinge" pos="0 0 0" axis="1 0 0" range="-1.8 1.8" damping="0.6" armature="0.03"/>
+                  <geom name="wrist3_geom" type="cylinder" size="0.04 0.03" material="arm_mat" mass="0.2"/>
+                  <site name="ee_site" pos="0 0 0" size="0.01" rgba="1 0 0 1"/>
+
+                  <!-- 抓取器基座 -->
+                  <body name="gripper_base" pos="0.05 0 0" euler="0 0 0">
+                    <geom name="gripper_base_geom" type="cylinder" size="0.045 0.035" material="gripper_mat" mass="0.15"/>
+                    <site name="grip_center" pos="0 0 0" size="0.008" rgba="0 1 0 1"/>
+
+                    <!-- 手指1 (0°) -->
+                    <body name="finger1_base" pos="0.04 0 0">
+                      <joint name="finger1_slide" type="slide" axis="0 0 1" range="-0.02 0.06" damping="0.4" stiffness="50"/>
+                      <geom name="finger1_base_geom" type="box" size="0.018 0.01 0.035" material="finger_mat" mass="0.04"/>
+
+                      <body name="finger1_proximal" pos="0 0 0.035">
+                        <joint name="finger1_hinge1" type="hinge" axis="0 1 0" range="-0.2 1.5" damping="0.3" stiffness="30" armature="0.01"/>
+                        <geom name="finger1_proximal_geom" type="box" size="0.015 0.008 0.028" material="finger_mat" mass="0.025"/>
+
+                        <body name="finger1_distal" pos="0 0 0.028">
+                          <joint name="finger1_hinge2" type="hinge" axis="0 1 0" range="0 1.2" damping="0.2" stiffness="20" armature="0.005"/>
+                          <geom name="finger1_distal_geom" type="box" size="0.012 0.006 0.022" material="finger_mat" mass="0.018"/>
+                          <site name="finger1_tip" pos="0 0 0.022" size="0.006" rgba="1 0.5 0 1"/>
                         </body>
                       </body>
                     </body>
 
-                    <!-- 手指2 (120度方向) - 改进的机械结构 -->
-                    <body name="finger_2_base" pos="-0.015 0.026 0">
-                      <joint name="finger_2_joint" type="slide" axis="0 0 1" range="0 0.06" damping="0.5"/>
-                      <geom name="finger_2_base_geom" type="box" size="0.015 0.008 0.03" material="finger_mat" mass="0.03"/>
+                    <!-- 手指2 (120°) -->
+                    <body name="finger2_base" pos="-0.02 0.035 0" euler="0 0 2.094">
+                      <joint name="finger2_slide" type="slide" axis="0 0 1" range="-0.02 0.06" damping="0.4" stiffness="50"/>
+                      <geom name="finger2_base_geom" type="box" size="0.018 0.01 0.035" material="finger_mat" mass="0.04"/>
 
-                      <body name="finger_2_proximal" pos="0 0 0.03">
-                        <joint name="finger_2_proximal_joint" type="hinge" axis="0.866 0.5 0" range="-0.3 1.2" damping="0.3"/>
-                        <geom name="finger_2_proximal_geom" type="box" size="0.012 0.006 0.025" material="finger_mat" mass="0.02"/>
+                      <body name="finger2_proximal" pos="0 0 0.035">
+                        <joint name="finger2_hinge1" type="hinge" axis="0 1 0" range="-0.2 1.5" damping="0.3" stiffness="30" armature="0.01"/>
+                        <geom name="finger2_proximal_geom" type="box" size="0.015 0.008 0.028" material="finger_mat" mass="0.025"/>
 
-                        <body name="finger_2_distal" pos="0 0 0.025">
-                          <joint name="finger_2_distal_joint" type="hinge" axis="0.866 0.5 0" range="0 1.0" damping="0.2"/>
-                          <geom name="finger_2_distal_geom" type="box" size="0.01 0.005 0.02" material="finger_mat" mass="0.015"/>
-                          <site name="finger_2_contact" pos="0 0 0.02" size="0.005" rgba="1 0 0 0.5"/>
+                        <body name="finger2_distal" pos="0 0 0.028">
+                          <joint name="finger2_hinge2" type="hinge" axis="0 1 0" range="0 1.2" damping="0.2" stiffness="20" armature="0.005"/>
+                          <geom name="finger2_distal_geom" type="box" size="0.012 0.006 0.022" material="finger_mat" mass="0.018"/>
+                          <site name="finger2_tip" pos="0 0 0.022" size="0.006" rgba="1 0.5 0 1"/>
                         </body>
                       </body>
                     </body>
 
-                    <!-- 手指3 (240度方向) - 改进的机械结构 -->
-                    <body name="finger_3_base" pos="-0.015 -0.026 0">
-                      <joint name="finger_3_joint" type="slide" axis="0 0 1" range="0 0.06" damping="0.5"/>
-                      <geom name="finger_3_base_geom" type="box" size="0.015 0.008 0.03" material="finger_mat" mass="0.03"/>
+                    <!-- 手指3 (240°) -->
+                    <body name="finger3_base" pos="-0.02 -0.035 0" euler="0 0 -2.094">
+                      <joint name="finger3_slide" type="slide" axis="0 0 1" range="-0.02 0.06" damping="0.4" stiffness="50"/>
+                      <geom name="finger3_base_geom" type="box" size="0.018 0.01 0.035" material="finger_mat" mass="0.04"/>
 
-                      <body name="finger_3_proximal" pos="0 0 0.03">
-                        <joint name="finger_3_proximal_joint" type="hinge" axis="-0.866 0.5 0" range="-0.3 1.2" damping="0.3"/>
-                        <geom name="finger_3_proximal_geom" type="box" size="0.012 0.006 0.025" material="finger_mat" mass="0.02"/>
+                      <body name="finger3_proximal" pos="0 0 0.035">
+                        <joint name="finger3_hinge1" type="hinge" axis="0 1 0" range="-0.2 1.5" damping="0.3" stiffness="30" armature="0.01"/>
+                        <geom name="finger3_proximal_geom" type="box" size="0.015 0.008 0.028" material="finger_mat" mass="0.025"/>
 
-                        <body name="finger_3_distal" pos="0 0 0.025">
-                          <joint name="finger_3_distal_joint" type="hinge" axis="-0.866 0.5 0" range="0 1.0" damping="0.2"/>
-                          <geom name="finger_3_distal_geom" type="box" size="0.01 0.005 0.02" material="finger_mat" mass="0.015"/>
-                          <site name="finger_3_contact" pos="0 0 0.02" size="0.005" rgba="1 0 0 0.5"/>
+                        <body name="finger3_distal" pos="0 0 0.028">
+                          <joint name="finger3_hinge2" type="hinge" axis="0 1 0" range="0 1.2" damping="0.2" stiffness="20" armature="0.005"/>
+                          <geom name="finger3_distal_geom" type="box" size="0.012 0.006 0.022" material="finger_mat" mass="0.018"/>
+                          <site name="finger3_tip" pos="0 0 0.022" size="0.006" rgba="1 0.5 0 1"/>
                         </body>
                       </body>
                     </body>
@@ -136,77 +160,120 @@
         </body>
       </body>
     </body>
-  </worldbody>
 
-  <actuator>
-    <!-- 6个关节电机 -->
-    <motor name="motor_1" joint="joint_1" gear="50"/>
-    <motor name="motor_2" joint="joint_2" gear="50"/>
-    <motor name="motor_3" joint="joint_3" gear="30"/>
-    <motor name="motor_4" joint="joint_4" gear="20"/>
-    <motor name="motor_5" joint="joint_5" gear="20"/>
-    <motor name="motor_6" joint="joint_6" gear="10"/>
-
-    <!-- 改进的抓取器电机 - 每个手指3个关节，共9个电机 -->
-    <motor name="finger_1_slide_motor" joint="finger_1_joint" gear="20"/>
-    <motor name="finger_1_proximal_motor" joint="finger_1_proximal_joint" gear="15"/>
-    <motor name="finger_1_distal_motor" joint="finger_1_distal_joint" gear="8"/>
-
-    <motor name="finger_2_slide_motor" joint="finger_2_joint" gear="20"/>
-    <motor name="finger_2_proximal_motor" joint="finger_2_proximal_joint" gear="15"/>
-    <motor name="finger_2_distal_motor" joint="finger_2_distal_joint" gear="8"/>
-
-    <motor name="finger_3_slide_motor" joint="finger_3_joint" gear="20"/>
-    <motor name="finger_3_proximal_motor" joint="finger_3_proximal_joint" gear="15"/>
-    <motor name="finger_3_distal_motor" joint="finger_3_distal_joint" gear="8"/>
-  </actuator>
+    <!-- 测试物体 -->
+    <body name="test_sphere" pos="0.2 0.1 0.1">
+      <joint type="free" name="sphere_joint"/>
+      <geom name="sphere_geom" type="sphere" size="0.06" material="red_mat" mass="0.25" friction="0.7 0.3 0.01"/>
+      <site name="sphere_site" pos="0 0 0" size="0.008"/>
+    </body>
 
-  <equality>
-    <!-- 手指滑动同步 -->
-    <joint name="finger_slide_sync_1" joint1="finger_1_joint" joint2="finger_2_joint" polycoef="0 1 0 0 0"/>
-    <joint name="finger_slide_sync_2" joint1="finger_1_joint" joint2="finger_3_joint" polycoef="0 1 0 0 0"/>
+    <body name="test_box" pos="0.1 -0.1 0.05">
+      <joint type="free" name="box_joint"/>
+      <geom name="box_geom" type="box" size="0.05 0.07 0.04" material="green_mat" mass="0.35" friction="0.8 0.4 0.01"/>
+      <site name="box_site" pos="0 0 0" size="0.008"/>
+    </body>
 
-    <!-- 近端关节同步 -->
-    <joint name="finger_proximal_sync_1" joint1="finger_1_proximal_joint" joint2="finger_2_proximal_joint" polycoef="0 1 0 0 0"/>
-    <joint name="finger_proximal_sync_2" joint1="finger_1_proximal_joint" joint2="finger_3_proximal_joint" polycoef="0 1 0 0 0"/>
+    <body name="test_cylinder" pos="0.3 0.0 0.1">
+      <joint type="free" name="cylinder_joint"/>
+      <geom name="cylinder_geom" type="cylinder" size="0.04 0.09" material="blue_mat" mass="0.3" friction="0.75 0.35 0.01"/>
+      <site name="cylinder_site" pos="0 0 0" size="0.008"/>
+    </body>
 
-    <!-- 远端关节同步 -->
-    <joint name="finger_distal_sync_1" joint1="finger_1_distal_joint" joint2="finger_2_distal_joint" polycoef="0 1 0 0 0"/>
-    <joint name="finger_distal_sync_2" joint1="finger_1_distal_joint" joint2="finger_3_distal_joint" polycoef="0 1 0 0 0"/>
-  </equality>
+    <body name="test_capsule" pos="-0.1 0.2 0.12">
+      <joint type="free" name="capsule_joint"/>
+      <geom name="capsule_geom" type="capsule" fromto="0 -0.04 0 0 0.04 0" size="0.03" material="yellow_mat" mass="0.2" friction="0.65 0.25 0.01"/>
+      <site name="capsule_site" pos="0 0 0" size="0.008"/>
+    </body>
+
+    <body name="test_ellipsoid" pos="0.0 -0.2 0.18">
+      <joint type="free" name="ellipsoid_joint"/>
+      <geom name="ellipsoid_geom" type="ellipsoid" size="0.05 0.07 0.04" material="purple_mat" mass="0.28" friction="0.6 0.2 0.01"/>
+      <site name="ellipsoid_site" pos="0 0 0" size="0.008"/>
+    </body>
+
+
+  </worldbody>
+
+  <actuator>
+    <!-- 机械臂关节执行器 -->
+    <motor name="motor_joint1" joint="joint1" gear="80" ctrlrange="-2.5 2.5" forcerange="-150 150"/>
+    <motor name="motor_joint2" joint="joint2" gear="70" ctrlrange="-2.5 2.5" forcerange="-120 120"/>
+    <motor name="motor_joint3" joint="joint3" gear="60" ctrlrange="-2.5 2.5" forcerange="-100 100"/>
+    <motor name="motor_joint4" joint="joint4" gear="40" ctrlrange="-2.0 2.0" forcerange="-80 80"/>
+    <motor name="motor_joint5" joint="joint5" gear="40" ctrlrange="-2.0 2.0" forcerange="-60 60"/>
+    <motor name="motor_joint6" joint="joint6" gear="30" ctrlrange="-2.0 2.0" forcerange="-40 40"/>
+
+    <!-- 抓取器执行器 -->
+    <motor name="motor_finger1_slide" joint="finger1_slide" gear="50" ctrlrange="-0.5 0.5" forcerange="-80 80"/>
+    <motor name="motor_finger1_hinge1" joint="finger1_hinge1" gear="35" ctrlrange="-0.5 1.5" forcerange="-60 60"/>
+    <motor name="motor_finger1_hinge2" joint="finger1_hinge2" gear="25" ctrlrange="0 1.2" forcerange="-40 40"/>
+
+    <motor name="motor_finger2_slide" joint="finger2_slide" gear="50" ctrlrange="-0.5 0.5" forcerange="-80 80"/>
+    <motor name="motor_finger2_hinge1" joint="finger2_hinge1" gear="35" ctrlrange="-0.5 1.5" forcerange="-60 60"/>
+    <motor name="motor_finger2_hinge2" joint="finger2_hinge2" gear="25" ctrlrange="0 1.2" forcerange="-40 40"/>
+
+    <motor name="motor_finger3_slide" joint="finger3_slide" gear="50" ctrlrange="-0.5 0.5" forcerange="-80 80"/>
+    <motor name="motor_finger3_hinge1" joint="finger3_hinge1" gear="35" ctrlrange="-0.5 1.5" forcerange="-60 60"/>
+    <motor name="motor_finger3_hinge2" joint="finger3_hinge2" gear="25" ctrlrange="0 1.2" forcerange="-40 40"/>
+  </actuator>
 
   <sensor>
-    <!-- 关节位置传感器 -->
-    <jointpos name="joint_1_pos" joint="joint_1"/>
-    <jointpos name="joint_2_pos" joint="joint_2"/>
-    <jointpos name="joint_3_pos" joint="joint_3"/>
-    <jointpos name="joint_4_pos" joint="joint_4"/>
-    <jointpos name="joint_5_pos" joint="joint_5"/>
-    <jointpos name="joint_6_pos" joint="joint_6"/>
-
-    <!-- 抓取器关节传感器 -->
-    <jointpos name="finger_1_slide_pos" joint="finger_1_joint"/>
-    <jointpos name="finger_1_proximal_pos" joint="finger_1_proximal_joint"/>
-    <jointpos name="finger_1_distal_pos" joint="finger_1_distal_joint"/>
-
-    <jointpos name="finger_2_slide_pos" joint="finger_2_joint"/>
-    <jointpos name="finger_2_proximal_pos" joint="finger_2_proximal_joint"/>
-    <jointpos name="finger_2_distal_pos" joint="finger_2_distal_joint"/>
-
-    <jointpos name="finger_3_slide_pos" joint="finger_3_joint"/>
-    <jointpos name="finger_3_proximal_pos" joint="finger_3_proximal_joint"/>
-    <jointpos name="finger_3_distal_pos" joint="finger_3_distal_joint"/>
+    <!-- 机械臂关节位置传感器 -->
+    <jointpos name="sensor_joint1" joint="joint1"/>
+    <jointpos name="sensor_joint2" joint="joint2"/>
+    <jointpos name="sensor_joint3" joint="joint3"/>
+    <jointpos name="sensor_joint4" joint="joint4"/>
+    <jointpos name="sensor_joint5" joint="joint5"/>
+    <jointpos name="sensor_joint6" joint="joint6"/>
+
+    <!-- 抓取器传感器 -->
+    <jointpos name="sensor_finger1_slide" joint="finger1_slide"/>
+    <jointpos name="sensor_finger1_hinge1" joint="finger1_hinge1"/>
+    <jointpos name="sensor_finger1_hinge2" joint="finger1_hinge2"/>
+
+    <jointpos name="sensor_finger2_slide" joint="finger2_slide"/>
+    <jointpos name="sensor_finger2_hinge1" joint="finger2_hinge1"/>
+    <jointpos name="sensor_finger2_hinge2" joint="finger2_hinge2"/>
+
+    <jointpos name="sensor_finger3_slide" joint="finger3_slide"/>
+    <jointpos name="sensor_finger3_hinge1" joint="finger3_hinge1"/>
+    <jointpos name="sensor_finger3_hinge2" joint="finger3_hinge2"/>
+
+    <!-- 力/力矩传感器 -->
+    <force name="sensor_finger1_force" site="finger1_tip"/>
+    <force name="sensor_finger2_force" site="finger2_tip"/>
+    <force name="sensor_finger3_force" site="finger3_tip"/>
 
     <!-- 触觉传感器 -->
-    <touch name="finger_1_touch" site="finger_1_contact"/>
-    <touch name="finger_2_touch" site="finger_2_contact"/>
-    <touch name="finger_3_touch" site="finger_3_contact"/>
+    <touch name="sensor_finger1_touch" site="finger1_tip"/>
+    <touch name="sensor_finger2_touch" site="finger2_tip"/>
+    <touch name="sensor_finger3_touch" site="finger3_tip"/>
+
+    <!-- 位置和姿态传感器 -->
+    <framepos name="sensor_gripper_pos" objtype="site" objname="grip_center"/>
+    <framequat name="sensor_gripper_quat" objtype="site" objname="grip_center"/>
+
+    <!-- 测试物体传感器 -->
+    <framepos name="sensor_sphere_pos" objtype="body" objname="test_sphere"/>
+    <framepos name="sensor_box_pos" objtype="body" objname="test_box"/>
+    <framepos name="sensor_cylinder_pos" objtype="body" objname="test_cylinder"/>
+    <framepos name="sensor_capsule_pos" objtype="body" objname="test_capsule"/>
+    <framepos name="sensor_ellipsoid_pos" objtype="body" objname="test_ellipsoid"/>
+
+    <!-- 接近度传感器 -->
+    <rangefinder name="sensor_gripper_range" site="grip_center" cutoff="0.5"/>
+  </sensor>
 
-    <!-- 抓取力矩传感器 -->
-    <jointactuatorfrc name="gripper_torque" joint="finger_1_proximal_joint"/>
+  <equality>
+    <!-- 抓取器同步约束（确保手指同步运动） -->
+    <joint name="equal_finger_slide_12" joint1="finger1_slide" joint2="finger2_slide" polycoef="0 1 0 0 0"/>
+    <joint name="equal_finger_slide_13" joint1="finger1_slide" joint2="finger3_slide" polycoef="0 1 0 0 0"/>
 
-    <!-- 目标接近度传感器 -->
-    <framepos name="grip_center_pos" objtype="site" objname="grip_center"/>
-    <framepos name="target_pos" objtype="site" objname="cube_site"/>
-  </sensor>
+    <joint name="equal_finger_hinge1_12" joint1="finger1_hinge1" joint2="finger2_hinge1" polycoef="0 1 0 0 0"/>
+    <joint name="equal_finger_hinge1_13" joint1="finger1_hinge1" joint2="finger3_hinge1" polycoef="0 1 0 0 0"/>
+
+    <joint name="equal_finger_hinge2_12" joint1="finger1_hinge2" joint2="finger2_hinge2" polycoef="0 1 0 0 0"/>
+    <joint name="equal_finger_hinge2_13" joint1="finger1_hinge2" joint2="finger3_hinge2" polycoef="0 1 0 0 0"/>
+  </equality>
 </mujoco>
\ No newline at end of file