Add contact visualisation to planar scene_graph visualizer

[kamiradi]

This PR addresses #13214

It extends the PlanarSceneGraphVisualizer matplotlib visualisation functionality to add contact information, PlanarSceneGraphContactVisualizer.

Possible points of improvement,

• Have one class for both scene_graph and contact_results visualization, and give the user the option to select if they want to visualise contact. This is as opposed to having PlanarSceneGraphVisualizer and PlanarSceneGraphContactVisualizer.
• At the moment, only 10 contact force lines are drawn by the matplotlib plot. I haven't found a way to dynamically register artists and update their data for each frame. This small addition can then allow all contact forces to be shown. In my experience using it, I work in an experimental setting where not more than 10 c ontacts need to be rendered. Althought, this may be needful for more contact-rich settings
• Ability to render contact forces between specified bodies. This would be handy if you want to visualise only a subset of contact forces.

Let me know what you think. Looking forward to your feedback.

---

This change is 

[jwnimmer-tri]

@drake-jenkins-bot ok to test

[SeanCurtis-TRI]

+@bernhardpg I'll defer to you if you want to do the feature review (as per your volunteering in the issue).

[kamiradi]

Hi, Indeed, I would say that most of the code is duplicated into the new class. I did initially think of extending PlanarSceneGraphVisualizer itself, however I thought from a users perspective giving a separate scene_graph and contact+scene_graph class would be beneficial. I'll do the relevant modifications. Thanks for your feedback @bernhardpg @SeanCurtis-TRI.

[kamiradi]

Hi @RussTedrake @bernhardpg @SeanCurtis-TRI,

I have made changes to the existing PlanarSceneGraphVisualizer and removed the PlanarSceneGraphContactVisualizer as per the comments above. I am running some sanity check tests on these modifications against the new MakeHardwareStation as used in hybrid_force_motion and have been facing a few issues.

Is there a change in the way contact results are being returned from the hardware station, as instantiated in the MakeHardwareStation function? I see that there is a prefinalize_parser_callback pointing to the Setup function, is this changing the contacts reported from the plant? It looks like when I evaluate the contact results coming in from the plant, even when the robot makes contact with the environment there is nothing populated in the point_pair_contact_info object or the hydroelastic_contact_info object. Am I instantiating the MakeHardwareStation incorrectly here?

Look forward to your feedback

[RussTedrake]

MakeHardwareStation should simply be exporting the contact results output port; it doesn't make any changes. In case it's useful, I also have this contact_inspector notebook; one could potentially add a planar scenegraph visualizer there (and compare the visualizations/recordings with Meshcat's).

[kamiradi]

MakeHardwareStation should simply be exporting the contact results output port; it doesn't make any changes. In case it's useful, I also have this contact_inspector notebook; one could potentially add a planar scenegraph visualizer there (and compare the visualizations/recordings with Meshcat's).

On further investigation, I see that the contacts reported back from the MakerHardwareStation are purely hydroelastic (which makes sense). This was helpful as I was not differentiating between hydroelastic and point_contact. So it helped catching the fact that I was completly missing visualizing Hydroelastic contact.

However, a new error when trying to visualise the hydroelastic contact is that quadrature data is not populated even though the num_hydroelastic_contacts is returning a single contact (the book lying on the table). Either that, or I am missing something in the way Im gathering hydroelastic_info from contact_results.

Cell In[23], line 437, in PlanarSceneGraphVisualizer.draw(self, context)
    435 for id in range(contact_results.num_hydroelastic_contacts()):
    436     contact_info = contact_results.hydroelastic_contact_info(id)
--> 437     qp_data = contact_info.quadrature_point_data()
    438     spatial_force = contact_info.SpatialForce()
    439     points.append(qp_data.p_WQ)

AttributeError: 'pydrake.multibody.plant.HydroelasticContactInfo' object has no attribute 'quadrature_point_data'

I'll give the contact_inspector a shot, thanks for the suggestion.

[kamiradi]

On the python side, you won't be able to handle the quadrature point data. It hasn't been bound. Only contact_surface() and F_Ac_W have been bound. On the other hand, we generally don't visualize that data in the meshcat contact visualizer either. Its greatest value was in debugging the initial implementation, but seeing a see of points, tractions, tangential velocities, and the like, doesn't provide much helpful feedback.

Reviewable status: LGTM missing from assignees SeanCurtis-TRI(platform),bernhardpg, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @kamiradi)

Ah I see, then i guess i can extract the c in F_Ac_W as the contact point in the world frame visualize the magnitude of the force on that point. How can I extract that point? Can you point me in the direction of where this alreadty done?

[SeanCurtis-TRI]

The C++ code provides some hints to that (check out the links below).

• In contact_visualizer.cc, we store the contact surface centroid.
• In hydroelastic_contact_visualizer.cc that quantity is used to position the contact. I'd interpret that the point at which the contact force is applied.

[kamiradi]

@SeanCurtis-TRI Is there a way that I can just run the python unittests? I tired looking for this in the developer docs but it points me towards the running the bazel python bindings tests. How do I discover the planar_scenegraph_visualizer_test.py?

[SeanCurtis-TRI]

The spelling you're looking for is:

bazel test //bindings/pydrake/systems:py/planar_scenegraph_visualizer_test

Note how the .py at the end has become a py/ at the beginning of the test name.

TO check for lint, do:

//bindings/pydrake/systems:py/planar_scenegraph_visualizer_py_pycodestyle

[kamiradi]

@SeanCurtis-TRI Thanks for this. I am running into another problem, when trying to run the test and building. It looks like the build process uses a lot of memory and RAM, and the whole system gets stuck, and slows down. My system specifications are as follows:

1. Building inside a docker container
2. RAM - 16gb
3. Processor: Intel Core i5-8365U CPU @ 1.6-GHz x 8
4. 64 bit

How do I get Drake to build on a laptop?

[SeanCurtis-TRI]

It sounds like you need to throttle the number of threads of build you have.

Here are your options:

1. Add the -j Nargument to a particular invocation (e.g., bazel test -j 2 //bindings/pydrake/systems:py/planar_scenegraph_visualizer_test to limit it to two threads.
2. Edit your ~/.bazrlc file to include the line build -j 2 to limit builds to two threads, system wide.
3. Edit your ~/.bazrlc file to include the line build -j HOST_CPUS*0.5 to limit builds to half of your available CPUs, system wide.

[SeanCurtis-TRI]

Sorry. that had a typo. it should be the ~/.bazelrc file.

[kamiradi]

Thanks for this @SeanCurtis-TRI. I tried with the steps you mentioned but ran into the same issue. As an alternative, do I need to build the repository to just perform the planar_scenegraph_visualizer_test? Can I somehow side step the build by testing against pre-built binaries (I guess not but wanted to see if I am missing something)?

[SeanCurtis-TRI]

Sadly, anything that depends on pydrake has to build all of Drake. If your machine isn't up to it, you can take the slower step of relying on CI to do the test for you.

That said, if you can get through the full build once, successive changes to the test or to planar_scenegraph_visualizer.py should not require further builds. So, you just have to let it grind through one long build.

[kamiradi]

@SeanCurtis-TRI Thank you for your feedback. Turns out once I build it on a slightly beefier machine, the build goes through (along with some limits on CPU and memory usage on docker's side). I am currently testing on the existing set of planar_scenegraph_visualizer_tests. I have modified the class such that the existing API for this class does not break. However, it seems that the test_mesh_file_parsing test fails with the following message. Any hints on where I could be going wrong here

Running tests...
----------------------------------------------------------------------
...E.
======================================================================
ERROR [0.047s]: test_mesh_file_parsing (planar_scenegraph_visualizer_test.TestPlanarSceneGraphVisualizer)
This test ensures we can load obj files or provide a reasonable error
----------------------------------------------------------------------
Traceback (most recent call last):
  File "/tmp/bazel-working-directory/drake/bazel-out/k8-opt/bin/bindings/pydrake/systems/py/planar_scenegraph_visualizer_test.runfiles/drake/bindings/pydrake/systems/test/planar_scenegraph_visualizer_test.py", line 167, in test_mesh_file_parsing
    PlanarSceneGraphVisualizer(scene_graph)
  File "/tmp/bazel-working-directory/drake/bazel-out/k8-opt/bin/bindings/pydrake/systems/py/planar_scenegraph_visualizer_test.runfiles/drake/bindings/pydrake/systems/planar_scenegraph_visualizer.py", line 143, in __init__
    self._build_body_patches(use_random_colors,
  File "/tmp/bazel-working-directory/drake/bazel-out/k8-opt/bin/bindings/pydrake/systems/py/planar_scenegraph_visualizer_test.runfiles/drake/bindings/pydrake/systems/planar_scenegraph_visualizer.py", line 285, in _build_body_patches
    raise RuntimeError(
RuntimeError: The given file /tmp/bazel-working-directory/drake/bazel-out/k8-opt/bin/bindings/pydrake/systems/py/planar_scenegraph_visualizer_test.runfiles/drake_models/iiwa_description/meshes/iiwa14/visual/link_0.STL is not supported and no alternate /tmp/bazel-working-directory/drake/bazel-out/k8-opt/bin/bindings/pydrake/systems/py/planar_scenegraph_visualizer_test.runfiles/drake_models/iiwa_description/meshes/iiwa14/visual/link_0.obj could be found.

It looks like it is not able to find a relevant .stl file

[kamiradi]

Ah I see, thanks for pointing that out. Then I guess I have one follow up meta question. What are the best practices when you are merging from the master to update all changes to your development branch (in this case 2dcontact). Mostly in the case that something like this happens.

This is more of a noob open source contribution question so that I can get my habits in order. Thanks in advance.

[SeanCurtis-TRI]

There are competing views about workflow. I can tell you my workflow and why I do it the way I do and you can decide if you like. Both workviews require addressing merge conflicts (which was undoubtedly the source of your problems here).

I almost always work with a curated branch. What does that mean? Git makes commits cheap. So, it's easy to start a branch and commit, commit, commit. There's nothing wrong with that. The challenge is when you master moves and you need to reconcile your branch with master. You can attempt to rebase your branch onto master, or you can merge master. When you have a long commit chain, it's generally easier to merge master into your branch. This, of course, extends your commit chain, often making it difficult to fully understand what in the chain you've changed.

What I (generally) do is have my branch consist of a single commit, always. When I make further changes on an existing branch, I amend commit the changes into the existing commit. This requires force pushing it to my remote fork repository. But it has the benefit that all of my branch's changes exist in a single commit and I can look at the contents of that commit and understand exactly what I've changed. With this single commit, if master changes, I can rebase the single commit on top of master. I really like the mental model of having my branch "floating on top" of master. It also makes it easier for reviewers to check out my branch and see the change in a single package. (And it means its ready to merge as is, as Drake requires these kind of curated commits for merging into master.)

The worst work flow would be to have a long commit chain and rebase. The reason for this is that there may be no conflicts between master and the final state of your branch, but there may be conflicts based on changes made in intermediate commits. Things you tried and then subsequently changed. So, don't do this. :)