Implements SpaceXYZFloatingMobilizer

[amcastro-tri]

Towards #14949.

Mobilizer to model free bodies using rpy angles instead of quaternions.
Joint + bindings and parsing will come in separate PRs.

---

This change is 

[amcastro-tri]

+@hongkai-dai for feature review please.

Reviewable status: LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @hongkai-dai)

[hongkai-dai]

I reviewed the header file. Could you clarify my question on the generalized velocity of this joint? Is it the time derivative of the Euler angles, or the angular velocity? Thanks!

Reviewable status: 2 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @amcastro-tri and @hongkai-dai)

---

multibody/tree/space_xyz_floating_mobilizer.h, line 43 at r1 (raw file):

// in F, both expressed in the inboard frame F.
//
// While the mapping MapVelocityToQDot() always exists, its inverse

Not sure if this is right. As the generalized velocity of this joint is v = [dθ₁/dt, dθ₂/dt, dθ₃/dt, vx_FM, vy_FM, vz_FM], hence v is qdot here. I would imagine that both MapVelocityToQDot() and MapQDotToVelocity() should return an identity matrix?

---

multibody/tree/space_xyz_floating_mobilizer.h, line 204 at r1 (raw file):

  // of each of the generalized velocities components packed in the order
  // documented in get_generalized_velocities(). For this mobilizer this
  // corresponds to `vdot = [alpha_FM; a_FM]`, with `alpha_FM = Dt_F(w_FM)` and

I am not sure about this. The documentation above says the generalized velocity contains v=[dθ₁/dt, dθ₂/dt, dθ₃/dt, vx_FM, vy_FM, vz_FM], but here vdot contains the time derivative of w_FM (the angular velocity).

[amcastro-tri]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @hongkai-dai)

---

multibody/tree/space_xyz_floating_mobilizer.h, line 43 at r1 (raw file):

As the generalized velocity of this joint is v = [dθ₁/dt, dθ₂/dt, dθ₃/dt, vx_FM, vy_FM, vz_FM]

Those are not the generalized velocities. The generalized velocities are the angular and translational velocities, i.e. v = [w_FM; v_FM].

Take a look at this part of the documentation just the paragraph above actually. Is that clear or should I paraphrase somehow?

---

multibody/tree/space_xyz_floating_mobilizer.h, line 204 at r1 (raw file):

The documentation above says the generalized velocity contains v=[dθ₁/dt, dθ₂/dt, dθ₃/dt, vx_FM, vy_FM, vz_FM]

That is not accurate. Could you tell me where you are reading from? I couldn't find the buggy piece of docs.

[sherm1]

With generalized velocities defined here as angular velocity this is compatible with the existing SpaceXYZ joint, which seems necessary. However it would be possible to define another joint that does use qdots as generalized velocities, as Paul did for the Bushing. That is convenient if you want to define per-axis damping.

Reviewable status: 2 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @hongkai-dai)

[hongkai-dai]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @amcastro-tri and @hongkai-dai)

---

multibody/tree/space_xyz_floating_mobilizer.h, line 85 at r1 (raw file):

  // Returns the generalized velocities for this mobilizer stored in `context`.
  // Generalized velocities v for this mobilizer are packed in exactly the
  // following order: v = [dθ₁/dt, dθ₂/dt, dθ₃/dt, vx_FM, vy_FM, vz_FM] that is,

This is where I read the generalized velocity is the time derivative of the Euler angle. Could you update this documentation?

[hongkai-dai]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @amcastro-tri and @hongkai-dai)

---

multibody/tree/space_xyz_floating_mobilizer.h, line 204 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

The documentation above says the generalized velocity contains v=[dθ₁/dt, dθ₂/dt, dθ₃/dt, vx_FM, vy_FM, vz_FM]

That is not accurate. Could you tell me where you are reading from? I couldn't find the buggy piece of docs.

It is in the documentation of get_generalized_velocities().

[amcastro-tri]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @hongkai-dai)

---

multibody/tree/space_xyz_floating_mobilizer.h, line 85 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

This is where I read the generalized velocity is the time derivative of the Euler angle. Could you update this documentation?

oh! I can't believe I wrote that. Maybe before I had my coffee this morning? anyway. Those docs are wrong. I just pushed a fix, PTAL.

---

multibody/tree/space_xyz_floating_mobilizer.h, line 204 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

It is in the documentation of get_generalized_velocities().

thanks. pushed a fix.

[hongkai-dai]

BTW, just curious if there is any range limit on the Euler angles. In some books the Euler angles are restricted in the range:
roll: [-pi, pi]
pitch: [-pi/2, pi/2]
yaw [-pi, pi]

I don't see we have an explicit range limit in this class, and I think that is reasonable. If I understand correctly, the behavior of this code is that:

1. We can set_angles with an Euler angle with arbitrary value, doesn't have to be within the range above. So if we call set_angles(rpy) with angles rpy outside of that range, and then call get_generalized_positions(), then the returned angles from get_generalized_positions() is the same as rpy.
2. If we call SetFromRigidTransform, then it converts the rotation matrix to Euler angles, and the converted Euler angle is within the range above. So if we call SetFromRigidTransform and then call get_generalized_positions(), then the returned angle is within the range above.

I am not sure how the Euler angles change when we integrate it, and the angle moves towards the boundary of the range. Does it wrap up (goes from pi to -pi)? Or it just ignore the range limit?

Reviewed 1 of 1 files at r2.
Reviewable status: LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @hongkai-dai)

---

multibody/tree/space_xyz_floating_mobilizer.h, line 85 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

oh! I can't believe I wrote that. Maybe before I had my coffee this morning? anyway. Those docs are wrong. I just pushed a fix, PTAL.

Thanks for the update

[sherm1]

Euler angles are free and capable of wrapping to +/- ∞π. That's actually useful if you have torsional springs. In any case it avoids discontinuities that would occur at wrapping points. (This is independent of the fact that there are singularities whenever the physical pitch angle (as distinct from the pitch coordinate) is at +/- π/2.

Reviewable status: LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @hongkai-dai)

[amcastro-tri]

I added comments to address the two questions above, @hongkai-dai.

I am not sure how the Euler angles change when we integrate it

That is a separate issue from the angular representations provided by this class, so I'll address here separately.
Even though we do not impose any range limits on the Euler angles, you have to keep in mind that, as documented, the mapping N(q) from spatial velocities to generalized positions is not well defined for pitch angles pi/2+k*pi, with k integer. In practice what that means is that in simulation if your quadrotor pitches by 90 degrees your simulation will blow up (luckily we actually throw an exception with a nice message in this class!). That's just a known limitation of Euler angles. But, hopefully your quadrotor doesn't need to pitch 90 degrees or your boat doesn't need to capsize.
If you do need to get those states in sim, say when simulating acrobatics, then you have no option but to use quaternions or some other representation without this issue (known as gimbal lock).

Reviewable status: LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @hongkai-dai)

[hongkai-dai]

I think there are several issues here, and I would like to discuss them separately.

1. The range of Euler angles. I agree that is can wrap to +/- ∞π. On the other hand, when we convert a rotation matrix to an Euler angle, we restrict its range to -pi <= roll <= pi, -pi/2 <= pitch <= pi/2, -pi <= yaw <= pi. We could also say that by adding 2pi to an Euler angle, it still represents the same rotation matrix.
2. I think it makes sense not to wrap up the Euler angles. For example for a quadrotor which often backflips in the air, the angle being 2pi versus the angle being 0 has different meanings, it counts how many circles the quadrotor has backflips.
3. The non-differentiability issue at pitch = ±pi/2. I think that's a separate issue as the range of the Euler angle. It happens that the non-differentiable point pitch = ±pi/2 coincide with the pitch range [-pi/2, pi/2], but whether we wrap-up the angle or not, the non-differentiability always exists. Besides, the angle could wrap-up at roll = pi or yaw = pi, where the mapping from Euler angle time derivative to angular velocity is well-defined.

To summarize, I don't think I am suggesting any action items here, but just to make sure that the Euler angle is not wrapped-up. It would be more clear if in the documentation we could mention that the Euler angle ranges are unbounded.

Reviewed 4 of 5 files at r1.
Reviewable status: LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers (waiting on @hongkai-dai)

[hongkai-dai]

Thanks for working on this @amcastro-tri , much appreciated!

+@sherm1 for platform review please, thanks!

Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on @hongkai-dai and @sherm1)

[hongkai-dai]

Reviewed 1 of 1 files at r3.
Reviewable status: LGTM missing from assignee sherm1(platform) (waiting on @sherm1)

[RussTedrake]

---

multibody/tree/space_xyz_floating_mobilizer.h, line 67 at r4 (raw file):

// @tparam_default_scalar
template <typename T>
class SpaceXYZFloatingMobilizer final : public MobilizerImpl<T, 6, 6> {

"SpaceXYZ" seems inconsistent with our language throughout the rest of drake. Can't with stick with RPY?

[hongkai-dai]

BTW, the python users cannot use this awesome new mobilizer yet. Will there be a follow-up PR to add the python binding?

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sherm1(platform) (waiting on @amcastro-tri, @hongkai-dai, and @sherm1)

[amcastro-tri]

to answer both @hongkai-dai and @RussTedrake here. Mobilizers are internal:: and therefore we do not have binding in Python?

As stated in the PR's description, this is the first of three PRs:

1. The mobilizer.
2. The user-facing joint API + bindings.
3. file parsing.

Also, the nomenclature for mobilizers is slightly different from those for joints @RussTedrake. Following the discussion in #14949, I believe we concluded "Free" would apply to joints since they do not impose any constraints. However mobilizers are sort of the dual of that, in that they "grant" dofs. In this case, the word "floating" seems to apply better (and we already do with the QuaternionFloatingMobilizer). This is a bit off topic, but I want to get to the point that the user-facing API, the joint, will be named FreeRpyJoint as discussed in #14949. With mobilizers we try to be more technical if you will so that the internal code communicates unambiguously our choice of parametrization (there are 27 Euler options even if in robotics saying "rpy" usually (not always) means space xyz).

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sherm1(platform) (waiting on @amcastro-tri, @hongkai-dai, and @sherm1)

[hongkai-dai]

That makes sense, thanks @amcastro-tri

Reviewable status: 1 unresolved discussion, LGTM missing from assignee sherm1(platform) (waiting on @amcastro-tri, @hongkai-dai, and @sherm1)

[hongkai-dai]

Reviewed 1 of 1 files at r4.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee sherm1(platform) (waiting on @amcastro-tri and @sherm1)

[RussTedrake]

---

multibody/tree/space_xyz_floating_mobilizer.h, line 67 at r4 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

"SpaceXYZ" seems inconsistent with our language throughout the rest of drake. Can't with stick with RPY?

i see now that there is already a SpaceXYZMobilizer, so this is consistent.

[sherm1]

+@jwnimmer-tri for platform review per rotation, please
( -@sherm1 )

Reviewable status: LGTM missing from assignee jwnimmer-tri(platform) (waiting on @jwnimmer-tri)

[jwnimmer-tri]

platform.

Reviewed 4 of 5 files at r1, 1 of 1 files at r4.
Reviewable status: 10 unresolved discussions (waiting on @amcastro-tri)

---

multibody/tree/space_xyz_floating_mobilizer.h, line 6 at r4 (raw file):

#include "drake/common/default_scalars.h"
#include "drake/common/drake_assert.h"

nit The drake_assert seems unused within the header.

---

multibody/tree/space_xyz_floating_mobilizer.h, line 71 at r4 (raw file):

  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SpaceXYZFloatingMobilizer)

  // Constructor for a %SpaceXYZFloatingMobilizer between an inboard frame F

nit % here does nothing to help the reader.

---

multibody/tree/space_xyz_floating_mobilizer.h, line 144 at r4 (raw file):

  // θ₂, θ₃]`.
  //
  // @param[out] context

nit For all of these context outputs here and below, the context is [in,out] not just [out] -- we are not fully overwriting it, we are leaving some of its original data intact.

---

multibody/tree/space_xyz_floating_mobilizer.h, line 274 at r4 (raw file):

  void DoCalcNplusMatrix(const systems::Context<T>& context,
                         EigenPtr<MatrixX<T>> Nplus) const final;

nit Here we have final, but nearby we also sometimes day override as well. It's not a huge deal, but it would be best to be consistent throughout -- assuming you're not actually trying to draw a distinction between the two keywords.

---

multibody/tree/space_xyz_floating_mobilizer.h, line 289 at r4 (raw file):

  // Bring the handy number of position and velocities MobilizerImpl enums into
  // this class' scope. This is useful when writing mathematical expressions
  // with fixed-sized vectors since we can do things like Vector<T, nq>.

nit kNq here, not nq?

---

multibody/tree/space_xyz_floating_mobilizer.cc, line 72 at r4 (raw file):

SpaceXYZFloatingMobilizer<T>::set_angular_velocity(
    systems::Context<T>* context, const Vector3<T>& w_FM) const {
  auto v = this->get_mutable_velocities(context).template head<3>();

nit Do you want w here instead of v? I could see it either way.

---

multibody/tree/space_xyz_floating_mobilizer.cc, line 100 at r4 (raw file):

    const systems::Context<T>& context) const {
  // N.B. We use get_positions() instead of get_angles() and get_translation()
  // to avoid temporary copies.

nit That micro-optimization pales in comparison to calling get_positions (i.e., get_state_segment) twice in a row, though?

---

multibody/tree/space_xyz_floating_mobilizer.cc, line 157 at r4 (raw file):

  // Demand for the computation to be away from a state for which Einv_F is
  // singular.
  DRAKE_DEMAND(abs(cp) > 1.0e-3);

nit The API docs imply that >= 1e-3 is fine, but this check is strictly >. Either choice is probably fine, but be consistent.

Ditto throughout.

---

multibody/tree/space_xyz_floating_mobilizer.cc, line 169 at r4 (raw file):

  Matrix3<T> Einv_F;
  Einv_F << cy_x_cpi, sy_x_cpi, 0.0, -sy, cy, 0.0, cy_x_cpi * sp, sy_x_cpi * sp,
      1.0;

nit Maybe my matrix-reading chops have dulled, but this would be much easier for me to follow if it had newlines between the rows.

Ditto throughout.

---

multibody/tree/space_xyz_floating_mobilizer.cc, line 351 at r4 (raw file):

  v->template head<3>() = Vector3<T>(cy * cp_x_rdot - sy * pdot, /*+ 0 * ydot*/
                                     sy * cp_x_rdot + cy * pdot, /*+ 0 * ydot*/
                                     -sp * rdot /*+   0 * pdot */ + ydot);

nit Maybe the commented-out + here could be better vertically aligned, by moving around dome whitespace within the comment? Shrug.

[amcastro-tri]

Reviewable status: complete! all discussions resolved, LGTM from assignees jwnimmer-tri(platform),hongkai-dai (waiting on @hongkai-dai and @jwnimmer-tri)

---

multibody/tree/space_xyz_floating_mobilizer.h, line 274 at r4 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Here we have final, but nearby we also sometimes day override as well. It's not a huge deal, but it would be best to be consistent throughout -- assuming you're not actually trying to draw a distinction between the two keywords.

I agree on this one. I however (and unfortunately) see lots of disagreement throughout Drake's code base, even at the systems:: level. Is there a preference here? on whether to use final or override when the class is already marked final?

---

multibody/tree/space_xyz_floating_mobilizer.cc, line 72 at r4 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Do you want w here instead of v? I could see it either way.

I actually meant v as in "generalized" velocities which, in this case, happen to coincide with the angular velocity w_FM. I could see also how it could go either way, that was my rationale when writing it though.

---

multibody/tree/space_xyz_floating_mobilizer.cc, line 100 at r4 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit That micro-optimization pales in comparison to calling get_positions (i.e., get_state_segment) twice in a row, though?

agree, I had forgotten what it took :-(

---

multibody/tree/space_xyz_floating_mobilizer.cc, line 351 at r4 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Maybe the commented-out + here could be better vertically aligned, by moving around dome whitespace within the comment? Shrug.

definitely much better aligning things. thanks

[jwnimmer-tri]

Reviewed 2 of 2 files at r5.
Reviewable status: complete! all discussions resolved, LGTM from assignees jwnimmer-tri(platform),hongkai-dai (waiting on @amcastro-tri)

---

multibody/tree/space_xyz_floating_mobilizer.h, line 274 at r4 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I agree on this one. I however (and unfortunately) see lots of disagreement throughout Drake's code base, even at the systems:: level. Is there a preference here? on whether to use final or override when the class is already marked final?

As far as I know, there is no rule nor consensus -- it's up to you. This way is fine.

---

multibody/tree/space_xyz_floating_mobilizer.cc, line 72 at r4 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I actually meant v as in "generalized" velocities which, in this case, happen to coincide with the angular velocity w_FM. I could see also how it could go either way, that was my rationale when writing it though.

It makes sense. A fun thought experiment.