Support add_frame directives when adding frozen children

[siddancha]

Fixes #322

Solution

• Implements a new DirectivesTree class in manipulation/directives_tree.py. This class creates a tree where:
  • Nodes: are defined by (1) model instances and (2) added frames.
  • Edges: are defined by (1) add_weld and (2) add_frame directives.
• The tree implements two recursive search functions:
  • Finding paths from the root ("world" frame) to a given set of robot instances. The use case is to find the minimal set of directives that supports the given robot instances.
  • Finding paths from given robot instances to leaves. The use case is to find child model instances that are welded to the robots (as well as accompanying directives), which is in-turn used to lock the childrens' joints and add their directives to a controller plant.
• This abstraction simplifies the code in _PopulatePlantOrDiagram().

Unit testing

• Added a test file: manipulation/test/test_directives_tree.py that tests the individual search member functions of DirectivesTree at the low level and MakeHardwareStation at the high-level.

---

This change is 

[RussTedrake]

Nice! I'm up to my ears in some work at the moment. @nepfaff ... could you take a first look?

[nepfaff]

Looks great! Thank you for adding the test case!

I left a couple of minor comments but could be merged from my point of view.

Reviewed 2 of 4 files at r1, all commit messages.
Reviewable status: 2 of 4 files reviewed, 11 unresolved discussions (waiting on @siddancha)

---

manipulation/directives_tree.py line 41 at r1 (raw file):

    def __init__(self, flattened_directives: typing.List[ModelDirective]):
        self.flattened_directives = flattened_directives
        self.add_model_directives: typing.Dict[str, ModelDirective] = dict()

nit I think people usually use nouns for variables and verbs for functions. However, I couldn't find this in the python guide. What do you think?
Maybe model_directives_to_add would be clearer?

Also, adding a comment that describes the variables purpose could be useful here. You have this in a later docstring but it requires some going back and fourth at the moment.

---

manipulation/directives_tree.py line 57 at r1 (raw file):

            if d.add_model:
                self.model_names.add(d.add_model.name)
                self.add_model_directives[d.add_model.name] = d

nit Slightly more readable when factoring out d.add_model.name:

name = d.add_model.name
self.model_names.add(name)
self.add_model_directives[name] = d

---

manipulation/directives_tree.py line 79 at r1 (raw file):

                    self.edges[parent] = {edge}
                else:
                    self.edges[parent].add(edge)

nit You could reduce code duplication here.

One option would be to create a helper function:

def _AddEdge(parent_name, child_name, directive):
   parent = self._MakeNode(parent_name)
   child = self._MakeNode(child_name)
   edge = Edge(parent, child, d)
   if parent not in self.edges:
     self.edges[parent] = {edge}
   else:
     self.edges[parent].add(edge)

This would also make the __init__ method more compact and thus more readable.

Code quote:

                parent = self._MakeNode(d.add_frame.X_PF.base_frame)
                child = self._MakeNode(d.add_frame.name)
                edge = Edge(parent, child, d)
                if parent not in self.edges:
                    self.edges[parent] = {edge}
                else:
                    self.edges[parent].add(edge)

---

manipulation/directives_tree.py line 93 at r1 (raw file):

        raise ValueError(
            f"Node {name} not found in the tree. It neither corresponds to a "
            + f"frame {self.frame_names} nor a model instance {self.model_names}."

nit The + is not needed

---

manipulation/directives_tree.py line 111 at r1 (raw file):

                    the input node.
                Set[ModelDirective]: The directives that need to be added to
                    weld the proper descendant models to the input node.

nit This might be more useful as part of the docstring of GetWeldedDescendantsAndDirectives to improve intellisense.
Generally, it would be nice if all public functions, public classes, and public class members had docstrings.

Code quote:

            Returns:
                Set[str]: Names of proper descendant models that are welded to
                    the input node.
                Set[ModelDirective]: The directives that need to be added to
                    weld the proper descendant models to the input node.

---

manipulation/directives_tree.py line 123 at r1 (raw file):

                if edge.child.type == "model":
                    _descendants.add(edge.child.name)
                    _directives.add(self.add_model_directives[edge.child.name])

nit Slightly more readable when factoring out the name:

name = edge.child.name
_descendants.add(name)
_directives.add(self.add_model_directives[name])

Code quote:

                    _descendants.add(edge.child.name)
                    _directives.add(self.add_model_directives[edge.child.name])

---

manipulation/directives_tree.py line 158 at r1 (raw file):

            Returns:
                Set[ModelDirective]: The directives that need to be added to
                    weld all `model_instance_names` to the "world" frame.

nit Similar to above, I would find this more useful as part of the docstring of the public member (for intellisense purposes).

Code quote:

            Returns:
                Set[ModelDirective]: The directives that need to be added to
                    weld all `model_instance_names` to the "world" frame.

---

manipulation/directives_tree.py line 191 at r1 (raw file):

        sorted order.
        """
        return [d for d in self.flattened_directives if d in directives]

The function name is misleading as its not doing any sorting.

Code quote:

    def TopologicallySortDirectives(
        self, directives: typing.Set[ModelDirective]
    ) -> typing.List[ModelDirective]:
        """
        This assumes that the flattened directives are in a valid topologically
        sorted order.
        """
        return [d for d in self.flattened_directives if d in directives]

---

manipulation/test/test_directives_tree.py line 19 at r1 (raw file):

class DirectivesTreeTest(unittest.TestCase):
    def GetFlattenedDirectives(self) -> typing.List[ModelDirective]:

nit Using snake case would be more consistent with the other methods in this class. This is also done in other test cases (see here for example)

---

manipulation/test/test_directives_tree.py line 19 at r1 (raw file):

class DirectivesTreeTest(unittest.TestCase):
    def GetFlattenedDirectives(self) -> typing.List[ModelDirective]:

Using @cache from functools would be more efficient as this is called many times.

---

manipulation/test/test_directives_tree.py line 112 at r1 (raw file):

            )
        }
        station = MakeHardwareStation(scenario, hardware=False)

nit An assert statement to test whether the station contains the wsg could be useful here.

[siddancha]

Reviewable status: 2 of 4 files reviewed, 4 unresolved discussions (waiting on @nepfaff)

---

manipulation/directives_tree.py line 41 at r1 (raw file):

Previously, nepfaff (Nicholas Pfaff) wrote…

nit I think people usually use nouns for variables and verbs for functions. However, I couldn't find this in the python guide. What do you think?
Maybe model_directives_to_add would be clearer?

Also, adding a comment that describes the variables purpose could be useful here. You have this in a later docstring but it requires some going back and fourth at the moment.

This was supposed to be a noun, to be read as "add_model directives". But thanks for catching this parsing ambiguity. Adding a comment clarifying what this variable stores.

# Dictionary of all `add_model` directives indexed by the model names.
self.add_model_directives: typing.Dict[str, ModelDirective] = dict()

---

manipulation/directives_tree.py line 191 at r1 (raw file):

Previously, nepfaff (Nicholas Pfaff) wrote…

The function name is misleading as its not doing any sorting.

Well, it is sorting in the sense that it is assigning a specific order to the unordered input set. And the order is a valid topological sorting where supporting directives always precede dependent directives. This is required by a Parser to successfully create an MBP. Although I agree that the ordering process itself is straightforward since it exploits the fact that the original flattened directives are topologically sorted.

---

manipulation/test/test_directives_tree.py line 19 at r1 (raw file):

Previously, nepfaff (Nicholas Pfaff) wrote…

Using @cache from functools would be more efficient as this is called many times.

Done.

---

manipulation/test/test_directives_tree.py line 112 at r1 (raw file):

Previously, nepfaff (Nicholas Pfaff) wrote…

nit An assert statement to test whether the station contains the wsg could be useful here.

Great suggestion! I'm now asserting the model contents of the controller plant in the station!

station: RobotDiagram = MakeHardwareStation(scenario, hardware=False)
controller_plant: MultibodyPlant = station.GetSubsystemByName(
      "iiwa_controller_plant_pointer_system"
).get()

# Check that the controller plant contains "iiwa" and "wsg" but not "table".
self.assertTrue(controller_plant.HasModelInstanceNamed("iiwa"))
self.assertTrue(controller_plant.HasModelInstanceNamed("wsg"))
self.assertFalse(controller_plant.HasModelInstanceNamed("table"))

[nepfaff]

Reviewed 1 of 1 files at r2, 1 of 1 files at r3, all commit messages.
Reviewable status: all files reviewed, 5 unresolved discussions (waiting on @siddancha)

---

manipulation/directives_tree.py line 41 at r1 (raw file):

Previously, siddancha (Siddharth Ancha) wrote…

This was supposed to be a noun, to be read as "add_model directives". But thanks for catching this parsing ambiguity. Adding a comment clarifying what this variable stores.

# Dictionary of all `add_model` directives indexed by the model names.
self.add_model_directives: typing.Dict[str, ModelDirective] = dict()

Ah, that makes sense. Thanks for adding the comment!

---

manipulation/directives_tree.py line 191 at r1 (raw file):

Previously, siddancha (Siddharth Ancha) wrote…

Well, it is sorting in the sense that it is assigning a specific order to the unordered input set. And the order is a valid topological sorting where supporting directives always precede dependent directives. This is required by a Parser to successfully create an MBP. Although I agree that the ordering process itself is straightforward since it exploits the fact that the original flattened directives are topologically sorted.

Ah, I missed that. Thanks for the clarification. All good then!

---

manipulation/directives_tree.py line 77 at r3 (raw file):

                self._AddEdge(parent_name, child_name, d)

    def _MakeNode(self, name: str):

nit Could you add a None return type. This is the default for intellisense but seems to be convention in this repo elsewhere (example).

---

manipulation/directives_tree.py line 92 at r3 (raw file):

        )

    def _AddEdge(self, parent_name: str, child_name: str, directive: ModelDirective):

nit Same as above: None return type

---

manipulation/test/test_directives_tree.py line 112 at r1 (raw file):

Previously, siddancha (Siddharth Ancha) wrote…

Great suggestion! I'm now asserting the model contents of the controller plant in the station!

station: RobotDiagram = MakeHardwareStation(scenario, hardware=False)
controller_plant: MultibodyPlant = station.GetSubsystemByName(
      "iiwa_controller_plant_pointer_system"
).get()

# Check that the controller plant contains "iiwa" and "wsg" but not "table".
self.assertTrue(controller_plant.HasModelInstanceNamed("iiwa"))
self.assertTrue(controller_plant.HasModelInstanceNamed("wsg"))
self.assertFalse(controller_plant.HasModelInstanceNamed("table"))

Perfect. Thanks!

[nepfaff]

I'd say its ready to squash + merge (only tiny nit comments left). @RussTedrake should we go ahead or do you want to have a final look?

[siddancha]

Resolved Nicholas' last nit comments (about None return type annotations).

Reviewable status: 3 of 4 files reviewed, 1 unresolved discussion (waiting on @nepfaff)

[nepfaff]

Reviewed 1 of 1 files at r4, all commit messages.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @siddancha)

[siddancha]

Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @nepfaff)

---

manipulation/directives_tree.py line 191 at r1 (raw file):

Previously, nepfaff (Nicholas Pfaff) wrote…

Ah, I missed that. Thanks for the clarification. All good then!

Done.

[siddancha]

Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @nepfaff)

[RussTedrake]

Reviewed 2 of 4 files at r1, 1 of 1 files at r3, 1 of 1 files at r4, all commit messages.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @nepfaff)

[RussTedrake]

Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @nepfaff)

---

manipulation/directives_tree.py line 191 at r1 (raw file):

Previously, siddancha (Siddharth Ancha) wrote…

Done.

Nick -- you're block on this, but clearly happy with the resolution. So I will power squash.