RigidBodyTree::computePositionNameToIndexMap() Does Not Support Multiple Model Instances

[liangfok]

Problem Definition

RigidBodyTree::computePositionNameToIndexMap() returns a std::map where the key is a joint position's name and the value is its index within the RigidBodyTree's output state vector. Problems arise when the RigidBodyTree contains multiple instances of the same model since there will be duplicate joint position names, meaning there will be a key collision in the returned std::map. The same collision will occur when different model instances with overlapping joint names exist in a RigidBodyTree.

Side note: Unit test RigidBodyPlanTest, InstancePortTest happened to pass because it queries for "joint4", which only exists in the four_dof_robot.

Suggested Course of Action

Option 1: Add a model_instance_id parameter

We can modify the method to take a model_instance_id parameter and have it return a std::map containing only the joint positions that belong to the specified model instance.

Option 2: Modify the return value to include model instance ID information

We can modify the method to return a std::map<int, std::map<std::string, int>> . The outer map's key is the model instance id, the inner map's key is the joint's position name, and the value is the joint position's index within the RigidBodyTree's output state vector.

Option 3: Do both options 1 and 2

There's nothing stopping us from doing both of the above.

[liangfok]

@sherm1 I assigned you as the dynamics team lead. Please confirm that this is indeed an issue that needs fixing, and whether the suggested solutions are valid; feel free to delegate back to me to implement the fix.

@sammy-tri I assigned you since you were the author of the aforementioned unit test. Feel free to unassign yourself once you've confirmed my comments above about how the unit test happened to pass.

[amcastro-tri]

why would a user need this method in the first place? users should not know about indexes at all. The API should hide these implementation details.

[liangfok]

See usage in schunk_wsg/schunk_wsg_simulation.cc where it is used to derive a feedback selection matrix, which is index-based. It's also used in several other files to verify that the tree's structure is correct.

[sherm1]

@amcastro-tri, @liangfok: can we live with this as-is and deal with it properly in MultibodyTree or do we need an RBTree retrofit?

[liangfok]

It is not blocking me right now. I will be OK leaving this unresolved given that there is a TODO comment in place.

[liangfok]

Here is the TODO comment:

https://github.com/liangfok/drake/blob/8c97437919d0b96bb1493d81feb2ee3f44146dab/drake/multibody/rigid_body_plant/test/rigid_body_plant_test.cc#L504

I will submit a PR that adds a TODO to the method's documentation itself.