MbP introspection

[amcastro-tri]

A number of user requests regarding topology and state introspection for an MBP model have been made. These include:

• [x] Retrieve the floating base bodies in an MBP. Proposed API: std::vector\<BodyIndex> GetFloatingBaseBodies().
• [x] Free body state introspection. Proposed API: Body::is_floating_base(), Body::is_quaternion_floating_base(), Body::get_position_start(), Body::get_velocity_start(), etc. Where the get_foo_index() methods would throw if the body is not a floating base.
• [x] Get the base for an articulated body for a given model instance (per request in #9747). Proposed API: BodyIndex GetUniqueBaseBody(ModelInstanceIndex instance).
• [ ] MBP::PrintStateNames(), MBP::PrintStateNames(ModelInstanceIndex) (or similar name), prints a table of "index into the state vs. state name".
• [ ] MBP::PrintInputActuationNames(), MBP::PrintInputActuationNames(ModelInstanceIndex) (or similar name), prints a table for input vectors u, also index vs. actuator name (it could also include actuated joint name).
• [ ] std::string MBP::get_state_name(int i), to retrieve the name of the i-th entry in a bare state vector.

cc'ing @mposa, @RussTedrake, @sammy-tri, @siyuanfeng-tri

[amcastro-tri]

11257 allows making some of our "physical representation" queries pre-finalize. Working on the design of model-specific queries with @sherm1, who has a draft in #10068.

[amcastro-tri]

First two bullets addressed in #11464

[amcastro-tri]

pumped priority to high go address ASAP given the high number of request for something like this.

[EricCousineau-TRI]

Is this still alive? (or high priority)

[sherm1]

It's been a year, so not high priority. But still needed so I lowered the priority.

[xuchenhan-tri]

Working on this issue as a side project now, and I want to clarify the request a little bit. For the fourth item:

MBP::PrintStateNames(), MBP::PrintStateNames(ModelInstanceIndex) (or similar name)...

What information of the state is expected? I'm thinking the name of joint/free body and whether it's a position or velocity dof. Is this reasonable? Anything else? @hongkai-dai, I saw you recently posted a thread in slack that's related. Do you want to share what would be helpful to you?

[sherm1]

I'm think "print" here is metaphorical -- we don't want methods that literally print anything. That brings up awkward issues of formatting and where to write the output. Whatever we decide is the right information it should come back as an std::vector<string> or something like that so we can let the caller figure out the tricky stuff. The value here is in the information returned.

[xuchenhan-tri]

Yes, I agree that we don't want to actually "print", and I was planning on returning a std::vector<string>. I'm not sure, however, what exactly is the information of interest. Same goes for the fifth item: anything else other the joint and actuator name?

[joemasterjohn]

I think the relevant information for each coordinate would be:

For joints:

<joint_name>_<local_joint_coordinate_name> and <joint_name>_<local_joint_coordinate_name_dot>

For example:

UniversalJoint3_theta1 UniversalJoint3_theta1_dot

And for a free body:

freebody1_q_FM_{w,x,y,z} freebody1_p_FM_{x,y,z} freebody1_w_FM_{x,y,z} freebody1_v_FM_{x,y,z}

Having _FM would follow the multibody quantity nomenclature, but currently for all freebodies F is the world frame and M is that body's BodyFrame.

For the joints you could add the method virtual void Joint::AddInStateNames(std::vector<std::string>*) and have each joint do its own thing. For the free bodies, you'll have to just iterate over them and grab their mobilizer to index into the coordinate name vector (MultibodyTree::GetFreeBodyMobilizerOrThrow and Mobilizer::position_start_in_q). You could probably encapsulate this all in a method like MultibodyTree::AddInFreeBodyStateNames(std::vector<std::string>*). What do you guys think?

[sherm1]

Would be good to get some end-user input as to what would be most useful. Fishing: @ggould-tri @hongkai-dai @avalenzu @EricCousineau-TRI ...

[jwnimmer-tri]

See also #5857 for the framework angle -- dumping the context may be another way to debug, instead of MbP methods.

Or, if we're going to provide human-usable-name getters for vector indices, it might also make sense to define those accessors on the system framework (ports, state, etc.) so they are usable generically for more than just the MbP (e.g., the inverse dynamics controller vector index names would help a lot). I thought there was an issue filed for that already, but I didn't immediately find it.

[RussTedrake]

As discussed in this afternoon's meeting... I would like to propose a step towards this for a slightly simpler/streamlined version of @joemasterjohn 's proposal above.

This is based on an example I've been working with doing optimization for littledog. In particular, I have the following methods that proved super useful:

def MakeNamedViewPositions(mbp, view_name):
    names = [None]*mbp.num_positions()
    for ind in range(mbp.num_joints()): 
        joint = mbp.get_joint(JointIndex(ind))
        # TODO: Handle planar joints, etc.
        assert(joint.num_positions() == 1)
        names[joint.position_start()] = joint.name()
    for ind in mbp.GetFloatingBaseBodies():
        body = mbp.get_body(ind)
        start = body.floating_positions_start()
        body_name = body.name()
        names[start] = body_name+'_qw'
        names[start+1] = body_name+'_qx'
        names[start+2] = body_name+'_qy'
        names[start+3] = body_name+'_qz'
        names[start+4] = body_name+'_x'
        names[start+5] = body_name+'_y'
        names[start+6] = body_name+'_z'
    return namedview(view_name, names)

def MakeNamedViewVelocities(mbp, view_name):
    names = [None]*mbp.num_velocities()
    for ind in range(mbp.num_joints()): 
        joint = mbp.get_joint(JointIndex(ind))
        # TODO: Handle planar joints, etc.
        assert(joint.num_velocities() == 1)
        names[joint.velocity_start()] = joint.name()
    for ind in mbp.GetFloatingBaseBodies():
        body = mbp.get_body(ind)
        start = body.floating_velocities_start() - mbp.num_positions()
        body_name = body.name()
        names[start] = body_name+'_wx'
        names[start+1] = body_name+'_wy'
        names[start+2] = body_name+'_wz'
        names[start+3] = body_name+'_vx'
        names[start+4] = body_name+'_vy'
        names[start+5] = body_name+'_vz'
    return namedview(view_name, names)

So specifically:

• Joints with num_positions==1
  • elements in a list of positions appear as joint.name()
  • elements in a list of velocities (only) appear as joint.name()
  • elements in a list of positions / velocities could appears as joint.name() / joint.name() + "dot".
• Joints with num_positions>=1
  • elements in a list of positions appear as joint.name() + "_" + joint.position_suffix(k)
  • elements in a list of velocities (only) appear as joint.name() + "_" + joint.velocity_suffix(k)
  • joint.velocity_suffix(k) != joint.position_suffix(k)
  • elements in a list of positions / velocities could appears as joint.name() + "_" + joint.position_suffix() / joint.name() + "_" + joint.velocity_suffix().

Proposal: Add the following new methods to Mobilizer:

virtual string position_suffix(int position_index_in_mobilizer) {
  DRAKE_DEMAND(num_positions==1);
  return "";
}

virtual string velocity_suffix(int velocity_index_in_mobilizer) {
  DRAKE_DEMAND(num_velocities==1);
  return "";
}

with overloads in mobilizers with num_positions>1, so that, e.g.

virtual string QuaternionMobilizer::position_suffix(int position_index_in_mobilizer) {
  switch(position_index_in_mobilizer) {
    case 0: return "qw";
    case 1: return "qx";
    ...
  }
}

@sherm1 , @amcastro-tri , @joemasterjohn (et al) - WDYT?

[sherm1]

Thanks for this proposal! A few thoughts:

• For 1-dof joints, consider giving them a suffix anyway for uniformity: elbow_q and elbow_v for example. I think that would be better than overloading "elbow" to mean all of: the joint, its position coordinate, and its velocity coordinate.
• I don't understand your proposal regarding "_dot" above. Normally we have q's and v's and the v's are not necessarily the same as qdot. Are you proposing to give access to q time derivatives that way, independent of whether they are actually v's? Also I'm not clear why you need different names for a list of joint positions and velocities -- the names are already distinct. Can you clarify that part of the proposal?
• nit: if you do need names for time derivatives, consider just "dot" rather than "_dot" since we usually just say qdot and vdot without the extra syntax. Quaternion time derivative would be qwdot, qxdot, etc.

[RussTedrake]

Thanks @sherm1

• I dislike the extra suffix for 1-dof joints. If you looked at that example, think of how much bloat that would add.
• I've updated the proposal above w.r.t. dot. I agree it was clunky.

Another alternative:

• joints with num_positions==1
  • positions by default appear as joint.name()
  • velocities by default appear as joint.name() + "dot"
• joints with num_positions > 1
  • positions always appear as joint.name() + "_" + joint.position_suffix(k)
  • velocities always appear as joint.name() + "_" + joint.velocity_suffix(k)
  • joint.velocity_suffix(k) != joint.position_suffix(k)

Maybe that's better.

[sherm1]

I do like the alternative better since the names don't change in context. Still some issues:

• The "dot" convention requires that the velocity variable is the time derivative of the position variable for 1-dof joints. That is very common (N there is 1x1 and you are requiring N=[1]) but a joint that incorporates scaling or a direction change can have N≠1. (We did toy with a joint like that for a motor model that Rick was considering but ultimately didn't use it.) We could ignore that but FTR I don't like it. I'd rather see "_v" than dot since it would always be correct (and would match the convention used for multi-dof joints).
• I still don't like the overloading of "joint_name" to mean both the joint and its position variable in the 1-dof case. Perhaps you can say for sure that this isn't a problem in use; I can't.
• nit: if you don't have any suffix and still want to use "dot", I think it should be "_dot". That is, I liked elbow_qdot but I don't like elbowdot with no separator.

As a multibody dynamicist I don't like treating 1-dof and n-dof joints differently and would use the same conventions for both. I can see that a roboticist may feel differently!

[RussTedrake]

To be clear, the proposal is that the default suffix for positions is "" and the default suffix for velocities is "dot" (which I had changed from "_dot" on your request?). That default will throw if num_positions != 1. But even a single DOF joint can overload it.

I'm happy to hear other opionons about "elbow" and "elbowdot" (or "elbow_dot") vs "elbow_q" and "elbow_v". But it seems like the basic setup is ok, so I will open a PR and we can easily change the default strings in the PR discususion.

[amcastro-tri]

This looks great @RussTedrake. My 2 cents:

• I also like the idea of all mobilizers having a suffix for uniformity, even if only of 1dof. I personally like "_q" and "_v" with option to overload. If those suffixes are always there, then dofs will always have the same name regardless of whether they show up in a vector of positions only, velocities only, or full state. I like that homogeneity. I believe the bloat you mention is only two extra chars?
• Personally, I do not like "dot" for two, small though, reasons: 1) true as @sherm1 mentioned that not always v=qdot, though as you pointed out, this design allows us to overload the names and 2) "_dot" is longer than "_v"? less bloat?

[EricCousineau-TRI]

Minor concern: Using _ as a delimiter is understandable for simplicity, and as a valid variable name, but perhaps there's another delim, like a period .? e.g. my_spherical_joint.rx, my_spherical_joint.rx_dot, etc?

[amcastro-tri]

nice!, I like @EricCousineau-TRI's idea. It has the advantage of allowing the _ to be used in the variable name!

[RussTedrake]

we discussed that in the dynamics meeting. doesn't it interfere with other syntactical structure? like in python, i would have my namedview class accessing an element by a string with a '.' in the middle?

[RussTedrake]

and of course you can use _ in the variable name either way.

[EricCousineau-TRI]

While it'd be a complication in namedview, I think it'd be a minor complication; it should be kinda trivial to quickly teach it how to nest fields in a Pythonic way; just some meta-programming hand-waving that Python makes easy ;)

[RussTedrake]

not anywhere as easy as just sticking with _?

[EricCousineau-TRI]

Yeah, but would be a flattening of hierarchy where you wouldn't necessarily need flattening. But yeah, I'm fine either way.