Drake: Need methods to extract unique floating body for a given model instance without requiring the name

[RussTedrake]

For example, I would like to add an object to manipulate into my plant, then set it's free body pose in the context. To accomplish this, I currently need to look into the sdf file and read out the name of the base link, call "GetBodyByName" and then "SetFreeBodyPoseOrThrow".

For this particular use case (which is an important one), it would be nice to have "SetFreeBodyPoseOrThrow(model_instance, pose, context)" which sets the pose of the (floating) base joint assuming that this model has exactly one floating base.

More generally, I feel unable to extract basic topology information related to a particular model instance of the tree.

[sherm1]

How about a method GetUniqueBaseBody(model_instance) which would return the body that connects that model instance to World (if unique) or else throw? What other per-instance topological information should be extractable from MBPlant?

[amcastro-tri]

A start liking that idea better. With a small caveat. Something like GetUniqueBaseBody() exposes a given preferential tree structure in our representation of the physical world. It is true for a generalized coordinates representation but it might not be true say for a maximal coordinates representation (in case we are interested in some future of providing that).
Another example; SDF says nothing about a tree structure, why would a user assume that there is a "base body"?

[sherm1]

I think it is OK to expose the idea that there are "multibodies" used by MBPlant to model the given sdf (it is kind of implied by the name!). We don't have to say exactly how we go from the input sdf to a set of multibodies, but having an API that can be used _in case_ there is a convenient multibody available makes sense. In Russ's case he's adding single free bodies so the multibody is identical to the maximal coordinate model.

[RussTedrake]

I think GetUniqueBaseBody is good to have. I still like the idea of having some sugar that sets the free-body pose of a model instance with a unique "free" base. I would say that, right now, the code for setting up the initial context is the most distressing part of the python workflow for people, and it's a good place to focus on making things nicer.

Regarding other API: I don't believe there is a way to get even, e.g. a list of bodies associated with a particular model instance? But if you guys don't see obvious missing API for working on the topology from a model instance, then I'm ok with restricting this feature request to making the SetFreePose workflow better; I'll add additional specific requests as I have them.

[RussTedrake]

fyi -- I've added a TODO for calling GetUniqueBaseBody in my geometry_inspector PR. #9926.

[rcory]

+1 for this. To reinforce this further, I've been looking over the manipulation station python code, and was particularly surprised at the amount of code required to set the initial pose for the brick (a single floating body):

    station.get_multibody_plant().tree().SetFreeBodyPoseOrThrow(
        station.get_multibody_plant().GetBodyByName("base_link",
                                                    object),
        X_WObject, station.GetMutableSubsystemContext(
            station.get_multibody_plant(),
            station_context))

I would have expected this to be much simpler. For example, in our RBT/P (C++) version, the WorldSimTreeBuilder made this particularly easy:

tree_builder->AddFloatingModelInstance("object", Vector3d(0, 0, 0), Vector3d(0, 0, 0));

which seems much more reasonable, especially given this is such a common use case. In general, I think it would be worth considering providing equivalent convenience methods to what WorldSimTreeBuilder previously provided for RBT/P.

[amcastro-tri]

@EricCousineau-TRI, I know you are not very fond of this proposed workflow. However, I do think we need a better API to set the pose (and spatial velocity) of a free body in the model. Do you have any ideas on how you'd like this to look like? for instance, @sherm1 suggested the implementation of a method like GetUniqueBaseBody(ModelInstanceIndex)

[EricCousineau-TRI]

To clarify, I am a fan of this workflow for accessing the coordinates, like pose and velocity of a body.

The part of the workflow I'm not a fan of is querying the implicit base body to then weld it or affect the model construction; @RussTedrake I can explain this part more if ya like.

And yeah, I'm fine with something like GetUniqueBaseBody(ModelInstanceIndex).

---

As a complete aside and FYI, Gazebo uses the term "canonical link" in its code base (not advocating for this name, though):
https://github.com/devrt/gazebo-mirror/blob/16589c29302bfec51079f62dd247864be54b71d3/gazebo/physics/Model.cc#L130-L132

[EricCousineau-TRI]

Upon re-reading this (xref from #13148), I would be fine if welding (via base body obtained from this method) had zero implications on the position of the body. Example: it just freezes in place at its current position.

What I don't support is having it change the positions of links, if we don't have a deterministic way of identifying this body. Example issue: it's super sensitive to tree breadth / depth, or has a non-deterministic (non-designed) determination.
If we do have unittested rules for ensuring that the base body is easy to predict (or specify), then yeah, I'd be totally down.
(And to reiterate: I totally support this in terms of how we specify q for stuff like optimization.)

For example, in SDFormat, there's now a __model__ frame, which is explicitly defined:
http://sdformat.org/tutorials?tut=pose_frame_semantics_proposal#2-2-referencing-the-implicit-model-frame-via-__model__-or-model-name

If you're using SDFormat files, then you could literally just get GetFrameByName("__model__", model_instance).
That won't translate well to URDF or custom-constructed MBP's, but it's a start.

---

As an uber minor update, the term "canonical" link is part of SDFormat 1.7:
http://sdformat.org/tutorials?tut=pose_frame_semantics_proposal#2-model-frame-and-canonical-link
It's used primarily for how the "model frame" is interpreted rather than how "islands" of floating bodies interact, so not super relevant here...

[xuchenhan-tri]

Reviving this as part of the effort towards #10736. The proposed solution is to provide GetUniqueBaseBodyOrThrow(ModelInstanceIndex), but not SetFreeBodyPose(Context*, ModelInstanceIndex, const RigidTransform&), which can be achieved by SetFreeBodyPose(context, GetUniqueBaseBodyOrThrow(model_instance), X_WB). Thoughts?

[jwnimmer-tri]

Just as a general process note -- when evaluating API trade-offs, I always find it useful to look at specific, fully-worked calling code examples. "If we do A, then the calling code would like [fully worked example 1]; if we do B, then the calling code would look like [fully worked example 2]." I often find it's too easy for people to mis-estimate what they really want when only speaking in the abstract. (Certainly, we should _also_ consider the question in the abstract and in terms of API uniformity, or making methods orthogonal, or others trade-offs, but part of the discussion should _always_ be fully-worked example, to help ground the final result.)

So specifically, in #14450's discussion of whether the method should even exist, it would help make the case for the value of the sugar if you _also_ ported a few call sites to use it. If there are no call sites for the sugar that could be ported, that is evidence that the sugar is not useful. (It's possible that the call sites to port are not in the Drake source tree, but even so you could post links to those potential future diffs.)

[sherm1]

Actually even that doesn't seem right. I looked through MBPlant and we have this already:
GetFloatingBaseBodies() but we don't have GetFloatingBaseBodies(ModelInstance). What's not captured above is that only _floating_ (or _free_) base bodies permit setting an arbitrary pose. So even if a ModelInstance has a unique base body, but that body is (say) welded to World, SetFreeBodyPose() won't work.

(BTW we have a naming inconsistency here between "Floating" and "Free". "Free" seems to be winning now so we may want to rename the existing method to GetFreeBaseBodies(). I'll assume that below.)

In any case I think what is needed is a way to find the unique, _free_, base body in a ModelInstance if there is one. And we also need _some_ way for the calling program to determine whether there is a unique, free body without having to get an exception thrown. GetFreeBaseBodies(ModelInstance) alone would be sufficient since it can be used to find the unique, free base body if there is one. Then GetUniqueFreeBaseBodyOrThrow(ModelInstance) would likely be useful sugar since its return can be fed directly to SetFreeBodyPose().

Jeremy's suggestion of looking at real code seems like a good way to nail this down to reality.

[sherm1]

One other design loose end we should tie down is what it means for a ModelInstance to have a base body. That is inherently a property of a tree (it is the "root" body). Maybe we don't care about that and the question is just whether any of the bodies with a given ModelInstance tag are themselves free bodies? That is a simpler question and avoids having to decide what to do with model instances that contain bodies but not the joints that interconnect those bodies. (We allow any MBPlant element to belong to any model instance.)

[xuchenhan-tri]

From the documentation for GetFloatingBaseBodies(), it looks like we don't care whether the bodies are "base bodies" or not, rather, we only care whether they are "free/floating". Following that, it seems reasonable to me to provide const Body<T>& GetUniqueFreeBodyOrThrow(ModelInstance). In particular, we do not mention anything about "base". To @sherm1 's point, I don't have enough background to know if there's value in getting the free bodies associated with a model instance (via GetFreeBodies(ModelInstance)), if only the free body that we care about are the unique ones, then it seems more reasonable to provide bool HasUniqueFreeBody(ModelInstance)?

I will work on porting call sites as @jwnimmer-tri suggests.

[sherm1]

As long as ~GetFreeBaseBodies()~ GetFloatingBaseBodies() returns only bodies whose parent body is World, those are by definition base bodies (root of a tree connected to World). Currently we don't permit explicit specification of Free (or Floating) joints, so all the ones present were added under the covers by us and we always connect them to World. Later, if we allow FreeJoint connections between bodies (and there are occasionally good reasons to allow that) then this method should restrict itself to the ones with World as the parent body.

But we should decide and document whether the new method is restricted to base bodies or not. The point of a unique base body is that moving it moves the entire model instance as though it were a rigid object. If that is what our application users are expecting, they won't get the right result from a method that simply finds all the free bodies. Even if there is only one free body, there could also be other base bodies (for say a robot arm attached to World) but that piece of the model instance won't move even if the free body is moved.

My suspicion is that users are actually looking to move the entire model instance, but I'm not certain. @RussTedrake as the OP can you say what semantics you are looking for with a request to find a model instance free body? Are you expecting to find that you can relocate the entire model instance by moving that body?

[xuchenhan-tri]

I see, thanks for the explanation @sherm1! I was assuming that a body being "base" is a necessary but not sufficient condition for a body being "floating/free" base on the implementation of GetFloatingBaseBodies() which looks to me like is not checking whether the parent body is World.

[sherm1]

Right, but since all Free joints currently are connected to World, and since this method doesn't imply that there aren't also other base bodies (via "unique"), the implementation is likely correct for now. Its documentation should note that there might be other, non-free, base bodies in existence and there ought to be a TODO saying it should be checking for inboard body=World if free joints are added as a user-specifiable joint type.

[xuchenhan-tri]

Summarizing the discussion here and in the review #14450, it looks like we should at least provide GetUniqueFreeBaseBodyOrThrow(ModelInstance) that throws unless 1) there exists a unique base body in the ModelInstance and 2) that base body is free. That would at least help with @rcory 's point (although that was already 2 years ago, not sure if it's still relevant).

[sherm1]

Can we get confirmation from @rcory and @RussTedrake ?

(And we also need some way for a programmer to check whether a throw would occur if that method were called since catches are prohibited. HasUniqueFreeBaseBody() or ?)

[xuchenhan-tri]

I want to give this issue an update before the change in #14450 gets too out of sync with master.

14450 will provide two new public methods for MbP:

1) HasUniqueFreeBaseBody(ModelInstance), which determines whether there exists a unique free base body associated with the given model instance, and
2) GetUniqueFreeBaseBodyOrThrow(ModelInstance), which retrieves the unique free base body if one exists.

These methods will enable the following work flow to set the free body pose of the unique free base body given a model instance tag:

if (HasUniqueFreeBaseBody(model_instance) {
  SetFreeBodyPose(context, GetUniqueFreeBaseBodyOrThrow(model_instance), X_WB);
}

This should at least make setting free-body pose of a model instance with a unique "free" base easier than it is now as it doesn't require a name look-up.

[EricCousineau-TRI]

Er, it looks like there are currently two possible parts to this issue:
(1) The *UniqueBaseBody*() functions - which I think Xuchen has well under way
(2) More general Get{Element}s(ModelInstanceIndex) like functions could be in scope, per the title and a free-wheeling interpretation of the last line of Russ's original post :P

It kewl if we downscope this issue to just (1)? (i.e. rename the issue to "Need methods to identify the free-body for a model instance"?)
(2) seems like it could be another issue.

[sherm1]

Yes, I think we should declare that Xuchen's fix closes this issue and open another one if necessary.

[EricCousineau-TRI]

Sweet! I went ahead and modded the title too.