Drake碰撞仅对低重量连杆生效的原因及锚定方案咨询

一、碰撞失效的核心原因 #

• 接触刚度与质量不匹配：当ground_mass2质量增大后，当前设置的10000接触刚度不足以提供足够的接触力抵消大质量带来的惯性力。接触力公式为F=k*x（k为刚度，x为穿透量），质量越大，需要的刚度越高才能限制穿透，否则穿透量会超过仿真允许阈值，导致碰撞约束失效。
• 碰撞体尺寸过小：ground_mass2的碰撞球半径仅为0.01，远小于视觉模型的0.05。大质量物体运动时，时间步长内的位移可能超过小碰撞体尺寸，导致碰撞检测滞后，无法及时触发接触约束。
• 关节无有效限位：prismatic关节仅允许沿Z轴自由移动，未设置位置限位或足够的弹簧阻尼，大质量物体受重力作用向下运动时，接触力不足以抗衡重力，直接穿透地面。

二、更优的地面锚定方案 #

1. 优化接触与碰撞参数 #

调整接触刚度和碰撞体尺寸，匹配大质量需求：

<!-- 修改ground_mass2的碰撞属性 -->
<collision name="collision_groundmass2">
  <origin xyz="0 0 -1" rpy="0 0 0"/>
  <geometry>
    <sphere radius=".05"/> <!-- 与视觉模型尺寸一致 -->
  </geometry>
  <drake:proximity_properties>
     <drake:hunt_crossley_dissipation value="5"/> <!-- 增大耗散减少反弹 -->
     <drake:point_contact_stiffness value="1000000"/> <!-- 刚度提升100倍 -->
  </drake:proximity_properties>
</collision>

2. 带弹簧阻尼与限位的棱柱关节 #

给prismatic关节添加弹簧阻尼和位置限位，直接约束ground_mass2在地面附近：

<joint name="ground_mass2" type="prismatic">
  <parent link="swing_leg"/>
  <child link="ground_mass2"/>
  <axis xyz="0 0 -1"/>
  <limit lower="-0.01" upper="0.01" effort="1000" velocity="1"/> <!-- 限制移动范围 -->
  <dynamics damping="100" spring="100000"/> <!-- 高刚度弹簧拉回地面位置 -->
</joint>

3. 连续时间距离约束 #

使用Drake的PointToPlaneDistanceConstraint添加连续时间约束，强制ground_mass2与地面保持最小距离：

// 获取地面和ground_mass2的Body对象
const auto& ground_body = plant->GetBodyByName("ground");
const auto& mass_body = plant->GetBodyByName("ground_mass2");

// 添加点到平面的距离约束，确保mass_body的指定点不穿透地面
plant->AddConstraint(
    MakePointToPlaneDistanceConstraint(
        *plant,
        mass_body.body_frame(), Eigen::Vector3d(0, 0, -1), // mass上的约束点
        ground_body.body_frame(), Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 1), // 地面平面（原点+法向量）
        0.0, 1.0)); // 距离下限0，上限可按需调整

4. 动态焊接关节（临时锚定） #

如果需要临时将腿部锚定到地面，可以使用weld关节，通过Context动态控制关节的锁定状态：

// 创建weld关节（在URDF中预先定义或代码中添加）
const auto weld_joint = plant->AddJoint<WeldJoint>(
    "leg_ground_weld",
    plant->GetBodyByName("ground"), {},
    plant->GetBodyByName("swing_leg"), {},
    Isometry3d::Identity());

// 在仿真中需要锚定时锁定关节
context->get_mutable_joint_actuator_port(weld_joint->actuator_index())->FixValue(context, 1.0);
// 需要解除锚定时解锁
context->get_mutable_joint_actuator_port(weld_joint->actuator_index())->FixValue(context, 0.0);

内容的提问来源于stack exchange，提问作者Jasmine Terrones