Improve Panda arm/hand collision model and filters

[calderpg-tri]

This change is 

[calderpg-tri]

Changes to the Panda collision model(s) and collision filtering, specifically:

1. Adds the additional collision spheres which we have been adding in-code to match the internal self-collision model to the URDF itself as two "extra" links attached to links 2 and 5.
2. Adjusts the collision filtering, particularly around the wrist assembly, so that collisions with the outboard end of link 5 are filtered while collisions with the inboard "extra" on link 5 are not.

See Anzu#15069 for accompanying internal code changes.

[calderpg-tri]

+@jwnimmer-tri for review or delegation, thanks!

cc @abeaulieu-tri @rcory

[calderpg-tri]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri, platform LGTM missing (waiting on @calderpg-tri and @jwnimmer-tri)

---

a discussion (no related file):
working This should not land until Anzu CI has run against it.

[jwnimmer-tri]

BTW I can review the change for suitability w.r.t. Drake, but I assume your objective here is to fix something in Anzu for the platform team, which I can't speak for -- if you want Anzu-relevant review to happen here, you'll need to add an additional reviewer for that perspective.

Reviewable status: 2 unresolved discussions, LGTM missing from assignee jwnimmer-tri, platform LGTM missing (waiting on @calderpg-tri)

---

a discussion (no related file):
Are these changes appropriate for use by simulation? For simulation, collision filters are helpful when the proximity geometry (or less often, joint kinematics) are modeled imprecisely and so would have false positives (i.e., nonsense physics) while computing contact forces.

The phrase "internal self-collision model" above makes me wonder if at least part (1) of this change might be specific to a plant used for control or planning (i.e., a collision avoidance model), instead of a simulation model.

(We do have the ability to use xacro codegen in drake_models, in case that's helpful.)

[calderpg-tri]

+@abeaulieu-tri for review as platform representative
+@rcory for review as sim representative

Reviewable status: 2 unresolved discussions, LGTM missing from assignee jwnimmer-tri, LGTM missing from assignee abeaulieu-tri, LGTM missing from assignee rcory, platform LGTM missing (waiting on @abeaulieu-tri, @jwnimmer-tri, and @rcory)

---

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Are these changes appropriate for use by simulation? For simulation, collision filters are helpful when the proximity geometry (or less often, joint kinematics) are modeled imprecisely and so would have false positives (i.e., nonsense physics) while computing contact forces.

The phrase "internal self-collision model" above makes me wonder if at least part (1) of this change might be specific to a plant used for control or planning (i.e., a collision avoidance model), instead of a simulation model.

(We do have the ability to use xacro codegen in drake_models, in case that's helpful.)

The "internal self-collision model" is that of the Franka control box itself, which faults on violations of its internal self-collision model (the geometry of which, to the best of our knowledge, is completely undocumented). Obviously it is most important that these changes are used by planner(s)/controller(s) so that the robot is never commanded into the problematic configurations in the first place.

I'm not opposed to having separate sim and planning versions of the Panda models, but I would argue that the change in sim behavior that results from these changes doesn't matter that much, since the configurations affected are those that are effectively unreachable on the real hardware anyways. If anyone is depending on sim behavior there, it will never make a sim->real transition.

In terms of the filters, the within-arm filters are necessary for both planning/control and simulation, since the extra links must be manually filtered as they are not automatically filtered as being kinematically adjacent. Hand filters are included for consistency, and practically speaking, and on the as-built hand with factory fingers, collisions between the fingers are impossible as the contacting configuration is the zero limit once the hand has initialized. (Also, I suspect basically no one is using the supplied fingers used in these models at all.)

[jwnimmer-tri]

I am going to let the other reviewers take the first pass. I imagine by the point it circles back to me I'll just platform-review stamp it too and we can merge.

Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri, LGTM missing from assignee abeaulieu-tri, LGTM missing from assignee rcory, platform LGTM missing (waiting on @abeaulieu-tri, @calderpg-tri, and @rcory)

---

a discussion (no related file):

Previously, calderpg-tri wrote…

The "internal self-collision model" is that of the Franka control box itself, which faults on violations of its internal self-collision model (the geometry of which, to the best of our knowledge, is completely undocumented). Obviously it is most important that these changes are used by planner(s)/controller(s) so that the robot is never commanded into the problematic configurations in the first place.

I'm not opposed to having separate sim and planning versions of the Panda models, but I would argue that the change in sim behavior that results from these changes doesn't matter that much, since the configurations affected are those that are effectively unreachable on the real hardware anyways. If anyone is depending on sim behavior there, it will never make a sim->real transition.

In terms of the filters, the within-arm filters are necessary for both planning/control and simulation, since the extra links must be manually filtered as they are not automatically filtered as being kinematically adjacent. Hand filters are included for consistency, and practically speaking, and on the as-built hand with factory fingers, collisions between the fingers are impossible as the contacting configuration is the zero limit once the hand has initialized. (Also, I suspect basically no one is using the supplied fingers used in these models at all.)

I will give it some more thought to be sure, but that all sounds basically reasonable.

---

a discussion (no related file):
The URDF files and the README need lots more comments, so that is crystal clear to future readers (1) how our URDF diverge from the upstream URDS and (2) why we've done that. Basically all of the expository text in the prior thread needs to appear in git, not reviewable. Probably both with inline comments in the URDFs, and some reminder in the README.

[rcory]

Reviewed all commit messages.
Reviewable status: 4 unresolved discussions, LGTM missing from assignee jwnimmer-tri, LGTM missing from assignee abeaulieu-tri, LGTM missing from assignee rcory, platform LGTM missing (waiting on @abeaulieu-tri, @calderpg-tri, and @jwnimmer-tri)

---

a discussion (no related file):
So we are clear on what the changes actually look like, I'm including some snapshots of the before & after collision geometries below.

My main concern is that these new collision geometries are grossly mismatched with the actual robot body. Perhaps that is ok for self-collision issues, but what about external contact with other items in the scene (e.g., manipulands, or a second arm)? These collision just seem too far off reality from a physics/contact standpoint.

I'm inclined to advocate that these changes instead go into the planning/control model, rather than the physical simulation model.

@dmcconachie-tri and @siyuanfeng-tri What are your thoughts?

For reference, here are the proposed new collision geometries:

All collisions shown, before this change:

All collisions shown, after this change:

[siyuanfeng-tri]

Previously, rcory (Rick Cory) wrote…

So we are clear on what the changes actually look like, I'm including some snapshots of the before & after collision geometries below.

My main concern is that these new collision geometries are grossly mismatched with the actual robot body. Perhaps that is ok for self-collision issues, but what about external contact with other items in the scene (e.g., manipulands, or a second arm)? These collision just seem too far off reality from a physics/contact standpoint.

I'm inclined to advocate that these changes instead go into the planning/control model, rather than the physical simulation model.

@dmcconachie-tri and @siyuanfeng-tri What are your thoughts?

For reference, here are the proposed new collision geometries:

All collisions shown, before this change:

All collisions shown, after this change:

waiiiiiiiiiit. i thought calder is just touching the planning models.. def no to sim with rick's above visualization.