过滤掉 pydrake 中的错误碰撞

Question

在我们的机器人项目中，我们使用 pydrake 查询对象来检查 iiwa 手臂中是否发生碰撞（这对于我们的 RRT 是必要的，并且对于仔细检查逆运动学模块的输出是否良好很有用）。

但是，在标准设置下，我们检测到一些不存在的碰撞，从而限制了机器人的运动。特别是，当对 iiwa 臂使用以下 sdf 时（场景仅包含此 iiwa 臂，不包含其他内容）：

- add_model:
    name: iiwa
    file: package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf

并设置其角度

example_joint_angles = np.array([0., 0., 0., 0., 0., 1.95, 0.])
environment.model.robot_arm.set_joint_angles(example_joint_angles)
print(environment.model.has_collisions())

我们被告知存在碰撞，但将第 6 个角度设置为 1.9 表示没有碰撞（作为参考，我们方法的实现如下）。

我认为这些角度不应该导致碰撞，原因有两个。首先，只要看一下网状猫，我就发现手臂还远未自行折叠。其次，我们一开始就没有预料到会发生这种碰撞，因为我们根据 iiwa 臂的 sdf 中列出的关节限制设置了逆运动学的角度限制（允许该关节上的角度最大为 2.0944） ).

所以我想第一个问题是：是否有更好的方法来检查与 iiwa 臂的碰撞（例如，使用具有稍微更精细的碰撞的不同 sdf）？

如果没有，是否有某种方法告诉查询对象/场景图/等跳过这两个对象之间的碰撞？

class IIWAArm:
    #...
    def set_joint_angles(self, q: np.ndarray, visualize: bool = True):
    '''
    Instantaneously moves the arm to the given angles (for visualization / motion planning).

    Args:
        q (np.ndarray): Angles to move to.
    '''
    self.iiwa = self.plant.GetModelInstanceByName("iiwa")
    self.plant.SetPositions(self.plant_context, self.iiwa, q)
    if visualize:
        self.diagram.ForcedPublish(self.context)


class SimulationModel:
    robot_arm: IIWAArm
    #...
    def has_collisions(self) -> bool:
        scene_graph = self.station.GetSubsystemByName("scene_graph")
        context = self.context
        scene_graph_context = scene_graph.GetMyMutableContextFromRoot(context)
        query_object = scene_graph.GetOutputPort("query").Eval(scene_graph_context)
        return query_object.HasCollisions()

    def get_collisions(self) -> List[PenetrationAsPointPair]:
        scene_graph = self.station.GetSubsystemByName("scene_graph")
        context = self.context
        scene_graph_context = scene_graph.GetMyMutableContextFromRoot(context)
        query_object = scene_graph.GetOutputPort("query").Eval(scene_graph_context)
        res = query_object.ComputePointPairPenetration()
        return res

我还尝试使用上面列出的函数 get_collisions，它可以让我提取碰撞中对象的 GeometryId。所以，从理论上讲，我认为我应该能够使用 get_collisions，然后跳过没有意义的碰撞（例如 iiwa 的关节 6 和 7 之间）。 我陷入了这种策略，因为我找不到一个函数可以让我提取有关给定 GeometryId 对应的对象的“人类可读”信息（例如，有一个 scene_graph.RenameGeometry 允许我设置与一个 GeometryId，但我找不到类似的“getter 函数”）。

我当前（极其hacky）的方法是更改SimulationModel的const'r，故意引起这种错误碰撞，以找到其相应的GeometryId并让has_collisions跳过它们。即，类似：

class SimulationModel:
    def __init__(self, ...):
        # ...
        self.robot_arm.set_joint_angles(BAD_ANGLES)
        self.fake_collisions = self.get_collisions()
        self.robot_arm.set_joint_angles(DEFAULT_ANGLES)

    def new_has_collisions(self) -> bool:
        suspected_collisions = self.get_collisions()
        filtered_collisions = filter_out(suspected_collisions, self.fake_collisions)
        return len(filtered_collisions) != 0

Answer 1

您遇到的问题只是邻近表示之一。顾名思义，您使用的 iiwa 将每个链接表示为邻近查询（也称为距离和联系人查询）的框。

盒子是简单但极其粗糙的几何近似（特别是对于像 iiwa 臂的链接这样的东西）。如果您在真实机器人配置空间的边缘附近推动，则需要更接近“实际”机器人几何形状的邻近几何形状。胶囊将是下一个值得尝试的产品。它们会更好地适应圆柱形链接，但仍然在查询中提供合理的性能。在极限情况下，您可以使用网格本身（它们将被网格接触外壳的低效表示所取代）。如果您在网格上收敛，您最好手动制作网格的低分辨率凸近似（从该数据来看，Drake 无法为您做到这一点）。如果您查看模型package://drake/manipulation/models/iiwa_description/sdf/iiwa14_polytope_collision.sdf，您会发现它的末端链接有一个手工制作的多胞体。

下面，我附加了一些步骤，您可以使用它们来反思并更好地理解模型。

如果您运行以下命令：

bazel run //tool:model_visualizer -- package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf

您可以准确地看到机器人（屏幕抓取显示控制面板的展开）：

如果我们打开“邻近”几何图形，您将看到这些框。

最后，如果我设置你建议的姿势（除了 iiwa_joint_6 之外全部为零），它会显示链接 7 和链接 5 之间的接触。