Support mobile robots in MakeMultibodyPlant

[nepfaff]

Currently, MakeMultibodyPlant assumes that all passed model_instance_names have a connection to the world frame. This is not the case for mobile robots.

A minimal example of what currently doesn't work:

from manipulation.station import LoadScenario, MakeMultibodyPlant

scenario_data = """
directives:
- add_model:
    name: mobile_iiwa
    file: package://manipulation/mobile_iiwa14_primitive_collision.urdf
    default_joint_positions:
        iiwa_joint_1: [-1.57]
        iiwa_joint_2: [0.1]
        iiwa_joint_3: [0]
        iiwa_joint_4: [-1.2]
        iiwa_joint_5: [0]
        iiwa_joint_6: [ 1.6]
        iiwa_joint_7: [0]
model_drivers:
    mobile_iiwa: !InverseDynamicsDriver {}
"""

scenario = LoadScenario(data=scenario_data)

controller_plant = MakeMultibodyPlant(scenario, model_instance_names=["mobile_iiwa"])
print("num controller_plant positions:", controller_plant.num_positions())

The reason is that _PopulatePlantOrDiagram uses GetDirectivesFromRootToModels to get the directives:

if not model_instance_names is None:
        tree = DirectivesTree(flattened_directives)
        directives = tree.GetDirectivesFromRootToModels(model_instance_names)

, but GetDirectivesFromRootToModels gets all directives that would connect model_instance_names to the world frame.

I added a quick fix that I don't think should have any side effects.

---

This change is 

[nepfaff]

+@siddancha for review please (if you have time 😊)

[nepfaff]

I now also added support for a single floating cluster.

This is very naive at the moment but I didn't see a way of solving the general case (multiple floating clusters/ multiple mobile robots) cleanly. We would probably need to add a directives graph + use graph search. Am I missing something?

For now, I'd say supporting a single mobile robot seems like a strict improvement.

Update: Taking this back. It works for multiple floating clusters.

[siddancha]

@nepfaff While this is a valid solution, it's a bit complex. There is a simpler solution to handle mobile robots not welded to the world frame. Currently in GetDirectivesFromWorldToRoot(), we perform downward traversals in the directives tree from the "world" root node to the model instances. Instead, we can perform upward traversals from the model instance nodes to some root node. The root node will not be the "world" node in case of mobile robots.

This solution avoids the need to compute directives between two models or within a model cluster. Furthermore, it actually simplifies the previous version of the code while automatically handling the new use case.

I have implemented this on top of your PR here: https://github.com/siddancha/manipulation/tree/support-mobile-robots-in-directives-tree . It passes all tests, including the new ones you wrote. If this looks good to you, you can merge that commit into this PR.

Reviewable status: 0 of 4 files reviewed, all discussions resolved

[siddancha]

Added a couple of minor suggestions in the test file.

Reviewed 2 of 4 files at r2, 2 of 2 files at r3, all commit messages.
Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @nepfaff)

---

manipulation/test/test_directives_tree.py line 113 at r3 (raw file):

        return directives

The order of declaring the directives is no longer topological, which may be confusing to the reader. What about rearranging these directives like:

class DirectivesTreeTest(unittest.TestCase):
    @cache
    def get_flattened_directives(
        self, mobile_iiwa: bool = False
    ) -> typing.List[ModelDirective]:
      # Directive to add iiwa.
      directives = [
            ModelDirective(
                add_model=AddModel(
                    name="iiwa",
                    file="package://manipulation/mobile_iiwa14_primitive_collision.urdf",
                )
            )
        ]
        if not mobile_iiwa:
            # Directives to weld iiwa to the world frame.
            directives += [
                ModelDirective(
                    add_frame=AddFrame(
                        name="iiwa_origin",
                        X_PF=Transform(
                            base_frame="world",
                            translation=[0, 0.765, 0],
                        ),
                    )
                ),
                ModelDirective(
                    add_weld=AddWeld(
                        parent="iiwa_origin",
                        child="iiwa::base",
                    )
                ),
            ]
        # Directives to add and weld wsg and table.
        directives += [
            ModelDirective(
                add_model=AddModel(
                    name="wsg",
                    file="package://drake_models/wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf",
                )
            ),
            ModelDirective(
                add_frame=AddFrame(
                    name="iiwa::wsg_attach",
                    X_PF=Transform(
                        base_frame="iiwa::iiwa_link_7",
                        translation=[0, 0, 0.114],
                        rotation=Rotation.Rpy(deg=[90.0, 0.0, 68.0]),
                    ),
                )
            ),
            ModelDirective(
                add_weld=AddWeld(
                    parent="iiwa::wsg_attach",
                    child="wsg::body",
                )
            ),
            ModelDirective(
                add_model=AddModel(
                    name="table",
                    file="package://drake_models/manipulation_station/table_wide.sdf",
                )
            ),
            ModelDirective(
                add_frame=AddFrame(
                    name="table_origin",
                    X_PF=Transform(
                        base_frame="world",
                        translation=[0.4, 0.3825, 0.0],
                        rotation=Rotation.Rpy(deg=[0.0, 0.0, 0.0]),
                    ),
                )
            ),
            ModelDirective(
                add_weld=AddWeld(
                    parent="table_origin",
                    child="table::table_body",
                )
            ),
        ]
        return directives

---

manipulation/test/test_directives_tree.py line 190 at r3 (raw file):

        self.assertEqual(plant.num_positions(), num_iiwa_positions)

    def test_make_multibody_plant(self):

Maybe rename this function to test_make_multibody_plant_welded_iiwa()?

[siddancha]

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

---

manipulation/test/test_directives_tree.py line 113 at r3 (raw file):

Previously, siddancha (Siddharth Ancha) wrote…

The order of declaring the directives is no longer topological, which may be confusing to the reader. What about rearranging these directives like:

class DirectivesTreeTest(unittest.TestCase):
    @cache
    def get_flattened_directives(
        self, mobile_iiwa: bool = False
    ) -> typing.List[ModelDirective]:
      # Directive to add iiwa.
      directives = [
            ModelDirective(
                add_model=AddModel(
                    name="iiwa",
                    file="package://manipulation/mobile_iiwa14_primitive_collision.urdf",
                )
            )
        ]
        if not mobile_iiwa:
            # Directives to weld iiwa to the world frame.
            directives += [
                ModelDirective(
                    add_frame=AddFrame(
                        name="iiwa_origin",
                        X_PF=Transform(
                            base_frame="world",
                            translation=[0, 0.765, 0],
                        ),
                    )
                ),
                ModelDirective(
                    add_weld=AddWeld(
                        parent="iiwa_origin",
                        child="iiwa::base",
                    )
                ),
            ]
        # Directives to add and weld wsg and table.
        directives += [
            ModelDirective(
                add_model=AddModel(
                    name="wsg",
                    file="package://drake_models/wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf",
                )
            ),
            ModelDirective(
                add_frame=AddFrame(
                    name="iiwa::wsg_attach",
                    X_PF=Transform(
                        base_frame="iiwa::iiwa_link_7",
                        translation=[0, 0, 0.114],
                        rotation=Rotation.Rpy(deg=[90.0, 0.0, 68.0]),
                    ),
                )
            ),
            ModelDirective(
                add_weld=AddWeld(
                    parent="iiwa::wsg_attach",
                    child="wsg::body",
                )
            ),
            ModelDirective(
                add_model=AddModel(
                    name="table",
                    file="package://drake_models/manipulation_station/table_wide.sdf",
                )
            ),
            ModelDirective(
                add_frame=AddFrame(
                    name="table_origin",
                    X_PF=Transform(
                        base_frame="world",
                        translation=[0.4, 0.3825, 0.0],
                        rotation=Rotation.Rpy(deg=[0.0, 0.0, 0.0]),
                    ),
                )
            ),
            ModelDirective(
                add_weld=AddWeld(
                    parent="table_origin",
                    child="table::table_body",
                )
            ),
        ]
        return directives

Sorry I didn't realize the IIWA models were different. Let me modify the above suggestion to:

class DirectivesTreeTest(unittest.TestCase):
    @cache
    def get_flattened_directives(
        self, mobile_iiwa: bool = False
    ) -> typing.List[ModelDirective]:
        if mobile_iiwa:
            # Add mobile iiwa.
            directives = [
                ModelDirective(
                    add_model=AddModel(
                        name="iiwa",
                        file="package://manipulation/mobile_iiwa14_primitive_collision.urdf",
                    )
                )
            ]
        else:
            # Add iiwa and weld it to the world frame.
            directives = [
                ModelDirective(
                    add_model=AddModel(
                        name="iiwa",
                        file="package://drake_models/iiwa_description/urdf/iiwa14_no_collision.urdf",
                    )
                ),
                ModelDirective(
                    add_frame=AddFrame(
                        name="iiwa_origin",
                        X_PF=Transform(
                            base_frame="world",
                            translation=[0, 0.765, 0],
                        ),
                    )
                ),
                ModelDirective(
                    add_weld=AddWeld(
                        parent="iiwa_origin",
                        child="iiwa::base",
                    )
                ),
            ]
        # Add and weld wsg + table.
        directives += [
            ModelDirective(
                add_model=AddModel(
                    name="wsg",
                    file="package://drake_models/wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf",
                )
            ),
            ModelDirective(
                add_frame=AddFrame(
                    name="iiwa::wsg_attach",
                    X_PF=Transform(
                        base_frame="iiwa::iiwa_link_7",
                        translation=[0, 0, 0.114],
                        rotation=Rotation.Rpy(deg=[90.0, 0.0, 68.0]),
                    ),
                )
            ),
            ModelDirective(
                add_weld=AddWeld(
                    parent="iiwa::wsg_attach",
                    child="wsg::body",
                )
            ),
            ModelDirective(
                add_model=AddModel(
                    name="table",
                    file="package://drake_models/manipulation_station/table_wide.sdf",
                )
            ),
            ModelDirective(
                add_frame=AddFrame(
                    name="table_origin",
                    X_PF=Transform(
                        base_frame="world",
                        translation=[0.4, 0.3825, 0.0],
                        rotation=Rotation.Rpy(deg=[0.0, 0.0, 0.0]),
                    ),
                )
            ),
            ModelDirective(
                add_weld=AddWeld(
                    parent="table_origin",
                    child="table::table_body",
                )
            ),
        ]
        return directives

[nepfaff]

That is indeed much smoother. Thank you for implementing this!

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

---

manipulation/test/test_directives_tree.py line 113 at r3 (raw file):

Previously, siddancha (Siddharth Ancha) wrote…

Sorry I didn't realize the IIWA models were different. Let me modify the above suggestion to:

class DirectivesTreeTest(unittest.TestCase):
    @cache
    def get_flattened_directives(
        self, mobile_iiwa: bool = False
    ) -> typing.List[ModelDirective]:
        if mobile_iiwa:
            # Add mobile iiwa.
            directives = [
                ModelDirective(
                    add_model=AddModel(
                        name="iiwa",
                        file="package://manipulation/mobile_iiwa14_primitive_collision.urdf",
                    )
                )
            ]
        else:
            # Add iiwa and weld it to the world frame.
            directives = [
                ModelDirective(
                    add_model=AddModel(
                        name="iiwa",
                        file="package://drake_models/iiwa_description/urdf/iiwa14_no_collision.urdf",
                    )
                ),
                ModelDirective(
                    add_frame=AddFrame(
                        name="iiwa_origin",
                        X_PF=Transform(
                            base_frame="world",
                            translation=[0, 0.765, 0],
                        ),
                    )
                ),
                ModelDirective(
                    add_weld=AddWeld(
                        parent="iiwa_origin",
                        child="iiwa::base",
                    )
                ),
            ]
        # Add and weld wsg + table.
        directives += [
            ModelDirective(
                add_model=AddModel(
                    name="wsg",
                    file="package://drake_models/wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf",
                )
            ),
            ModelDirective(
                add_frame=AddFrame(
                    name="iiwa::wsg_attach",
                    X_PF=Transform(
                        base_frame="iiwa::iiwa_link_7",
                        translation=[0, 0, 0.114],
                        rotation=Rotation.Rpy(deg=[90.0, 0.0, 68.0]),
                    ),
                )
            ),
            ModelDirective(
                add_weld=AddWeld(
                    parent="iiwa::wsg_attach",
                    child="wsg::body",
                )
            ),
            ModelDirective(
                add_model=AddModel(
                    name="table",
                    file="package://drake_models/manipulation_station/table_wide.sdf",
                )
            ),
            ModelDirective(
                add_frame=AddFrame(
                    name="table_origin",
                    X_PF=Transform(
                        base_frame="world",
                        translation=[0.4, 0.3825, 0.0],
                        rotation=Rotation.Rpy(deg=[0.0, 0.0, 0.0]),
                    ),
                )
            ),
            ModelDirective(
                add_weld=AddWeld(
                    parent="table_origin",
                    child="table::table_body",
                )
            ),
        ]
        return directives

Done.

---

manipulation/test/test_directives_tree.py line 190 at r3 (raw file):

Previously, siddancha (Siddharth Ancha) wrote…

Maybe rename this function to test_make_multibody_plant_welded_iiwa()?

Done.

[siddancha]

Looks great, thanks! I think this is ready to merge!

Reviewed 3 of 3 files at r4, all commit messages.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @nepfaff)