Update RigidBodyPlant's Output to Provide Named Accessors

[liangfok]

Problem Definition

The output of RigidBodyPlant is a BasicVector that contains the RigidBodyTree's state vector. This is problematic since users of this state vector are forced to use meaningless indices to access values within the state vector. Without having a reference to a RigidBodyTree, downstream systems cannot interpret the meaning of the RigidBodyPlant's output.

Current Proposal Solution

Modify the output of RigidBodyPlant to be a subclass of BasicVector that contains named accessors. Downstream systems should be able to take the output of RigidBodyPlant, downcast it to something like RigidBodyPlantOutput and make calls like:

• Eigen::VectorBlock GetPositions(const DrakeJoint& joint)
• Eigen::VectorBlock GetVelocities(const DrakeJoint& joint)
• void set_positions SetPositions(const DrakeJoint& joint, VectorX positions)
• void set_positions SetVelocities(const DrakeJoint& joint, VectorX velocities)

  Older Proposed Solution 1

Modify the output of RigidBodyPlant to be a subclass of BasicVector that contains named accessors. Downstream systems should be able to take the output of RigidBodyPlant, downcast it to something like RigidBodyPlantOutput and make calls like get_velocity("right_wheel_joint").

[amcastro-tri]

What would the return of get_velocity("right_wheel_joint") be? would that be an O(1) operation? what are the use cases. And, wouldn't #3471 solve this issue?

[tkoolen]

I think it would be nice to add something like get_velocity(const DrakeJoint& joint) to KinematicsResults (the output of RigidBodyPlant proposed in #3471). I like that better than using a std::string as the argument. It's currently a little awkward though, since DrakeJoints only know about their number of positions and number of velocities, but not the starting index into the position and velocity vectors (those are stored in RigidBody...). As for computational efficiency: it will probably be a map lookup, so log(n), but I don't think that will be a huge problem and I think there's a lot of value in having a more usable interface.

[amcastro-tri]

I like the option you propose @tkoolen, with the joint as an argument. And I think it is completely ok for the joint to know its starting position in the tree for fast queries.

[liangfok]

How about let's add KinematicsResults::get_velocity(const DrakeJoint& joint, int index) where index is the index of the velocity DoF within the joint? I assume this will be an O(1) method since KinematicsCache has a v member variable that I presume can be accessed in O(1) time.

[liangfok]

Example Use Case Scenario 1

We're about to develop simulations that involve dynamics-based vehicles (e.g., the ego car) and kinematics-based vehicles (the other cars). To do this, we're going to instantiate a RigidBodyTreeLcmPublisher that publishes LCM messages to Drake Visualizer. The input to this system will be a long BasicVector containing the state vector of its RigidBodyTree. A MuxSystem will be used to combine the outputs of a RigidBodyPlant (which models the dynamics-based vehicles) and numerous other kinematic-simulation-systems each "pretending" to be a RigidBodyPlant in terms of its output. The implementation of these kinematic-simulation-systems and the MuxSystem could be aided if the BasicVector consumed by RigidBodyTreeLcmPublisher could have a semantically rich API rather than a long sequence of nameless doubles.

[tkoolen]

I would just have get_velocity return an Eigen::VectorBlock instead of passing in an additional index. Then the user can just index into it like any other vector if they want.

What I did in my Julia rigid body dynamics module was just store a map from joint to range (i.e. an object containing start index and length) in the equivalent of RigidBodyTree, instead of having this information be in the joint or rigid body itself (see https://github.com/tkoolen/RigidBodyDynamics.jl/blob/master/src/mechanism.jl#L7).

[liangfok]

Nice. I updated this issue's original description.

One slight concern I have is systems that want to set the value of the port that's consumed by RigidBodyPlantLcmPublisher needs to have a reference to the RigidBodyPlantLcmPublisher's RigidBodyTree (or another tree that's either identical or compatible in terms of being a superset of it). This is more coupling than would be ideal. Allowing the joint positions and velocities to be set by name could alleviate this problem.

@tkoolen, would you be against providing both forms of setters, one that takes as input a reference to a DrakeJoint and another that takes as input a model instance ID integer and a joint name std::string?

[tkoolen]

But where are those strings going to come from (unless you hardcode them)? Just holding on to a const reference to a RigidBodyTree shared between subsystems or a new instance created from the same URDF seems like the easiest way to access that information anyway, and then you already have the DrakeJoints set up, so why not use those?

[liangfok]

That's a good point; I think you're right.

I'm basically trying to avoid requiring that certain systems like SimpleCar and TrajectoryCar maintain const references to the world-representing RigidBodyTree that is used by the RigidBodyTreeLcmPublisher. These systems operate at a higher level of abstraction involving road networks, signage, traffic lights, etc., as opposed to multi-body-dynamics, which I consider to be a lower level of abstraction. They don't need to know the full RigidBodyTree. They only need to know which joints in the RigidBodyTree are under their jurisdiction for these are the joints that they can "control".

I now believe the above can be achieved by obtaining const references to DrakeJoint objects from the RigidBodyTree and passing them as input parameters to SimpleCar and TrajectoryCar during construction. This is why I'm backing down from seeking additional methods for specifying joint state by name and model instance ID.

[amcastro-tri]

@tkoolen: What I did in my Julia rigid body dynamics module was just store a map from joint to range (i.e. an object containing start index and length) in the equivalent of RigidBodyTree, instead of having this information be in the joint or rigid body itself

what's wrong with rigid bodies or joints knowing their start indexes in the world? For a simulation engine that is the fastest way to get this information and IMO much preferred to having a map (slow access).

@liangfok: One slight concern I have is systems that want to set the value of the port that's consumed by RigidBodyPlantLcmPublisher needs to have a reference to the RigidBodyPlantLcmPublisher's RigidBodyTree

Is that true? look at the pendulum example. As long as it's output is in the format that the RigidBodyTreeLcmPublisher needs no RBT is needed on the Pendulum side of things.

@liangfok: The implementation of these kinematic-simulation-systems and the MuxSystem could be aided if the BasicVector consumed by RigidBodyTreeLcmPublisher could have a semantically rich API rather than a long sequence of nameless doubles.

could be aided? I am confused by that statement. As far as I understand the RigidBodyTreeLcmPublisher does not use any "semantically richer" information. Does it need it?. If that is the original problem you are trying to solve I think it is already solved and none of the API's proposed above (like GetVelocities(const DrakeJoint& joint) are needed).

[tkoolen]

what's wrong with rigid bodies or joints knowing their start indexes in the world

First, if in the future there will be a 'maximum coordinates' mechanism class, then there may not even be a joint position/joint velocity vector to index into. This could also be the case for loop joints in a RigidBodyTree. Second, it doesn't make sense to me conceptually. If you just look at a single joint outside of the concept of a RigidBodyTree, then it doesn't really make sense to talk about its indices into some vectors that live outside of that joint; it's a strange conceptual cross-dependency. Third, I think map lookups are among the least of our performance worries, and if we have to start worrying about those, we've already won the game of creating efficient tools.

[amcastro-tri]

@tkoolen: First, if in the future there will be a 'maximum coordinates' mechanism class, then there may not even be a joint position/joint velocity vector to index into. This could also be the case for loop joints in a RigidBodyTree.

That is because those are "constraints" and what you call "joints" should probably be called "mobilizers" as @sherm1 advocated for so many years with a very mature piece of software like Simbody.

@tkoolen: Second, it doesn't make sense to me conceptually. If you just look at a single joint outside of the concept of a RigidBodyTree, then it doesn't really make sense to talk about its indices into some vectors that live outside of that joint; it's a strange conceptual cross-dependency.

That's because the joint always belongs to the tree, the tree owns it.

@tkoolen: Third, I think map lookups are among the least of our performance worries, and if we have to start worrying about those, we've already won the game of creating efficient tools.

I agree. However I always think that attention to details is what makes our work differentiate it from others. Besides, and again, what's so wrong with integer indexes when our final solving stage will use, no matter what, some sort of linear algebra package to solve our system of equations where entries are indexed by integers? we always need those indexes and having them in a useful place, out of reach from users, is always a good thing.

[jwnimmer-tri]

I assume this is WONTFIX. Please reopen if not.