Modify collision geometry pose for a given Body

[mposa]

Is there a way, online and after finalizing the plant, to update the transform from a Body to one of it's collision geometries?

[SeanCurtis-TRI]

While there is no API to directly change only the pose, there is an API that will give you the end result: SceneGraph::ChangeShape() (and, more particularly, it's context-modifying variant).

Basic sample code -- modify SceneGraph's model

SceneGraph<double>& scene_graph = ...;  // SceneGraph from somewhere.
const GeometryId geom_id = ...;   // The geometry id you've found/tracked/whatever.
const SourceId source_id = ...;   // The id of the "owner" of the geometry.
const SceneGraphInspector<double>& inspector = scene_graph.model_inspector();
const RigidTransformd X_FG = ...;  // Your new pose.
scene_graph.ChangeShape(source_id, geom_id, inspector.GetShape(geom_id), X_FG);

Notes:

1. This variant modifies the underlying SceneGraph model. You will only see the effect in the next contexts you allocate. To modify an existing context, see the example below.
2. The SourceId. If you are modifying data that was parsed from a model file via Parser, you get it from MultibodyPlant.
3. The ChangeShape() API requires a shape. In this case, we use SceneGraphInspector to get the existing shape and simply pass it back through.

Modifying an existing context

Context<double>& sg_context = ...;  // You got the SceneGraph context from somewhere.
SceneGraph<double>& scene_graph = ...;  // SceneGraph from somewhere.
const GeometryId geom_id = ...;   // The geometry id you've found/tracked/whatever.
const SourceId source_id = ...;   // The id of the "owner" of the geometry.
const SceneGraphInspector<double>& inspector =
    scene_graph.get_query_output_port().Eval<QueryObject<double>>(sg_context).inspector();
const RigidTransformd X_FG = ...;  // Your new pose.
scene_graph.ChangeShape(&sg_context, source_id, geom_id, inspector.GetShape(geom_id), X_FG);

Notes:

1. To get the Context for the SceneGraph, you may have to extract it from the diagram context.
2. This only changes the current context. Other copies of that context will be unchanged, and newly allocated contexts will not have the new pose.
3. Note that we're getting the SceneGraphInspector from the context and not scene_graph itself. If nothing has changed for the geometry indicated by geom_id since allocation, there won't be any difference in which inspector you use. But it's generally safer to query the context you're actually modifying to avoid potential traps.