在PyDrake中获取Kuka Iiwa机械臂关节的世界坐标系3D位姿

首先要明确：RigidBody_[float]类本身并没有get_pose_in_world()方法，因为连杆的姿态/位置依赖于系统上下文（Context）（包含当前关节位置等状态），必须结合Context才能计算。以下是具体实现步骤：

核心步骤 #

1. 准备MultibodyPlant与Context #

确保你已经从URDF/SDF加载了Kuka Iiwa 7的MultibodyPlant实例，并且拥有对应的Context：

• 若为仿真场景，直接使用仿真器提供的Context；
• 若为离线计算，创建默认Context后通过plant.SetPositions()设置关节角度thetas。

2. 获取目标Frame #

根据需求选择要查询的Frame：

• 连杆本体Frame：通过plant.GetBodyByName("iiwa_link_N")获取对应连杆（N为0~7），再调用body.body_frame()得到其本体Frame；
• 关节位置Frame：如果需要关节旋转轴的原点位置，可通过关节对象获取，比如plant.GetJointByName("iiwa_joint_N").frame_on_child()（子连杆侧的关节Frame）或frame_on_parent()（父连杆侧）。

3. 计算Frame在目标坐标系中的位置 #

使用plant.CalcRelativeTransform()计算Frame相对于世界/基座坐标系的变换，再提取其中的平移分量即为3D坐标：

• 世界坐标系：plant.world_frame()；
• 基座坐标系：plant.GetBodyByName("iiwa_link_0").body_frame()（Iiwa的基座为link_0）。

代码示例 #

import numpy as np
from pydrake.multibody.plant import MultibodyPlant

# 假设已加载plant，且context为对应的上下文对象，thetas是长度为7的关节角度数组
plant.SetPositions(context, thetas)

# 示例1：获取iiwa_link_5在世界坐标系中的3D坐标
target_body = plant.GetBodyByName("iiwa_link_5")
target_frame = target_body.body_frame()
world_frame = plant.world_frame()

# 计算世界到目标Frame的变换
X_WB = plant.CalcRelativeTransform(context, frame_A=world_frame, frame_B=target_frame)
# 提取x/y/z坐标
world_position = X_WB.translation()
print(f"iiwa_link_5 世界坐标: {world_position}")

# 示例2：获取iiwa_link_5在基座坐标系中的3D坐标
base_body = plant.GetBodyByName("iiwa_link_0")
base_frame = base_body.body_frame()

X_BB = plant.CalcRelativeTransform(context, frame_A=base_frame, frame_B=target_frame)
base_position = X_BB.translation()
print(f"iiwa_link_5 基座坐标: {base_position}")

# 示例3：获取iiwa_joint_3的关节位置（世界坐标系）
target_joint = plant.GetJointByName("iiwa_joint_3")
joint_frame = target_joint.frame_on_child()
X_WJ = plant.CalcRelativeTransform(context, world_frame, joint_frame)
joint_world_position = X_WJ.translation()
print(f"iiwa_joint_3 世界坐标: {joint_world_position}")

关于Triad的补充 #

如果之前添加Triad是为了可视化，无需手动调用姿态查询方法——只要更新Context中的关节位置，Triad会自动同步绑定Frame的姿态，因为它的状态是由MultibodyPlant驱动的。

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