如何在pydrake中创建URDF对象后手动修改其位姿？

你之前用WeldFrames的方式会把模型和世界帧永久固定，初始化后无法修改位姿。要实现运行时动态调整位姿，核心是让模型与世界之间保留可动自由度，而非直接焊接。以下是具体实现方案：

核心思路 #

1. 加载URDF时，不为模型添加固定焊接，而是通过浮动基座关节（Floating Base Joint）让模型拥有6自由度的可动空间（3个位置自由度+3个姿态自由度）。
2. 在Plant初始化完成后，通过Context对象修改模型的状态，直接设置目标位姿。

完整代码示例 #

from pydrake.all import (
    FindResourceOrThrow, Parser, MultibodyPlant,
    RigidTransform, RollPitchYaw, Simulator,
    FloatingBaseType
)

# 1. 初始化MultibodyPlant
plant = MultibodyPlant(time_step=0.0)  # 连续系统，若用离散系统可设置非0时间步
parser = Parser(plant)

# 2. 加载URDF并添加浮动基座关节
my_urdf_path = "path/to/your/model.urdf"  # 替换为你的URDF路径
urdf = FindResourceOrThrow(my_urdf_path)
# 指定浮动基座类型，自动为模型添加6自由度的可动关节
model_instance = parser.AddModelFromFile(urdf, floating_base_type=FloatingBaseType.kRollPitchYaw)

# 3. 完成Plant初始化
plant.Finalize()

# 4. 创建模拟器与上下文（用于修改模型状态）
simulator = Simulator(plant)
context = simulator.get_mutable_context()

# 5. 第一次设置模型位姿
initial_pos = [0.0, 0.0, 0.5]
initial_rpy = [0.0, 0.0, 0.0]
initial_T = RigidTransform(RollPitchYaw(initial_rpy), initial_pos)
# 通过你指定的帧找到对应的刚体
target_body = plant.GetFrameByName("my_frame", model_instance).body()
plant.SetFreeBodyPose(context, target_body, initial_T)

# 6. 运行模拟一段时间后，动态修改位姿
simulator.AdvanceTo(1.0)

# 定义新的目标位姿
new_pos = [1.0, 0.5, 0.8]
new_rpy = [0.2, 0.3, 0.1]
new_T = RigidTransform(RollPitchYaw(new_rpy), new_pos)
plant.SetFreeBodyPose(context, target_body, new_T)

# 继续运行模拟
simulator.AdvanceTo(2.0)

# 可重复修改位姿
another_pos = [-0.5, 1.0, 0.3]
another_rpy = [-0.1, 0.0, 0.5]
another_T = RigidTransform(RollPitchYaw(another_rpy), another_pos)
plant.SetFreeBodyPose(context, target_body, another_T)
simulator.AdvanceTo(3.0)

关键细节说明 #

• 浮动基座类型：除了kRollPitchYaw（用欧拉角描述姿态），还可以选择kQuaternion（用四元数描述，避免欧拉角奇点问题）。
• SetFreeBodyPose方法：无需手动处理关节位置，直接通过RigidTransform设置刚体位姿，是最直观的方式。
• 避免焊接：如果必须保留焊接逻辑（比如部分模型固定），可以将需要动的部分拆分为独立模型，单独添加浮动基座。

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