Mujoco 学习系列（五）与ROS之间的通讯
这篇文章是 mujoco 学习系列第五篇，在你通过前四篇文章掌握了最基本的使用方法之后，这一篇主要介绍如何与 ROS 框架进行通讯。ROS是学习机器人与具身智能的必备组件，尽管现在对ROS的框架结构能否适应具身存在争议，但当前必须承认的一点是很多机器人硬件与算法提供了ROS接口，能让我们方便地使用机器人并调试算法，因此我个人认为ROS还是会在具身智能这个行业中起到很大的作用。
【Note】由于这篇文章主要介绍两个系统之间的通讯方式，因此需要你已经安装了ROS并掌握基本使用方法，这里不会就ROS的安装和部署额外介绍。
- Operation System：Ubuntun 20.04 Desktop
- Linux Kernel：Linux Server 5.15.0-139-generic
- ROS1 Version：noetic
- ROS2 Version：foxy
有关具身使用 ROS1 还是 ROS2 的讨论，我个人认为会以 ROS2 为主，因为具身机器人通常有很多个传感器，一个典型结构如星海图的 R1 系列机器人有 3个 RGBD 相机、21个主动关节，如果你的业务需要触觉或激光雷达的话传感器还会进一步增加，ROS1最大的短板就是数据传输效率，因此我认为 ROS2 的分布式通讯框架是一个解决方案，但我并不认为 ROS2 适合具身智能，具身智能仍然需要一个属于自己的操作系统或框架 Embodied Operation System。
1. 准备一个 xml 文件
无论你想要ROS1还是ROS2进行试验，都需要准备一个 xml 文件用来描述机器人，这里以之前那篇博客中使用的滑块demo为例：
- Mujoco 学习系列（三）机器人状态IO与仿真操作：第3章节 “设置joint值”
<mujoco><worldbody><light diffuse="0.5 0.5 0.5" pos="0 0 5" dir="0 0 -1"/> <geom name="ground" type="plane" size="5 5 0.1" pos="0 0 0" rgba="0.5 0.5 0.5 1" solimp="0.9 0.95 0.001" solref="0.02 1"/><body name="slider" pos="0 0 0.1"><joint name="slide_joint" type="slide" axis="1 0 0" damping="0.2" range="-2 2"/><geom type="box" size="0.2 0.1 0.1" rgba="0 0.5 0.8 1" mass="0.5" /></body></worldbody><actuator><position name="position" joint="slide_joint" kp="100" kv="10"/></actuator>
</mujoco>
2.与 ROS1 交互
与ROS1进行交互需要在ROS工程包中启动 mujoco 仿真器，你可以将 mujoco 视为 Gazebo 的替代品。
2.1 初始化工作空间
创建工作空间 mujoco_ws：
(mujoco) $ mkdir mujoco_ws && cd mujoco_ws
(mujoco) $ catkin_make
然后创建一个名为 demo 的 package，然后返回工作空间目录进行编译确保这部分没有报错：
(mujoco) $ cd src
(mujoco) $ catkin_create_pkg demo std_msgs rospy roscpp
(mujoco) $ cd ../
(mujoco) $ catkin_make
然后在demo package 包的几个不同的位置创建几个文件夹：
(mujoco) $ cd mujoco_ws/demo # 进入到demo包目录中
(mujoco) $ mkdir src/scripts # 存放python脚本的文件夹
(mujoco) $ touch src/scripts/robot.py
(mujoco) $ mkdir src/mjcf # 存在xml文件的文件夹
(mujoco) $ touch src/mjcf/robot.xml
再将第1节中的xml内容复制到 src/mjcf 中，此时的你的文件结构应该如下：
(mujoco) $ tree -L 4.
├── build
│ ├── atomic_configure...
├── devel
│ ├── cmake.lock...
└── src├── CMakeLists.txt -> /opt/ros/noetic/share/catkin/cmake/toplevel.cmake└── demo # demo 包├── CMakeLists.txt├── include│ └── demo├── mjcf # 存放xml文件的文件夹│ └── robot.xml # 机器人模型xml文件├── package.xml├── scripts # 存放python脚本的文件夹│ └── robot.py # python脚本文件└── src
2.2 编写 python 脚本
打开 src/demo/scripts/robot.py 这个文件并编写下面的内容：
import mujoco
import mujoco.viewer
import numpy as np
import rospy
from sensor_msgs.msg import JointState# --------- 初始化 ROS 节点 ---------
rospy.init_node('mujoco_joint_publisher')
joint_pub = rospy.Publisher('/joint_states', JointState, queue_size=10)# --------- 加载 MuJoCo 模型 ---------
# 替换为你的 MJCF 文件路径
xml_path = "/home/gaohao/Desktop/mujoco_ws/src/demo/mjcf/robot.xml"
model = mujoco.MjModel.from_xml_path(xml_path)
data = mujoco.MjData(model)# --------- 模拟 + 发布循环 ---------
joint_names = ['slide_joint'] # 替换为你需要的关节名
joint_indices = [model.joint(name).qposadr for name in joint_names]rate = rospy.Rate(100) # 100Hzwith mujoco.viewer.launch_passive(model=model, data=data) as viewer:while not rospy.is_shutdown():mujoco.mj_step(model, data)# 获取关节位置positions = [data.qpos[i] for i in joint_indices]# 构造 ROS 消息msg = JointState()msg.header.stamp = rospy.Time.now()msg.name = joint_namesmsg.position = positionsjoint_pub.publish(msg)rospy.loginfo(msg)# 同步GUI渲染viewer.sync()rate.sleep()
写好 python 脚本之后记得修改可执行权限，否则运行时会报错：
(mujoco) $ chmod +x src/demo/scripts/robot.py
2.3 改写 CMakeLists.txt 文件
修改这个 src/demo/CMakeLists.txt 文件，添加以下内容：
catkin_install_python(PROGRAMSscripts/robot.pyDESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)install(DIRECTORY mjcf/DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/mjcf
)
2.4 编译工作空间
因为这里使用了conda虚拟环境，所以在编译的时候需要明确python解释器。
首先查看自己 mujoco 环境解释器位置：
(mujoco) $ conda info --envs# conda environments:
#
base /home/gaohao/miniconda3
mujoco * /home/gaohao/miniconda3/envs/mujoco
查看路径的 bin 文件夹中是否有python3解释器（通常情况下肯定有），如果下面的命令没有输出你需要自己找到对应的python3解释器位置并记下来，后面编译时要用：
(mujoco) $ ls /home/gaohao/miniconda3/envs/mujoco/bin/ | grep pythonpython
python3
python3.1
python3.10
python3.10-config
python3-config
编译工作空间 mujoco_ws：
(mujoco) $ cd mujoco_ws
(mujoco) $ catkin_make -DPYTHON_EXECUTABLE=/home/gaohao/miniconda3/envs/mujoco/bin/python3
2.5 运行示例
在运行前确保已经有一个 roscore 正在另一个终端中运行：
(base) $ roscore
然后运行ros节点：
(mujoco) $ source devel/setup.bash
(mujoco) $ rosrun demo robot.py
可以在弹出的仿真GUI中拖动右侧 Control 面板的滑块就可以看到终端的 position 数组字段发生变化。
3.与 ROS2 交互
这里也提供了一个 ROS2 的交互示例，后面关于 mujoco 与 ROS 框架的交互我会尽量提供 ROS1 与 ROS2 两个方案，但会以 ROS2 为主。
【Note】：因为 ROS2 在编译的时候需要用安装的conda环境进行编译，我这里为了演示和上面ROS1 写了相同的环境名，实际上我用的是自己的 ros2 环境，如果你的OS也是同时安装了ROS1和ROS2，那么留意conad环境切换。
3.1 初始化工作空间
使用下面的命令创建并初始化工作空间：
(mujoco) $ mkdir -p mujoco_ws/src
(mujoco) $ cd mujoco_ws/src(mujoco) $ ros2 pkg create --build-type ament_python demo
编译一下工作空间：
(mujoco) $ cd mujoco_ws
(mujoco) $ colcon build
创建两个文件用来保存脚本和xml内容：
(mujoco) $ touch src/demo/demo/robot.py
(mujoco) $ mkdir src/demo/mjcf
(mujoco) $ touch src/demo/mjcf/robot.xml
3.2 编写 python 脚本
在 src/demo/demo/robot.py 文件中添加下面代码：
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointStateimport mujoco
import mujoco.viewerclass MujocoJointPublisher(Node):def __init__(self):super().__init__('mujoco_joint_publisher')self.publisher_ = self.create_publisher(JointState, 'joint_states', 10)# 模型路径xml_path = "/home/gaohao/Desktop/mujoco_ws/src/demo/mjcf/robot.xml"# xml_path = os.path.join(os.path.dirname(__file__), 'mjcf', 'robot.xml')self.model = mujoco.MjModel.from_xml_path(xml_path)self.data = mujoco.MjData(self.model)self.joint_names = ['slide_joint']self.joint_indices = [self.model.joint(name).qposadr for name in self.joint_names]self.timer = self.create_timer(0.01, self.timer_callback) # 100Hz# 启动 mujoco viewerself.viewer = mujoco.viewer.launch_passive(model=self.model, data=self.data)def timer_callback(self):mujoco.mj_step(self.model, self.data)msg = JointState()msg.header.stamp = self.get_clock().now().to_msg()msg.name = self.joint_namesmsg.position = [float(self.data.qpos[i]) for i in self.joint_indices]msg.velocity = [float(self.data.qvel[i]) for i in self.joint_indices]msg.effort = [0.0] * len(self.joint_indices)self.publisher_.publish(msg)self.get_logger().info(f"Position: '{msg.position}'")self.viewer.sync()def main(args=None):rclpy.init(args=args)node = MujocoJointPublisher()rclpy.spin(node)node.destroy_node()rclpy.shutdown()if __name__ == '__main__':main()
然后将第1章节的 xml 文件内容复制到 src/demo/mjcf/robot.xml 中，此时你的工作 mujoco_ws/src 空间文件结构如下：
(mujoco) $ tree -L 3
...
└── demo├── demo│ ├── __init__.py│ └── robot.py # python脚本├── mjcf│ └── robot.xml # 机器人描述文件├── package.xml├── resource│ └── demo├── setup.cfg├── setup.py└── test├── test_copyright.py├── test_flake8.py└── test_pep257.py
3.3 改写 setup.py 文件
修改 src/demo/setup.py 文件，在 console_scripts 中添加以下内容：
"robot = " + package_name + ".robot:main"
3.4 编译工作空间
(mujoco) $ colon build
3.5 运行示例
(mujoco) $ source install/setup.bash
(mujoco) $ ros2 run demo robot
可以在弹出的仿真GUI中拖动右侧 Control 面板的滑块就可以看到终端的 position 数组字段发生变化。
后面的有关ROS的示例就不会写的这么详尽了，我将默认认为你已经很熟悉ROS1和ROS2框架，至少在编译之前遇到的问题能够自己解决。