如何在Drake中实现仿真可视化？求C++可用测试示例

一、项目结构 #

drake_visualization_template/
├── CMakeLists.txt
└── src/
    └── main.cpp

二、CMakeLists.txt 配置 #

适配apt预编译包与源码编译两种Drake安装方式，预留ROS1整合接口：

cmake_minimum_required(VERSION 3.16)
project(drake_visualization_template)

# 查找Drake（自动适配apt/源码安装路径）
find_package(Drake REQUIRED)

# 生成可执行文件
add_executable(visualization_demo src/main.cpp)

# 链接Drake核心库
target_link_libraries(visualization_demo PRIVATE Drake::drake)

# 指定C++标准（Drake要求C++17）
target_compile_features(visualization_demo PRIVATE cxx_std_17)

# -------------------------- ROS1 整合预留配置 --------------------------
# find_package(catkin REQUIRED COMPONENTS roscpp std_msgs)
# include_directories(${catkin_INCLUDE_DIRS})
# target_link_libraries(visualization_demo PRIVATE ${catkin_LIBRARIES})

三、C++ 核心仿真代码 #

支持drake-visualizer/meldis/meshcat三种可视化方式，可通过命令行参数切换，内置简单立方体刚体仿真：

#include <memory>
#include <string>
#include <iostream>

#include "drake/geometry/scene_graph.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/geometry/drake_visualizer.h"
#include "drake/geometry/meshcat_visualizer.h"
#include "drake/geometry/meldis_visualizer.h"

using namespace drake;
using namespace drake::geometry;
using namespace drake::multibody;
using namespace drake::systems;

int main(int argc, char* argv[]) {
    // 可视化方式选择：默认drake-visualizer，可通过参数指定为meldis/meshcat
    std::string viz_type = "drake_visualizer";
    if (argc > 1) viz_type = argv[1];
    std::cout << "Visualization mode: " << viz_type << "\n";

    // 1. 构建仿真图
    DiagramBuilder<double> builder;
    auto scene_graph = builder.AddSystem<SceneGraph>();
    scene_graph->set_name("scene_graph");

    // 2. 创建多体仿真植物
    const double time_step = 0.001;
    auto plant = builder.AddSystem<MultibodyPlant>(time_step);
    plant->set_name("plant");
    plant->RegisterAsSourceForSceneGraph(scene_graph);

    // 3. 添加立方体刚体
    const double cube_size = 0.5;
    const double cube_mass = 1.0;
    Body<double>& cube = plant->AddRigidBody(
        "cube",
        SpatialInertia<double>::SolidBox(cube_mass, cube_size, cube_size, cube_size));
    // 设置初始悬浮位置
    plant->SetDefaultFreeBodyPose(&cube, Isometry3<double>::Translation(Vector3<double>(0, 0, 1.0)));

    plant->Finalize();

    // 4. 连接植物与SceneGraph的几何数据流
    builder.Connect(
        plant->get_geometry_poses_output_port(),
        scene_graph->get_source_pose_port(plant->get_source_id()));
    builder.Connect(
        scene_graph->get_query_output_port(),
        plant->get_geometry_query_input_port());

    // 5. 绑定可视化工具
    if (viz_type == "drake_visualizer") {
        auto viz = builder.AddSystem<DrakeVisualizer>();
        builder.Connect(scene_graph->get_pose_bundle_output_port(), viz->get_input_port(0));
    } else if (viz_type == "meldis") {
        auto viz = builder.AddSystem<MeldisVisualizer>();
        builder.Connect(scene_graph->get_pose_bundle_output_port(), viz->get_input_port(0));
    } else if (viz_type == "meshcat") {
        auto viz = builder.AddSystem<MeshcatVisualizer<double>>();
        builder.Connect(scene_graph->get_pose_bundle_output_port(), viz->get_input_port(0));
        std::cout << "Meshcat access URL: " << viz->web_url() << "\n";
    } else {
        std::cerr << "Unsupported visualization type: " << viz_type << "\n";
        return 1;
    }

    // 6. 启动仿真
    auto diagram = builder.Build();
    Simulator<double> simulator(*diagram);
    simulator.set_publish_every_time_step(true);
    std::cout << "Starting 10-second simulation...\n";
    simulator.AdvanceTo(10.0);

    return 0;
}

四、编译与运行步骤 #

编译 #

1. 创建编译目录并进入：

mkdir build && cd build

2. CMake配置：

• apt安装Drake：

cmake ..

• 源码编译Drake（假设源码在~/drake）：

cmake .. -DCMAKE_PREFIX_PATH=~/drake/build/install

3. 编译：

make -j$(nproc)

运行 #

方式1：drake-visualizer

1. 新开终端启动可视化工具：

drake-visualizer

2. 运行仿真：

./visualization_demo drake_visualizer

方式2：meldis

1. 新开终端启动工具：

meldis

2. 运行仿真：

./visualization_demo meldis

方式3：meshcat

直接运行程序，按提示在浏览器打开输出的URL：

./visualization_demo meshcat

五、空窗口问题排查 #

1. 确保可视化工具先于仿真程序启动，否则无法接收初始几何数据；
2. 验证Drake安装完整性：单独运行drake-visualizer/meldis，确认能正常打开空窗口；
3. 检查OpenGL版本：运行glxinfo | grep "OpenGL version"，需3.3及以上；
4. 确认图形驱动正常：Ubuntu 20.04需确保NVIDIA/Mesa驱动安装无误。

六、ROS1整合提示 #

取消CMakeLists.txt中预留的ROS1配置注释，在代码中添加ROS节点初始化、话题收发逻辑即可实现与ROS1的交互，例如将刚体位姿发布为geometry_msgs/PoseStamped话题，或订阅指令控制刚体运动。

内容的提问来源于stack exchange，提问作者MIKE PAPADAKIS