当 QueryObject 离开函数作用域时出现段错误

Question

以下代码段错误：

from pydrake.all import (
    Diagram,
    DiagramBuilder,
    AddMultibodyPlantSceneGraph,
    UnitInertia,
    SpatialInertia,
    MultibodyPlant,
    SceneGraph,
    QueryObject,
)
import numpy as np


def get_query_object(
    diagram: Diagram,
    plant: MultibodyPlant,
    scene_graph: SceneGraph,
    q: np.ndarray,
) -> QueryObject:
    context = diagram.CreateDefaultContext()
    plant_context = diagram.GetMutableSubsystemContext(plant, context)
    plant.SetPositions(plant_context, q)
    sg_context = diagram.GetMutableSubsystemContext(scene_graph, context)
    query_object = (
        scene_graph.get_query_output_port().EvalAbstract(sg_context).get_value()
    )
    return query_object


builder = DiagramBuilder()

plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 1e-4)
plant.AddRigidBody(
    "body",
    SpatialInertia(1, [0, 0, 0], UnitInertia(1, 1, 1)),
)

plant.Finalize()
diagram = builder.Build()

q = np.zeros(7)

query_object = get_query_object(
    diagram=diagram,
    plant=plant,
    scene_graph=scene_graph,
    q=q,
)
signed_distance_pair = query_object.ComputeSignedDistancePairwiseClosestPoints()

段错误可以通过内联get_query_object的内容或者在其中执行ComputeSignedDistancePairwiseClosestPoints来避免，所以我认为问题在于使用离开其创建范围的 QueryObject 进行几何查询。德雷克文档有一个关于 QueryObject 的警告，我认为它与此有关：

"The const reference returned by the input port is considered 'live' - it is linked to the context, system, and cache (making full use of all of those mechanisms). This const reference should never be persisted; doing so can lead to erroneous query results. It is simpler and more advisable to acquire it for evaluation in a limited scope (e.g., CalcTimeDerivatives()) and then discard it. If a QueryObject is needed for many separate functions in a LeafSystem, each should re-evaluate the input port. The underlying caching mechanism should make the cost of this negligible."

这是我的用例：我写了一堆使用几何查询的函数，比如非渗透约束和接触雅可比。所有这些都使用大量样板来设置 QueryObject，所以我想我应该将样板包装在 get_query_object[=25= 中] 函数，其他函数将调用该函数，然后运行他们在其中需要的任何几何查询。这 听起来 对我来说是安全的，因为每个函数都在设置自己的 QueryObject 并且它不会退出该函数的范围，所以我们应该't 运行进入文档中描述的问题。但它显然不起作用---那么推荐的工作流程是什么？

Answer 1

您遇到的问题是 get_query_object 创建了 一个在 return 上被丢弃的上下文。在最高级别，您应该将 QueryObject 视为可以执行特定查询的视图到 Context 中；它不存储任何数据。没有 Context，你的 QueryObject 就死了。

您的函数试图做两件事：

为多body 植物设置一些位置。
获取查询 object 以便计算有符号距离。

尽管您对查询 object 现在已死这一事实感到困惑，但您的另一个问题是您设置的 body 位置也随上下文一起被破坏。

我建议进行以下更改：

创建上下文并将其存储在与图表、工厂和场景图相似的范围内。
获取与 that 上下文关联的查询 object 并坚持下去。只要上下文有效，它就会有效且“有效”（即，它将始终是该上下文数据的视图）。或者，如文档所示，QueryObject 个实例很便宜。您可以在需要时从上下文中获取一个。
创建一个方法，其唯一目的是在同一上下文中为植物设置 body 位置。

这应该可以为您提供所需的功能。

您粘贴的文档中的警告是指查询 object 是实时的。如果您继续使用该参考文献并且 上下文中的数据发生变化，您将得到不同的答案。答案将始终反映相关上下文的当前状态。

当 QueryObject 离开函数作用域时出现段错误

Segfault when QueryObject leaves function scope

drake