Drake: LcmAndVectorBaseTranslator interface is too limiting

[tkoolen]

Consider this use case: I need to populate the following fields of a single LCM message:

• joint positions/velocities
• joint torques
• force/torque sensor data

Joint positions and velocities come from the output port of RigidBodyPlant. Joint torques would come from the same source that is connected to the input port of RigidBodyPlant. Force/torque sensor data would probably also be a future output of RigidBodyPlant.

Currently, the TranslateLcmToVectorBase and TranslateVectorBaseToLcm methods of LcmAndVectorBaseTranslator accept a single VectorBase pointer/reference. This is too limiting to handle the case described above.

One way of handling this would be to add a Multiplexer class that concatenates all of the required signals together and to keep the LcmAndVectorBaseTranslator the same, but I would personally really dislike that. What are your thoughts on handling this case?

[tkoolen]

This is one of the big issues currently blocking a proper C++ Valkyrie/Atlas simulator that interfaces with the InstantaneousQPController and the OpenHumanoids stack.

[jwnimmer-tri]

My first reaction is that it would make sense to update the Translator interface to allow for its use in a multi-ported System. This would be especially true if the multiple-inputs-single-message is a common case; less so if its a rare case.

Another option would be to have a new class JointStateLcmPublisherSystem extend LcmPublisherSystem and override DoPublish, where it gets to declare its own input ports and how they map into the message (without using a Translator). Being its own class, perhaps there's a way to make it more amenable to correctly wiring up into a Diagram (e.g., you can only add it to a Diagram that has a RigidBodyPlant, and the JointStateLcmPublisherSystem self-wires to the ports correctly).

[liangfok]

Hi @tkoolen. I'm curious why you would really dislike a custom HumanoidJointAndSensorStateMultiplexerSystem that combines all of the disparate signals into a single vector that can then be consumed by a LcmAndVectorBaseTranslator? That seems to me, at first glance, to be the cleanest in terms of not needing to generalize LcmPublisherSystem and LcmSubscriberSystem to support multiple ports.

[tkoolen]

new class JointStateLcmPublisherSystem

That's definitely an option, and perhaps the right thing to do.

[tkoolen]

@liangfok, I dislike it because it requires the LcmAndVectorBaseTranslator to index into this big vector of general, agglomerated stuff, which is error prone and not very user friendly.

[jwnimmer-tri]

@liangfok A HumanoidJointAndSensorStateMultiplexerSystem would be quite a lot of boilerplate, and (I suspect) difficult to wire directly into a Diagram. The dataflow feature it implies is already present in Diagram, so if we can just reuse that with a multi-ported translator, we should end up with better code economy.

I have a few ideas of how we can update the Publisher for multi-porting (or allow for multi-ported subclassing), without making it more difficult for single-ported use cases. (If we decide to go in that direction.)

[jwnimmer-tri]

... it requires the LcmAndVectorBaseTranslator to index into this big vector of general, agglomerated stuff, which is error prone and not very user friendly.

The simple implementation of a fan-in mux System would do this, yes. A more complicated implementation would offer (through type erasure) structured accessors to the underlying data, so that the translator code would not be error-prone. (In other words, the joint state translator would be able to downcast its input to a more specific type produced by the mux, that expresses the underlying structure.) But it would still be fairly verbose, and not really offer much value IMO for all of the code that it would take to achieve it.

[liangfok]

Suppose we have an advanced HumanoidJointAndSensorStateVector that is a subclass of BasicVector with all of the necessary bookkeeping and accessors to easily obtain the relevant state within it. Would there be more consumers of such a signal in addition to LcmPublisherSystem? If so, it may be worthwhile to implement it.

[liangfok]

FTR, I would be happy to generalize LcmPublisherSystem and LcmSubscriberSystem to support multiple ports. @jwnimmer-tri, I look forward to learning about your ideas on how this can be done without incurring undue burden on those who only need a single port.

[jwnimmer-tri]

Suppose have have an advanced HumanoidJointAndSensorStateVector ... Would there be more consumers of such a signal in addition to LcmPublisherSystem?

In intuition is that such a vector would be extremely tailored to the contents of the LCM message that we want, and so would be extremely unlikely to be reusable for other purposes.

[liangfok]

Another question is can InstantaneousQPController and the OpenHumanoids stack components that would receive the LCM messages be pulled into Drake and thus be native System 2 systems?

[liangfok]

Also, can multiple LcmPublisherSystem systems be wired into the diagram, one for each type of data in this issue's original description, and publishing on a different LCM channel? Could InstantaneousQPController and the OpenHumanoids stack be modified to receive the data across multiple LCM channels?

[tkoolen]

can InstantaneousQPController and the OpenHumanoids stack components that would receive the LCM messages be pulled into Drake and thus be native System 2 systems

For InstantaneousQPController, I believe that is the plan, at some point. It's already in Drake, but it's not System 2 yet. Other OpenHumanoids stack components should be considered on a case-by-case basis. For example, I don't think the state estimator should be part of Drake (nor do I think you'll be able to convince Maurice to make it part of Drake).

This is kind of orthogonal to the current discussion though. Regardless of whether these blocks become System 2.0 at some point, we need a simulator that allows us to do full-stack integration tests. We should only be replacing the Valkyrie/Atlas hardware; all of the LCM communication and threading in use on the real system should be kept as is. So the simulator should listen for torque commands, and publish robot_state_t and foot_contact_estimate_t, just like the computer on the physical robot.

Also, can multiple LcmPublisherSystem systems be wired into the diagram, one for each type of data in this issue's original description, and publishing on a different LCM channel? Could InstantaneousQPController and the OpenHumanoids stack be modified to receive the data across multiple LCM channels?

That shouldn't be necessary. This would further complicate the already complex OpenHumanoids stack by requiring it to deal with more data synchronization issues.

I'm satisfied with just building my own custom LcmPublisherSystem-like LeafSystem. It's not a lot of code anyway. No changes to LcmAndVectorBaseTranslator required. I'll close the issue; if you think we need a more general solution, feel free to re-open.

[jwnimmer-tri]

I think it was an illuminating discussion (I certainly hadn't considered multi-ported message sending when helping consult on the design of the current code). If we see more examples of multi-ported message sending, we should definitely look towards supporting it within the framework.

[liangfok]

@jwnimmer-tri said:

HumanoidJointAndSensorStateMultiplexerSystem would be quite a lot of boilerplate, and (I suspect) difficult to wire directly into a Diagram.

Is the increased difficulty due to the general fact that the more ports a system has the more difficult it will be to wire into a Diagram?

[jwnimmer-tri]

Is the increased difficulty due to the general fact that the more ports a system has the more difficult it will be to wire into a Diagram?

It's a bit hard to say without sketching out an example. It would have to both connect its inputs to the correct places from upstream, and also provide enough metadata in its sole output port for the downstream consumer to publish it correctly. The alternative, of a single System going directly from many input ports to the single LCM message we need, is likely to be easier to understand and maintain.