Add a 6dof (free) RollPitchYaw joint

[hongkai-dai]

Currently for floating joint, we default to use quaternion to represent its orientation. There are cases when an Euler-angle representation (like roll-pitch-yaw) is preferred

1. In trajectory optimization, using quaternion requires adding the unit length constraint, and extra constraint on the integration scheme.
2. For some applications like planes, it is common to use Euler angles.

Hence I would like to request adding roll-pitch-yaw as another representation for floating base orientation.

cc @RussTedrake

[RussTedrake]

Strongly agree! I think it should be sufficient to add a RollPitchYawJoint (e.g. a joint; we can leave the Quaternion as the strange special case for "floating" body). This would be similar to how we've used the PlanarJoint that @mpetersen94 wrote, where we can optionally add this joint between the world and the base.

[sherm1]

A RollPitchYawJoint would be a great addition. BTW you can produce that effect today with three revolute and three prismatic joints and a few intermediate massless bodies.

[sherm1]

From f2f discussion:

• better workaround: use BallRpyJoint plus x-y-z prismatics.
• @amcastro-tri says he'll push the new 6 dof joint.

For consistency with the ball I think it should be called FreeRpyJoint ?

[amcastro-tri]

I like FreeRpyJoint, it seems to convey the mental idea our users need IMO.

[RussTedrake]

notably, neither urdf nor sdf seem to offer it: http://wiki.ros.org/urdf/XML/joint http://sdformat.org/spec?ver=1.8&elem=joint

in our old matlab we called it FloatingRpy, but I'm ok with FreeRpy

[amcastro-tri]

I believe @sherm mostly wanted to keep the "rpy" part of the name in consistency with BallRpyJoint. We use the "floating" terminology already in Drake, so it'd seem to keep naming consistent RpyFloatingJoint would be best?

We've been adding custom drake:joint, see for instance the planar joint parsing.

[sherm1]

For consistency it should be FloatingRpyJoint, or we would have to rename BallRpyJoint -> RpyBallJoint. But I think FloatingRpyJoint and BallRpyJoint are marginally better because they start with the joint type and would alphabetize next to other Floating and Ball joints that use different generalized coordinates. Those would be easier for people to find.

Personally I prefer Free to Floating. The MBP API already uses the term "FreeBody" extensively -- to me it seems better to have a FreeBody use a FreeJoint. Also "Free" correctly refers to the constraints imposed by the joint (none), while "Floating" isn't a property of joints.

[xuchenhan-tri]

My understanding is that @joemasterjohn will add a free joint soon. Will that be represented as rpy and solve this isse?

[sherm1]

No, Joe is planning to add a 6-dof joint that uses a quaternion. Would be good to have the Rpy option also but that isn't needed for Joe's purpose.

[xuchenhan-tri]

Good to know thanks. I'll put this in the backburner for now then.

[RussTedrake]

I think backburner is ok. I will note that we SpaceXYZFloatingMobilizer in the code (great!) but it's completely untouchable with no joint nor any other public API nor parsing logic for actually adding one to a MultibodyPlant. So we're leaving this in a bit of a funny state.

(I actually got to this issue after a bit of searching because I remembered having to touch SpaceXYZFloatingMobilizer, so was trying to find the corresponding joint to recommend to a student)

[RussTedrake]

For anyone that feels like they need this, there is a less elegant solution already: you can just add all of the joints individually: https://github.com/RussTedrake/manipulation/blob/5e5981147079e69f03d1b42707b8db0386dc8824/manipulation/scenarios.py#L107

[sherm1]

FYI -- If you want to emulate the future FloatingRpyJoint exactly, put the BallRpyJoint first, followed by three PrismaticJoints (with two massless links in between). Caveat: the BallRpyJoint doesn't allow actuation of individual axes -- if you need that you would have to start with three RevoluteJoints instead of the BallRpy.

[RussTedrake]

just a bump because I've run into this yet again as a pain point. I was hoping to pose some objects in the scene using the "authoring multibodyplant" tutorial workflow, but JointSliders does not support quaternion floating bases and we don't make it easy to add these RPY joints.

[sherm1]

Any thoughts / progress on this, @joemasterjohn ?

[amcastro-tri]

@joemasterjohn is working on this but run into some design choices. @joemasterjohn will ask in the DrakeDevelopers channel for people to weigh in for options.

[xuchenhan-tri]

The design discussion mentioned above is linked here.

[EricCousineau-TRI]

In trajectory optimization, using quaternion requires adding the unit length constraint, and extra constraint on the integration scheme.

(At the risk of being ignorant) Why not use 3-vector axis-angle / so(3)? No gimbal lock; not sure if you'd need bounding box / polytope to approximate the 2*pi "wraparound" sphere.

[sherm1]

Sadly, no 3-variable representation of so(3) can avoid having a singularity. See the Hairy Ball Theorem. It's not that you can't represent all the orientations nicely; it's that the derivative (tangent) must be singular somewhere.

[hongkai-dai]

(At the risk of being ignorant) Why not use 3-vector axis-angle / so(3)?

I suppose you mean to take the axis a and the angle θ, and form a 3-vector aθ? This representation has a singularity at θ=π. You can see that if you have a 3-vector πa and another 3-vector -πa, they corresponds to the same rotation. Hence from the rotation matrix to this 3-vector, you have two branches, one gives πa, the other branch gives -πa. Hence this function from a rotation matrix to the 3-vector is ill-defined (hence bad derivative) at θ=π

[EricCousineau-TRI]

Sadly, no 3-variable representation of so(3) can avoid having a singularity.

Makes sense - but I think the point here is to have a less (nonlinearly) constrained space - per above, Hongkai would like to avoid quaternions given the unit-vector constraint. Also, yay on the hairy ball, thank you for the reference! Had struggled with that for some ML perception based stuff, and didn't know how to articulate that exact concept haha.

Hence this function from a rotation matrix to the 3-vector is ill-defined (hence bad derivative) at θ=π

Gotcha, that makes sense for f(R) = aθ (and is very nice embodiment of the antipodal hairy ball stuff!). And that's what I tried to mention with the bounding box / polytope for the 2*pi wraparound.

Additionally, doesn't this give you a wider surface of well-defined mappings (a span of almost 2pi along at least* one axis of rotation) than gimbal lock w/ rpy?

Also, if you're optimizing against aθ, don't you really care about f⁻¹(aθ) = R? How well is that conditioned? (Sorry, I haven't yet consulted a book on the math - feel free to tell me to go read :sweat_smile:)

[hongkai-dai]

Also, if you're optimizing against aθ, don't you really care about f⁻¹(aθ) = R?

The function f⁻¹(aθ) = R is smooth and well-defined. But at some point we will need to convert R back to aθ, and this conversion is not well defined everywhere. In this regard, using aθ is the same as using roll-pitch-yaw angle. The function from roll-pitch-yaw to R is also smooth and well-defined, the inverse mapping from R to roll-pitch-yaw is not well defined everywhere.

If we only care about trajectory optimization, then we can choose either aθ or roll-pitch-yaw angle. The function from aθ or roll-pitch-yaw to R is smooth, hence trajectory optimization will work fine.

If we do simulation, then both aθ and roll-pitch-yaw will run into problem, as we need to convert R back to aθ or roll-pitch-yaw in the state. This conversion is not well-defined everywhere.

And that's what I tried to mention with the bounding box / polytope for the 2*pi wraparound

I don't think wraparound will help in this case, in the θ=π case, the angle is π , there is no need to wrap-around the angle. The ambiguity is in the rotation axis; both a and -a will give the same rotation matrix.

[amcastro-tri]

@joemasterjohn-TRI, will tackle this one for our tech debt. Thanks @joemasterjohn-TRI! @xuchenhan-tri, is that ok? or were you thinking someone else would work on this? Thus far only @sherm1 and I were assigned.

[xuchenhan-tri]

Of course! Thanks @joemasterjohn!

[jwnimmer-tri]

I hit this again trying to make ModelVisualizer better. It currently gives the user 7 sliders each to pose the floating base(s) of the visualized model file(s). I would like to instead provide rpy sliders instead of qw qx qy qz sliders for ease of use. Doing that by adding a FloatingRpyJoint to my plant would be very nice!

[amcastro-tri]

See if you like the API in atlas_run_dynamics.cc in WIP PR #19065. I can clean up and merge if you find it useful. With that API, users can change the default floating joint in the config, or simply add an rpy joint with MBP::AddJoint().

[jwnimmer-tri]

For ModelVisualizer, yes that would fit the bill. I'd just switch my plant to use rpy exclusively before loading any models.

[amcastro-tri]

Thanks for the feedback. I'll start cleaning up towards a PR.

[aamice-bdai]

Hitting this exact situation myself both in the Trajectory Optimization context and in the model visualization with sliders context. Would be a great addition!

[sherm1]

FYI -- I'm going to add a 6dof joint now using roll-pitch-yaw rather than a quaternion. Despite the discussion above, I'm going to name it rpy_floating_joint for consistency with the existing quaternion_floating_joint.