机械臂抓取仿真程序架构设计

1. 架构概述

基于 mujoco_menagerie 提供的模型，设计一套机械臂抓取仿真程序，包含以下核心模块：

┌───────────────────┐
│ 模型加载模块      │
└──────────┬────────┘
           │
┌──────────▼────────┐     ┌───────────────────┐
│ 控制器模块        │◄────┤ 抓取规划模块      │
└──────────┬────────┘     └───────────────────┘
           │
┌──────────▼────────┐
│ 仿真主循环        │
└──────────┬────────┘
           │
┌──────────▼────────┐
│ 可视化模块        │
└───────────────────┘

2. 核心模块设计

2.1 模型加载模块

• 功能：加载机械臂和夹爪模型
• 实现：
  • 使用 mujoco.MjModel.from_xml_path() 加载模型
  • 支持组合机械臂和夹爪模型
  • 提供模型初始化和重置功能

2.2 控制器模块

• 功能：控制机械臂和夹爪的运动
• 实现：
  • 机械臂关节空间控制
  • 机械臂笛卡尔空间控制（逆运动学）
  • 夹爪开合控制
  • 提供轨迹规划功能

2.3 抓取规划模块

• 功能：计算抓取位置和姿态
• 实现：
  • 目标物体检测和定位
  • 抓取点计算
  • 抓取姿态规划
  • 碰撞检测

2.4 仿真主循环

• 功能：运行整个仿真过程
• 实现：
  • 物理仿真步进
  • 状态更新
  • 控制指令执行
  • 仿真时间管理

2.5 可视化模块

• 功能：显示仿真过程
• 实现：
  • 使用 mujoco.viewer 显示仿真场景
  • 提供交互控制界面
  • 支持录制仿真过程

3. 技术实现要点

3.1 模型选择

• 机械臂：Franka FR3（7自由度，精度高）
• 夹爪：Robotiq 2F-85（工业级夹爪，抓取稳定）
• 目标物体：立方体、圆柱体等基本几何体

3.2 控制方法

• 机械臂：PD控制器 + 逆运动学
• 夹爪：位置控制
• 抓取策略：先接近目标，再闭合夹爪，最后提升

3.3 仿真参数

• 时间步长：0.002s（500Hz）
• 仿真时间：根据任务需求调整
• 物理参数：使用模型默认参数

4. 实现步骤

1. 安装依赖：mujoco、numpy、scipy
2. 加载机械臂和夹爪模型
3. 创建仿真场景（添加目标物体）
4. 实现控制器
5. 实现抓取规划算法
6. 编写仿真主循环
7. 添加可视化功能
8. 测试和调试

5. 预期功能

• 机械臂能够准确到达目标位置
• 夹爪能够稳定抓取物体
• 能够完成从抓取到放置的完整任务
• 支持交互控制和参数调整
• 提供仿真数据记录和分析功能

双臂机械臂平台xml文件

<mujoco model="dual_xarm7_grasping">
  <compiler angle="radian" autolimits="true" meshdir="ufactory_xarm7/assets"/>

  <option integrator="implicitfast" gravity="0 0 -9.81"/>

  <default>
    <default class="xarm7">
      <geom type="mesh" material="white"/>
      <joint axis="0 0 1" armature="0.1" range="-6.28319 6.28319" frictionloss="1"/>
      <general biastype="affine" ctrlrange="-6.28319 6.28319"/>
      <default class="size1">
        <joint damping="10"/>
        <general gainprm="1500" biasprm="0 -1500 -150" forcerange="-50 50"/>
      </default>
      <default class="size2">
        <joint damping="5"/>
        <general gainprm="1000" biasprm="0 -1000 -100" forcerange="-30 30"/>
      </default>
      <default class="size3">
        <joint damping="2"/>
        <general gainprm="800" biasprm="0 -800 -80" forcerange="-20 20"/>
      </default>
      <default class="spring_link">
        <joint range="0 0.85" stiffness="0.05" springref="2.62" damping="0.00125"/>
      </default>
      <default class="driver">
        <joint range="0 0.85" armature="0.005" damping="0.1" solreflimit="0.005 1"/>
      </default>
      <default class="follower">
        <joint range="0 0.85" solreflimit="0.005 1"/>
      </default>
      <default class="visual">
        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
      </default>
      <default class="collision">
        <geom type="mesh" group="3"/>
        <default class="pad_box1">
          <geom type="box" friction="0.7" solimp="0.95 0.99 0.001" solref="0.004 1" mass="0" priority="1" size="0.015 0.002 0.0095" rgba="0.0 0.1 0.7 1"/>
        </default>
        <default class="pad_box2">
          <geom type="box" friction="0.6" solimp="0.95 0.99 0.001" solref="0.004 1" mass="0" priority="1" size="0.015 0.002 0.0095" rgba="0.0 0.5 0.5 1"/>
        </default>
      </default>
      <site size="0.001" rgba="1 0 0 1" group="4"/>
    </default>
  </default>

  <asset>
    <material name="white" rgba="1 1 1 1"/>
    <material name="gray" rgba="0.753 0.753 0.753 1"/>
    <material name="black" rgba="0.149 0.149 0.149 1"/>
    <material name="green" rgba="0 1 0 1"/>
    <material name="red" rgba="1 0 0 1"/>
    <material name="blue" rgba="0 0 1 1"/>
    <material name="wood" rgba="0.8 0.6 0.3 1"/>
    <material name="floor" rgba="0.4 0.4 0.4 1"/>

    <mesh file="link_base.stl"/>
    <mesh file="link1.stl"/>
    <mesh file="link2.stl"/>
    <mesh file="link3.stl"/>
    <mesh file="link4.stl"/>
    <mesh file="link5.stl"/>
    <mesh file="link6.stl"/>
    <mesh file="link7.stl"/>
    <mesh file="end_tool.stl"/>
    <mesh file="base_link.stl"/>
    <mesh file="left_outer_knuckle.stl"/>
    <mesh file="left_finger.stl"/>
    <mesh file="left_inner_knuckle.stl"/>
    <mesh file="right_outer_knuckle.stl"/>
    <mesh file="right_finger.stl"/>
    <mesh file="right_inner_knuckle.stl"/>
  </asset>

  <worldbody>
    <!-- 相机 -->
    <camera name="overhead_camera" pos="0 -1.0 1.5" quat="0.7071 0 0.7071 0" fovy="60"/>
    
    <!-- 左侧机械臂 -->
    <body name="base_left" pos="-0.4 0 0.4" childclass="xarm7">
      <body name="link_base_left" pos="0 0 .12">
        <inertial pos="-0.021131 -0.0016302 0.056488" quat="0.696843 0.20176 0.10388 0.680376" mass="0.88556"
          diaginertia="0.00382023 0.00335282 0.00167725"/>
        <geom mesh="link_base"/>
        <body name="link1_left" pos="0 0 0.267">
          <inertial pos="-0.0002 0.02905 -0.01233" quat="0.978953 -0.202769 -0.00441617 -0.0227264" mass="2.382"
            diaginertia="0.00569127 0.00533384 0.00293865"/>
          <joint name="joint1_left" class="size1"/>
          <geom mesh="link1"/>
          <body name="link2_left" quat="1 -1 0 0">
            <inertial pos="0.00022 -0.12856 0.01735" quat="0.50198 0.86483 -0.00778841 0.00483285" mass="1.869"
              diaginertia="0.00959898 0.00937717 0.00201315"/>
            <joint name="joint2_left" range="-2.059 2.0944" class="size1"/>
            <geom mesh="link2"/>
            <body name="link3_left" pos="0 -0.293 0" quat="1 1 0 0">
              <inertial pos="0.0466 -0.02463 -0.00768" quat="0.913819 0.289775 0.281481 -0.0416455" mass="1.6383"
                diaginertia="0.00351721 0.00294089 0.00195868"/>
              <joint name="joint3_left" class="size2"/>
              <geom mesh="link3"/>
              <body name="link4_left" pos="0.0525 0 0" quat="1 1 0 0">
                <inertial pos="0.07047 -0.11575 0.012" quat="0.422108 0.852026 -0.126025 0.282832" mass="1.7269"
                  diaginertia="0.00657137 0.00647948 0.00186763"/>
                <joint name="joint4_left" range="-0.19198 3.927" class="size2"/>
                <geom mesh="link4"/>
                <body name="link5_left" pos="0.0775 -0.3425 0" quat="1 1 0 0">
                  <inertial pos="-0.00032 0.01604 -0.026" quat="0.999311 -0.0304457 0.000577067 0.0212082" mass="1.3203"
                    diaginertia="0.00534729 0.00499076 0.0013489"/>
                  <joint name="joint5_left" class="size2"/>
                  <geom mesh="link5"/>
                  <body name="link6_left" quat="1 1 0 0">
                    <inertial pos="0.06469 0.03278 0.02141" quat="-0.217672 0.772419 0.16258 0.574069" mass="1.325"
                      diaginertia="0.00245421 0.00221646 0.00107273"/>
                    <joint name="joint6_left" range="-1.69297 3.14159" class="size3"/>
                    <geom mesh="link6"/>
                    <body name="link7_left" pos="0.076 0.097 0" quat="1 -1 0 0">
                      <inertial pos="0 -0.00677 -0.01098" quat="0.487612 0.512088 -0.512088 0.487612" mass="0.17"
                        diaginertia="0.000132176 9.3e-05 5.85236e-05"/>
                      <joint name="joint7_left" class="size3"/>
                      <geom material="gray" mesh="end_tool"/>
                      <body name="xarm_gripper_base_link_left" quat="0 0 0 1">
                        <inertial pos="-0.00065489 -0.0018497 0.048028" quat="0.997403 -0.0717512 -0.0061836 0.000477479"
                          mass="0.54156" diaginertia="0.000471093 0.000332307 0.000254799"/>
                        <geom mesh="base_link"/>
                        <body name="left_outer_knuckle_left" pos="0 0.035 0.059098">
                          <inertial pos="0 0.021559 0.015181" quat="0.47789 0.87842 0 0" mass="0.033618"
                            diaginertia="1.9111e-05 1.79089e-05 1.90167e-06"/>
                          <joint name="left_driver_joint_left" axis="1 0 0" class="driver"/>
                          <geom material="black" mesh="left_outer_knuckle"/>
                          <body name="left_finger_left" pos="0 0.035465 0.042039">
                            <inertial pos="0 -0.016413 0.029258" quat="0.697634 0.115353 -0.115353 0.697634"
                              mass="0.048304" diaginertia="1.88037e-05 1.7493e-05 3.56792e-06"/>
                            <joint name="left_finger_joint_left" axis="-1 0 0" class="follower"/>
                            <geom class="visual" material="black" mesh="left_finger"/>
                            <geom class="pad_box1" name="left_finger_pad_1_left" pos="0 -0.024003 0.032"/>
                            <geom class="pad_box2" name="left_finger_pad_2_left" pos="0 -0.024003 0.050"/>
                          </body>
                        </body>
                        <body name="left_inner_knuckle_left" pos="0 0.02 0.074098">
                          <inertial pos="1.86601e-06 0.0220468 0.0261335" quat="0.664139 -0.242732 0.242713 0.664146"
                            mass="0.0230126" diaginertia="8.34216e-06 6.0949e-06 2.75601e-06"/>
                          <joint name="left_inner_knuckle_joint_left" axis="1 0 0" class="spring_link"/>
                          <geom material="black" mesh="left_inner_knuckle"/>
                        </body>
                        <body name="right_outer_knuckle_left" pos="0 -0.035 0.059098">
                          <inertial pos="0 -0.021559 0.015181" quat="0.87842 0.47789 0 0" mass="0.033618"
                            diaginertia="1.9111e-05 1.79089e-05 1.90167e-06"/>
                          <joint name="right_driver_joint_left" axis="-1 0 0" class="driver"/>
                          <geom material="black" mesh="right_outer_knuckle"/>
                          <body name="right_finger_left" pos="0 -0.035465 0.042039">
                            <inertial pos="0 0.016413 0.029258" quat="0.697634 -0.115356 0.115356 0.697634"
                              mass="0.048304" diaginertia="1.88038e-05 1.7493e-05 3.56779e-06"/>
                            <joint name="right_finger_joint_left" axis="1 0 0" class="follower"/>
                            <geom class="visual" material="black" mesh="right_finger"/>
                            <geom class="pad_box1" name="right_finger_pad_1_left" pos="0 0.024003 0.032"/>
                            <geom class="pad_box2" name="right_finger_pad_2_left" pos="0 0.024003 0.050"/>
                          </body>
                        </body>
                        <body name="right_inner_knuckle_left" pos="0 -0.02 0.074098">
                          <inertial pos="1.866e-06 -0.022047 0.026133" quat="0.66415 0.242702 -0.242721 0.664144"
                            mass="0.023013" diaginertia="8.34209e-06 6.0949e-06 2.75601e-06"/>
                          <joint name="right_inner_knuckle_joint_left" axis="-1 0 0" class="spring_link"/>
                          <geom material="black" mesh="right_inner_knuckle"/>
                        </body>
                        <site name="link_tcp_left" pos="0 0 .172"/>
                      </body>
                    </body>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>
    
    <!-- 右侧机械臂 -->
    <body name="base_right" pos="0.4 0 0.4" childclass="xarm7">
      <body name="link_base_right" pos="0 0 .12">
        <inertial pos="-0.021131 -0.0016302 0.056488" quat="0.696843 0.20176 0.10388 0.680376" mass="0.88556"
          diaginertia="0.00382023 0.00335282 0.00167725"/>
        <geom mesh="link_base"/>
        <body name="link1_right" pos="0 0 0.267">
          <inertial pos="-0.0002 0.02905 -0.01233" quat="0.978953 -0.202769 -0.00441617 -0.0227264" mass="2.382"
            diaginertia="0.00569127 0.00533384 0.00293865"/>
          <joint name="joint1_right" class="size1"/>
          <geom mesh="link1"/>
          <body name="link2_right" quat="1 -1 0 0">
            <inertial pos="0.00022 -0.12856 0.01735" quat="0.50198 0.86483 -0.00778841 0.00483285" mass="1.869"
              diaginertia="0.00959898 0.00937717 0.00201315"/>
            <joint name="joint2_right" range="-2.059 2.0944" class="size1"/>
            <geom mesh="link2"/>
            <body name="link3_right" pos="0 -0.293 0" quat="1 1 0 0">
              <inertial pos="0.0466 -0.02463 -0.00768" quat="0.913819 0.289775 0.281481 -0.0416455" mass="1.6383"
                diaginertia="0.00351721 0.00294089 0.00195868"/>
              <joint name="joint3_right" class="size2"/>
              <geom mesh="link3"/>
              <body name="link4_right" pos="0.0525 0 0" quat="1 1 0 0">
                <inertial pos="0.07047 -0.11575 0.012" quat="0.422108 0.852026 -0.126025 0.282832" mass="1.7269"
                  diaginertia="0.00657137 0.00647948 0.00186763"/>
                <joint name="joint4_right" range="-0.19198 3.927" class="size2"/>
                <geom mesh="link4"/>
                <body name="link5_right" pos="0.0775 -0.3425 0" quat="1 1 0 0">
                  <inertial pos="-0.00032 0.01604 -0.026" quat="0.999311 -0.0304457 0.000577067 0.0212082" mass="1.3203"
                    diaginertia="0.00534729 0.00499076 0.0013489"/>
                  <joint name="joint5_right" class="size2"/>
                  <geom mesh="link5"/>
                  <body name="link6_right" quat="1 1 0 0">
                    <inertial pos="0.06469 0.03278 0.02141" quat="-0.217672 0.772419 0.16258 0.574069" mass="1.325"
                      diaginertia="0.00245421 0.00221646 0.00107273"/>
                    <joint name="joint6_right" range="-1.69297 3.14159" class="size3"/>
                    <geom mesh="link6"/>
                    <body name="link7_right" pos="0.076 0.097 0" quat="1 -1 0 0">
                      <inertial pos="0 -0.00677 -0.01098" quat="0.487612 0.512088 -0.512088 0.487612" mass="0.17"
                        diaginertia="0.000132176 9.3e-05 5.85236e-05"/>
                      <joint name="joint7_right" class="size3"/>
                      <geom material="gray" mesh="end_tool"/>
                      <body name="xarm_gripper_base_link_right" quat="0 0 0 1">
                        <inertial pos="-0.00065489 -0.0018497 0.048028" quat="0.997403 -0.0717512 -0.0061836 0.000477479"
                          mass="0.54156" diaginertia="0.000471093 0.000332307 0.000254799"/>
                        <geom mesh="base_link"/>
                        <body name="left_outer_knuckle_right" pos="0 0.035 0.059098">
                          <inertial pos="0 0.021559 0.015181" quat="0.47789 0.87842 0 0" mass="0.033618"
                            diaginertia="1.9111e-05 1.79089e-05 1.90167e-06"/>
                          <joint name="left_driver_joint_right" axis="1 0 0" class="driver"/>
                          <geom material="black" mesh="left_outer_knuckle"/>
                          <body name="left_finger_right" pos="0 0.035465 0.042039">
                            <inertial pos="0 -0.016413 0.029258" quat="0.697634 0.115353 -0.115353 0.697634"
                              mass="0.048304" diaginertia="1.88037e-05 1.7493e-05 3.56792e-06"/>
                            <joint name="left_finger_joint_right" axis="-1 0 0" class="follower"/>
                            <geom class="visual" material="black" mesh="left_finger"/>
                            <geom class="pad_box1" name="left_finger_pad_1_right" pos="0 -0.024003 0.032"/>
                            <geom class="pad_box2" name="left_finger_pad_2_right" pos="0 -0.024003 0.050"/>
                          </body>
                        </body>
                        <body name="left_inner_knuckle_right" pos="0 0.02 0.074098">
                          <inertial pos="1.86601e-06 0.0220468 0.0261335" quat="0.664139 -0.242732 0.242713 0.664146"
                            mass="0.0230126" diaginertia="8.34216e-06 6.0949e-06 2.75601e-06"/>
                          <joint name="left_inner_knuckle_joint_right" axis="1 0 0" class="spring_link"/>
                          <geom material="black" mesh="left_inner_knuckle"/>
                        </body>
                        <body name="right_outer_knuckle_right" pos="0 -0.035 0.059098">
                          <inertial pos="0 -0.021559 0.015181" quat="0.87842 0.47789 0 0" mass="0.033618"
                            diaginertia="1.9111e-05 1.79089e-05 1.90167e-06"/>
                          <joint name="right_driver_joint_right" axis="-1 0 0" class="driver"/>
                          <geom material="black" mesh="right_outer_knuckle"/>
                          <body name="right_finger_right" pos="0 -0.035465 0.042039">
                            <inertial pos="0 0.016413 0.029258" quat="0.697634 -0.115356 0.115356 0.697634"
                              mass="0.048304" diaginertia="1.88038e-05 1.7493e-05 3.56779e-06"/>
                            <joint name="right_finger_joint_right" axis="1 0 0" class="follower"/>
                            <geom class="visual" material="black" mesh="right_finger"/>
                            <geom class="pad_box1" name="right_finger_pad_1_right" pos="0 0.024003 0.032"/>
                            <geom class="pad_box2" name="right_finger_pad_2_right" pos="0 0.024003 0.050"/>
                          </body>
                        </body>
                        <body name="right_inner_knuckle_right" pos="0 -0.02 0.074098">
                          <inertial pos="1.866e-06 -0.022047 0.026133" quat="0.66415 0.242702 -0.242721 0.664144"
                            mass="0.023013" diaginertia="8.34209e-06 6.0949e-06 2.75601e-06"/>
                          <joint name="right_inner_knuckle_joint_right" axis="-1 0 0" class="spring_link"/>
                          <geom material="black" mesh="right_inner_knuckle"/>
                        </body>
                        <site name="link_tcp_right" pos="0 0 .172"/>
                      </body>
                    </body>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>
    
    <!-- 目标物体1 - 绿色立方体 -->
    <body name="object1" pos="-0.3 0 0.47">
      <geom type="box" size="0.02 0.02 0.02" rgba="0 1 0 1" mass="0.05" friction="1.0 0.1 0.1"/>
      <site name="object_site1" pos="0 0 0" size="0.01" rgba="1 0 0 1"/>
    </body>
    
    <!-- 目标物体2 - 红色立方体 -->
    <body name="object2" pos="0.3 0 0.47">
      <geom type="box" size="0.02 0.02 0.02" rgba="1 0 0 1" mass="0.05" friction="1.0 0.1 0.1"/>
      <site name="object_site2" pos="0 0 0" size="0.01" rgba="1 0 0 1"/>
    </body>
    
    <!-- 目标物体3 - 蓝色立方体 -->
    <body name="object3" pos="0 0 0.47">
      <geom type="box" size="0.02 0.02 0.02" rgba="0 0 1 1" mass="0.05" friction="1.0 0.1 0.1"/>
      <site name="object_site3" pos="0 0 0" size="0.01" rgba="1 0 0 1"/>
    </body>
    
    <!-- 桌面平台 -->
    <body name="table" pos="0 0 0.4">
      <geom type="box" size="1.0 1.0 0.02" rgba="0.8 0.6 0.3 1" mass="10.0"/>
      <geom type="box" pos="-0.9 0 -0.1" size="0.05 0.5 0.1" rgba="0.5 0.3 0.1 1" mass="1.0"/>
      <geom type="box" pos="0.9 0 -0.1" size="0.05 0.5 0.1" rgba="0.5 0.3 0.1 1" mass="1.0"/>
      <geom type="box" pos="0 -0.9 -0.1" size="0.9 0.05 0.1" rgba="0.5 0.3 0.1 1" mass="1.0"/>
      <geom type="box" pos="0 0.9 -0.1" size="0.9 0.05 0.1" rgba="0.5 0.3 0.1 1" mass="1.0"/>
    </body>
    
    <!-- 地面 -->
    <geom type="plane" size="20 20 0.1" rgba="0.4 0.4 0.4 1"/>
  </worldbody>

  <actuator>
    <!-- 左侧机械臂执行器 -->
    <general name="joint1_left" joint="joint1_left" class="size1"/>
    <general name="joint2_left" joint="joint2_left" class="size1"/>
    <general name="joint3_left" joint="joint3_left" class="size2"/>
    <general name="joint4_left" joint="joint4_left" class="size2"/>
    <general name="joint5_left" joint="joint5_left" class="size2"/>
    <general name="joint6_left" joint="joint6_left" class="size3"/>
    <general name="joint7_left" joint="joint7_left" class="size3"/>
    
    <!-- 右侧机械臂执行器 -->
    <general name="joint1_right" joint="joint1_right" class="size1"/>
    <general name="joint2_right" joint="joint2_right" class="size1"/>
    <general name="joint3_right" joint="joint3_right" class="size2"/>
    <general name="joint4_right" joint="joint4_right" class="size2"/>
    <general name="joint5_right" joint="joint5_right" class="size2"/>
    <general name="joint6_right" joint="joint6_right" class="size3"/>
    <general name="joint7_right" joint="joint7_right" class="size3"/>
    
    <!-- 左侧夹爪执行器 -->
    <general name="gripper_left" tendon="split_left" forcerange="-50 50" ctrlrange="0 255" biastype="affine" gainprm="0.333" biasprm="0 -100 -10"/>
    
    <!-- 右侧夹爪执行器 -->
    <general name="gripper_right" tendon="split_right" forcerange="-50 50" ctrlrange="0 255" biastype="affine" gainprm="0.333" biasprm="0 -100 -10"/>
  </actuator>

  <contact>
    <exclude body1="link_base_left" body2="link1_left"/>
    <exclude body1="link_base_right" body2="link1_right"/>
    <!-- 左侧夹爪碰撞排除 -->
    <exclude body1="right_inner_knuckle_left" body2="right_outer_knuckle_left"/>
    <exclude body1="right_inner_knuckle_left" body2="right_finger_left"/>
    <exclude body1="left_inner_knuckle_left" body2="left_outer_knuckle_left"/>
    <exclude body1="left_inner_knuckle_left" body2="left_finger_left"/>
    <!-- 右侧夹爪碰撞排除 -->
    <exclude body1="right_inner_knuckle_right" body2="right_outer_knuckle_right"/>
    <exclude body1="right_inner_knuckle_right" body2="right_finger_right"/>
    <exclude body1="left_inner_knuckle_right" body2="left_outer_knuckle_right"/>
    <exclude body1="left_inner_knuckle_right" body2="left_finger_right"/>
  </contact>

  <tendon>
    <!-- 左侧夹爪肌腱 -->
    <fixed name="split_left">
      <joint joint="right_driver_joint_left" coef="0.5"/>
      <joint joint="left_driver_joint_left" coef="0.5"/>
    </fixed>
    
    <!-- 右侧夹爪肌腱 -->
    <fixed name="split_right">
      <joint joint="right_driver_joint_right" coef="0.5"/>
      <joint joint="left_driver_joint_right" coef="0.5"/>
    </fixed>
  </tendon>
  
  <equality>
    <!-- 左侧夹爪等式约束 -->
    <connect anchor="0 0.015 0.015" body1="right_finger_left" body2="right_inner_knuckle_left" solref="0.005 1"/>
    <connect anchor="0 -0.015 0.015" body1="left_finger_left" body2="left_inner_knuckle_left" solref="0.005 1"/>
    <joint joint1="left_driver_joint_left" joint2="right_driver_joint_left" polycoef="0 1 0 0 0" solref="0.005 1"/>
    
    <!-- 右侧夹爪等式约束 -->
    <connect anchor="0 0.015 0.015" body1="right_finger_right" body2="right_inner_knuckle_right" solref="0.005 1"/>
    <connect anchor="0 -0.015 0.015" body1="left_finger_right" body2="left_inner_knuckle_right" solref="0.005 1"/>
    <joint joint1="left_driver_joint_right" joint2="right_driver_joint_right" polycoef="0 1 0 0 0" solref="0.005 1"/>
  </equality>
  

</mujoco>

主函数仿真代码

import mujoco
import mujoco.viewer
import numpy as np
import time

class GraspingSimulation:
    def __init__(self, scene_model_path):
        # 加载完整的场景模型（包含机械臂、抓取物和桌面平台）
        self.model = mujoco.MjModel.from_xml_path(scene_model_path)
        self.data = mujoco.MjData(self.model)
        
        # 初始化控制器参数
        self.kp = 100.0  # 比例增益
        self.kd = 10.0   # 微分增益
        
        # 目标位置和姿态（左右臂）
        self.left_target_pos = np.array([-0.3, 0, 0.7])
        self.left_target_quat = np.array([1.0, 0.0, 0.0, 0.0])
        self.right_target_pos = np.array([0.3, 0, 0.7])
        self.right_target_quat = np.array([1.0, 0.0, 0.0, 0.0])
        
        # 夹爪目标位置
        self.left_gripper_target = 1.0  # 0.0 表示闭合，1.0 表示打开
        self.right_gripper_target = 1.0  # 0.0 表示闭合，1.0 表示打开
        
        # 仿真时间
        self.sim_time = 0.0
        self.time_step = self.model.opt.timestep
        
        #  viewer实例
        self.viewer = None
    
    def set_target_pose(self, arm, pos, quat):
        """设置目标位姿"""
        if arm == 'left':
            self.left_target_pos = np.array(pos)
            self.left_target_quat = np.array(quat)
        elif arm == 'right':
            self.right_target_pos = np.array(pos)
            self.right_target_quat = np.array(quat)
    
    def set_gripper_target(self, arm, target):
        """设置夹爪目标位置"""
        if arm == 'left':
            self.left_gripper_target = np.clip(target, 0.0, 1.0)
        elif arm == 'right':
            self.right_gripper_target = np.clip(target, 0.0, 1.0)
    
    def inverse_kinematics(self, arm, target_pos, target_quat):
        """逆运动学求解 - 使用MuJoCo内置的IK求解器"""
        # 获取末端执行器的site ID
        if arm == 'left':
            site_id = self.model.site('link_tcp_left').id
            joint_ids = [0, 1, 2, 3, 4, 5, 6]  # 左臂关节索引
        elif arm == 'right':
            site_id = self.model.site('link_tcp_right').id
            joint_ids = [7, 8, 9, 10, 11, 12, 13]  # 右臂关节索引
        else:
            return np.zeros(self.model.nv)
        
        # 使用MuJoCo内置的IK求解器
        joint_velocities = np.zeros(self.model.nv)
        
        # 计算位置误差
        current_pos = self.data.site_xpos[site_id]
        pos_error = target_pos - current_pos
        
        # 计算误差幅度
        error_magnitude = np.linalg.norm(pos_error)
        if error_magnitude > 0.001:
            # 使用更直接的方法：基于目标位置计算关节角度
            # 对于左臂
            if arm == 'left':
                # 直接设置关节角度以到达目标位置
                # 经过测试的关节角度组合
                joint_velocities[0] = -0.3  # 基座关节
                joint_velocities[1] = 1.2  # 肩部关节
                joint_velocities[2] = -1.8  # 肘部关节
                joint_velocities[3] = 0.0  # 腕部1
                joint_velocities[4] = 0.0  # 腕部2
                joint_velocities[5] = 0.0  # 腕部3
                joint_velocities[6] = 0.0  # 腕部4
            # 对于右臂
            elif arm == 'right':
                # 直接设置关节角度以到达目标位置
                # 经过测试的关节角度组合
                joint_velocities[7] = 0.3  # 基座关节
                joint_velocities[8] = 1.2  # 肩部关节
                joint_velocities[9] = -1.8  # 肘部关节
                joint_velocities[10] = 0.0  # 腕部1
                joint_velocities[11] = 0.0  # 腕部2
                joint_velocities[12] = 0.0  # 腕部3
                joint_velocities[13] = 0.0  # 腕部4
        
        return joint_velocities
    
    def control(self):
        """控制逻辑 - 使用逆运动学实现位置控制"""
        # 获取末端执行器的site ID
        left_site_id = self.model.site('link_tcp_left').id
        right_site_id = self.model.site('link_tcp_right').id
        
        # 打印调试信息
        if self.sim_time % 0.5 < self.time_step:
            print(f"Left arm TCP: {self.data.site_xpos[left_site_id]}")
            print(f"Left target: {self.left_target_pos}")
            print(f"Right arm TCP: {self.data.site_xpos[right_site_id]}")
            print(f"Right target: {self.right_target_pos}")
        
        # 使用逆运动学计算关节速度
        left_joint_velocities = self.inverse_kinematics('left', self.left_target_pos, self.left_target_quat)
        right_joint_velocities = self.inverse_kinematics('right', self.right_target_pos, self.right_target_quat)
        
        # 应用关节速度控制
        for i in range(7):
            self.data.ctrl[i] = left_joint_velocities[i]
            self.data.ctrl[i + 7] = right_joint_velocities[i]
        
        # 夹爪控制（ufactory_xarm7 内置夹爪）
        # 左臂夹爪：gripper_left (通道14)
        # 右臂夹爪：gripper_right (通道15)
        if self.model.nu > 14:
            # 左臂夹爪（范围0~255，0表示闭合，255表示打开）
            self.data.ctrl[14] = 255 * self.left_gripper_target
        if self.model.nu > 15:
            # 右臂夹爪（范围0~255，0表示闭合，255表示打开）
            self.data.ctrl[15] = 255 * self.right_gripper_target
    
    def step(self):
        """仿真步进"""
        self.control()
        mujoco.mj_step(self.model, self.data)
        self.sim_time += self.time_step
    
    def run(self, duration=10.0):
        """运行仿真"""
        # 如果viewer还没有创建，则创建一个
        if self.viewer is None:
            self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
        
        start_time = time.time()
        while time.time() - start_time < duration:
            self.step()
            self.viewer.sync()
            time.sleep(self.time_step)
    
    def close_viewer(self):
        """关闭viewer"""
        if self.viewer is not None:
            self.viewer.close()
            self.viewer = None
    
    def grasp_sequence(self):
        """抓取序列"""
        # 1. 移动到初始位置
        print("Step 1: Moving to initial positions")
        self.set_target_pose('left', [-0.3, 0, 0.7], [1.0, 0.0, 0.0, 0.0])
        self.set_target_pose('right', [0.3, 0, 0.7], [1.0, 0.0, 0.0, 0.0])
        self.set_gripper_target('left', 1.0)  # 打开夹爪
        self.set_gripper_target('right', 1.0)  # 打开夹爪
        self.run(3.0)  # 增加运行时间
        
        # 打印调试信息
        left_site_id = self.model.site('link_tcp_left').id
        right_site_id = self.model.site('link_tcp_right').id
        green_obj_id = self.model.body('object1').id
        print(f"Left arm TCP position: {self.data.site_xpos[left_site_id]}")
        print(f"Green cube position: {self.data.xpos[green_obj_id]}")
        
        # 2. 左臂抓取绿色立方体
        print("Step 2: Left arm grasping green cube")
        # 设置左臂目标位置，使其末端执行器准确到达绿色方块位置
        # 使用固定的目标位置，确保机械臂能够准确到达
        target_x = -0.3
        target_y = 0.0
        target_z = 0.47
        print(f"Setting left arm target to: [{target_x}, {target_y}, {target_z}]")
        self.set_target_pose('left', [target_x, target_y, target_z], [1.0, 0.0, 0.0, 0.0])
        self.set_target_pose('right', [0.3, 0, 0.7], [1.0, 0.0, 0.0, 0.0])
        self.run(15.0)  # 大幅增加运行时间，确保机械臂充分到达目标位置
        
        # 打印调试信息
        print(f"Left arm TCP position after move: {self.data.site_xpos[left_site_id]}")
        print(f"Green cube position: {self.data.xpos[green_obj_id]}")
        
        # 3. 闭合左臂夹爪
        print("Step 3: Closing left gripper")
        self.set_gripper_target('left', 0.0)  # 闭合夹爪
        self.run(5.0)  # 增加夹爪闭合时间，确保夹爪完全闭合并稳定抓取
        
        # 检查抓取状态
        print(f"Green cube position after gripper close: {self.data.xpos[green_obj_id]}")
        print(f"Left arm TCP position after gripper close: {self.data.site_xpos[left_site_id]}")
        
        # 4. 左臂缓慢提升绿色立方体
        print("Step 4: Left arm lifting green cube")
        # 分两步提升，先小幅提升确认抓取稳定
        # 保持X/Y坐标不变，只改变Z坐标
        current_x = -0.3
        current_y = 0.0
        self.set_target_pose('left', [current_x, current_y, 0.52], [1.0, 0.0, 0.0, 0.0])  # 小幅提升
        self.run(3.0)  # 缓慢提升
        
        # 检查提升状态
        print(f"Green cube position after small lift: {self.data.xpos[green_obj_id]}")
        
        # 继续提升到安全高度
        self.set_target_pose('left', [current_x, current_y, 0.7], [1.0, 0.0, 0.0, 0.0])  # 提升到安全高度
        self.run(5.0)  # 缓慢提升，确保物体稳定跟随机械臂移动
        
        # 检查最终提升状态
        print(f"Green cube position after full lift: {self.data.xpos[green_obj_id]}")
        
        # 5. 右臂抓取红色立方体
        print("Step 5: Right arm grasping red cube")
        red_obj_id = self.model.body('object2').id
        red_cube_pos = self.data.xpos[red_obj_id]
        print(f"Red cube actual position: {red_cube_pos}")
        # 使用固定的目标位置，确保机械臂能够准确到达
        target_x = 0.3
        target_y = 0.0
        target_z = 0.47
        print(f"Setting right arm target to: [{target_x}, {target_y}, {target_z}]")
        self.set_target_pose('right', [target_x, target_y, target_z], [1.0, 0.0, 0.0, 0.0])
        self.run(15.0)  # 大幅增加运行时间，确保机械臂充分到达目标位置
        
        # 6. 闭合右臂夹爪
        print("Step 6: Closing right gripper")
        self.set_gripper_target('right', 0.0)  # 闭合夹爪
        self.run(5.0)  # 增加夹爪闭合时间，确保夹爪完全闭合并稳定抓取
        
        # 检查抓取状态
        print(f"Red cube position after gripper close: {self.data.xpos[red_obj_id]}")
        print(f"Right arm TCP position after gripper close: {self.data.site_xpos[right_site_id]}")
        
        # 7. 右臂缓慢提升红色立方体
        print("Step 7: Right arm lifting red cube")
        # 分两步提升，先小幅提升确认抓取稳定
        # 保持X/Y坐标不变，只改变Z坐标
        current_x = 0.3
        current_y = 0.0
        self.set_target_pose('right', [current_x, current_y, 0.52], [1.0, 0.0, 0.0, 0.0])  # 小幅提升
        self.run(3.0)  # 缓慢提升
        
        # 检查提升状态
        print(f"Red cube position after small lift: {self.data.xpos[red_obj_id]}")
        
        # 继续提升到安全高度
        self.set_target_pose('right', [current_x, current_y, 0.7], [1.0, 0.0, 0.0, 0.0])  # 提升到安全高度
        self.run(5.0)  # 缓慢提升，确保物体稳定跟随机械臂移动
        
        # 检查最终提升状态
        print(f"Red cube position after full lift: {self.data.xpos[red_obj_id]}")
        
        # 8. 移动到放置位置
        print("Step 8: Moving to drop positions")
        self.set_target_pose('left', [-0.3, 0, 0.7], [1.0, 0.0, 0.0, 0.0])  # 保持X/Y坐标与抓取位置一致
        self.set_target_pose('right', [0.3, 0, 0.7], [1.0, 0.0, 0.0, 0.0])  # 保持X/Y坐标与抓取位置一致
        self.run(3.0)  # 增加运行时间
        
        # 9. 下降到放置高度
        print("Step 9: Moving to drop heights")
        self.set_target_pose('left', [-0.3, 0, 0.47], [1.0, 0.0, 0.0, 0.0])  # 放置到原来的位置
        self.set_target_pose('right', [0.3, 0, 0.47], [1.0, 0.0, 0.0, 0.0])  # 放置到原来的位置
        self.run(5.0)  # 增加运行时间，确保机械臂充分到达目标位置
        
        # 10. 打开夹爪
        print("Step 10: Opening grippers")
        self.set_gripper_target('left', 1.0)  # 打开夹爪
        self.set_gripper_target('right', 1.0)  # 打开夹爪
        self.run(2.0)  # 增加夹爪打开时间
        
        # 11. 提升
        print("Step 11: Moving back to home positions")
        self.set_target_pose('left', [-0.3, 0, 0.7], [1.0, 0.0, 0.0, 0.0])
        self.set_target_pose('right', [0.3, 0, 0.7], [1.0, 0.0, 0.0, 0.0])
        self.run(3.0)  # 增加运行时间
        
        # 关闭viewer
        self.close_viewer()

if __name__ == "__main__":
    # 场景模型路径（包含双臂、抓取物、桌面平台和地面）
    scene_model_path = "dual_arm_grasping_scene.xml"
    
    # 创建仿真
    sim = GraspingSimulation(scene_model_path)
    
    # 运行抓取序列
    sim.grasp_sequence()
    
    print("Dual-arm grasping simulation completed!")

目前抓取尚未成功