Visualize robot positions in HardwareStationInterface

[siddancha]

Problem

Slack thread: https://drakedevelopers.slack.com/archives/C2CK4CWE7/p1720146312718089

When using MakeHardwareStation(meshcat, hardware=True) , _MakeHardwareStationInterface(meshcat) completely ignores its meschat. However, a strong use-case when running real robot hardware is to run Meshcat and visualize robot positions.

It seems that a (partial) solution was previously implemented, but was commented out.

Solution

Philosophy: When the user passes a meshcat instance when running on real hardware, we assume that the user wants to visualize robot positions using this instance. In this case, the relevant systems are added to the station and wired appropriately (see below). When meshcat=None, these are not added.

• Un-commented the partial solution. The partial solution adds a SceneGraph, MultibodyPositionToGeometryPose and runs ApplyVisualizationConfig(). However, MultibodyPositionToGeometryPose is not wired.
• Wired output positions from IiwaStatusReceiver and/or SchunkWsgStatusReceiver to the MultibodyPositionToGeometryPose. This allows the user to successfully visualize the plant in Meshcat.
• Added a test to check that MakeHardwareStation(hardware=True) works with and without meshcat.

System diagrams

The following system diagrams were generated from manipulation/test/test_hardware_station_interface.py:

HardwareStationInterface when meshcat=None

HardwareStationInterface when a meshcat instance is passed by the user

---

This change is 

[RussTedrake]

I don't by the documented argument for nesting the class. But otherwise

Thank you for pushing these upstream (and writing beautiful code).

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

---

manipulation/station.py line 1255 at r1 (raw file):

    # We define the class inside the function because the number of input ports
    # depends on the number of model instances.

why does that necessitate being inside a function? you can just pass the necessary arguments into the constructor?

---

manipulation/station.py line 1398 at r1 (raw file):

            config,
            builder=builder,
            plant=plant,

btw -- it's too bad that we have to pass the plant in here. for this visualization config, it think it should not be needed. but according to the drake docs, I agree with you that it's necessary.

fyi @jwnimmer-tri.
(in the implementation of ApplyVisualizationConfigImpl, plant it only used for publishing contact results and inertias).

[jwnimmer-tri]

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

---

manipulation/station.py line 1398 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

btw -- it's too bad that we have to pass the plant in here. for this visualization config, it think it should not be needed. but according to the drake docs, I agree with you that it's necessary.

fyi @jwnimmer-tri.
(in the implementation of ApplyVisualizationConfigImpl, plant it only used for publishing contact results and inertias).

If the only thing you're publishing is illustration, then you could choose to call MeshcatVisualizer.AddToBuilder instead of ApplyVisualizationConfig. In that design, the plant reference is not necessary.

ApplyVisualizationConfig is syntactic sugar for common use cases. It's OK to set it aside if it doesn't meet the need, especially when you're not actually providing your users access to the config object to tweak anyway.

[siddancha]

Reviewable status: 3 of 4 files reviewed, 2 unresolved discussions (waiting on @jwnimmer-tri and @RussTedrake)

---

manipulation/station.py line 1255 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

why does that necessitate being inside a function? you can just pass the necessary arguments into the constructor?

My bad -- you're right! That comment was incorrect and I've now removed it.

Would you prefer having ConcatenatePositions defined outside the function with constructor arguments? Here's what it would look like: https://github.com/siddancha/manipulation/blob/9b18aa4226fbbeb1570267678e85b14f0b545c8d/manipulation/station.py#L1247-L1302

---

manipulation/station.py line 1398 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

If the only thing you're publishing is illustration, then you could choose to call MeshcatVisualizer.AddToBuilder instead of ApplyVisualizationConfig. In that design, the plant reference is not necessary.

ApplyVisualizationConfig is syntactic sugar for common use cases. It's OK to set it aside if it doesn't meet the need, especially when you're not actually providing your users access to the config object to tweak anyway.

Makes sense! I can confirm that I've tried using MeshcatVisualizer instead of ApplyVisualizationConfig and it works well for me! What this doesn't do is add DrakeVisualizer systems that I don't need anyway.

[siddancha]

I moved ConcatenateVectors outside the function with constructor args, and followed Jeremy's suggestion to replace ApplyVisualizationConfig() with MeshcatVisualizer.AddToBuilder() (which slightly simplifies the diagram).

I think we're ready to merge.

Reviewable status: 2 of 4 files reviewed, 2 unresolved discussions (waiting on @jwnimmer-tri and @RussTedrake)

---

manipulation/station.py line 1255 at r1 (raw file):

Previously, siddancha (Siddharth Ancha) wrote…

My bad -- you're right! That comment was incorrect and I've now removed it.

Would you prefer having ConcatenatePositions defined outside the function with constructor arguments? Here's what it would look like: https://github.com/siddancha/manipulation/blob/9b18aa4226fbbeb1570267678e85b14f0b545c8d/manipulation/station.py#L1247-L1302

Moved ConcatenateVectors outside the function with constructor args.

---

manipulation/station.py line 1398 at r1 (raw file):

Previously, siddancha (Siddharth Ancha) wrote…

Makes sense! I can confirm that I've tried using MeshcatVisualizer instead of ApplyVisualizationConfig and it works well for me! What this doesn't do is add DrakeVisualizer systems that I don't need anyway.

Commented out ApplyVisualizationConfig()and replaced it with MeshcatVisualizer.AddToBuilder(). Now, the system diagram is a bit simpler.

[RussTedrake]

Reviewed 2 of 2 files at r3, all commit messages.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @jwnimmer-tri and @siddancha)

---

manipulation/station.py line 1398 at r1 (raw file):

Previously, siddancha (Siddharth Ancha) wrote…

Commented out ApplyVisualizationConfig()and replaced it with MeshcatVisualizer.AddToBuilder(). Now, the system diagram is a bit simpler.

of course you could publish both collision and visual geometry by adding the two MeshcatVisualizers (that's what was happening before). do we think the collision / visual geometry option is less relevant for the hardware version?

[siddancha]

Reviewable status: 2 of 4 files reviewed, all discussions resolved (waiting on @RussTedrake)

---

manipulation/station.py line 1398 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

of course you could publish both collision and visual geometry by adding the two MeshcatVisualizers (that's what was happening before). do we think the collision / visual geometry option is less relevant for the hardware version?

I think collision geometries are indeed useful in the hardware version. Reverting back to ApplyVisualizationConfig().

I think it's fine if ApplyVisualizationConfig() takes the plant as an argument but doesn't use it. At least it provides a comprehensive set of visualization options that one day we might let the user tweak.

[RussTedrake]

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