如何在Drake C++中获取水弹性接触力及接触力起点？

问题描述 #

我有一个基于Drake C++的Robotiq-140夹爪抓取不规则网格的仿真程序（代码基于Issue 19842提及的robotiq.cc），仿真已生成带两个红色平移接触力的可视化结果，现在需要获取这些红色接触力的矢量及其起点。

解决方案 #

Drake的MultibodyPlant提供了直接获取接触结果的接口，通过订阅对应输出端口或在仿真过程中查询离散接触对，即可拿到接触力的矢量、方向及作用点信息，以下是具体实现方式：

1. 核心逻辑 #

针对你代码中使用的离散接触近似（kLagged），需调用MultibodyPlant的get_discrete_contact_results_output_port()接口获取接触数据，从中提取每个接触对的力矢量和作用点。

2. 实时查询实现（修改仿真循环） #

直接在仿真过程中添加循环，每步步进后查询并输出接触信息：

// 修改原do_main函数中的仿真部分
systems::Simulator simulator(*diagram);
auto context = simulator.get_mutable_context();
auto plant_context = &diagram->GetMutableSubsystemContext(plant, context);

// 替换原simulator.AdvanceTo(20.0)为循环查询
while (simulator.get_context().get_time() < 20.0) {
    simulator.AdvanceTo(simulator.get_context().get_time() + 0.1);
    
    // 获取离散接触结果
    const auto& contact_results = plant.get_discrete_contact_results(*plant_context);
    
    // 遍历所有接触对
    for (const auto& pair : contact_results.contact_pairs()) {
        // 世界坐标系下的接触力矢量
        const Vector3d& force_vec = pair.contact_force().body_world_forces().force();
        // 世界坐标系下的接触力作用点（即红色矢量起点）
        const Vector3d& force_point = pair.contact_force().body_world_forces().application_point();
        
        // 输出结果
        fmt::print("接触力矢量: [{}, {}, {}], 作用点: [{}, {}, {}]\n",
                   force_vec.x(), force_vec.y(), force_vec.z(),
                   force_point.x(), force_point.y(), force_point.z());
    }
}

3. 自定义系统订阅端口（更模块化） #

创建一个专用日志系统订阅接触结果端口，适合复杂场景：

// 自定义接触力日志系统
class ContactForceLogger : public systems::LeafSystem<double> {
public:
    ContactForceLogger() {
        // 声明输入端口，接收离散接触结果
        DeclareInputPort("contact_results", systems::Value<multibody::DiscreteContactResults<double>>());
        // 每0.1秒触发一次日志输出
        DeclarePeriodicDiscreteUpdateEvent(0.1, 0, &ContactForceLogger::LogForces);
    }

private:
    void LogForces(const systems::Context<double>& context,
                   systems::DiscreteValues<double>* state) const {
        const auto& contact_results = get_input_port().Eval<multibody::DiscreteContactResults<double>>(context);
        
        for (const auto& pair : contact_results.contact_pairs()) {
            const Vector3d& force = pair.contact_force().body_world_forces().force();
            const Vector3d& point = pair.contact_force().body_world_forces().application_point();
            
            fmt::print("接触力: ({:.3f}, {:.3f}, {:.3f})，作用点: ({:.3f}, {:.3f}, {:.3f})\n",
                       force.x(), force.y(), force.z(),
                       point.x(), point.y(), point.z());
        }
    }
};

在Diagram构建阶段添加并连接该系统：

// 在builder.Build()之前添加以下代码
auto force_logger = builder.AddSystem<ContactForceLogger>();
builder.Connect(plant.get_discrete_contact_results_output_port(), force_logger->get_input_port());

注意事项 #

• 接触近似类型需匹配：如果使用连续接触，需替换为get_continuous_contact_results_output_port()接口。
• 力的方向：Drake中输出的接触力是作用在第一个碰撞物体上的力，第二个物体受到的力为其反方向。
• 作用点坐标：输出的作用点是世界坐标系下的位置，与Meshcat中显示的红色力矢量起点完全对应。

内容的提问来源于stack exchange，提问作者Daniel He