RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.34k stars 1.26k forks source link

Visualization when T=AutoDiffXd #15992

Open hongkai-dai opened 3 years ago

hongkai-dai commented 3 years ago

If I have system A with scalar type double, and system B with scalar type AutoDiffXd and want to connect A's output port to B's input port. Even if these two ports transmit the same abstract data type, when I try builder->Connect(A->get_output_port(), B->get_input_port()), the compilation would fail.

One example is that I have ContactResultsToLcmSystem<AutoDiffXd>, whose output port transmits abstract data type lcmt_contact_results_for_viz. I want to connect this output port to an LcmPublisherSystem's input port. This connection fails because one port is of type OutputPort<AutoDiffXd>, while the other port is of type InputPort<double>.

cc @SeanCurtis-TRI

jwnimmer-tri commented 3 years ago

The only way to connect System ports is via a Diagram. All systems added to a Diagram must have a homogeneous scalar type. This is working as intended.

hongkai-dai commented 3 years ago

@jwnimmer-tri I agree. Currently I have a ContactResultsToLcmSystem<AutoDiffXd>, which means I will have to use LcmPublisherSystem<AutoDiffXd>? We don't have a templated version of LcmPublisherSystem. Should we add that?

jwnimmer-tri commented 3 years ago

Should we add that?

If you need it, then sure.

Or maybe drake::geometry::DrakeVisualizer (which is already scalar-convertible) should be sending contact results anyway. There's not really any benefit to making the user make two function calls to connect a MbP+SG to a drake-visualizer -- everyone always wants both, as best I can tell.

hongkai-dai commented 3 years ago

Or maybe drake::geometry::DrakeVisualizer (which is already scalar-convertible) should be sending contact results anyway. There's not really any benefit to making the user make two function calls to connect a MbP+SG to a drake-visualizer -- everyone always wants both, as best I can tell.

@jwnimmer-tri just to make sure I understand it correctly. Currently inside ConnectContactResultsToDrakeVisualizer, we connect MBP.get_contact_results_output_port() to the input port of ContactResultsToLcmSystem, and then connect the output port of ContactResultsToLcmSystem to an lcm publisher, that publishes the contact results to CONTACT_RESULTS channel. The drake-visualizer binary (not our class DrakeVisualizer) listens to this LCM channel and then visualize the contact results.

Are you suggesting to modify drake::geometry::DrakeVisualizer, such that we connect MBP.get_contact_results_output_port() to a new port in DrakeVisualizer, and then drake::geometry::DrakeVisualizer probably add a new function like SendContactResultMessage which publishes the contact result to the LCM channel CONTACT_RESULTS (similar to how DrakeVisualizer::SendDrawMessage constructs lcm message and publishes to the DRAKE_VIEWER_DRAW channel)?

SeanCurtis-TRI commented 3 years ago

On the other hand, if we did add templates to LcmPublisherSystem, it wouldn't be a blocker in the future (if located in a scalar-converted diagram). That doesn't seem a bad thing.

The primary argument against having DrakeVisualizer visualize contact results is that it brings multibody in as a dependency for geometry -- that's something I'm keen to avoid.

SeanCurtis-TRI commented 3 years ago

And, yet another thought, would be to change how we visualize ContactResults so it doesn't depend on LcmPublisherSystem. That's how we used to do scene graph visualization. Now, the (now clearly misnamed) DrakeVisualizer handles publication on its own. We could do the same for ContactResults.

jwnimmer-tri commented 3 years ago

Are you suggesting to modify drake::geometry::DrakeVisualizer ...

Yes, that seems like an improvement to me. (The contact_results input port should be optional, so that if a user doesn't connect it then we just skip sending the CONTACT_RESULTS message.)

Turning on drake-visualizer for a DiagramBuilder should be a one-liner. Turning on meshcat for a DiagramBuilder should be a one-liner. Anything else is just more boilerplate that users need to wade through and remember.

The primary argument against having DrakeVisualizer visualize contact results is that it brings multibody in as a dependency for geometry -- that's something I'm keen to avoid.

Avoiding the dependency is good; I agree that we should avoid it. However, in that case the correct conclusion is that drake_visualizer.h (and contact_results_to_lcm.h and contact_results_to_meshcat.h and ...) are all in the wrong package, not that we should fracture the code required to successfully operate drake-visualizer into ten different packages so that users need to call ten different functions in a row just visualize a simulation. The organizing principle for that code should be which visualizer it is targeting (and thus, whether we are depending on liblcm vs libumsgpack), not what concepts are being visualized.

I do support having the visualization code partitioned into separately-unit-testable functions or classes, so that it's easy to maintain. But there is no reason that our implementation partitioning needs to be exposed to users.

On the other hand, if we did add templates to LcmPublisherSystem, it wouldn't be a blocker in the future (if located in a scalar-converted diagram). That doesn't seem a bad thing.

I don't necessarily disagree, but in practice I only know about one system that is instantiated on AutoDiffXd and has an lcm-message-valued output port (which is ContactResultsToLcmSystem). Maybe this is a chicken-and-egg problem, and now that @hongkai-dai is for the first time trying to run a Simulator<AutoDiffXd> that we need to go back and template any and all of our primitive systems that only allow for double.

SeanCurtis-TRI commented 3 years ago

...are all in the wrong package.

I totally agree with this. But that's going to be a larger effort before Hongkai's life is sufficiently simplified.

But there is no reason that our implementation partitioning needs to be exposed to users.

Complete agree here. In principle, I love the idea of having visualizers in their own corner than can depend on whatever they want to visualize. I like the idea of picking a single visualizer type (based on what you're broadcasting to) and what gets visualized is purely a function of what one "turns on". But, again, timeframe. I think patching two classes (which would in some sense get subsumed/dismissed by this grander solution) is a reasonable short-term solution.

...now that @hongkai-dai is for the first time...

It's worth noting that he's also the reason that ContactResultsToLcmSystem supports T. He's one of the few users that's actually attempting to meaningfully use T != double. As such, he's the one most likely to be the canary in the coal mine and run into the ergonomic inefficiencies of systems that prevent diagrams from being scalar converted.

jwnimmer-tri commented 3 years ago

I think patching two classes ...

I think you under-appreciate how difficult the deprecation mechanics would be here. Adding a tiny bit of new logic to DrakeVisualizer (to call the existing code as a subroutine) is trivial by comparison.

jwnimmer-tri commented 1 year ago

Perhaps the best victory condition here is that ApplyVisualizationConfig and AddDefaultVisualization can accept a T=AutoDiffXd diagram builder, instead of only double. (They would discard the derivatives when visualizing, of course.)

Whether the functions do that by adding derivative-discarding pass through systems, or by adding templates to more other kinds of systems, or etc. would be up to the implementation.