[parsing] Script for converting model directives into sdformat files

[marcoag]

This is to follow up on the work done at: https://github.com/aaronchongth/drake/pull/1.

This adds a script to convert model directives (.yaml) to sdformat (.sdf). It is still a work in progress with a few existing issues to be ironed out.

The script is multibody/parsing/model_directives_to_sdf.py, while a number of testing files have been included in multibody/parsing/test/convert_model_directives_test (Note: these test model directives are the same as the ones in multibody/parsing/test/process_model_directives, just that they don't use the package:// tag and all referenced assets are relative)

Build and test

For example, converting multibody/parsing/test/convert_model_directives_test/inject_frames.yaml,

git clone https://github.com/aaronchongth/drake \
    -b aaron/convert-model-directives \
    ~/drake

cd ~/drake
bazel build multibody/parsing:model_directives_to_sdf

cd ~/drake/multibody/parsing/test/convert_model_directives_test
python3 ~/drake/bazel-bin/multibody/parsing/model_directives_to_sdf \
    -m inject_frames.yaml \
    -o inject_frames.sdf

ign sdf -k inject_frames.sdf

At the moment, the output is still invalid due to some of the issues listed below, a valid output of converting inject_frams.yaml has been committed too for reference.

Outstanding issues

• [x] Conversion of implicitly named frames and it's welds.

Currently, with the model directives mechanism, users can either implicitly or explicitly determine which model instance a frame is attached to. In the implicit case, the frame will be placed under the same model instance as its base_frame. In the example below, even though we know that the frame should be considered top_level_model::top_injected_frame, we see that a weld with the parent just top_injected_frame works as well,

- add_frame:
    name: top_injected_frame
    X_PF:
      base_frame: top_level_model::base
      translation: [1, 2, 3]

- add_weld:
    parent: top_injected_frame
    child: mid_level_model::base

Sources: directives, test

This could lead to undesired behaviors when there are multiple frames with the same name present, just not in the same model instance.

Possible solution: Model directives only allow explicitly naming frames alongside their model instance,

- add_frame:
    name: top_level_model::top_injected_frame
    X_PF:
        base_frame: top_level_model::base
        translation: [1, 2 3]

- add_weld:
    parent: top_level_model::top_injected_frame
    child: mid_level_model::base

---

• [x] Fixed joints are not expecting frames to be implicitly scoped by their base_frame. (@marcoag: same as above? marking as solved in the meantime)

Possible solution: Make sure parent and child frames in add_weld are explicitly named.

---

• [x] Merge included models are currently using the //include/placement_frame and //include/pose[@relative_to] mechanism, when it should be the top level model that implements it, //model[@placement_frame] and //model/pose[@relative_to].

This can be seen in the output of converting inject_frames.yaml,

    <model name="mid_level_model">
      <frame name="mid_injected_frame">
        <pose relative_to="base">1 2 3   0 0 0</pose>
      </frame>
      <include merge="true">
        <name>mid_level_model</name>
        <uri>simple_model.sdf</uri>
        <placement_frame>base</placement_frame>
        <pose relative_to="top_injected_frame"/>
      </include>
    </model>

The correct one should be,

    <model name="mid_level_model" placement_frame="base">
      <pose relative_to="top_level_model::top_injected_frame"/>
      <frame name="mid_injected_frame">
        <pose relative_to="base">1 2 3   0 0 0</pose>
      </frame>
      <include merge="true">
        <name>mid_level_model</name>
        <uri>simple_model.sdf</uri>
      </include>
    </model>

---

• [x] Support add_directive in the script, the implementation is incomplete at the moment.

---

This change is 

[EricCousineau-TRI]

@marcoag I think this is probably still a draft? Assuming so, I've marked this do not review currently.

For traceability, I'm also assigning Addisu and myself for review (once it is ready).

[jwnimmer-tri]

One thought from me -- totally not a blocker for this PR, but something to consider as you work on this idea over time...

As phrased now, this is a command-line tool only, with global variables. Ideally, I would like to have pydrake offer this features as a part of a library module. That would mean refactoring this logic to live in a class. (We'd still have a command-line tool that can run the class, but the actual conversion logic would all be modular.)

Factoring into a class might also be easier for unit testing. If the only entry point is main(), your unit testing will need to deal with a lot of manual file shuffling. With library API, it might be easier to test.

[EricCousineau-TRI]

Sounds good to me - thank you for the suggestion!

[marcoag]

It seems that the current script (https://github.com/RobotLocomotion/drake/pull/17223/commits/c3a34fb80b54581e19e1be55dc4b5dc137e537fe) generates the valid output stated above.

To confirm, I printed the Bodies, Frames and Joints for each ModelInstance for a MultibodyPlant loaded through LoadModelDirectives and another for one that loads the sdf string generated through the convert_directive function, this is the result:

Using LoadModelDirectives:

Model Instance:  WorldModelInstance
Body: WorldBody
Frame: WorldBody
Model Instance:  DefaultModelInstance
Model Instance:  top_level_model
Body: base
Frame: base
Frame: __model__
Frame: frame
Frame: top_injected_frame
Model Instance:  mid_level_model
Body: base
Frame: base
Frame: __model__
Frame: frame
Frame: mid_injected_frame
Joint: top_injected_frame_welds_to_base
Model Instance:  bottom_level_model
Body: base
Frame: base
Frame: __model__
Frame: frame
Joint: mid_injected_frame_welds_to_base

Using convert_directive and AddModelFromString:

Model Instance:  WorldModelInstance
Body: WorldBody
Frame: WorldBody
Model Instance:  DefaultModelInstance
Model Instance:  inject_frames
Frame: __model__
Model Instance:  inject_frames::top_level_model
Body: base
Frame: base
Frame: __model__
Frame: top_injected_frame
Frame: _merged__top_level_model__model__
Frame: frame
Frame: 
Model Instance:  inject_frames::mid_level_model
Body: base
Frame: base
Frame: __model__
Frame: mid_injected_frame
Frame: _merged__mid_level_model__model__
Frame: frame
Frame: 
Frame: 
Joint: top_level_model__top_injected_frame___to___mid_level_model__base___weld_joint
Model Instance:  inject_frames::bottom_level_model
Body: base
Frame: base
Frame: __model__
Frame: frame
Frame: 
Joint: mid_level_model__mid_injected_frame___to___bottom_level_model__base___weld_joint

Biggest differences I noted:

• There is a top level model that gets added in the conversion process with the name of the file (inject_frames). I assume this was added in order to be able to add the the corresponding joints. This also changes other model names which could affect code that references them by name (and of course by instance number), is this a concern?
• There's a bunch of extra Frames, some created by the merges and some others that show an empty name. I'm not sure if this is an issue that should be considered or we are ok with them.

Thoughts? Any other issues on the output we should consider?

[jwnimmer-tri]

@drake-jenkins-bot ok to test

[azeey]

multibody/parsing/model_directives_to_sdformat.py line 278 at r17 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Sorry, I'm not sure if I understand the technical limitation - why can we only process one level of nesting? (is that an effective "TODO"?)

I think we would run into a problem when trying to weld that deeply nested model to something else because you'll need to add a placement_frame for the nested model. But you can only add placement_frame in an include.

Here's one example:

directives:

- add_model:
    name: top_level_model
    file: package://sdf_parser_test/model_with_directly_nested_models.sdf

- add_model:
    name: tool_model
    file: package://process_model_directives_test/simple_model.sdf

- add_weld:
    parent: tool_model::base
    child: top_level_model::parent_model::robot1::base_link

Another issue is when we try to inject a frame into a deeply nested model. Merge-including the deeply nested model would not be enough to be able to put the <frame> in the right model:

directives:

- add_model:
    name: top_level_model
    file: package://sdf_parser_test/model_with_directly_nested_models.sdf

- add_frame:
    name: top_injected_frame
    X_PF:
      base_frame: top_level_model::parent_model::robot1::tool_frame
      translation: [1, 2, 3]

- add_model:
    name: tool_model
    file: package://process_model_directives_test/simple_model.sdf

- add_weld:
    parent: top_level_model::parent_model::robot1::tool_frame
    child: tool_model::base

[azeey]

multibody/parsing/model_directives_to_sdformat.py line 278 at r17 (raw file):

Yeah, that's what I thought but then I tested the sdf and it wasn't throwing any error, so I wasn't sure. Shouldn't it complain it's not able to find top_level_model::nested_model::base?

No, the script doesn't look inside an included model and so won't check if the link base actually exists.

[azeey]

multibody/parsing/model_directives_to_sdformat.py line 278 at r17 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

Per Slack, is it possible to provide a link to a commit w/ command to reproduce the failure? I'd be fine with something like:

# TODO(eric.cousineau): See if there is a better structure for converting across multiple layers of nesting.
# See failure case encoding by <link to commit w/ command to run>

I've pushed some code in the convert-model-directives branch of https://github.com/azeey/drake/

After checking it out, run

bazel run //multibody/parsing:py/model_directives_to_sdformat_test -- "TestConvertModelDirectiveToSDF.test_list_frames" multibody/parsing/test/model_directives_to_sdformat_files/test_deeply_nested.dmd.yaml

The test simply lists all the frames found in an MbP. That command currently fails with

E
======================================================================
ERROR [0.375s]: test_list_frames (model_directives_to_sdformat_test.TestConvertModelDirectiveToSDF)
----------------------------------------------------------------------
Traceback (most recent call last):
  File "~/.cache/bazel/_bazel_addisuzt/f644823b694186bfa32d01d1532653cf/execroot/drake/bazel-out/k8-dbg/bin/multibody/parsing/py/model_directives_to_sdformat_test.runfiles/drake/multibody/parsing/test/model_directives_to_sdformat_test.py", line 417, in test_list_frames
    ProcessModelDirectives(directives=directives,
SystemExit: Failure at multibody/parsing/detail_dmd_parser.cc:48 in AddWeld(): condition 'found' failed.

However, swapping the parent and child of the weld, it succeeds with

WorldModelInstance::world: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
top_level_model::parent_model::robot1::base_link: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
top_level_model::parent_model::robot1::moving_link: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
top_level_model::parent_model::robot1::__model__: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
top_level_model::parent_model::robot1::slider_child: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
top_level_model::parent_model::robot2::base_link: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
top_level_model::parent_model::robot2::moving_link: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
top_level_model::parent_model::robot2::__model__: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
top_level_model::parent_model::robot2::slider_child: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
top_level_model::parent_model::__model__: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
top_level_model::parent_model::robot2::weld_robots_child: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
top_level_model::__model__: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
tool_model::base: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
tool_model::__model__: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[0. 0. 0.]
tool_model::frame: RollPitchYaw(roll=0.0, pitch=-0.0, yaw=0.0) xyz=[1. 2. 3.]
.
----------------------------------------------------------------------
Ran 1 test in 0.097s

OK

[jwnimmer-tri]

I think there is another DMD feature not yet supported in this PR. See https://github.com/RobotLocomotion/drake/issues/13065#issuecomment-1273843047 for details or #17802 for the DMD feature.

[marcoag]

I think there is another DMD feature not yet supported in this PR. See #13065 (comment) for details or #17802 for the DMD feature.

As per DM with @EricCousineau-TRI: added a temporal fail fast for this and we should address on a follow up PR.

[jwnimmer-tri]

I am surveying the list of stalled-out pull requests, and came across this one.

To get this unstuck, I wonder if we could land an incomplete version of the tool on master, where sometimes it throws an exception if it can't handle certain kinds of input yet. The longer this sits as an unmerged PR, the more it will bitrot, and the more CI resources it will chew up.

[azeey]

I am surveying the list of stalled-out pull requests, and came across this one.

To get this unstuck, I wonder if we could land an incomplete version of the tool on master, where sometimes it throws an exception if it can't handle certain kinds of input yet. The longer this sits as an unmerged PR, the more it will bitrot, and the more CI resources it will chew up.

Yes, it would be great if this was merged. I'm not sure if we have any outstanding issues, but it looks like there's a new CI failure. Looks like lxml was dropped in https://github.com/RobotLocomotion/drake/pull/19027. @marcoag can you make similar changes as that PR here to avoid using lxml?

[jwnimmer-tri]

I've started reviewing. There is a lot of code here that is far enough from being satisfactory that I'll need to provide patches as the feedback mechanism, instead of review discussion comments.

Would you prefer that I open PR(s) against this branch so you guys can check those over before it goes in, or should I just (non-force) push patches to this branch directly?

[jwnimmer-tri]

Thanks! For now, I think doing meta-PR(s) would be best.

https://reviewable.io/reviews/marcoag/drake/4 to kick things off (my checkpoint for today). I can't seem to assign reviewers though, so I'm mentioning it here as well.

[jwnimmer-tri]

@EricCousineau-TRI / @marcoag do you still plan to try to finish this pull request? Should I try to finish it, instead?

See also #20122, which adds a new directive, so might need more conversion code added here also.

[marcoag]

@jwnimmer-tri I have no more cycles associated to TRI anymore, it would be great if you can finish whatever is needed to merge this.

[EricCousineau-TRI]

I have plans to finish it off, but may take some time before I get there (handling other tasks).

Should I try to finish it, instead?

If you can that would be excellent. If not, I will pick it back up (most likely in a couple of month's time).