A possible workflow for tool changing in simulation

[huihuaTRI]

Problem statement

This issue tries to explain an approach for tool switching in simulation. This approach is not yet feasible with the current state of Drake, but probably only requires a small amount of functionalities for this particular tool change use case.

The main idea contains three steps.
Step 1: Define a robot model (via urdf/sdf) that contains all the necessary joints needed for both the robot and the tools. For example, a robot may use grippers (two joints) or wipes (no extra joint). Then the urdf/sdf should include the joint definition of the gripper, which has the most number of joints of all the tools the robot may use for this case. This allows the robot model to have the maximum number of states that the robot may need.

Step 2: In the urdf/sdf, set the necessary properties (for example, mass/inertia and size of the visual geometry) of the gripper, which are required to make the Drake parser happy. These default values do not matter. Essentially, these are just place holders for the properties of the tool links that we are going to modify later.

Step 3: In the code, the user could pragmatically set the actual properties (mass/inertial, visual and contact geometries, etc.) of a particular tool. Every time when the robot needs to switch to a different tool, the same routine will be called to update the properties of that tool.

Advantages and drawbacks

Advantages:

1. A possible shortcut to realize tool change in simulation without waiting the official Drake solution to be available. It is likely faster since there is no model reloading or model reconfiguration.
2. The robot will have fixed state size and tree topology. Only the properties of the tool link need to be updated.

Drawbacks:

1. The robot description will be inaccurate if the robot uses the non-default tools considering the number of degrees are different between different tools. For example, the finger joints and links of the gripper will still be in the MBP even though the robot now uses a wipe on the hand. The plant controller should aware that and do calculations accordingly.

Pseudo steps

To realize this approach, several functions are needed and some of them may already available in Drake. I just list them below to show the flow.

1. Load a valid urdf/sdf using Drake parser
2. ConfigToolProperties(const ToolParameters ) which includes
   a. ConfigToolMassAndInertia(const ToolParameters )
   b. ConfigToolVisualGeometries(const ToolParameters )
   c. ConfigToolCollisionGeometires(const ToolParameters )
   All the config functions should be able to change the transforms of the corresponding properties.
3. Finalize the MBP and run simulation
4. If an event is received to change the tool, pause the simulation and UnFinalize() the MBP.
5. Do Step 2 again with the new tool parameters.

An alternative of Step 4 is to clone the original plant (non-finalized version), then jump to Step 2.

Questions

1. Is Step 2 possible with the current state of Drake? Can we modify all the properties of a link before finalization?
2. I know Step 4 is not supported yet. How much effort would be needed to make this routine possible?
3. Is unfinalizing MBP the one thing required in order to do Step 2 during the simulation?

I am tagging multiple people here for discussion purpose. Thank you in advance!
@sherm1 @amcastro-tri @SeanCurtis-TRI @jwnimmer-tri @sammy-tri

[sherm1]

Also @joemasterjohn -- more parameterization!

[amcastro-tri]

Thank for the very clear problem statement @huihuaTRI!

@joemasterjohn is working currently towards (2) for mass and inertia properties.
Regarding (4), I don't think you need it! that's what's great about your proposed approach.
We might need however the "freeze" dofs capability @sherm1 mentioned in the past to make sure "unused dofs" do not go out of control. The "free dofs" capability would definitely be something nice to have anyway. What do you think @sherm1

[huihuaTRI]

Regarding (4), I don't think you need it! that's what's great about your proposed approach.

I don't fully understand which part is not needed. Are you saying that I could update the geometries without UnFinalize() the plant? Or you are saying that this is the case after everything is parameterized. Thanks.

[SeanCurtis-TRI]

I believe 4 isn't necessary because (and correct me if I'm wrong):

1. in MBP the things you're interested in changing become parameters which is merely changes in context-stored values, and
2. Changing geometries is also a context-value thing. "Simply" remove the old geometry and put in the new geometry. (How "simple" it really is still depends on some coordination between MBP and SG about what collision geometries exist. But I believe @joemasterjohn is currently working on having MBP learn about collision geometries directly from SG.)

[sherm1]

Regarding implementation of MBP::UnFinalize(): Finalize() does the following:

1. create a MultibodyTree model
2. allocates System resources (ports, cache entries, etc.)
3. does some internal precalculations
4. creates collision exclusion groups (in SG)
5. deletes the pointer to SG.

Items 1-3 would be easy enough to undo. The primary impediment is the interaction with SG. Of course deleting the pointer is fatal, but we could change that. The exclusion groups cannot currently be undone, and I'm not sure that can be fixed due to potential interactions with other exclusion groups. What do you think @SeanCurtis-TRI ?

My inclination at the moment is not to attempt unfinalizing, but rather focus on fast re-creation of a new (and unfinalized) MBP/SG pair.

[SeanCurtis-TRI]

The magnitude of the difficulty depends on what's happening with the unfinalized plant.

• A body gets removed. The corresponding frame and geometries get removed from SG (somehow in order to keep the systems compatible). So, not taking any explicit act for worrying about collision filters is largely harmless. Filtering contact with an existing geometry has the same effect as filtering contact with a non-existent geometry.
• A body is inserted
  • This would be a bit of a problem. If the original change were A -> B then geometries affixed to A and B would have been filtered from collision (because they are "adjacent" bodies).
  • When we insert a new link to get: A -> C -> B
    • Collision between the geometries between A and B should possibly be allowed (the adjacency justification for filtering contact is gone). There are two ugly issues here:
      • There is currently no SG API to undo collision filtering. Right now it's a one-way operation. Not the end of the world, but...
      • If there are redundant reasons why two bodies have been filtered (it might be declared that way in the SDF), then just because they are no longer adjacent doesn't mean we should undo the filtering. There is no infrastructure for being able to puzzle out this case.
    • There should be collision filtering between (A, C) and (C, B). But MBP cannot do it because it no longer has a SG pointer. SG can't do it because it doesn't know anything about MBP adjacency. So, someone else would have to do it. This is resolved simply by making the SG available to MBP when it wants to "finalize" the change.

[huihuaTRI]

After multiple offline conversations, here is my understanding

Requests of this issue can be summarized into two items.

• R1) modify mass/inertia properties of a certain link/body. This request includes both the values of these two properties and the transformation from the CoM to the link.
• R2) remove existing collision/visual geometries and add new collision/visual geometries.

Solutions to this issue

• S1) Joe is working on parameterizing the mass/inertia properties, which should solve R1. This does not require unfinalize.
• S2) Sean and Joe are also working on to remove the geometry information (the map between geometry ID and body index) stored inside MBP. Hence, I could add/remove geometries in SceneGraph freely. Essentially, R2 will have no string attached to MBP anymore, therefore, no unfinalize is required.

Questions remaining:

• Q1) Would the transformation between the link and the CoM be parameterized also? @joemasterjohn
• Q2) Thanks for Sherm bringing up a good question, i.e. how to update the collision filter group. In this case, there will be no bodies added or removed. Is it possible to tell SceneGraph that the geometries of this body have been changed, please update the associated collision filter group accordingly?

[sherm1]

@SeanCurtis-TRI: yeah, that matches how I see the problem. I don't think anything like that is feasible. A robust UnFinalize() method would have to put MBP and SG precisely back into the state they had prior to Finalize(). That would need to be done by something like a stack that could be popped.

[SeanCurtis-TRI]

Essentially, R2 will have no string attached to MBP anymore, therefore, no unfinalize is required.

Not quite true. In the text above I confronted the question of collision filtering. Currently, collisions between adjacent bodies is not allowed. That gets enforced at finalization. If you delete geometries (that had their filter settings set by the original call to Finalize()) and add new geometries, those new geometries will not respect the "do-not-collide-with-adjacent-geometry" rule.

In order to patch that, you'd have to

1. identify the bodies adjacent to the body you're modifying. (I don't know if we have an API for that.)
2. Get the FrameId for each of those adjacent bodies and place them in a GeometrySet.
3. Place the FrameId for the modified body in a a different GeometrySet.
4. Invoke SG::ExcludeCollisionsBetween with the two sets.

[huihuaTRI]

The existence of Q2 basically contradicts the statement "no string attached" of R2. Thanks for answering Q2.

I will investigate whether "identify adjacent bodies" is available. If not, it won't be an issue for this use-case since pretty much everything is already pre-defined.

[SeanCurtis-TRI]

The point that I was trying to make is identifying adjacent bodies is an issue. Without doing so, you risk having the tool fly off when it experiences artificial collision with the arm end.

[sherm1]

This has been partially addressed by parameterization. Ongoing improvements to remodeling will eventually solve tool changing as a side effect. Closing.