问题描述
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],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。 Drake 文档有一个关于 QueryObject 的警告，我认为这与此有关：
"输入端口返回的const引用被认为是'live'- 它链接到上下文、系统和缓存（充分利用所有 这些机制）。这个 const 引用不应该被持久化； 这样做会导致错误的查询结果。它更简单，更多 建议获取它以在有限范围内进行评估（例如， CalcTimeDerivatives()) 然后丢弃它。如果需要 QueryObject 对于 LeafSystem 中的许多独立功能，每个功能都应该重新评估 输入端口。底层缓存机制应该使成本 这个可以忽略不计。”
这是我的用例：我写了一堆使用几何查询的函数，比如非渗透约束和联系雅可比。所有这些都使用了很多样板来设置 QueryObject，所以我想我会把样板包装在一个 get_query_object 函数中，其他函数会调用它并然后运行他们需要的任何几何查询。这对我来说听起来是安全的，因为每个函数都在设置自己的 QueryObject 并且它不会退出该函数的范围，所以我们不应该遇到这个问题文档中描述。但它显然不起作用---那么推荐的工作流程是什么？
解决方法
您遇到的问题是 get_query_object 创建一个在返回时被抛出的上下文。在最高级别，您应该将 QueryObject 视为可以执行特定查询的 Context 的视图；它不存储任何数据。没有Context，你的QueryObject会死。
您的函数尝试做两件事：
- 为多体植物设置一些位置。
- 获取一个查询对象，以便计算有符号距离。
尽管您对查询对象现在已死的事实感到困惑，但您的另一个问题是您设置的主体位置也被上下文破坏了。
我建议进行以下更改：
- 创建上下文并将其存储在与图表、工厂和场景图类似的范围内。
- 获取与那个上下文相关联的查询对象，然后挂在它上面。只要上下文（即，它将总是成为该上下文数据的视图），它将是有效的和“活的”。或者，如文档所示，
QueryObject实例很便宜。您可以在需要时从上下文中获取一个。 - 创建一个方法，其唯一目的是在同一上下文中设置植物的身体位置。
这应该为您提供了您正在寻找的功能。
您粘贴的文档中的警告是指查询对象处于活动状态这一事实。如果您坚持使用该引用并且上下文中的数据发生变化，您将得到不同的答案。答案将始终反映相关上下文的当前状态。