如何在Drake仿真中让低刚度连杆以指定压缩量启动？

我尝试将连杆建模为弹簧，已在URDF中添加drake:point_contact_stiffness参数。现在有两个需求：

• 能否让仿真以连杆处于任意挠度的状态启动？
• 能否让低刚度系数的连杆以特定压缩量启动仿真？

我曾尝试在腿部连杆与髋部质量块之间添加棱柱关节，通过“下压”髋部质量块让连杆初始轻微压缩，但仿真效果差，腿部连杆与髋部质量块会相互飞散。

以下是地面和腿部连杆的碰撞动力学URDF示例：

<visual>
  <origin xyz="0 0 -.25" rpy="0 0 0"/>
  <geometry>
    <box size="10 1 .5" />
  </geometry>
  <material name="green">
    <color rgba="0 1 0 1" />
  </material>
</visual>

<collision name="collision_ground">
  <origin xyz="0 0 -.25" rpy="0 0 0"/>
  <geometry>
    <box size="10 1 .5" />
  </geometry>
  <drake:proximity_properties>
     <drake:hunt_crossley_dissipation value="0.1"/>
     <drake:point_contact_stiffness value="10000"/>
    </drake:proximity_properties>
</collision> 

仿真代码示例（期望腿部连杆初始带压缩量）：

builder = DiagramBuilder()
compass_walker, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
Parser(compass_walker).AddModelFromFile(file_name="compass.urdf")

compass_walker.Finalize()
compass_walker.set_name("cw")

meshcat.Delete()
visualizer = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
visualizer.set_name("visualizer")
logger = LogVectorOutput(compass_walker.get_state_output_port(), builder)


# finish building the block diagram
diagram = builder.Build()
simulator = Simulator(diagram)


context = simulator.get_mutable_context()
plant_context = compass_walker.GetMyMutableContextFromRoot(context)

context.SetContinuousState([0.0, 0.0, 0.156, -0.312, 0.0, 0.0, -0.3975, -0.3065]) #8 states


visualizer.StartRecording()
simulator.AdvanceTo(0.908)
visualizer.PublishRecording()

log = logger.FindLog(context)

核心思路：通过位姿初始化制造预压缩 #

用碰撞刚度模拟弹簧的本质是依赖接触力实现弹性形变，这类形变属于约束动力学范畴，不能通过硬改关节位置实现，否则会因初始状态违反约束产生巨大冲力导致模型飞散。推荐两种可行方案：

1. 直接调整刚体初始位姿（适配碰撞刚度方案） #

手动修改连杆两端刚体的相对位姿，让它们处于压缩接触状态，Drake会自动计算初始接触力维持预压缩：

# 获取目标刚体（替换为你的模型中实际的刚体名称）
leg_mass = compass_walker.GetBodyByName("leg_mass")
leg_link = compass_walker.GetBodyByName("leg_link")

# 获取默认位姿，向下移动髋部质量块制造压缩（0.05m为示例，可根据刚度调整）
mass_pose = leg_mass.default_pose()
current_z = mass_pose.translation()[2]
mass_pose.set_translation([0, 0, current_z - 0.05])

# 设置初始位姿
compass_walker.SetFreeBodyPose(plant_context, leg_mass, mass_pose)

# 清零所有刚体初始速度，避免冲力
compass_walker.SetFreeBodySpatialVelocity(plant_context, leg_mass, SpatialVelocity.Zero())
compass_walker.SetFreeBodySpatialVelocity(plant_context, leg_link, SpatialVelocity.Zero())

2. 改用弹簧阻尼约束（更可控的弹性连杆方案） #

如果连杆本质是弹性部件，直接用Drake的LinearSpringDamperConstraint替代碰撞刚度，不仅更贴合物理模型，还能直接设置初始形变：

# 在MultibodyPlant Finalize之前添加弹簧约束
leg_mass = compass_walker.GetBodyByName("leg_mass")
leg_link = compass_walker.GetBodyByName("leg_link")

# 添加线性弹簧阻尼：原长0.5m，刚度1000N/m，阻尼10N·s/m
compass_walker.AddLinearSpringDamperConstraint(
    bodyA=leg_mass,
    pointA=[0, 0, 0],          # 髋部质量块的连接点
    bodyB=leg_link,
    pointB=[0, 0, 0.5],        # 腿部连杆的连接点（对应弹簧原长）
    rest_length=0.5,
    stiffness=1000,
    damping=10
)

# 初始化时调整髋部位置，让弹簧处于压缩状态（原长0.5m，初始距离0.45m）
mass_pose = leg_mass.default_pose()
mass_pose.set_translation([0, 0, mass_pose.translation()[2] - 0.05])
compass_walker.SetFreeBodyPose(plant_context, leg_mass, mass_pose)

棱柱关节方案失败原因 #

添加的棱柱关节会让髋部与连杆成为串联刚体，而碰撞刚度是基于接触的约束，两者同时作用会产生冗余约束冲突：关节强制保持特定距离，碰撞又禁止穿透，冲突的约束会瞬间产生巨大冲力，导致模型飞散。

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