在DRAKE中通过Weldjoint实现力/扭矩传感器检测碰撞力的技术求助

1. 用专用ForceTorqueSensor替代WeldJoint反作用力端口 #

Drake内置的ForceTorqueSensor是专门用于测量刚体间作用力与扭矩的组件，比WeldJoint的反作用力端口更适配你的碰撞检测场景。不要直接将圆柱体焊死在固定基座上，而是在固定基座（比如世界坐标系body）和圆柱体之间加装该传感器，它能直接输出两者之间的所有相互作用力与扭矩，包含碰撞产生的外力部分。

2. 排查WeldJoint反作用力失效的核心问题 #

如果坚持使用WeldJoint的get_reaction_forces_output_port()，需确认三个关键环节：

• WeldJoint必须连接固定基座（world body）与圆柱体，且圆柱体的所有运动自由度被完全约束。
• 碰撞系统需正常启用：场景中必须添加SceneGraph，同时圆柱体和碰撞物体都要注册CollisionGeometry，还要确认MultibodyPlant的碰撞检测开关处于开启状态（默认开启，但需检查set_discrete_contact_approximation或set_contact_solver的参数设置）。
• 反作用力输出是基于关节帧的数值，需要转换到目标帧（比如圆柱体质心帧）才能正确解读碰撞力数据。

3. 代码调整关键步骤 #

针对你的现有代码，可按以下方式修改：

• 移除直接焊接圆柱体的WeldJoint，替换为添加ForceTorqueSensor：

  const auto& world = plant->world_body();
  ForceTorqueSensor<double> cylinder_ft_sensor(
      "cylinder_ft_sensor",
      world.body_frame(),
      cylinder_body.body_frame(),
      Eigen::Isometry3d::Identity());
  plant->AddSensor(&cylinder_ft_sensor);
  

• 读取传感器输出数据：

  const auto& ft_output_port = cylinder_ft_sensor.get_output_port();
  // 在仿真循环内读取实时数据
  const auto ft_data = ft_output_port.Eval(*context);
  // 通过ft_data.force()获取作用力，ft_data.torque()获取扭矩
  

• 确保碰撞几何体正确注册：

  // 为圆柱体添加碰撞几何
  plant->RegisterCollisionGeometry(
      cylinder_body,
      Eigen::Isometry3d::Identity(),
      Cylinder(radius, height),
      "cylinder_collision",
      CoulombFriction<double>(0.5, 0.5));
  // 碰撞物体也需注册对应的CollisionGeometry
  

4. 仿真验证小技巧 #

• 先关闭重力，测试碰撞发生时传感器是否有输出，确认碰撞力检测正常后，再开启重力验证叠加效果。
• 给碰撞物体设置简单的运动轨迹，让其主动撞击圆柱体，便于直观观察传感器数据变化。

内容的提问来源于stack exchange，提问作者Lin YANG