当前位置：首页 > news >正文

（7）ROS2-MUJOCO联合仿真环境迁移优化

news 2025/7/22 12:36:03

前言：

之前在这篇帖子（5）ROS：MUJOCO仿真环境迁移教程_mujoco文件用gezabo打开的方法和注意事项-CSDN博客中介绍过如何将ros2的仿真环境由gazebo迁移到mujoco，但是当时使用的ros官方源码的迁移程序中，对mujoco的渲染是使用的自定义的mujoco_rendering.cpp，相当于实现了极简的纯仿真环境，但是没有mujoco原版那样默认丰富的侧边可交互UI，本篇介绍如何使用mujoco官方定义的simulate仿真UI替换原来的mujoco_rendering.cpp。

正文：

1. 找到mujoco的源码路径，找到simulate文件夹

2. 把simulate文件夹完整copy到/your_workspace/src/mujoco_ros2_control/mujoco_ros2_control下，目录结构如图

3. 下载lodepng依赖，是simulate包编译需要的一个依赖程序，主要用于处理png，mujoco官方包里没有这个https://github.com/lvandeve/lodepng/

git下来之后解压找到lodepng.cpp和lodepng.h，单独把这两个复制到simulate文件夹下就可以，simulate完整版长这样

4. 配置主循环，打开main.cc，因为现在是希望用这个main.cc代替原来的主循环mujoco_ros2_control_node.cpp，也就是它除了要实现官方原定义的UI渲染，还要实现ros2 controller manager的加载和仿真步进，因此做三处修改：

第一步，头部导入mujoco_ros2_control.hpp，并创建将来用于加载控制器的静态指针

#if defined(mjUSEUSD)
#include <mujoco/experimental/usd/usd.h>
#endif
#include <mujoco/mujoco.h>
#include "glfw_adapter.h"
#include "simulate.h"
#include "array_safety.h"// 增加如下两行，用于 PhysicsLoop 访问 mujoco_control
#include "mujoco_ros2_control/mujoco_ros2_control.hpp"
static mujoco_ros2_control::MujocoRos2Control* g_mujoco_control = nullptr;

第二步，修改void PhysicsLoop函数，确保每次步进我们的控制器也被更新，完整的函数程序如下，主要就是加了两行g_mujoco_control->update和executor.spin_some(); 保证ros2、mujoco仿真环境、mujoco仿真UI的时钟保持一致

void PhysicsLoop(mj::Simulate& sim, rclcpp::executors::MultiThreadedExecutor& executor) {// cpu-sim syncronization pointstd::chrono::time_point<mj::Simulate::Clock> syncCPU;mjtNum syncSim = 0;// run until asked to exitwhile (!sim.exitrequest.load()) {if (sim.droploadrequest.load()) {sim.LoadMessage(sim.dropfilename);mjModel* mnew = LoadModel(sim.dropfilename, sim);sim.droploadrequest.store(false);mjData* dnew = nullptr;if (mnew) dnew = mj_makeData(mnew);if (dnew) {sim.Load(mnew, dnew, sim.dropfilename);// lock the sim mutexconst std::unique_lock<std::recursive_mutex> lock(sim.mtx);mj_deleteData(d);mj_deleteModel(m);m = mnew;d = dnew;mj_forward(m, d);} else {sim.LoadMessageClear();}}if (sim.uiloadrequest.load()) {sim.uiloadrequest.fetch_sub(1);sim.LoadMessage(sim.filename);mjModel* mnew = LoadModel(sim.filename, sim);mjData* dnew = nullptr;if (mnew) dnew = mj_makeData(mnew);if (dnew) {sim.Load(mnew, dnew, sim.filename);// lock the sim mutexconst std::unique_lock<std::recursive_mutex> lock(sim.mtx);mj_deleteData(d);mj_deleteModel(m);m = mnew;d = dnew;mj_forward(m, d);} else {sim.LoadMessageClear();}}// sleep for 1 ms or yield, to let main thread run//  yield results in busy wait - which has better timing but kills battery lifeif (sim.run && sim.busywait) {std::this_thread::yield();} else {std::this_thread::sleep_for(std::chrono::milliseconds(1));}// lock the sim mutex for the entire physics/control/ros2 updateconst std::unique_lock<std::recursive_mutex> lock(sim.mtx);// run only if model is presentif (m) {// runningif (sim.run) {bool stepped = false;// record cpu time at start of iterationconst auto startCPU = mj::Simulate::Clock::now();// elapsed CPU and simulation time since last syncconst auto elapsedCPU = startCPU - syncCPU;double elapsedSim = d->time - syncSim;// requested slow-down factordouble slowdown = 100 / sim.percentRealTime[sim.real_time_index];// misalignment condition: distance from target sim time is bigger than syncMisalignbool misaligned =std::abs(Seconds(elapsedCPU).count()/slowdown - elapsedSim) > syncMisalign;// out-of-sync (for any reason): reset sync times, stepif (elapsedSim < 0 || elapsedCPU.count() < 0 || syncCPU.time_since_epoch().count() == 0 ||misaligned || sim.speed_changed) {// re-syncsyncCPU = startCPU;syncSim = d->time;sim.speed_changed = false;// run single step, let next iteration deal with timingmj_step(m, d);// --- 保证控制器 update 被周期性调用 ---if (g_mujoco_control) {g_mujoco_control->update();}// ROS2 spin_some 也在锁内if (rclcpp::ok()) {executor.spin_some();}const char* message = Diverged(m->opt.disableflags, d);if (message) {sim.run = 0;mju::strcpy_arr(sim.load_error, message);} else {stepped = true;}}// in-sync: step until ahead of cpuelse {bool measured = false;mjtNum prevSim = d->time;double refreshTime = simRefreshFraction/sim.refresh_rate;// step while sim lags behind cpu and within refreshTimewhile (Seconds((d->time - syncSim)*slowdown) < mj::Simulate::Clock::now() - syncCPU &&mj::Simulate::Clock::now() - startCPU < Seconds(refreshTime)) {// measure slowdown before first stepif (!measured && elapsedSim) {sim.measured_slowdown =std::chrono::duration<double>(elapsedCPU).count() / elapsedSim;measured = true;}// inject noisesim.InjectNoise();// call mj_stepmj_step(m, d);// --- 保证控制器 update 被周期性调用 ---if (g_mujoco_control) {g_mujoco_control->update();}// ROS2 spin_some 也在锁内if (rclcpp::ok()) {executor.spin_some();}const char* message = Diverged(m->opt.disableflags, d);if (message) {sim.run = 0;mju::strcpy_arr(sim.load_error, message);} else {stepped = true;}// break if resetif (d->time < prevSim) {break;}}}// save current state to history bufferif (stepped) {sim.AddToHistory();}}// pausedelse {// run mj_forward, to update rendering and joint slidersmj_forward(m, d);sim.speed_changed = true;}}}
}

第三步，修改main函数启动循环及ros2节点，创建node节点来启动controller_manager，分离ros2控制线程和UI渲染主线程，可直接复制

int main(int argc, char** argv) {// display an error if running on macOS under Rosetta 2
#if defined(__APPLE__) && defined(__AVX__)if (rosetta_error_msg) {DisplayErrorDialogBox("Rosetta 2 is not supported", rosetta_error_msg);std::exit(1);}
#endif// print version, check compatibilitystd::printf("MuJoCo version %s\n", mj_versionString());if (mjVERSION_HEADER!=mj_version()) {mju_error("Headers and library have different versions");}// scan for libraries in the plugin directory to load additional pluginsscanPluginLibraries();#if defined(mjUSEUSD)// If USD is used, print the version.std::printf("OpenUSD version v%d.%02d\n", PXR_MINOR_VERSION, PXR_PATCH_VERSION);
#endifmjvCamera cam;mjv_defaultCamera(&cam);mjvOption opt;mjv_defaultOption(&opt);mjvPerturb pert;mjv_defaultPerturb(&pert);// simulate object encapsulates the UIauto sim = std::make_unique<mj::Simulate>(std::make_unique<mj::GlfwAdapter>(),&cam, &opt, &pert, /* is_passive = */ false);// ROS2 初始化rclcpp::init(argc, argv);auto ros_node = rclcpp::Node::make_shared("mujoco_ros2_control_node",rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));// 读取 launch 传入参数std::string mujoco_model_path;if (ros_node->has_parameter("mujoco_model")) {mujoco_model_path = ros_node->get_parameter("mujoco_model").as_string();}if (mujoco_model_path.empty()) {for (int i = 1; i < argc; ++i) {std::string arg = argv[i];if (arg.size() > 4 && (arg.substr(arg.size()-4) == ".xml" || arg.substr(arg.size()-4) == ".mjb")) {mujoco_model_path = arg;break;}}}if (mujoco_model_path.empty()) {std::cerr << "ERROR: No MuJoCo model path found! Please set 'mujoco_model' ROS2 parameter, or pass .xml/.mjb file as argument." << std::endl;return 1;}const char* filename = mujoco_model_path.c_str();// 1. 先定义 executor，后面传递给物理线程rclcpp::executors::MultiThreadedExecutor executor;// 2. 启动物理线程加载模型和数据和推进仿真std::thread physicsthreadhandle([&]() {PhysicsThread(sim.get(), filename, executor);});// 2. 主线程等待 d 就绪std::cout << "[MuJoCo] 主线程: 等待物理线程加载模型和数据..." << std::endl;while (!d && rclcpp::ok() && !sim->exitrequest.load()) {std::this_thread::sleep_for(std::chrono::milliseconds(100));}if (!d) {std::cerr << "[MuJoCo] 主线程: 等待 d 超时或被中断，退出" << std::endl;if (physicsthreadhandle.joinable()) physicsthreadhandle.join();return 1;}std::cout << "[MuJoCo] 主线程: m/d 已就绪，初始化控制器和 ROS2 executor" << std::endl;// 3. m/d 就绪后再初始化控制器和 executormujoco_ros2_control::MujocoRos2Control mujoco_control(ros_node, m, d);g_mujoco_control = &mujoco_control;mujoco_control.init();executor.add_node(ros_node);// 4. 主线程只负责 UI 渲染std::cout << "[MuJoCo] 主线程: 进入 sim->RenderLoop() (UI 事件循环)" << std::endl;sim->RenderLoop();physicsthreadhandle.join();mj_deleteData(d);mj_deleteModel(m);return 0;
}

5. 打开Cmakelist.txt，设置新主程序的依赖，这里叫它simulate_main

file(GLOB SIMULATE_SRC"${CMAKE_CURRENT_SOURCE_DIR}/simulate/*.cc""${CMAKE_CURRENT_SOURCE_DIR}/simulate/lodepng.cpp"
)add_executable(simulate_main${SIMULATE_SRC}src/mujoco_ros2_control.cppsrc/mujoco_rendering.cppsrc/mujoco_cameras.cpp
)
ament_target_dependencies(simulate_main ${THIS_PACKAGE_DEPENDS})
target_link_libraries(simulate_main${MUJOCO_LIB}glfw
)
target_include_directories(simulate_mainPUBLIC${CMAKE_CURRENT_SOURCE_DIR}/simulate$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>$<INSTALL_INTERFACE:include>${MUJOCO_INCLUDE_DIR}${EIGEN3_INCLUDE_DIR}${CMAKE_CURRENT_SOURCE_DIR}/src${CMAKE_CURRENT_SOURCE_DIR}/simulate
)
install(TARGETSsimulate_mainDESTINATION lib/${PROJECT_NAME})

6. 最后，launch的时候的启动改成用simulate_main就好了

node_mujoco_ros2_control = Node(package='mujoco_ros2_control',executable='simulate_main',output='screen',parameters=[robot_description,controller_config_file,{'mujoco_model_path': os.path.join(mujoco_ros2_control_demos_path, 'mujoco_xml', 'my_mujoco_arm.xml')},])

最后你的仿真环境就能达到这种原生态效果：