Output KinematicsCache from RBP

[amcastro-tri]

Per discussion in #3316, we need to output the KinematicsCache as an output for BotVisualizer not having to recomputed already available quantities in the KinematicsCache.
The output will be in the form of an AbstractValue and BotVisualizer will have an input port for it.

For systems like a simple pendulum dynamics but that we want to render as a three dimensional multibody system, we will just leave the KinematicsCache port unconnected case in which the BotVisualizer will just instantiate its own KinematicsCache as it is done right now. Therefore we would need the capability to leave inputs unconnected and for System<T>'s to query whether these are connected or not.

Two questions arise @david-german-tri and @sherm1:

1. Can we leave input ports disconnected? would it be a good idea to do so?
2. Could we (should we?) provide a System<T> method to check whether an input port is connected or not? like System<T>::is_input_port_conneted(port_number).

[jwnimmer-tri]

I think this design proposal is a super-confusing way to address the challenges that #3316 was trying to solve, and that #3316 was more on the right track.

[amcastro-tri]

I am open to close this issue and re-open #3316. But let me see if I can share with you some thoughts.
Just think about how the design in #3316 would get connected.

Example 1. RBP --> RbpTransalator --> BotVisualizer
In order for RbpTransalator to not recompute the KinematicsCache it'd need it as an input form RBP. So the desing here proposed does not add this new AbstractValue port but it was implicit in #3316 (in case with "super-confusing" this is what you were referring to).

Example 2. SimplePendulum --> RbpTransalator --> BotVisualizer
In this case SimplePendulum does not have a KinematicsCache. Somebody needs to compute it. Well, in that case it seems we'd then need to leave the KinematicsCache port unconnected and then RbpTransalator would compute the kinematics cache.

So, all the super-confusing components were already present in #3316. Actually here I am just simplifying to two classes instead of three. Again, I might be missing something. Let me know what you think.

[jwnimmer-tri]

This is tagged as high priority; is that true? Is the only top-level goal to be able to write something like BotVisualizer? I feel like a whiteboard would really help, which means deferring to next week.

[liangfok]

And a System 2.0 versions of RosTfPublisherSystem, and ImuSensorSystem. OK for deferring till next week.

[amcastro-tri]

Yes, that is why the high priority. It should be easy to implement once we agree on what exactly to implement. The high priority is because we at least need RBP, and BotVisuzlizer to simulate something and visualize it (as done with the Kuka demo).
I like the idea of a f2f with whiteboard! and for next week, yes.

[sherm1]

KinematicsCache is going to have a short lifetime since its functionality should be absorbed by System2's built-in caching facility. So I don't think we should be designing in any new dependence on it (unless this is meant as a short-term hack). Exposing meaningful information on output ports makes more sense and is more general since any System could output such data, while KinematicsCache is only available from Systems that have a RigidBodyTree embedded somewhere.

[liangfok]

Is it possible to incrementally transform the KinematicsCache into System2's cache? Are we still planning on incrementally morphing RigidBodyTree into the version used by Dynamics 2.0?

[sherm1]

Is it possible to incrementally transform the KinematicsCache into System2's cache?

That will be much easier if we don't expose KinematicsCache outside RBTree and RBPlant. Then we can gradually move its contents in the Sys2 cache until it is empty.

[tkoolen]

Sharing of KinematicsCache (or whatever will replace it in the future) is also pretty much required for an efficient implementation of RigidBodySensors. Perhaps a quick and dirty solution is to just construct these RigidBodySensors with a reference to the KinematicsCache internal to RigidBodyPlant for now (which should by the way not be constructed on every call to EvalTimeDerivatives!).

[liangfok]

Would it be too against the spirit of System 2.0 to have RigidBodyPlant maintain a pointer to a mutable KinematicsCache object and all consumers of the KinematicsCache maintain a const reference to it?

I hesitate to recommend such a design because it feels like it's a side-channel around System 2.0's port or cache abstractions.

[sherm1]

System 2 has its own nascent caching mechanism which should not be strangled in the womb by outputting some other caching mechanism!

[tkoolen]

For future reference, I'll restate an example I gave on slack earlier: consider a robot with 20 accelerometers attached to it (we've actually discussed doing this for Valkyrie; this is not some contrived example). If every RigidBodyAccelerometer has to redo the work to compute the data currently stored in KinematicsCache, that would be terrible for performance. In my view, this demonstrates the need for some sort of sharing of cached results between Systems.

So what is the long-term vision for this? The discussion on slack seemed to be leaning towards only outputting a minimal representation of state (q and v for a RigidBodyPlant), and that cache is only an implementation detail, hidden to someone assembling a Diagram. That would imply that sharing cached results should be done 'underhand'.

Instead, I'd like to argue that passing data between systems should always be done through ports; otherwise this nice, decoupled system design is in name only. So I'm in favor of @amcastro-tri's original plan of having a KinematicsCache output port for RigidBodyPlant (or whatever the cache for a RigidBodyPlant will be renamed to in the future).

[liangfok]

How about instead of having one output port that contains a KinematicsCache object, we have n output ports, one for each of the n intermediate values stored in what is now the KinematicsCache? We can always add more output ports if and when we want more intermediate values.

Maybe RigidBodyPlant can optimize itself by detecting whether there are any downstream consumers of a particular output port and only computing the intermediate value for that output port if (1) it has to anyway and (2) there are downstream consumers of the value on the port. I'm actually not sure if System 2.0 currently supports this optimization.

[sherm1]

The System 2 cache is contained inside the Context and is available to all subsystems for every computation performed at run time (in a carefully managed way that guarantees up to date results). In a sense it is always exported implicitly. It should never be exposed explicitly on an output port.

If every RigidBodyAccelerometer has to redo the work to compute the data currently stored in KinematicsCache, that would be terrible for performance.

Agreed! That would never happen making proper use of the System 2 facilities.

[amcastro-tri]

The System 2 cache is contained inside the Context and is available to all subsystems for every computation performed at run time

I wonder what that API would look like. Right now we can access subsystem contexts like done here for the PID controller. The access can be done if you have the pointer to the system which subcontext you want to access to.
Consider a simple example like BotVisuzlizer, should BotVisualizer keep a (non-owning) pointer to the RigidBodyPlant?

[tkoolen]

@sherm1, hmm, I mean it works, but the output ports of a RigidBodyPlant are kind of just for show then (except for properly setting up the dependency graph). RigidBodySensors would be able to do their work just fine without any information from RigidBodyPlant output ports.

I suppose maybe you're planning to require having access to the state vector in order to access the cache for a system (this is not what's currently implemented), but in that case, why not just have a scalar output port, outputting a hash value computed from the state?

[david-german-tri]

From @sherm1:

If every RigidBodyAccelerometer has to redo the work to compute the data currently stored in KinematicsCache, that would be terrible for performance.

Agreed! That would never happen making proper use of the System 2 facilities.

I agree as well, and for the same reasons. Of course, this would be clearer if the System2 cache facilities actually existed, so I will dust off that branch this week. It was previously PR #3135. We tabled it because there were competing priorities, and because @sherm1 and I have an unresolved design disagreement. That disagreement is tangential to this thread, where we entirely agree.

From @tkoolen:

Instead, I'd like to argue that passing data between systems should always be done through ports; otherwise this nice, decoupled system design is in name only.

I absolutely, 100% agree with this. If we allowed mutable members of System<T> subclasses, the whole framework would be pointless.

So I'm in favor of @amcastro-tri's original plan of having a KinematicsCache output port for RigidBodyPlant (or whatever the cache for a RigidBodyPlant will be renamed to in the future).

As an interim solution, I think this (or anything similar) is fine. It also gets at an important point. The System2 cache infrastructure will supersede the aspects of KinematicsCache that manage data lifetime. It will not supersede the aspects of KinematicsCache that are a notation for the result of particular, expensive computations about RigidBodyTree. We will still need that notation, to be stored in System2 cache, and to be transmitted on a RigidBodyPlant output port to downstream consumers.

[david-german-tri]

Sigh. My post crossed the most recent from @amcastro-tri and @tkoolen in flight.

I do not think @sherm1 meant to claim that multiple subsystems would share a cache. If he did, then I strongly disagree, for the reasons @tkoolen mentioned! I believe Sherm meant, and I also would assert, that (a) each subsystem always has its own private cache available and (b) cache entries which depend on a given input are invalidated whenever that input changes.

So, a downstream consumer of KinematicsCache can check whether its KinematicsCache input has changed, and redo dependent computations only if it hasn't.

[sherm1]

I do not think @sherm1 meant to claim that multiple subsystems would share a cache.

Right! What I meant is that the full cache is always lurking behind every computation within a Diagram so that (as managed carefully via input and output ports) all needed information can be obtained without (a) passing caches around, (b) recomputing anything that has already been computed, or (c) computing things that don't get used. Each subsystem sees only its own cache, but the framework as a whole has the Diagram perspective and can efficiently deliver computations between subsystems via the ports.

[tkoolen]

OK, that makes sense. Back to the original issue :-). What ports do you think should be added to RigidBodyPlant to enable efficient RigidBodySensors and visualizers?

[RussTedrake]

fyi - this is also blocking the valkyrie sim.

[liangfok]

To get the conversation started how about let's make it output the following at least initially? We can always add more output ports with additional data as necessary.

1. A vector of joint positions (q). Meta data include model instance IDs and joint names.
2. A vector of joint velocities (q_dot). Meta data include model instance IDs and joint names.
3. A newly defined WorldVisualizationDescription object that contains, for each RigidBody in the world, a description of how it should be visualized. It should contain all of the information needed by BotVisualizerSystem::initialize_drake_visualizer().
4. A newly defined RigidBodyPoses object that contains, for each RigidBody in the world, its 6-DOF pose position and quaternion orientation in the world frame. It should contain all of the information needed by BotVisualizerSystem::DoPublish().
5. A newly defined ContactWrenches object that contains, for each contact in the world, the two bodies in contact and the contact wrench between them.

[RussTedrake]

@liangfok asked me to comment on this

@liangfok, i think that the conceptual output of the rigid body plant is very clear -- it should contain the generalized state of the plant (which is positions and velocities). in my view most of the discussion here is about passing the result of computations on that state (transforms, accelerations, etc) simply to make downstream computations more efficient for sensors, etc.

[liangfok]

Yes, the additional state that we were thinking about outputting from RigidBodyPlant is indeed mainly for computational efficiency. Another reason I've heard is to prevent downstream systems from needing to have a const reference to the RigidBodyTree (i.e., an encapsulation argument).

I believe one of the original instigators that resulted in this discussion is #3228 (comment), where it was pointed out that BotVisualizer 2.0 could be vastly simplified if it were provided rigid body pose information.

In #3325 (comment), @tkoolen makes a strong argument for why efficiency is important with respect to supporting sensors.

In System 1.0, the RigidBodyPlant and Sensors were all inside RigidBodySystem, which sidesteps the question of what RigidBodyPlant should output.

[tkoolen]

@jwnimmer-tri, I'm aware of that. My reasons for not liking fat outputs so much are outlined here: #3325 (comment). But fat output ports certainly are a valid solution to this issue; just a matter of preference.

[liangfok]

One option is to initially provide both fat ports and small ports, then over time we can see if both are in fact useful for different purposes.

[sherm1]

I'm a little unclear on @sherm1's stance [on sharing of cache]

I believe mine is the same as @david-german-tri's: we have a beautiful caching mechanism designed for System 2 that is completely sufficient for efficiently and correctly connecting subsystems via input/output ports. So my stance is that this is a non-issue. The cache should be invisible and the solutions should involve designing useful output ports that expose meaningful data.

[david-german-tri]

I was able to chat about this quickly with @RussTedrake and @tkoolen this afternoon. We cleared up a few misunderstandings. (Please correct me if I misrepresent anything!)

First, Russ confirmed that it's not necessarily a requirement that RigidBodyPlant, or other systems, have parsimonious output. In principle, it's OK for RigidBodyPlant to output not just x = [q v], but also other ancillary data like the mass matrix. He also agreed that the System2 cache infrastructure (#3376 et. seq.) can remain private to a System. So, that resolves the major blocking issue from #3325 (comment).

Second, Russ pointed out a couple of additional requirements that I hadn't thought of. (a) It should be possible to have a network boundary between RigidBodyPlant and its consumers. (b) If N sensors depend on some ancillary data output of RigidBodyPlant, they shouldn't have to recompute it, even if RigidBodyPlant wouldn't compute it by default.

That suggests an implementation spiral like the following. My primary interest is in the application of the System2 architecture; if I've missed some dynamics details please accept my apologies!

step 0 (fat ports, no cache). RigidBodyPlant outputs KinematicsCache (and, separately, contact forces). It may make sense to rename KinematicsCache to RigidBodyConfiguration or similar, since it wouldn't really be a cache anymore. AIUI this is what @amcastro-tri proposed originally!

step 1 (fat ports, cache). Once the System2 cache is available, KC/RBC can be stored there (and invalidated on changes to x or u) instead of recomputed on every call to EvalOutput.

step 2, option a (thin ports, cache). Once #2890 is resolved, it may be worthwhile to break up KC/RBC into smaller outputs. Then the N sensors mentioned above can depend on foo_matrix_port, and it will be computed exactly once - or zero times, if no one depends on it at all. (@RussTedrake, @tkoolen, this deviates a little bit from our conversation. I realized afterwards that we don't have to resort to mutable members of KC/RBC if we have thin ports.)

step 2, option b (no ports, cache). You could alternatively glom RigidBodyPlant and the sensors into a single System with a shared cache. I don't think anyone sees a good reason to prefer this, but the door wouldn't be closed to it.

[tkoolen]

Just going to add a couple of things I took away from the meeting. Let me know if I failed to understand something. But first:

RigidBodyConfiguration

How about RigidBodyPlantState? Configuration normally refers to only the 'q' (generalized positions) part of the state, and including Plant makes it clear that it's not the state of a single rigid body. I'll tentatively use RigidBodyPlantState below.

@david-german-tri and I were initially confused about how @RussTedrake wanted to 1) output only q and v from RigidBodyPlant, 2) not redo work, and 3) not share RigidBodyPlant's private cache 'under the table'. Russ cleared that up by stating he is in favor of @amcastro-tri's original plan (and his own) of outputting KinematicsCache (RigidBodyPlantState), i.e. an object that contains q and v and can be serialized as just q and v, but also contains ancillary computation results that depend (only) on q and v.

So in a sense, RigidBodyPlantState would be a fancy state vector that has its own 'cache' of q-and-v-dependent computation results, separate from each subsystem's private cache. If the additional computation results are missing (e.g. if a RigidBodyPlantState was received over LCM, serialized as just q and v), then the receiver will just have to do a bit more computation, but the 'cache' in a RigidBodyPlantState will be invisible in the sense of #3325 (comment).

Consumers of a RigidBodyPlantState get a const view of it, and so if the consumer receives a state that doesn't have the right ancillary computation results stored, computing and storing those results becomes a problem. Our solution at the meeting was to make the ancillary fields mutable. David still doesn't like this, witnessed by 2a above, but I think it's a correct application of mutable: the RigidBodyPlantState is logically const in the sense that q and v don't change (so the state remains the same), but not bitwise const (see http://stackoverflow.com/a/105061). Do note that currently, computing the mass matrix and dynamics bias term in RigidBodyPlant actually requires all ancillary parts of KinematicsCache to be up to date, so for a simple diagram where RigidBodyPlantState goes straight from RigidBodyPlant to sensors (instead of being serialized in between), this problem disappears.

On a somewhat related note, I think we should move towards not exposing the CacheElements of KinematicsCache through a getElement method, but rather move the methods of RigidBodyTree to KinematicsCache, so that there is no need to expose this caching detail. These methods will then form the interface used by consumers.

RigidBodyPlant outputs KinematicsCache (and, separately, contact forces).

Our reasoning for separating out contact forces is that they may depend on the RigidBodyPlant's inputs in a direct feedthrough manner (although they currently don't, due to the penalty-based ground contact model), making them distinct from the stuff that's currently in KinematicsCache. So @amcastro-tri's ContactResults from #3362 sounds like the right way to go.

[sherm1]

Making parts of an output mutable would be a complete end-around the System 2 caching system, identical to exporting the KinematicsCache. We should not do that. If we don't have suitable caching available yet, we can just have the RBPlant do extra work to make sure everything it outputs is valid.

BTW, I agree that caching is an appropriate use of mutable; that is exactly how the System 2 caching system works (will work). But it manages that internally and guarantees correctness.

[liangfok]

So, basically, RigidBodyPlantState is a "fat port". SGTM.

Why not have downstream consumers of it simply make a mutable copy of the const version they receive?

[tkoolen]

@sherm1, thinking about networking is what convinced me the approach we came up with at the meeting is the right way to go. Consider this: how would you send a ContactResults over the network? I'm using ContactResults here because that is less controversial at this point. Would you send over one giant LCM message that is an exact mirror of all of the fields of ContactResults, with all its implementation details exposed, or would you send over a more minimal representation and then recompute ancillary data on the consumer (receiver) side? I would argue the latter is the right way to go.

If you choose the minimal LCM message, you could have the LCMSubscriber system recompute all the ancillary data from the data received over LCM, and that would work fine. But you might be recomputing more than what the consumer needs. Making that recomputation be lazy - but invisible to the consumer - is the optimization we wanted to implement. If this is not covered by the System 2 caching design, perhaps it should be.

Why not have downstream consumers of it simply make a [non-]mutable copy of the const version they receive?

@liangfok, that's an option, but my argument is that the input will remain const in the way that matters anyway (logically const), even if it's not bitwise const.

[david-german-tri]

RigidBodyPlantState

SGTM.

---

Although I agree with @sherm1 that it's a bad idea, it sounds like we should also track an option 2c. Importantly, we don't have to choose among the 2x's to do steps 0 and 1, which will unblock most real use cases.

step 2, option c (fat port, separate caching). Make the ancillary computation results in the KC/RBPS mutable. This allows downstream consumers can write those values back, sharing them with other consumers, without the RBPlant (or LCM receiver) having to know to compute them in advance.

On a somewhat related note, I think we should move towards not exposing the CacheElements of KinematicsCache through a getElement method, but rather move the methods of RigidBodyTree to KinematicsCache, so that there is no need to expose this caching detail. These methods will then form the interface used by consumers.

👍 to this part. I knew I was forgetting something.

[tkoolen]

we don't have to choose among the 2x's to do steps 0 and 1, which will unblock most real use cases.

Just step 0 already does this, and it will even make the non-networked case be as efficient as possible: computing the dynamics in RigidBodyPlant requires all ancillary parts of KinematicsCache/RigidBodyPlantState to be up to date.

[sherm1]

@tkoolen: Making that recomputation be lazy - but invisible to the consumer - is the optimization we wanted to implement.

I totally agree and this is how the System 2 caching mechanism should behave. Surprisingly this is somewhat controversial at this point but I hope we'll resolve it soon.

[tkoolen]

The controversial thing is sharing cache that is supposed to be private to a subsystem implicitly (i.e., not through output ports) with other subsystems in a diagram (e.g., #3325 (comment)).

The proposal here is to associate cache with the output of a subsystem, not with the subsystem itself, so that it passes through an output port, and the dependencies are explicit.

Maybe I completely misunderstood, but I was under the impression that the current cache design only associates cache with a subsystem.

[david-german-tri]

@tkoolen Your understanding is mine also. I think @sherm1 is trying to connect this thread to the question of whether System outputs themselves should be computed in push order or pull/lazy order. I claim that's a totally separate, unrelated issue.

[tkoolen]

The point is that in a networked situation, the RigidBodyPlantState (or rather, a minimal representation of it as an LCM message) may be coming over the network, and there may not be any RigidBodyPlant on the receiver side whose cache consumers can access. The natural thing in this case is to associate the cache directly with the RigidBodyPlantState output, not with the RigidBodyPlant.

[jwnimmer-tri]

If it helps: another architecture for the networked plant state is to transmit the minimal representation, and then have an explicit System on the receiving side that takes minimal representation as input and provides the supplemental representation as (cached) output. Then you get minimalism, caching, explicit documentation of the computations' structure, and it would (also) be reusable code pattern if you had different plants with different supplemental state.

[david-german-tri]

@jwnimmer-tri beat me to it. But this whole argument is 2a vs 2c and can wait.

[tkoolen]

Completely agree.

[tkoolen]

@jwnimmer-tri, yep, exactly what I meant in #3325 (comment) with

you could have the LCMSubscriber system recompute all the ancillary data from the data received over LCM, and that would work fine [... there's a but after this.]

[jwnimmer-tri]

It sounds like I should abstain from this discussion for the time being 😄. I am happy to talk though the computation / caching for the future numbered items if and when we need them, if anyone wants.

[tkoolen]

Sorry, didn't mean to scare you away, haha :-)

[jwnimmer-tri]

I'm good. It's an interesting discussion, just not a problem I need to own.

[sherm1]

Maybe I completely misunderstood, but I was under the impression that the current cache design only associates cache with a subsystem.

Caching is supposed to apply to output ports as well (and also other computations like derivatives, discrete updates, energy calculations, and anything a concrete System may want to add besides the general API). Cached output ports provide a disciplined way to allow caching to reach downstream from the System that owns that piece of cache real estate, without having to expose any of the internal workings (even the fact that a cache exists at all should be evident only through performance).

[tkoolen]

Seeing as there is still no consensus on this issue (given #3471), I propose clearing things up with a meeting, this time with everybody involved in this issue.

[sherm1]

Please consider that accessing an output port triggers the associated computation (that is, the output port's value is guaranteed to be up to date when you see it). Consequently output ports need to be segregated appropriately; asking for body poses should not trigger acceleration computations. Also, a port that includes accelerations is necessarily a direct feedthough from the RBPlant's actuator inputs. That is not what you want from a port that returns kinematics, which is not direct feedthrough and could thus serve as input to a sensor System whose output goes to a controller that generates actuation inputs to RBPlant.

Returning KinematicsCache as a single output port thus implies that everything in it gets calculated at once. That likely means we have to include all the sensor and controller code in RBPlant rather than using System interconnections. That could be done but seems like a complete end-around the System 2 framework.

IMO, a nice solution would involve at least 3 output ports:

• one for one-time data such as body names, mass properties, dimensions, and other "parameterizable" quantities
• one for state-based outputs (position and velocity kinematics)
• one for dynamics outputs (accelerations, reaction forces)

It is not clear to me that we should be exporting internal quantities through output ports (rather than bespoke RBPlant methods) but if we do those would be good candidates for additional ports so that expensive computations (mass matrix, for example) could be delayed unless someone asks for them. (Accelerations can be calculated without forming a mass matrix.)

@tkoolen: I propose clearing things up with a meeting

That seems like a good idea!

[liangfok]

The above seems to be a strong argument for using "thin" ports.

I did not realize that the act of accessing a port triggers the associated computation, though it sounds like a really nice optimization. I'm now thinking about the implications of this fact on the implementation of the system that owns the output port. If a particular output port is accessed only 50% of the simulation cycles, is it the responsibility of the system that owns the output port to only compute the value supplied on that port 50% of the time?

[sherm1]

The dataflow model (a.k.a. lazy evaluation) that we're now using never computes anything unless someone asks for it. So an output port that gets accessed occasionally only gets evaluated occasionally. The same scheme works for all the rest of the computations also, like derivatives, discrete updates, energy calculations, initial conditions, and so on. We don't know how often those need to be computed until someone asks.

[david-german-tri]

The dataflow model ... that we're now using

Not on master yet - that's #3455. (pending your review)