parsing, urdf: `//joint[@type="floating"]` - confusing?

[EricCousineau-TRI]

Triggered by discussion here: https://github.com/RobotLocomotion/drake/pull/13128#pullrequestreview-438592326

For spec: http://wiki.ros.org/urdf/XML/joint#Attributes

Currently, this is our code for handling //joint[@type="floating"]: https://github.com/RobotLocomotion/drake/blob/86c27959bd75b1f29d98b09dc96ed2d3fde10997/multibody/parsing/detail_urdf_parser.cc#L439-L444

It says it's not supported, but then, it uh, does almost the right thing?

My suggestions:

• If we want type="floating" in Drake, we support it fully, and fix it -- we use X_PJ + SetDefaultFreeBodyPose (and maybe track some basic kinematics, I guess?)
• If not, we completely disable it, and fail fast.

@sammy-tri Thoughts?

[sammy-tri]

I think we should just go ahead and support "floating" as a joint type. I've thought of the following cases, in increasing order of difficulty to support:

• Parent body is world and we set the default free body pose to X_PJ
• Parent body is welded to world and we can calculate the world-relative origin using that body + X_PJ.
• (maybe, this is pushing it) Parent body is a free body and we can use its default position (as known to the parser when the model is parsed!) + X_PJ to set the position for the child of the floating joint.
• (this is really pushing it) Parent body has articulated joints between the floating joint and the world, and we calculate the default free body pose using the default joint positions + X_PJ (can MBP even calculate this before Finalize?) I'm of the opinion that anyone who writes this URDF can get a warning/error and keep both halves of their broken model.

[sherm1]

FYI other internal-coordinate multibody codes do support a 6dof floating joint type. They are useful for establishing a convenient coordinate frame between two bodies (that is, they define the meaning of the corresponding generalized coordinates q and v). Simbody has actually has two flavors -- a "Free" joint that uses quaternions for the rotational dofs and a "Bushing" joint that uses gimbal angles (more convenient for applying forces but singular at odd configurations). Both have been useful.

[EricCousineau-TRI]

@sherm1 Just curious - do those usages permit changing the frame of for the floating joint DoFs? e.g. the pose of the free body w.r.t. the inertial frame vs. some other (possibly non-inertial) frame?

For the case of URDF, my assumption is the floating joint DoFs will represent the pose of the free body relative to the world frame. However, I may need to see if it's possibly to confirm :P

[sherm1]

I believe your hands may be tied for urdf due to lack of any way to properly model a joint as a relationship between two frames. Should be better in sdf. The general model of a joint should relate a frame F fixed on the parent to a frame M fixed on the child, like in the Simbody picture here. That formulation includes all possible joints, including floating joints.

[EricCousineau-TRI]

@sammy-tri W.r.t. your points, I think it'd be easy enough to do some bookkeeping to record X_PJ and X_PL? (ignore joint mobilization, and only consider X_FM = eye(4)?)

[EricCousineau-TRI]

@sherm1 Gotcha. And sorry, I don't think our hands are necessarily tied - my goal is to simply interpret the URDF as other tools do for minimal coordinates (since URDFs are simple).

If the other tools interpret it as inboard=world, outboard=child, then great! Drake's public API supports that! And if it's actually inboard=parent, outboard=child, then oops! I don't think Drake's public API supports that yet (e.g. I don't think we can tune the inboard/outboard bodies for QuaternionFloatingMobilizer), and I'd rather fail fast or perhaps make a more explicit warning - before trying to figure out how to delineate between "free bodies" and "bodies with a floating joint".

[sammy-tri]

@sammy-tri W.r.t. your points, I think it'd be easy enough to do some bookkeeping to record X_PJ and X_PL? (ignore joint mobilization, and only consider X_FM = eye(4)?)

Sorry, I'm struggling today... Which transforms are X_PL and X_FM?

[EricCousineau-TRI]

Ah, sorry - P is a link's parent (identified through joints), J is the joint, L is the link (or child, rather). X_FM is really the inboard/outboard transform (F - inboard frame, M - outboard / "mobilized" frame), it should've beenX_PL - my bad!

[sammy-tri]

after reading the other comments again, my original concerns only involved calculating where the initial world pose of the floating body would be, and I ignored the question of the generalized coordinates used to represent the position of that body in the state vector. I think the only case we could suppose easily right now is parent==world and X_PJ==X_PL so that we could set the free body pose to the origin specified in the URDF (and the generalized coordinates are world relative, like drake uses for free bodies already).

[RussTedrake]

I just stumbled on this myself. It seems worth fixing (and might be even easier now that @joemasterjohn added QuaternionFloatingJoint. @rpoyner-tri -- perhaps we should move this to your queue, as owner of the parsing component, since I don't think @EricCousineau-TRI is likely to move on it?

[rpoyner-tri]

I've added myself.

[RussTedrake]

I just ran into this again. I made a simple example of using joint locking/unlocking to simulate suction here. But to do that I (unfortunately) need to make a free joint between the suction tip and the objects it could suck. I can't do that through the parser yet, until we implement the free joint with a QuaternionFloatingJoint.

(ftr -- i had hoped to use constraints, but the constraint API wouldn't allow me to change the kinematics of the constraint after construction; so I would probably need a relative spatial velocity constraint to do it).