Дрейк: Почему для одной и той же ссылки в MultibodyPlant используется несколько geometryId?

Question

У меня есть 6 реальных звеньев (названные левым дистальным, левым проксимальным, правым дистальным, правым проксимальным, ладонью и шаром) + 2 фиктивных звена (так что шар может свободно перемещаться в плоскости xy) в моих URDF. Я хочу знать, какой geometryId соответствует какой ссылке, чтобы проверить контакт. Однако зарегистрированных в системе geometryId гораздо больше, чем ссылок. Поэтому я пытаюсь распечатать имена тел через geometryIds и обнаружил несколько geometryIds с одинаковым именем тела. Затем из учебника Drake render_multibody_plant.ipynb я увидел эту строку geometry_label = inspector.GetPerceptionProperties(geometry_id).GetProperty("label", "id"), поэтому я напечатал то же самое для своих geometryIds. Однако некоторые из них возвращают None. Когда я распечатываю int(geometry_label) для тех, которые не None, я получаю ровно 6 чисел (но кратные одному и тому же числу для некоторых ссылок!). Я не понимаю, для чего нужны эти дополнительные geometry_ids и как найти те geometryIds, которые мне действительно нужны.

Вот соответствующий код:

builder = DiagramBuilder()
# plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.00001)
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.)
file_name = FindResource("models/myhand.urdf")
gripper_model = Parser(plant).AddModelFromFile(file_name)
file_name = FindResource("models/sphere.urdf")
object_model = Parser(plant).AddModelFromFile(file_name)
scene_graph_context = scene_graph.AllocateContext()
plant.Finalize()
plant.set_name('hand')


controller = ConstantVectorSource([-7, 7])
torque_system = builder.AddSystem(controller)
builder.Connect(torque_system.get_output_port(0), plant.get_actuation_input_port())

# Setup visualization
T_xy = np.array([[ 1., 0., 0., 0.], [ 0., 1., 0., 0.], [ 0., 0., 0., 1.]])
T_xz = np.array([[ 1., 0., 0., 0.], [ 0., 0., 1., 0.], [ 0., 0., 0., 1.]])
visualizer = builder.AddSystem(
    PlanarSceneGraphVisualizer(scene_graph, T_VW=T_xy, xlim=[-0.3, 0.3], ylim=[-0.3, 0.3], show=plt_is_interactive))

builder.Connect(scene_graph.get_pose_bundle_output_port(),
                visualizer.get_input_port(0))


diagram = builder.Build()

diagram_context = diagram.CreateDefaultContext()
scene_graph_context = scene_graph.GetMyContextFromRoot(diagram_context) 
plant_context = plant.GetMyContextFromRoot(diagram_context) 

# Set up a simulator to run this diagram
simulator = Simulator(diagram)
context = simulator.get_mutable_context()

# simulate from zero to duration
simulator.Initialize()


# Simulate
duration = 0.5 if get_ipython() else 0.1 # sets a shorter duration during testing
context.SetTime(0.0)

AdvanceToAndVisualize(simulator, visualizer, duration)

query_object = scene_graph.get_query_output_port().Eval(scene_graph_context)
inspector = query_object.inspector()


for geometry_id in inspector.GetAllGeometryIds():
    body = plant.GetBodyFromFrameId(inspector.GetFrameId(geometry_id))
    print(body.name())

    geometry_label = inspector.GetPerceptionProperties(
        geometry_id)
    if geometry_label != None:
        print(int(geometry_label.GetProperty("label", "id")))

Вот вывод оператора print:

ball
ball
6
right_distal
right_distal
5
right_distal
5
left_distal
left_distal
4
left_distal
4
right_proximal
right_proximal
3
right_proximal
3
left_proximal
left_proximal
2
left_proximal
2
palm
palm
1

Примечание: запуск PyDrake на ноутбуке Jupyter

Answer 1

В частности, каждая ссылка может регистрировать столкновения и визуальную геометрию. Даже если две геометрии задают формы с одинаковыми параметрами, эти формы не объединяются в единую геометрию. Таким образом, вы получите один GeometryId для каждого столкновения и визуальной геометрии, связанной со ссылкой.

Те, которые объявлены как визуальные в вашем URDF, будут иметь не-None PerceptionProperties (и IllutrationProperties). Но их ProximityProperties будет None. И наоборот, геометрии, объявленные как коллизии, будут иметь не None ProximityProperties, но None для двух других типов.

Answer 2

Я ожидал бы иметь отдельный идентификатор геометрии для каждого визуального элемента в вашем теле (а не для каждого тела). Есть ли в вашем URDF несколько визуальных элементов в каждом теле? *

Учитывая GeometryId, SceneGraph не может сообщить, к какому телу он прикреплен. Он может сообщать только о фрейме псевдонима SceneGraph F. Но следующая идиома может сообщать о теле:

const MultibodyPlant<T>& plant = ...;
const SceneGraphInspector<T>& inspector =  ...;
const GeometryId g_id = id_from_some_query;
const FrameId f_id = inspector.GetFrameId(g_id);
const Body<T>* body = plant.GetBodyFromFrameId(f_id);

См. Документацию по геометрии :: SceneGraphInspector о том, где получить инспектора.