如何在PyDrake中获取IIWA机械臂两个本体坐标系间的变换

你遇到的TypeError是因为CalcPoseInBodyFrame()必须传入**系统上下文（Context）**作为参数，且该方法仅用于获取当前框架在其所属刚体本体坐标系中的位姿，无法直接计算两个框架间的相对变换。

要计算f1（iiwa_link_0）和f2（iiwa_link_2）之间的相对位姿变换，推荐以下两种方法：

方法一：调用Plant的CalcRelativeTransform #

直接通过多体植物对象计算两个框架的相对变换，参数依次为上下文、参考框架、目标框架：

# 先创建或获取已配置好的Context
context = plant.CreateDefaultContext()

# 计算f2相对于f1的位姿变换（X_F1F2）
X_F1F2 = plant.CalcRelativeTransform(context, frame_A=f1, frame_B=f2)

# 若需f1相对于f2的变换，调换框架参数即可
X_F2F1 = plant.CalcRelativeTransform(context, frame_A=f2, frame_B=f1)

方法二：调用单个框架的CalcPoseInFrame #

通过其中一个框架的CalcPoseInFrame方法，传入上下文和目标参考框架，得到相对位姿：

context = plant.CreateDefaultContext()

# 获取f2在f1坐标系下的位姿
X_F1F2 = f2.CalcPoseInFrame(context, f1)

# 获取f1在f2坐标系下的位姿
X_F2F1 = f1.CalcPoseInFrame(context, f2)

关键注意事项 #

• 若需要非零位姿的关节状态，需手动修改Context：通过context.get_mutable_continuous_state_vector()获取关节状态向量，设置对应关节角度值。
• 得到的RigidTransform结果，可通过.rotation()提取旋转矩阵，.translation()提取平移向量，用于后续计算或控制逻辑。

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