Adding frozen children doesn't support `add_frame` directives

[siddancha]

Current logic

When add_frozen_child_instances=True, the current logic in _PopulatePlantOrDiagram() searches over a tree whose edges are defined only by weld directives to find child instances to add (see _FindChildren()).

[Permalink]:

tree = dict()
children = set()
for d in flattened_directives:
    if d.add_weld:
        parent = ScopedName.Parse(d.add_weld.parent).get_namespace()
        child = ScopedName.Parse(d.add_weld.child).get_namespace()
        if parent not in tree:
            tree[parent] = {child}
        else:
            tree[parent].add(child)

Furthermore, only two types of directives are retained to be added to the controller plant -- add_model and add_weld:

[Permalink]:

directives = []
for d in flattened_directives:
    if d.add_model and (d.add_model.name in all_model_instances):
        directives.append(d)
    if (
        d.add_weld
        and (
            ScopedName.Parse(d.add_weld.child).get_namespace()
            in all_model_instances
        )
        and (
            d.add_weld.parent == "world"
            or ScopedName.Parse(d.add_weld.parent).get_namespace()
            in all_model_instances
        )
    ):
        directives.append(d)

The problem

This logic is problematic when the scenario file contains add_frame directives. For example, a common workflow is to add a custom frame relative to a body frame of one robot, and weld the custom frame to a body frame of another robot. In this case, the controller plant throws an error because it cannot find the declaration of the custom frame.

Furthermore, _FindChildren() will not find children that are separated by at least one add_frame directive.

Proposed solution

We should treat add_frame directives as edges just like add_weld in the search tree. This will detect any robot instance separated by a sequence of add_frame and add_weld directives as a child.

Secondly, we should add the relevant add_frame directives into the controller plant.

Temporary workaround

I'm temporarily working around this issue by avoiding add_frame directives in my scenario file, and directly welding body frames instead.

[RussTedrake]

I agree that this is an issue. Good to have it on the queue, and happy to take contributions if you find it easier to resolve it.

[siddancha]

I agree that this is an issue. Good to have it on the queue, and happy to take contributions if you find it easier to resolve it.

@RussTedrake Added PR #323 fixing this issue.