视频讲解：
ROS2下MoveIt+Rviz+MuJoCo 三剑合璧！Panda 机械臂联动仿真！
仓库代码：GitHub - LitchiCheng/ros2_package
今天介绍下，ros2下使用moveit在Rviz和mujoco联合仿真，结合上一期视频《MuJoCo 仿真 Panda 机械臂关节空间运动｜含完整代码》
首先创建一个ros2的package
ros2 pkg create --build-type ament_python joint_state_pkg --dependencies rclpy sensor_msgs在src中创建joint_state_subscriber.py
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
import mujoco
import numpy as np
import glfwclass JointStateSubscriber(Node):def __init__(self):super().__init__("joint_state_subscriber")self.subscription = self.create_subscription(JointState,"/joint_states",self.callback,10)# 加载模型self.model = mujoco.MjModel.from_xml_path('/home/dar/MuJoCoBin/mujoco_menagerie/franka_emika_panda/scene.xml')self.data = mujoco.MjData(self.model)# 打印所有 body 的 ID 和名称print("All bodies in the model:")for i in range(self.model.nbody):body_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_BODY, i)print(f"ID: {i}, Name: {body_name}")# 初始化 GLFWif not glfw.init():returnself.window = glfw.create_window(1200, 900, 'Panda Arm Control', None, None)if not self.window:glfw.terminate()returnglfw.make_context_current(self.window)# 设置鼠标滚轮回调函数glfw.set_scroll_callback(self.window, self.scroll_callback)# 初始化渲染器self.cam = mujoco.MjvCamera()self.opt = mujoco.MjvOption()mujoco.mjv_defaultCamera(self.cam)mujoco.mjv_defaultOption(self.opt)self.pert = mujoco.MjvPerturb()self.con = mujoco.MjrContext(self.model, mujoco.mjtFontScale.mjFONTSCALE_150.value)self.scene = mujoco.MjvScene(self.model, maxgeom=10000)# 找到末端执行器的 body idself.end_effector_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, 'hand')print(f"End effector ID: {self.end_effector_id}")if self.end_effector_id == -1:print("Warning: Could not find the end effector with the given name.")glfw.terminate()return# 初始关节角度self.initial_q = self.data.qpos[:7].copy()print(f"Initial joint positions: {self.initial_q}")self.timer1 = self.create_timer(0.01, # 100ms周期self.timer_callback1)def scroll_callback(self, window, xoffset, yoffset):# 调整相机的缩放比例self.cam.distance *= 1 - 0.1 * yoffsetdef limit_angle(self, angle):while angle > np.pi:angle -= 2 * np.piwhile angle < -np.pi:angle += 2 * np.pireturn angle def timer_callback1(self):if not glfw.window_should_close(self.window):# 获取当前末端执行器位置mujoco.mj_forward(self.model, self.data)self.end_effector_pos = self.data.body(self.end_effector_id).xposself.initial_q[0] = self.initial_q[0] + 0.1self.initial_q[0] = self.limit_angle(self.initial_q[0])new_q = self.initial_q# 设置关节目标位置self.data.qpos[:7] = self.positions# 模拟一步mujoco.mj_step(self.model, self.data)# 更新渲染场景viewport = mujoco.MjrRect(0, 0, 1200, 900)mujoco.mjv_updateScene(self.model, self.data, self.opt, self.pert, self.cam, mujoco.mjtCatBit.mjCAT_ALL.value, self.scene)mujoco.mjr_render(viewport, self.scene, self.con)# 交换前后缓冲区glfw.swap_buffers(self.window)glfw.poll_events()def callback(self, msg: JointState):# 过滤Panda机械臂的7个关节（名称以panda_joint开头）panda_joints = [name for name in msg.name if "panda_joint" in name]self.positions = [msg.position[i] for i, name in enumerate(msg.name) if "panda_joint" in name]for name, pos in zip(panda_joints, self.positions):self.get_logger().info(f"{name}: {pos:.4f}")def main(args=None):rclpy.init(args=args)node = JointStateSubscriber()rclpy.spin(node)rclpy.shutdown()if __name__ == "__main__":main()# 清理资源glfw.terminate()修改setup.py
entry_points={'console_scripts': ['joint_state_subscriber = joint_state_pkg.joint_state_subscriber:main',],
},先运行panda rviz场景
ros2 launch moveit2_tutorials demo.launch.py编译运行
colcon build --packages-select joint_state_subscriber
source install/setup.bash
ros2 run joint_state_pkg joint_state_subscriber拖动Rviz末端的控制，点击planning中的plan和excute
可以看到mujoco中机械臂跟着一起动啦！
