def add_to(self,
plant_dest,
scene_graph_dest=None,
model_instance_remap=model_instance_remap_same_name):
"""Adds this subgraph onto a given plant_dest and scene_graph_dest.
Args:
plant_dest: "Destination plant".
scene_graph_dest: "Destination scene graph".
If this is specified, there must be a source scene_graph.
model_instance_remap:
Either a function of the form:
func(plant_src, model_instance_src, plant_dest)
-> model_instance_dest
Or a string, which simply remaps all source model instances to
a model instance of the given name (which may be added if it
does not already exist).
Returns:
src_to_dest (MultibodyPlantElementsMap) used to copy elements and
record the associations from this subgraph's plant_src (and
scene_graph_src) to plant_dest (and scene_graph_dest).
"""
assert isinstance(plant_dest, MultibodyPlant)
if isinstance(model_instance_remap, str):
model_instance_name = model_instance_remap
model_instance_remap = create_model_instance_remap_by_name(
lambda x: model_instance_name)
plant_src = self.plant_src
scene_graph_src = self.scene_graph_src
elem_src = self._elem_src
if scene_graph_dest is not None:
assert scene_graph_src is not None
# Create mapping.
src_to_dest = MultibodyPlantElementsMap(
plant_src,
plant_dest,
scene_graph_src=scene_graph_src,
scene_graph_dest=scene_graph_dest,
)
# Remap and register model instances.
for model_instance_src in _sorted_indices(elem_src.model_instances):
model_instance_dest = model_instance_remap(plant_src,
model_instance_src,
plant_dest)
src_to_dest.register_model_instance(model_instance_src,
model_instance_dest)
# Register world body and frame if we're using that source model
# instance.
if world_model_instance() in src_to_dest.model_instances:
src_to_dest.register_world_body_and_frame()
# Copy bodies.
for body_src in _sorted_elements(elem_src.bodies):
src_to_dest.copy_body(body_src)
# Copy frames.
for frame_src in _sorted_elements(elem_src.frames):
src_to_dest.copy_frame(frame_src)
# Copy joints.
for joint_src in _sorted_elements(elem_src.joints):
src_to_dest.copy_joint(joint_src)
# Copy joint actuators.
for joint_actuator_src in _sorted_elements(elem_src.joint_actuators):
src_to_dest.copy_joint_actuator(joint_actuator_src)
# Copy geometries (if applicable).
if scene_graph_dest is not None:
for geometry_id_src in _sorted_identifiers(elem_src.geometry_ids):
src_to_dest.copy_geometry_by_id(geometry_id_src)
# Apply policies to new mapping.
for policy in self._policies:
policy.after_add(src_to_dest)
return src_to_dest
def test_multibody_plant_elements(self):
plant, scene_graph, _ = self.make_arbitrary_multibody_stuff(
num_bodies=1)
other_plant, _, _ = self.make_arbitrary_multibody_stuff(num_bodies=1)
# Test nominal usage.
elem = mut.get_elements_from_plant(plant, scene_graph)
actual_count = MultibodyPlantElementsCount.of(elem)
expected_count = MultibodyPlantElementsCount()
expected_count.__dict__.update(
model_instances=3,
bodies=2,
frames=6,
joints=1,
joint_actuators=1,
geometry_ids=3,
)
self.assert_count_equal(actual_count, expected_count)
# Test copying.
elem_copy = copy.copy(elem)
self.assertIsNot(elem, elem_copy)
self.assertEqual(elem, elem_copy)
# Test subgraph invariant.
mut.check_subgraph_invariants(elem)
@contextmanager
def check_subgraph_negative():
elem_copy = copy.copy(elem)
yield elem_copy
with self.assertRaises(AssertionError):
mut.check_subgraph_invariants(elem_copy)
# Check negative cases:
# - subgraph model instance in plant model instances
with check_subgraph_negative() as elem_copy:
elem_copy.model_instances.add(ModelInstanceIndex(100))
# - subgraph bodies in subgraph model instances.
with check_subgraph_negative() as elem_copy:
elem_copy.model_instances.remove(world_model_instance())
# - subgraph element must be part of the subgraph plant.
with check_subgraph_negative() as elem_copy:
elem_copy.bodies.add(other_plant.world_body())
# - subgraph frames must be attached to subgraph bodies
# - subgraph joints only connected to subgraph bodies
# - subgrpah geometries must be attached to subgraph bodies
with check_subgraph_negative() as elem_copy:
elem_copy.bodies.remove(plant.world_body())
# - subgraph joint actuators must solely act on subgraph joints
with check_subgraph_negative() as elem_copy:
joint, = elem.joints
elem_copy.joints.remove(joint)
# Test usage without SceneGraph.
elem_copy_no_scene_graph = copy.copy(elem)
elem_copy_no_scene_graph.scene_graph = None
elem_copy_no_scene_graph.geometry_ids = set()
self.assertEqual(mut.get_elements_from_plant(plant),
elem_copy_no_scene_graph)
# Tests that we can't add elements with any intersection.
with self.assertRaises(AssertionError):
elem += elem_copy
# Tests that we can add "disjoint" stuff.
elem_copy = copy.copy(elem)
last_body = list(elem.bodies)[-1]
elem_copy.bodies.remove(last_body)
self.assertNotEqual(elem, elem_copy)
elem_world_body_only = mut.MultibodyPlantElements(plant, scene_graph)
elem_world_body_only.bodies.add(last_body)
elem_copy += elem_world_body_only
self.assertEqual(elem, elem_copy)
def copy_geometry_by_id(self, geometry_id_src):
"""Copes the geometry represented by geometry_id_src (and its roles /
properties) to be added to the destination plant / scene_graph.
At present, collision geometries that are to be used for
simulation must be registered with the plant (rather than
queried). For this reason, all collision geometries must *only*
be collision geometries, and have to go through the plant.
"""
assert isinstance(geometry_id_src, GeometryId)
plant_src = self.plant_src
scene_graph_src = self.scene_graph_src
plant_dest = self.plant_dest
scene_graph_dest = self.scene_graph_dest
assert scene_graph_src is not None
assert scene_graph_dest is not None
inspector_src = scene_graph_src.model_inspector()
frame_id_src = inspector_src.GetFrameId(geometry_id_src)
body_src = plant_src.GetBodyFromFrameId(frame_id_src)
assert body_src is not None
body_dest = self.bodies[body_src]
frame_id_dest = plant_dest.GetBodyFrameIdOrThrow(body_dest.index())
assert frame_id_dest is not None
geometry_instance_dest = inspector_src.CloneGeometryInstance(
geometry_id_src)
# Use new "scoped" name.
if body_src.model_instance() != world_model_instance():
model_instance_name_src = plant_src.GetModelInstanceName(
body_src.model_instance())
prefix_src = f"{model_instance_name_src}::"
else:
prefix_src = ""
if body_dest.model_instance() != world_model_instance():
model_instance_name_dest = plant_dest.GetModelInstanceName(
body_dest.model_instance())
prefix_dest = f"{model_instance_name_dest}::"
else:
prefix_dest = ""
scoped_name_src = geometry_instance_dest.name()
assert scoped_name_src.startswith(prefix_src), (
f"'{scoped_name_src}' should start with '{prefix_src}'")
unscoped_name = scoped_name_src[len(prefix_src):]
scoped_name_dest = f"{prefix_dest}{unscoped_name}"
geometry_instance_dest.set_name(scoped_name_dest)
# TODO(eric.cousineau): How to relax this constraint? How can we
# register with SceneGraph only? See Sean's TODO in
# MultibodyPlant.RegisterCollisionGeometry.
proximity_properties = (geometry_instance_dest.proximity_properties())
if proximity_properties is not None:
assert geometry_instance_dest.perception_properties() is None
assert geometry_instance_dest.illustration_properties() is None
geometry_id_dest = plant_dest.RegisterCollisionGeometry(
body=body_dest,
X_BG=geometry_instance_dest.pose(),
shape=geometry_instance_dest.release_shape(),
name=unscoped_name,
properties=proximity_properties)
else:
# Register as normal.
source_id_dest = plant_dest.get_source_id()
geometry_id_dest = scene_graph_dest.RegisterGeometry(
source_id_dest, frame_id_dest, geometry_instance_dest)
_add_item(self.geometry_ids, geometry_id_src, geometry_id_dest)