基于PyDrake的KUKA IIWA抓取模拟：锁定关节附着失败排查

环境配置 #

• Ubuntu 22.04
• PyDrake 1.19.0
• ROS Humble
• PyCharm

任务描述 #

基于SDF构建多体模型，用KUKA IIWA机械臂模拟大箱子的取放任务。计划在机械臂末端工具与箱子间添加QuaternionFloatingJoint，通过关节的锁定/解锁实现抓取、放置动作，但代码运行触发错误。

SDF模型说明 #

包含IIWA机械臂与箱子的World SDF文件（示意图）

代码片段 #

rdb = RobotDiagramBuilder(time_step=0)
parser = rdb.parser()
self.models = parser.AddModels(resource_file)
self.model = get_model_by_name(rdb.plant(), 'robot')
robot_tip_frame_name = 'iiwa_link_7'
box_target_frame_name = 'base'
box_lock = QuaternionFloatingJoint('box_lock',rdb.plant().GetFrameByName(robot_tip_frame_name),rdb.plant().GetFrameByName(box_target_frame_name))
self.box_lock = rdb.plant().AddJoint(box_lock)
floating_joint_placement = RigidTransform(RollPitchYaw(np.array([np.pi, 0.0, 0.0])), np.array([0.6, 0.0, 0.05]))
self.box_lock.SetDefaultPose(floating_joint_placement)
rdb.plant().Finalize()
self.robot = rdb.Build()
cc = SceneGraphCollisionChecker(model=self.robot, robot_model_instances=[self.model],
                            configuration_distance_function=l2_dist, edge_step_size=0.01,
                            env_collision_padding=0.0, self_collision_padding=0.0)

错误信息 #

Failure at bazel-out/k8-opt/bin/multibody/tree/_virtual_includes/multibody_tree_core/drake/multibody/tree/body.h:293 in floating_positions_start(): condition 'is_floating()' failed.

已尝试但无效的方案 #

• 通过主体框架与连杆框架创建漂浮关节
• 使用简单上下文替代碰撞上下文（运行IK时仍报相同错误）
• 在SDF中添加/移除RSO起始位姿
• 升级至PyDrake 1.20.0（进一步升级需处理弃用问题，判断为自身代码问题）

这个错误的核心原因是：QuaternionFloatingJoint要求关节连接的两个主体中，被连接的从动主体必须是无基座约束的漂浮体（不能与世界或其他固定主体有关节约束）。你的代码直接将箱子的base框架连到机械臂末端，但箱子在SDF中大概率是固定在世界坐标系下的，导致箱子并非漂浮体，调用SetDefaultPose或后续碰撞检查时触发断言失败。

修正步骤：

1. 确保箱子为漂浮体：在SDF中删除箱子与世界之间的固定关节；如果已加载模型，可通过代码移除箱子的固定关节。
2. 正确关联框架：获取箱子的主体（Body）对应框架，而非可能带约束的框架，再与机械臂末端框架创建漂浮关节。
3. 调整位姿设置逻辑：SetDefaultPose设置的是箱子相对于机械臂末端的相对位姿，而非绝对位姿。

修正后的关键代码片段：

# 加载模型后，找到箱子的模型实例（假设箱子模型名为box）
box_model = get_model_by_name(rdb.plant(), 'box')
# 若箱子存在固定关节，先移除（假设固定关节名为box_fixed）
# rdb.plant().RemoveJoint(rdb.plant().GetJointByName('box_fixed', box_model))

robot_tip_frame = rdb.plant().GetFrameByName(robot_tip_frame_name)
# 获取箱子的主体及对应框架
box_body = rdb.plant().GetBodyByName('base', box_model)
box_frame = box_body.body_frame()

# 创建漂浮关节
box_lock = QuaternionFloatingJoint('box_lock', robot_tip_frame, box_frame)
self.box_lock = rdb.plant().AddJoint(box_lock)

# 设置箱子相对于机械臂末端的相对位姿
relative_pose = RigidTransform(RollPitchYaw(np.array([np.pi, 0.0, 0.0])), np.array([0.0, 0.0, 0.05]))
self.box_lock.SetDefaultPose(relative_pose)

另外，使用SceneGraphCollisionChecker时，需根据需求确认是否将箱子加入机器人模型实例列表，或正确区分机器人与环境主体。

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