Drake中SetFreeBodyPose未按世界坐标系设置位姿的问题咨询

问题现象 #

在Drake模拟器中，我将零件通过可锁止的吸盘关节（QuaternionFloatingJoint）连接到末端执行器，想要在世界坐标系下初始化零件到工作台位姿。但调用plant.SetFreeBodyPose(plant_sim_context, part_body, world_to_part)时，零件实际是相对于末端执行器坐标系变换，而非世界坐标系。只有计算末端执行器到零件的相对变换ee_to_part，再传入SetFreeBodyPose才能让零件处于世界坐标系的目标位姿，这和文档中SetFreeBodyPose设置刚体世界位姿X_WB的描述不符。

核心原因 #

QuaternionFloatingJoint的关节状态定义的是子刚体相对于父刚体的位姿，即使关节处于解锁状态，零件也并非完全自由的刚体——它和末端执行器之间存在关节约束关系。当调用SetFreeBodyPose时，Drake会通过求解约束（更新关节变量）来满足目标位姿，而非直接覆盖刚体的世界位姿。也就是说，此时SetFreeBodyPose传入的位姿会被解析为零件相对于其父关节（末端执行器）的位姿，而非世界位姿。

解决方案 #

方案1：直接计算关节所需的相对位姿（当前可行方案） #

保持现有逻辑，先计算末端执行器的世界位姿world_to_ee，再通过逆变换得到零件相对于末端执行器的位姿ee_to_part = world_to_ee.InvertAndCompose(world_to_part)，传入SetFreeBodyPose。这种方式直接适配了浮关节的变量定义。

方案2：通过关节变量直接设置相对位姿 #

既然QuaternionFloatingJoint的位置变量就是子刚体相对于父刚体的位姿（四元数+平移向量），可以直接构造关节位置向量，用plant.SetJointPositions设置：

# 计算零件相对于末端执行器的位姿
ee_to_part = world_to_ee.InvertAndCompose(world_to_part)
# 将位姿转为关节位置格式：[qw, qx, qy, qz, x, y, z]
joint_pos = np.hstack([ee_to_part.rotation().wxyz(), ee_to_part.translation()])
plant.SetJointPositions(plant_sim_context, suction_joint, joint_pos)

方案3：先设置零件位姿再添加关节 #

如果希望先在世界坐标系下定位零件，再建立吸盘关节约束，可以调整流程：

1. 添加零件模型后，先调用SetFreeBodyPose设置其世界位姿
2. 再添加QuaternionFloatingJoint连接到末端执行器
3. 注意添加关节时可以通过suction_joint.SetDefaultPose()设置初始相对位姿，确保关节创建后零件位置不变

代码修正示例 #

以方案2为例，修改原代码中设置零件位姿的部分：

# ... 原有代码 ...
part_body = plant.GetBodyByName("part1_link")

# World to end effector transform
world_to_ee = plant.CalcRelativeTransform(
    plant_sim_context,
    plant.world_frame(),
    plant.GetFrameByName("combo_end_effector_link"),
)

# Part pose in world frame
world_to_part = RigidTransform(pose=[0, 0.5, 0])

# 计算零件相对于末端执行器的位姿并设置关节变量
ee_to_part = world_to_ee.InvertAndCompose(world_to_part)
joint_pos = np.hstack([ee_to_part.rotation().wxyz(), ee_to_part.translation()])
plant.SetJointPositions(plant_sim_context, suction_joint, joint_pos)
# ... 原有代码 ...

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