MuJoCo 全流程实战教程：从零搭建一个仿真实验

原创已于 2025-08-22 23:11:14 修改 · 1.9w 阅读

138

325 ·

CC 4.0 BY-SA版权

文章标签：

#机器人 #python #MuJoCo #物理仿真 #触觉反馈

于 2025-05-14 18:02:27 首次发布

MuJoCo 专栏收录该内容

1 篇文章

订阅专栏

该文章已生成可运行项目，

前言：

本教程基于MuJoCo官方文档中提供的python接口（对应的MuJoCo版本为3.2.7），并结合论文《Goal-Driven Robotic Pushing Using Tactile and Proprioceptive Feedback》中的物体推动实验（如下图），让读者通过代码实操快速上手MuJoCo。

通过在MuJoCo中复现论文中的物体推动实验，读者可以学习到：

如何获取并导入机械臂模型
如何修改MJCF文件（MuJoCo中定义模型的xml文件）
如何通过IKPy库简单控制机械臂
如何获取仿真过程中的各种数据（如力传感器的数据）

注意事项：

1、在开始本教程前，建议读者先去官方文档中简单运行下Tutorial notebook提供的入门教程。

2、MuJoCo的官方文档写的不够通俗，但是它确实是将所有的API都清晰列了出来，在自学时可以借助AI工具快速了解各个函数的作用。（本教程是编者自己摸索出来的其中一种实现方式，水平有限，写法可能不是最优解）

3、目前在具身智能的强化学习中主要用dm_control库，这是DeepMind公司在MuJoCo的基础上，补充了强化学习的框架等别的功能，里面是包含了完整的MuJoCo的。本教程不涉及强化学习相关概念，读者可以按需要直接去学习dm_control。

4、获取本教程的代码请点击：https://github.com/VincentXun/tutorial_for_mujoco

此代码对应着第五章物体推动实验。

一、安装MuJoCo

我这里选择新建一个Anaconda虚拟环境：

conda create -n tutorial_for_mujoco python=3.9

接着在虚拟环境中执行：

pip install mujoco

下载时记得开个VPN，不然下载速度很慢。

然后打开PyCharm，或是其他python编辑器，选择好对应的装有MuJoCo的环境。

二、获取并导入UR5e机械臂模型

论文中使用的常见的UR5e机械臂，我们可以在MuJoCo官方模型库Model Gallery中找到对应的模型。

打开后可以看到UR5e的MuJoCo模型文件"universal_robots_ur5e"：

我们将模型下载，并放到工程目录里：

接下来创建一个main.py，并运行如下代码：

import mujoco.viewer

def main():
    model = mujoco.MjModel.from_xml_path('model/universal_robots_ur5e/scene.xml')
    data = mujoco.MjData(model)

    with mujoco.viewer.launch_passive(model, data) as viewer:
        while viewer.is_running():
            mujoco.mj_step(model, data)
            viewer.sync()

if __name__ == "__main__":
    main()

运行后可以看到MuJoCo的官方图形用户界面：

点击左侧Simulation下的"Load key"，可以看到机械臂的姿态变成了上图所示的样子。Load key指加载UR5e模型默认的keyframe，即机械臂的默认姿态对应的六个关节值。同时读者可以左右滑动改变右侧Control的关节电机设定值来观察机械臂的运动。

接下来修改下代码：

import mujoco.viewer
import time

def main():
    model = mujoco.MjModel.from_xml_path('model/universal_robots_ur5e/scene.xml')
    data = mujoco.MjData(model)
    data.ctrl[:6] = [-1.57, -1.34, 2.65, -1.3, 1.55, 0]

    with mujoco.viewer.launch_passive(model, data) as viewer:
        while viewer.is_running():
            mujoco.mj_step(model, data)
            viewer.sync()
            time.sleep(0.002) # 让动画速度变慢，不然更新太快看不清机械臂的运动过程

if __name__ == "__main__":
    main()

运行后可以看到UR5e慢慢地运动到如下位姿：

其中，

data.ctrl[:6] = [-1.57, -1.34, 2.65, -1.3, 1.55, 0]

表示MJCF文件中的前6个驱动器actuator的控制值，此时对应着机械臂的6个关节电机的控制值。具体可以在ur5e.xml中看到：

  <actuator>
    <general class="size3" name="shoulder_pan" joint="shoulder_pan_joint"/>
    <general class="size3" name="shoulder_lift" joint="shoulder_lift_joint"/>
    <general class="size3_limited" name="elbow" joint="elbow_joint"/>
    <general class="size1" name="wrist_1" joint="wrist_1_joint"/>
    <general class="size1" name="wrist_2" joint="wrist_2_joint"/>
    <general class="size1" name="wrist_3" joint="wrist_3_joint"/>
  </actuator>

这里对应着6个PD控制的关节电机，data.ctrl设置的就是这6个电机的目标值。

至此，我们已经掌握了如何导入模型、如何开始仿真、如何将仿真过程可视化这几个重要步骤了。接下来将详细讲解下每一行代码。

首先看到这两行：

    model = mujoco.MjModel.from_xml_path('model/universal_robots_ur5e/scene.xml')
    data = mujoco.MjData(model)

可以看到读取了scene.xml模型文件。这是MuJoCo的特点之一，模型model与仿真过程中产生的各种动态数据data分开储存。

下面打开scene.xml文件，这是一个用来定义仿真场景的文件（例如里面定义了光照与地板），其中可以看到里面包含了UR5e的机械臂模型：

<include file="ur5e.xml"/>

之后我们需要对scene.xml与ur5e.xml这两个MJCF文件进行修改，添加所需的力传感器与被推动物体的模型。

接着往下看代码，打开图形用户界面后，便进入了循环，不断运行

mujoco.mj_step(model, data)

这是更新仿真状态的函数，非常重要，每执行一次mj_step，仿真便会前进0.002s（默认值）。简单而言，比如我定义了一个初始位置在半空中的小球，那么每执行一次mj_step，MuJoCo就会根据重力加速度的值计算出0.002s后小球会下降多少的距离，同时更新一次data中小球的xyz坐标信息。

下一行代码：

viewer.sync()

是用来更新图形用户界面中的动画的。

三、通过IKPy库求解逆运动学，控制机械臂运动

3.1 前言

逆运动学，Inverse Kinematics，即给定机械臂末端执行器的目标位置与姿态，然后求出机械臂对应的各个关节值。

在本教程中我们将通过IKPy这个库来实现这一点，并将得到的关节值通过线性插值的方法传给MuJoCo中ur5e机械臂的各个关节电机，以此实现对机械臂的控制。

读者可以先去IKPy的官方GitHub简单了解下，为了方便阅读，我把官方提供的教程截图了。

简而言之，IKPy库需要我们提供机械臂的URDF文件。下面我们先在环境中安装所需要的库：

pip install ikpy matplotlib transforms3d

接下来去获取ur5e的URDF文件，可以看到在MuJoCo模型库中也是很贴心地给出了ur5e的官方URDF文件，

点进去后，将整个ur_e_description文件下载，

打开urdf文件，可以看到官方提供的.xacro格式的文件，这需要我们通过ROS将其转换为.urdf文件。

这是获取ur5e的urdf文件的官方途径，方便起见，读者可以直接在教程代码https://github.com/VincentXun/tutorial_for_mujoco中获取转换好的ur5e_orig.urdf。

下面我将原始的ur5e_orig.urdf文件重命名为ur5e.urdf，并将ur5e.urdf文件放在model文件夹目录下。

3.2 IK计算

如下图，我们的目标是让ur5e从初始姿态运动到准备推动物块的姿态：

我们先利用IKPy提供的可视化工具来简单验证下，运行如下代码：

import ikpy.chain
import ikpy.utils.plot as plot_utils
import matplotlib.pyplot as plt
import transforms3d as tf

def main():
    my_chain = ikpy.chain.Chain.from_urdf_file("model/ur5e.urdf")
    ee_pos = [-0.13, 0.5, 0.1] # 末端执行器在世界坐标系下的目标位置的xyz 
    ee_euler = [3.14, 0, 1.57] # 末端执行器的目标欧拉角姿态（弧度）
    ee_orientation = tf.euler.euler2mat(*ee_euler) # 将欧拉角姿态用旋转矩阵表示
    ref_pos = [0, -1.57, -1.34, 2.65, -1.3, 1.55, 0, 0] # 提供初始关节角猜测，用于优化求解
    
    fig, ax = plot_utils.init_3d_figure()
    my_chain.plot(my_chain.inverse_kinematics(ee_pos, ee_orientation, "all", initial_position=ref_pos), ax)
    plt.show()

if __name__ == "__main__":
    main()

运行后可以看到IKPy计算出的逆运动学符合期望位置与姿态：

下面解释inverse_kinematics()函数：

my_chain.inverse_kinematics(ee_pos, ee_orientation, "all", initial_position=ref_pos)

前两个参数是目标位置与目标姿态（这里参考了深蓝课堂的课程，发现姿态这里要用旋转矩阵来表示，用欧拉角得到的结果会有偏差)；第三个参数"all"表示要绕着xyz三个轴进行旋转；

第四个参数initial_position为IK的初始猜测解。一次逆运动学可能有很多种不同的关节角结果，猜测解就是尽可能让它收敛到我希望的那个结果，因此猜测解需要尽可能的准确。如下图，我们可以通过MuJoCo的图形用户界面来手动调整机械臂的姿态，当此时的姿态大致符合我们的要求时，记录下对应的关节角，并以这一组关节角作为初始猜测解。在本次实验中，由于机械臂只需要进行简单的向前推动的动作，所以不需要频繁地修改这个猜测值。

至于为什么除了ur5e的6个关节角外还多了两个数值0，那是因为URDF中，"base_link"与"ee_fixed_joint"即使不参与IK计算，也必须赋0，否则IKPy会报错。

ok，下面把IK结果传给MuJoCo，看看仿真效果如何。

3.3 在MuJoCo中验证IK

在开始前，先在scene.xml文件中可视化MuJoCo世界坐标系原点的坐标轴，以及可视化目标点，方便我们直观地判断IK结果是否准确。

打开scene.xml，在其中添加两个body，一个名字为"world_frame"，一个名字为"target"：

    <worldbody>
        <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
        <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>

        <!--你需要添加的-->
        <body name="world_frame" pos="0 0 0">
            <geom name="X" type="cylinder" size="0.005" fromto="0 0 0 0.3 0 0" rgba="1 0 0 1" contype="0"
                  conaffinity="0"/>
            <geom name="Y" type="cylinder" size="0.005" fromto="0 0 0 0 0.3 0" rgba="0 1 0 1" contype="0"
                  conaffinity="0"/>
            <geom name="Z" type="cylinder" size="0.005" fromto="0 0 0 0 0 0.3" rgba="0 0 1 1" contype="0"
                  conaffinity="0"/>
        </body>
        
        <body name="target" pos="-0.13 0.5 0.1">
            <geom size="0.02" rgba="0 1 0 0.5" contype="0" conaffinity="0"/>
        </body>

    </worldbody>

这是我们第一次对MJCF文件进行修改，读者可以先看下MJCF的body标签。这里我们在<worldbody>这个标签下添加了一个新的<body>，名字叫"world_frame"，坐标在世界坐标系的原点处。其中，这个"world_frame"的body还拥有3个子标签<geom>，分别用来描述指向X、Y、Z坐标轴正方向的圆柱形几何体。

接下来将IKPy算出的关节值传给MuJoCo，运行如下代码：

import mujoco.viewer
import time
import ikpy.chain
import transforms3d as tf


def main():
    model = mujoco.MjModel.from_xml_path('model/universal_robots_ur5e/scene.xml')
    data = mujoco.MjData(model)
    my_chain = ikpy.chain.Chain.from_urdf_file("model/ur5e.urdf")

    ee_pos = [-0.13, 0.5, 0.1]
    ee_euler = [3.14, 0, 1.57]
    ref_pos = [0, -1.57, -1.34, 2.65, -1.3, 1.55, 0, 0]
    ee_orientation = tf.euler.euler2mat(*ee_euler)

    joint_angles = my_chain.inverse_kinematics(ee_pos, ee_orientation, "all", initial_position=ref_pos)
    ctrl = joint_angles[1:-1]
    data.ctrl[:6] = ctrl

    with mujoco.viewer.launch_passive(model, data) as viewer:
        while viewer.is_running():
            mujoco.mj_step(model, data)
            viewer.sync()
            time.sleep(0.002)

if __name__ == "__main__":
    main()

可得到的仿真结果并不符合预期，机械臂的末端执行器并没有抵达绿色球处的目标点[-0.13,0.5,0.1]：

这说明URDF文件的坐标系与MuJoCo的坐标系不一致。从结果上看，感觉URDF的坐标系的XY轴与MuJoCo的XY轴相反。为了确保IKPy计算的坐标与MuJoCo保持一致，我们需要知道此时的误差是什么，才能准确地调整坐标系。

下面先打印出此时的末端执行器的xyz坐标看一下，运行如下代码：

import mujoco.viewer
import time
import ikpy.chain
import transforms3d as tf

def main():
    model = mujoco.MjModel.from_xml_path('model/universal_robots_ur5e/scene.xml')
    data = mujoco.MjData(model)
    my_chain = ikpy.chain.Chain.from_urdf_file("model/ur5e.urdf")

    ee_pos = [-0.13, 0.5, 0.1]
    ee_euler = [3.14, 0, 1.57]
    ref_pos = [0, -1.57, -1.34, 2.65, -1.3, 1.55, 0, 0]
    ee_orientation = tf.euler.euler2mat(*ee_euler)
    ee_id = model.site("attachment_site").id # 获取末端执行器对应的id

    joint_angles = my_chain.inverse_kinematics(ee_pos, ee_orientation, "all", initial_position=ref_pos)
    ctrl = joint_angles[1:-1]
    data.ctrl[:6] = ctrl

    with mujoco.viewer.launch_passive(model, data) as viewer:
        while viewer.is_running():
            mujoco.mj_step(model, data)
            viewer.sync()
            time.sleep(0.002)

    ee_pos = data.site_xpos[ee_id] # 获取此时末端执行器对应的xyz坐标信息
    print(ee_pos)

if __name__ == "__main__":
    main()

此处的"attachment_site"对应着ur5e.xml中的机械臂最后一个关节处，用于连接工具的位置点，此时我们直接认为这个<site>标签对应的标记点就是ur5e的末端执行器。值得注意的是，用户在MJCF中创建的所有实体<body>与标记点<site>，在加载xml文件后，MuJoCo都会按照定义的顺序给每个元素进行标号，在仿真过程中我们可以实时通过id来获取对象当前的各种仿真数据。

打印出来的结果是：[ 0.12974408 -0.49788067 0.0925184 ]，与给定的目标[-0.13，0.5，0.1]相比，可以确定URDF文件的XY轴与MJCF文件的相反了。我的解决办法是：将URDF文件的坐标系绕着Z轴旋转180°。具体实现方式是在base_link处增加一个虚拟关节，此虚拟关节会绕着base_link旋转180°，那么后面的所有关节都相当于是跟着一起旋转了（这个办法有点复杂，应该存在更优的解决办法）。

对ur5e.urdf文件的修改如下，添加一个"global_transform"关节，同时记得修改"shoulder_pan_joint"的父关节。

...  
<link name="base_link">
    <visual>
      <geometry>
        <mesh filename="package://test_and_learn/meshe/ur5e/visual/base.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://test_and_learn/meshe/ur5e/collision/base.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="4.0"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
    </inertial>
  </link>
  
  <!--你需要添加的：-->
  <!-- Global coordinate transformation (rotate 180 degrees around Z axis) -->
  <joint name="global_transform" type="fixed">
    <origin xyz="0 0 0" rpy="0 0 3.141592653589793"/>
    <parent link="base_link"/>
    <child link="transformed_base_link"/>
  </joint>
  <link name="transformed_base_link"/>

  <joint name="shoulder_pan_joint" type="revolute">
    <parent link="transformed_base_link"/>  <!--记得修改parent_link-->
    <child link="shoulder_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.163"/>
    <axis xyz="0 0 1"/>
    <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.14"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="shoulder_link">
...

修改完URDF文件后，不要忘了代码中也要进行相应修改：

...
ref_pos = [0, 0, -1.57, -1.34, 2.65, -1.3, 1.55, 0, 0]
...
ctrl = joint_angles[2:-1]
...

最后，运行代码，打印出的末端执行器的坐标为[-0.13002427 0.49752776 0.0925558 ]，成功！

至于在运行时IKPy会有一些warning：

想要消除，只需要在读取URDF时将不参与计算的关节显式设置为False：

    my_chain = ikpy.chain.Chain.from_urdf_file("model/ur5e.urdf",
                                               active_links_mask=[False, False] + [True] * 6 + [False])

至此，我们已经实现了通过IKPy进行逆运动学求解，在MuJoCo中对机械臂进行控制了。

3.4 对关节空间下的轨迹进行线性插值

本小节的目标是：让机械臂末端执行器沿着Y轴正方向近似匀速地运动一段距离。这里设置起始点为[-0.13,0.3,1]，终点为[-0.13,0.6,1]。为了实现"近似匀速"这一效果，一种简单的办法是：对两点之间的关节空间坐标系下的轨迹进行线性插值处理。具体代码如下，读者也可以获取末端执行器"wrist_3_link"的线速度来简单验证：

import mujoco.viewer
import ikpy.chain
import transforms3d as tf
import numpy as np


def viewer_init(viewer):
    """渲染器的摄像头视角初始化"""
    viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE
    viewer.cam.lookat[:] = [0, 0.5, 0.5]
    viewer.cam.distance = 2.5
    viewer.cam.azimuth = 180
    viewer.cam.elevation = -30


class JointSpaceTrajectory:
    """关节空间坐标系下的线性插值轨迹"""
    def __init__(self, start_joints, end_joints, steps):
        self.start_joints = np.array(start_joints)
        self.end_joints = np.array(end_joints)
        self.steps = steps
        self.step = (self.end_joints - self.start_joints) / self.steps
        self.trajectory = self._generate_trajectory()
        self.waypoint = self.start_joints

    def _generate_trajectory(self):
        for i in range(self.steps + 1):
            yield self.start_joints + self.step * i
        # 确保最后精确到达目标关节值
        yield self.end_joints

    def get_next_waypoint(self, qpos):
        # 检查当前的关节值是否已经接近目标路径点。若是，则更新下一个目标路径点；若否，则保持当前目标路径点不变。
        if np.allclose(qpos, self.waypoint, atol=0.02):
            try:
                self.waypoint = next(self.trajectory)
                return self.waypoint
            except StopIteration:
                pass
        return self.waypoint


def main():
    model = mujoco.MjModel.from_xml_path('model/universal_robots_ur5e/scene.xml')
    data = mujoco.MjData(model)
    my_chain = ikpy.chain.Chain.from_urdf_file("model/ur5e.urdf",
                                               active_links_mask=[False, False] + [True] * 6 + [False])

    start_joints = np.array([-1.57, -1.34, 2.65, -1.3, 1.55, 0])  # 对应机械臂初始位姿[-0.14, 0.3, 0.1, 3.14, 0, 1.57]
    data.qpos[:6] = start_joints # 确保渲染一开始机械臂便处于起始位置，而非MJCF中的默认位置

    # 设置目标点
    ee_pos = [-0.13, 0.6, 0.1]
    ee_euler = [3.14, 0, 1.57]
    ref_pos = [0, 0, -1.57, -1.34, 2.65, -1.3, 1.55, 0, 0]
    ee_orientation = tf.euler.euler2mat(*ee_euler)

    joint_angles = my_chain.inverse_kinematics(ee_pos, ee_orientation, "all", initial_position=ref_pos)
    end_joints = joint_angles[2:-1]

    joint_trajectory = JointSpaceTrajectory(start_joints, end_joints, steps=100)

    with mujoco.viewer.launch_passive(model, data) as viewer:
        viewer_init(viewer)
        while viewer.is_running():
            waypoint = joint_trajectory.get_next_waypoint(data.qpos[:6])
            data.ctrl[:6] = waypoint

            mujoco.mj_step(model, data)
            viewer.sync()


if __name__ == "__main__":
    main()

viewer_init()函数是用来初始化MuJoCo渲染器的初始视角的，方便用户观察仿真动画。

值得注意的是，代码在一开始通过直接设置机械臂6个关节值data.qpos[:6]，来实现初始化机械臂的位姿。qpos指储存在data中的所有<joint>，这也是按照MJCF文件中的定义顺序来一一存储的。由于在我们的MJCF中，最先读取的MJCF就是ur5e.xml，所以最先定义的<joint>，即前6位qpos，就是对应着ur5e的关节值。另外，<joint>不单指机械臂的关节值。比如一个进行自由落体的物体，描述它的位姿需要3轴位置与3轴位姿信息，那么这里也应该给这个物体添加上拥有6轴自由度的<joint>标签，这在下一章添加物体模型的时候会提到。

还有一点，设置data.qpos[:6]与data.ctrl[:6]的目标关节值来调节机械臂位姿，这两种控制方式是不一样的。前者是直接修改机械臂的关节值，后者是修改机械臂的关节电机的设定值。

四、添加触觉传感器与被推动物体的模型

论文中使用的触觉传感器为TacTip，外观如下图。TacTip就像人的手指尖，由半球形橡胶表面与内置摄像头组成，当传感器接触物体时，橡胶表面发生形变，同时带动内表面上的标记点发生移动，内置摄像头记录下标记点的位移，对图像进行分析后便可以判断出接触的位置、方向等信息。论文中是通过CNN去学习这种标记点位移图像到力的映射关系的。

下面我们通过MuJoCo的<sensor>标签中提供的"force"传感器去实现一个TacTip传感器的大致建模。这个"force"传感器可以求出指定物体所受到的合外力矢量。

首先修改ur5e.xml文件，添加一个名叫"force_sensor"的body用来设置传感器的物理外形，让其长得像TacTip传感器。

                                ...
                                <body name="wrist_3_link" pos="0 0 0.1">
                                    <inertial mass="0.1889" pos="0 0.0771683 0" quat="1 0 0 1"
                                              diaginertia="0.000132134 9.90863e-05 9.90863e-05"/>
                                    <joint name="wrist_3_joint" class="size1"/>
                                    <geom material="linkgray" mesh="wrist3" class="visual"/>
                                    <geom class="eef_collision" pos="0 0.08 0" quat="1 1 0 0" size="0.04 0.02"/>
                                    <site name="attachment_site" pos="0 0.1 0" quat="-1 1 0 0"/>

                                    <!--你需要添加的-->
                                    <body name="force_sensor" pos="0 0.1 0" euler="3.14 0 3.14">
                                        <geom name='force_sensor_geom' type="sphere" mass="0.0" size="0.04"
                                              rgba="0 1 0 0.3"/>
                                        <site name="force_sensor_site" pos="0 0 0" quat="1 0 0 0"/>
                                    </body>
                                </body>
                                ...

接下来，在与<worldbody>同级的标签下添加一个<sensor>标签，并将"force"传感器“固定”在"force_sensor_site"这个点上。那么这个"force"传感器便会求出所属的<body>，即"force_sensor"所受到的合外力，包括重力与接触力。因此这里会将mass设置为0，避免重力的干扰。

    <worldbody>
    ...
    </worldbody>

    <!--你需要添加的-->
    <sensor>
        <force site="force_sensor_site"/>
    </sensor>

此时运行main.py，可以看到我们建模出来的TacTip传感器：

至此我们已经添加了一个半球形的"force"传感器了，下面在scene.xml文件中添加被推动物体的模型，在<worldbody>标签里面添加一个拥有6自由度关节的正方体：

        <body name="red_box" pos="-0.1 0.5 0.1" euler="0 0 45">
            <joint name="red_box_joint" type="free"/>
            <geom name="red_box_geom" type="box" size="0.1 0.1 0.1" rgba="1 0 0 1" mass="1.0"/>
        </body>

同时要将ur5e.xml中的<keyframe>部分注释掉：

    <!--    The number of keyframe qpos must match the total number of joints specified in the XML!-->
    <!--    <keyframe>-->
    <!--        <key name="home" qpos="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0"-->
    <!--             ctrl="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0"/>-->
    <!--    </keyframe>-->

这是因为由于"red_box"引入了一个新的<joint>，而原先的<keyframe>只给出了UR5e的六个关节值，若不进行修改则会报错。

五、物体推动实验

将main.py修改为如下代码并运行：

import mujoco.viewer
import ikpy.chain
import transforms3d as tf
import numpy as np
from collections import deque
import matplotlib.pyplot as plt


def viewer_init(viewer):
    """渲染器的摄像头视角初始化"""
    viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE
    viewer.cam.lookat[:] = [0, 0.5, 0.5]
    viewer.cam.distance = 2.5
    viewer.cam.azimuth = 180
    viewer.cam.elevation = -30


class ForcePlotter:
    """实时可视化接触力"""

    def __init__(self, update_interval=20):
        plt.ion()
        self.fig = plt.figure()
        self.ax = self.fig.add_subplot(111, projection='3d')
        self.update_interval = update_interval  # 更新间隔帧数
        self.frame_count = 0  # 帧计数器

    def plot_force_vector(self, force_vector):
        self.frame_count += 1
        if self.frame_count % self.update_interval != 0:
            return  # 跳过本次渲染

        self.ax.clear()

        origin = np.array([0, 0, 0])
        force_magnitude = np.linalg.norm(force_vector)
        force_direction = force_vector / force_magnitude if force_magnitude > 1e-6 else np.zeros(3)

        # 主箭头
        arrow_tip = force_direction * 1.5
        self.ax.quiver(*origin, *arrow_tip, color='r', arrow_length_ratio=0)

        # 蓝色箭头
        self.ax.quiver(*arrow_tip, *(0.5 * force_direction), color='b', arrow_length_ratio=0.5)

        # XY平面投影
        self.ax.plot([0, arrow_tip[0]], [0, arrow_tip[1]], [-2, -2], 'g--')

        # XZ平面投影
        self.ax.plot([0, 0], [2, 2], [0, arrow_tip[2]], 'm--')

        # 力大小指示条
        scaled_force = min(max(force_magnitude / 50, 0), 2)
        self.ax.plot([-2, -2], [2, 2], [0, scaled_force], 'c-')
        self.ax.text(-2, 2, scaled_force, f'Force: {force_magnitude:.1f}', color='c')

        # 坐标系设置
        self.ax.scatter(0, 0, 0, color='k', s=10)
        self.ax.set_xlim([-2, 2])
        self.ax.set_ylim([-2, 2])
        self.ax.set_zlim([-2, 2])
        self.ax.set_title(f'Force Direction')

        plt.draw()
        plt.pause(0.001)
        self.frame_count = 0  # 重置计数器


class ForceSensor:
    def __init__(self, model, data, window_size=100):
        self.model = model
        self.data = data
        self.window_size = window_size
        self.force_history = deque(maxlen=window_size)

    def filter(self):
        """获取并滑动平均滤波力传感器数据(传感器坐标系下)"""
        # 获取MjData中的传感器数据
        force_local_raw = self.data.sensordata[:3].copy() * -1

        # 添加新数据到滑动窗口
        self.force_history.append(force_local_raw)

        # 计算滑动平均
        filtered_force = np.mean(self.force_history, axis=0)

        return filtered_force


class JointSpaceTrajectory:
    """关节空间坐标系下的线性插值轨迹"""

    def __init__(self, start_joints, end_joints, steps):
        self.start_joints = np.array(start_joints)
        self.end_joints = np.array(end_joints)
        self.steps = steps
        self.step = (self.end_joints - self.start_joints) / self.steps
        self.trajectory = self._generate_trajectory()
        self.waypoint = self.start_joints

    def _generate_trajectory(self):
        for i in range(self.steps + 1):
            yield self.start_joints + self.step * i
        # 确保最后精确到达目标关节值
        yield self.end_joints

    def get_next_waypoint(self, qpos):
        # 检查当前的关节值是否已经接近目标路径点。若是，则更新下一个目标路径点；若否，则保持当前目标路径点不变。
        if np.allclose(qpos, self.waypoint, atol=0.02):
            try:
                self.waypoint = next(self.trajectory)
                return self.waypoint
            except StopIteration:
                pass
        return self.waypoint


def main():
    model = mujoco.MjModel.from_xml_path('model/universal_robots_ur5e/scene.xml')
    data = mujoco.MjData(model)
    my_chain = ikpy.chain.Chain.from_urdf_file("model/ur5e.urdf",
                                               active_links_mask=[False, False] + [True] * 6 + [False])

    start_joints = np.array([-1.57, -1.34, 2.65, -1.3, 1.55, 0])  # 对应机械臂初始位姿[-0.13, 0.3, 0.1, 3.14, 0, 1.57]
    data.qpos[:6] = start_joints  # 确保渲染一开始机械臂便处于起始位置，而非MJCF中的默认位置

    # 设置目标点
    ee_pos = [-0.13, 0.6, 0.1]
    ee_euler = [3.14, 0, 1.57]
    ref_pos = [0, 0, -1.57, -1.34, 2.65, -1.3, 1.55, 0, 0]
    ee_orientation = tf.euler.euler2mat(*ee_euler)

    joint_angles = my_chain.inverse_kinematics(ee_pos, ee_orientation, "all", initial_position=ref_pos)
    end_joints = joint_angles[2:-1]

    joint_trajectory = JointSpaceTrajectory(start_joints, end_joints, steps=100)

    force_sensor = ForceSensor(model, data)
    force_plotter = ForcePlotter()

    with mujoco.viewer.launch_passive(model, data) as viewer:
        viewer_init(viewer)
        while viewer.is_running():
            waypoint = joint_trajectory.get_next_waypoint(data.qpos[:6])
            data.ctrl[:6] = waypoint

            filtered_force = force_sensor.filter()
            force_plotter.plot_force_vector(filtered_force)

            mujoco.mj_step(model, data)
            viewer.sync()


if __name__ == "__main__":
    main()

运行后可以看到UR5e在推动正方体，同时左侧会实时显示出“TacTip”传感器受到的接触力的大小与方向（传感器坐标系下）。

回到代码中，相较之前的多出了两个class，分别是ForceSensor与ForcePlotter。前者是获取MuJoCo仿真中的"force"传感器的原始数据，即XYZ力矢量，并对其进行滑动平均滤波后输出滤波后的力矢量；后者是通过matplotlab库将传入的XYZ力矢量实时可视化出来。

那么通过得到的接触力数据，我们就可以按照论文的思路设计出PID控制器，来实现“将正方体推动到目标位置”这一目的。根据论文，一共需要2个PID控制器：

第1个PID控制器：用来确保机械臂末端执行器始终垂直推动正方体。如下图，输入误差为θ，通过调节末端执行器的偏航角Yaw，使得θ误差趋向于0。