基于PyDrake的可变形物体位姿获取与Runtime状态控制技术咨询

• 获取可变形物体的整体参考位姿：用MultibodyPlant.GetBodyPoseInWorld()，传入可变形物体关联的参考刚体（创建时会自动绑定一个参考框架），得到的就是这个参考体在世界坐标系下的位姿，代表物体的整体基准位置和姿态。
• 获取变形后所有质点的全局位置：用MultibodyPlant.GetDeformableBodyVerticesPositions()接口，传入可变形物体实例，返回的数组里包含每个质点在世界坐标系下的三维坐标。
• 自定义局部坐标系：基于参考体的位姿，用Frame类的接口创建局部坐标系，或者通过参考体位姿矩阵把质点的全局坐标转换成局部坐标系下的坐标。

可变形物体的自由度是所有质点的位置集合，和刚体的广义坐标逻辑不一样，所以刚体那套applied_generalized_force、actuation端口对它没用，得用下面这些方法：

• 给质点施加外力：通过MultibodyPlant.get_applied_spatial_force_input_port()（给参考体加力）或者专门的可变形质点力输入端口，写个LeafSystem来计算每个质点需要的控制力，输出到对应端口。比如在PyDrake里可以用ApplyExternalSpatialForce给参考体加空间力，或者直接给单个质点施加力。
• 用软约束实现位置控制：要是想把物体移到目标位置，可以做个弹簧-阻尼模型：算每个质点当前位置和目标位置的偏差，生成F = k*(x_target - x_current) - d*v_current这样的恢复力，把这个力输出到可变形物体的力输入端口，靠持续施加这种力拉着物体到目标点。
• 用内置约束辅助控制：可以加DeformableBodyConstraint相关约束（比如位置约束），结合InverseDynamicsController或者自定义控制器，间接控制可变形物体的形态和位置。

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