No information on layout of model instance arrays in MbP

[edrumwri]

It appears to be impossible at the moment to determine how position, velocity, and actuator indices map into a model instance array (that returned by, e.g., GetPositionsFromArray(ModelInstanceIndex,...)).

It is possible to get the mapping from joint positions/velocities and actuators into the full MBP position, velocity, and actuation arrays. For example:

VectorX q = plant.GetPositions(context);
JointType t = plant.GetJointByName("my_joint");
q[t->position_start()] = 1.0;
plant.SetPositions(&context, q);

Using this example, here's what I can't currently do:

VectorX q = plant.GetPositions(context, model_instance);
JointType t = plant.GetJointByName("my_joint", model_instance);
q[t->position_start_in_model_instance()] = 1.0;
plant.SetPositions(&context, model_instance, q);

[amcastro-tri]

@sammy-tri, do you have any good suggestions here?

[sammy-tri]

It's never actually been a problem for me, though I admit the functionality is lacking. Up to this point I've generally been dealing either with model instances where the ordering is trivial (the 7 joints along the iiwa arm, 2 on the Schunk gripper, or free bodies where MBP handles the pose for me), and/or controllers which have their own plant with one (not including the world) model instance, in which case the state/actuation vectors for that plant correspond to the instance vectors in the larger plant being simulated.

This seems like it's only a problem in the case where you've got access to q_instance but not the full q? If you have access to q, and a reference to the joint, just go ahead and get/set it.

[EricCousineau-TRI]

@sherm1 I'm gonna semi-hijack this issue to discuss usability issues with ModelInstanceIndex in MultibodyPlant, per this discussion: https://github.com/robotlocomotion/drake/pull/12839#issuecomment-606752290

Basically, model instances are, in my mind, unduly coupled to construction. e.g. What if I have a controller that wants to an arm and a gripper as a unit, but I parse them from separate files via some form of composition?

I effectively have to infect the API (either in MBP or downstream) to deal with lists of model instances, lists of bodies / positions, etc. (In my mind, compiling them into one model description (e.g. via xacro or other forms of glorified text preprocessing) is a hack, and should always be labeled as such.)

It's totally usable in this current state, but does require a whole bunch of extra bookkeeping.

It would be nice if (a) model instance tracking were proper objects, not just indices, and (b) if they provided some form of non-exclusive composition (e.g. hierarchy for "arm", "gripper", "arm+gripper", etc.).

FYI @jwnimmer-tri on having proper objects (but sorry to blare noise on other things)

[sherm1]

@EricCousineau-TRI I don't think extending "model instance" is the right approach. I'm fine with that limited, construction-associated feature as is. A more-general mechanism is needed for constructing arbitrary, overlapping sets of MultibodyElements. For example, if we were able to associate an arbitrary set of tags with any element, it would be easy to construct sets of elements "with tag1 and tag2 but not tag3" and that sort of thing.

The original complaint in this issue was about (lack of) introspection. Not clear to me how that is related to your comment above?

[edrumwri]

It would be nice if (a) model instance tracking were proper objects, not just indices, and (b) if they provided some form of non-exclusive composition (e.g. hierarchy for "arm", "gripper", "arm+gripper", etc.).

A related problem to that which @EricCousineau-TRI broached is computing kinematics and dynamics quantities for a subset of MBP. The prevalent pattern for dealing with this in Drake has been to construct a MBP(+SG) for each combination of bodies that one is interested in (e.g., a MBP+SG containing a robot and manipulands for simulation plus a MBP containing only the robot for inverse dynamics control). That pattern works poorly if the sets of bodies of interest are variable. Since MBP+SG construction times are significant when parsing from SDF (see #12703), this is a significant problem for us.

Happy to open an issue for this.

[sherm1]

Happy to open an issue for this.

That would be good. I'd be interested to know the specific list of quantities that would be useful for arbitrary sets of bodies, or more generally arbitrary sets of MultibodyElements. Some things wouldn't make sense for unconnected subsets.

[jwnimmer-tri]

Closing as "not planned".

The workaround is to call the full plant.GetPositions(model_instance) instead of the subset plant.GetPositions(context, model_instance), if there is a need to pull out specific joint positions after the fact.