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 https://github.com/RobotLocomotion/drake/pull/3228#issuecomment-242509888, where it was pointed out that BotVisualizer 2.0 could be vastly simplified if it were provided rigid body pose information.

In https://github.com/RobotLocomotion/drake/issues/3325#issuecomment-244989985, @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]

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)

@RussTedrake, I disagree; I think that the state of a rigid body plant is very clear, but I don't think that the state should be the only possible output presented on a rigid body plant's output ports, even conceptually.

[amcastro-tri]

I will summarize here a f2f conversation we just had with @sherm1 about this.

About visualizing a RigidBodyPlant

First, We thought that a more appropriate name for System 2 BotVisualizer would be something like RbtLcmPublisher (other similar name suggestions are welcome) so that it is abolutely clear what this system does: it publishes LCM messages for our drake-visualizer that are only valid to visualize a RigidBodyTree. Also, it then makes sense that this system is then initialized from a constant reference to a RigidBodyTree.

Second, RigidBodyPlant would have output ports for each of its RigidBody poses so that they can be inputs to RbtLcmPublisher. Since outputs in System 2.0 will be cached, accessing these from RbtLcmPublisher won't incur in any additional computation solving the problem of unnecessarily re-doing computations.

About visualizing a system like a Pendulum.

We think that in a case of a system like say a simple pendulum that clearly has all the information to plot itself and was written by a user with a clear idea in mind of how that system would be visualized, it makes sense for this system to also output the poses for its links. Therefore System 2.0 pendulum would output the rigid body pose of its link. Similarly for other systems like say SpringMassSystem.

[jwnimmer-tri]

I think it will be a common occurrence for Systems that connect to the RigidBodyPlant output to want the position and velocity for all bodies, not just some. The above proposal reads to me like it proposes one port per body. Offering that is fine if you want, but I would also like to see offered a single output port that provides the entire state for all bodies (in addition to whatever other outputs we also end up offering). The so-called RbtLcmPublisher would use that all-in-one port, of course.

[liangfok]

I would also prefer a single port that contains all of the poses. It seems cleaner and more tidy that way.

[tkoolen]

RigidBodyPlant would have output ports for each of its RigidBody poses

and

I would also like to see offered a single output port that provides the entire state for all bodies

This is a difficult one.

I think the 'small' output ports for each of the RigidBody poses should exist in any case, because otherwise you're probably requiring some systems downstream to accept more data than what they need to do their jobs: 'fat' output ports require fat input ports. This would be kind of like only wanting a little pet rat, but getting the whole pet store and having to deal with it. Fat output ports increase coupling in the code base, and make it harder to write unit tests for individual systems. Small output ports provide better data abstraction.

But should there be a fat output port with all poses in addition to the small output ports? I could be persuaded both ways, but I'm leaning towards a "no", just because it feels a bit redundant. Maybe what's needed instead is some syntactic sugar for connecting a whole bunch of ports in a one-liner?

RbtLcmPublisher

I prefer not to abbreviate RigidBodyTree (or rather, to abbreviate in general). LCM is a very well-established abbreviation, but RBT is more likely to confuse people that are new to the code base.

[liangfok]

:+1: for providing sugar for wiring up a whole bunch of related ports in one line of code.

Question: Is it OK for the number of output ports to be dependent on the model that's being loaded? In other words, will the decision to make the number of output ports proportional to the number of rigid bodies introduce too much coupling between the diagram and the model (URDF, SDF, etc.) that it can load? It would be a bummer to have to recompile the code just to support a URDF or SDF with a different number of DOFs.

[RussTedrake]

I still think the output of the RigidBodySystem should always be the generalized state. We could have a sensor/modifier that turns that into poses of some end effector. Again, the design which was thought out and discussed (at length) before is described briefly in https://github.com/RobotLocomotion/drake/blob/master/drake/systems/plants/RigidBodySystem.h#L15

I think we should either consider RigidBodyPlant to be what I was calling RigidBodyTree in that doc, or include sensors/actuators and call it RigidBodySystem (where the output is now the sensors).

[liangfok]

OK. I suppose we can stick with the System 1.0 design of lumping the RigidBodyTree, Sensors, and ForceElements all into a single System 2.0-based RigidBodySystem.

@RussTedrake, just to be clear, are you opposed to having this system also output the individual rigid body poses in addition to its generalized state? Only outputting the generalized state will force downstream systems like BotVisualizer to recompute the pose information.

Oops, I just now understood the comment above saying sensors can be added that output the poses of each rigid body. In that design, the generalized state and pose information will all be lumped into a single very fat output port (a port containing a very long BasicVector). This is not necessarily a regression since it is the current state of System 1.0. I suppose System 2.0 is better in that the output port can output a subclass of BasicVector, which then provides named accessors to the data within it. All of the index bookkeeping can be hidden within this subclass of BasicVector.

[liangfok]

Thinking about this some more, I suppose there's nothing preventing us from aggregating the RigidBodyTree, Sensors, and Force Elements all into a single System 2.0-based RigidBodySystem, but still have multiple output ports, one for the generalized state of the tree, and other ports for each individual sensor and force element.

[tkoolen]

lumping the RigidBodyTree, Sensors, and ForceElements all into a single System 2.0-based RigidBodySystem.

That works too.

I still think the output of the RigidBodySystem should always be the generalized state.

I really think that just can't work because of https://github.com/RobotLocomotion/drake/issues/3325#issuecomment-244989985. If sensors are separate systems (as was the case in the design you linked) and the only outputs available to the sensors are q and v, then you either have to redo lots of work, or share caching between systems, making the port-based design be just for show.

[amcastro-tri]

@jwnimmer-tri, @liangfok. Yes, I am sorry if I didn't explain correctly but my proposal is to provide a single port with all body poses, not a port per body.

@RussTedrake

I still think the output of the RigidBodySystem should always be the generalized state. We could have a sensor/modifier that turns that into poses of some end effector.

Yes, the proposal here is to have that output port with the state plus an additional port with poses that could for instance be used by the bot visualizer. The reason for this port is to avoid re-doing computations in the bot visualizer which were already performed by the RigidBodyPlant. These poses (and potentially other kinematic quantities) could additionally be used by sensor systems which then won't require to recompute the body poses or any kinematics in order to figure out say an acceleration.

Thinking about this some more, I suppose there's nothing preventing us from aggregating the RigidBodyTree, Sensors, and Force Elements all into a single System 2.0-based RigidBodySystem

I do not like that idea. Dividing our system into a RigidBodyPlant and into a bigger RigidBodySystem containing now additional sensors and actuators IMO allows for greater flexibility and a more incremental development. That discussion does not belong to this issue though.

[amcastro-tri]

Please let me summarize the plan of action into what I think covers all of your concerns. I am intentionally leaving out the discussion of sensors/actuators and other systems that can be attached to a RigidBodyPlant because System 2.0 now allow us to develop into smaller increments by composing simpler systems into a Diagram. RigidBodyPlant ONLY contains the RigidBodyTree model of the world with: Inputs: the actuators' generalized forces. Outputs: state + convenience output port (a single one) with bodies' kinematics.

The later can be used for either visualization or by smaller systems like sensors so that this information is already cached and does not need re-computations.

The output port with the system kinematics will be wrapped in a nice API so that a request on per-body basis can easily be performed as well. So this output port won't just be a bare array of doubles but more like an array of RigidBodyKinematics (for the lack of a better name).

[liangfok]

@amcastro-tri I am in general agreement with you, but was only considering mirroring System 1.0's approach of combining everything into a RigidBodySystem based on @RussTedrake's comments above.

In what way does making sensors and force elements independent systems increase flexibility and incremental development? Even if they were contained in a RigidBodySystem along with a RigidBodyTree, they would still be their own independent objects that are instantiated at run-time either programmatically or via a specification file like SDF. By combining them into a single system, there will be no ports between the tree, sensors, and force elements, which eliminates the concern that they are "just for show".

[amcastro-tri]

I am in general agreement with you

Good.

but was only considering mirroring System 1.0's approach

System 2.0 does not need to mirror System 1.0 (of course we still have similar concepts, but not implementations). We can do better with @david-german-tri's design now.

In what way does making sensors and force elements independent systems increase flexibility and incremental development?

A simple example: #3245 could be merged right now as it is without having to design right now the right abstractions to add sensors. Even adding the output poses I propose here would be a separate PR. IMO that is incremental development.

How it is more flexibly to design sensor systems? well, that'd require a f2f or VC but I don't think that belongs to this issue anyway.

[liangfok]

I see, so you're saying incremental w.r.t. pull requests. That is true.

However, would you agree that lumping the tree, sensors, and actuators into a single RigidBodySystem does not enable additional incremental development w.r.t the creation of a model?

[sherm1]

@tkoolen: I think the 'small' output ports for each of the RigidBody poses should exist in any case, because otherwise you're probably requiring some systems downstream to accept more data than what they need to do their jobs: 'fat' output ports require fat input ports.

In System 2 there is no cost to having a fat poses port even if only a few poses are used, because RigidBodyPlant has to compute them anyway. When the System 2 implementation is complete there will be no copying from output ports to input ports. The connecting lines in a System Diagram should not be thought of as pipes along which data flows, but just an expression of the connectivity. So picking a particular pose from the output port is actually just obtaining a reference to a particular element of RBPlant's internal pose cache.

There would be nothing to prevent us from adding options to RBPlant to make it produce more specific output ports. That would be a performance win for quantities that wouldn't otherwise be computed, but for poses we wouldn't gain anything.

[tkoolen]

@sherm1, I wasn't talking about runtime cost.

[sherm1]

I wasn't talking about runtime cost.

Oh. To me it seems messier to add construction options to an RBPlant to generate body-specific pose outputs, then use those to make a custom-wired Diagram, than simply to select the poses you care about from the always-available general supply. Same goes for state -- it's easy just to output the whole x even if downstream only needs some of it.

[liangfok]

I was just thinking that if we end up lumping RigidBodyTree, Sensors, and Force Elements all within a single System 2.0 version of RigidBodySystem, we might as well also throw BotVisualizer into RigidBodySystem. I'm not sure if this is an argument for or against lumping everything together. Just something to think about. Maybe the port-based architecture is only for connecting things that are less coupled like perception ←→planner ←→ controller ←→ plant?

[RussTedrake]

@tkoolen et al. Perhaps I've missed some of the discussion. Is passing caching between systems really so bad? i think that's exactly the right thing to do. I do not like the idea of passing out additional kinematic outputs, because it is conceptually incorrect (over-specified). If we are doing more complicated design/analysis on these systems, then we want to have the conceptually correct (and minimal) signals passing between them -- not just for show. The caching can make the runtime performance ok.

[liangfok]

@RussTedrake, I believe @david-german-tri's post above (https://github.com/RobotLocomotion/drake/issues/3325#issuecomment-245105482) concisely describes the System 2.0 vision of caching. Basically, each system will have its own private cache that's not shared with other systems. However, the Diagram itself will have a cache that ensures efficiency within its constituent systems.

[sherm1]

@RussTedrake, KinematicsCache will be replaced (gradually) by System 2's more-general caching mechanism so we don't want to expose KC in a System 2 system. Sys2 has more information so can trigger computation when necessary and avoid computing expensive quantities that aren't being consumed.

[RussTedrake]

If that is the model, then i would argue for the rigodbodysystem approach, where the single system contains the list of sensors and actuators -- acting effectively as a very special case of diagram where cache sharing is allowed.

Perhaps the botvisualizer tools also live as a part of this system so they share the same privilege?

[RussTedrake]

@sherm1 -- totally agreed that the more general caching mechanism will be in play here. But is that mechanism restricted to a single system, or can it be shared across systems?

[sherm1]

is that mechanism restricted to a single system, or can it be shared across systems?

It can be shared among subsystems contained in the same Diagram (however deeply nested). Sharing is carefully controlled though to prevent out-of-date references. Subsystem A can do anything it wants with its own portion of the Context (which contains the cache), but if it wants to use something from subsystem B that is mediated by the Diagram.

[liangfok]

Are we accidentally delaying ourselves by prematurely optimizing things? @sherm1 has previously emphasized that the entire caching mechanism is optional and that it will be easily togglable on / off. From a system user's point of view, the cache does not even exist. It will only have non-functional impacts on the overall execution of the software.

Given the above, how about let's charge forward implementing the RigidBodyPlant, Sensors, ForceElements, BotVisualizer, etc., as individual System 2.0 systems that input and output only the minimal generalized state (and have multiple instantiations of RigidBodyTree as necessary)? We can then perform some benchmarks to prove that we have a performance problem, which will justify (1) a sophisticated caching mechanism, (2) exposure of non-minimal state through ports, and/or (3) lumping multiple software abstractions into a single RigidBodySystem so they can share the same cache.