multibody: Implement UniversalJoint

[mpetersen94]

Creates UniversalJoint, a two degree of freedom rotational joint. The joint includes damping but not any spring force (much like RevoluteJoint). In addition, joint limits in MBP are not enforced for this joint and there is no parsing added in this pr. I would love to discuss fixing either/both of these deficiencies.

• Requires ability to mutate joint position limits (multibody: Implement BallRpyJoint #13039 )
• Requires UniversalMobilizer (multibody: Implement UniversalMobilizer #13054 )

Towards #12404

Also removes unused friend class JointTester from several joints and fixes a bug that made SpaceXYZMobilizer a floating mobilizer.

cc @sherm1 @amcastro-tri

---

This change is 

[mpetersen94]

Ready for review. Commits are properly curated.

@amcastro-tri for feature review.

[amcastro-tri]

Excellent!

@ggould-tri for platform review please.

Reviewed 6 of 6 files at r1.
Reviewable status: 12 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mpetersen94)

---

bindings/pydrake/multibody/tree_py.cc, line 341 at r1 (raw file):

        .def("get_angular_rates", &Class::get_angular_rates, py::arg("context"),
            cls_doc.get_angular_rates.doc)
        .def("set_angular_rates", &Class::set_angular_rates, py::arg("context"),

nit, none of these bindings are unit tested.

---

multibody/tree/universal_joint.h, line 17 at r1 (raw file):

namespace multibody {

/// This joint model a universal joint allowing two bodies to rotate relative to

nit, model --> models.

---

multibody/tree/universal_joint.h, line 24 at r1 (raw file):

/// y-axis. No translational motion of M in F is allowed and the origins, `Mo`
/// and `Fo`, of frames M and F respectively remain coincident. The angles of
/// rotation about F's x-axis and M's y-axis, along with their rates, specifies

specifies --> specify

---

multibody/tree/universal_joint.h, line 83 at r1 (raw file):

  /// angular rates for `this` joint (see get_angular_rates())and τ the torque
  /// on child body B about frame F's x-axis and M's y-axis.
  double damping() const { return damping_; }

A few border line ambiguous things here. It is not quite clear from the symbols whether ω is a 3D angular velocity or actual rates (though you do say it) and whether τ is a 3D torque vector or not.
I'd just rephrase to something like:

Viscous damping is modeled as

τᵢ = -damping ⋅  ωᵢ,  i = 1, 2

with ωᵢ the angular rate about the i-th axis and τᵢ is the torque on body B about the same i-th axis.

---

multibody/tree/universal_joint.h, line 95 at r1 (raw file):
"The x-axis of F and I are aligned and the axes are offset by the rotation, θ₁, about their shared x-axis." doesn't parse quite well.

I'd probably have a text saying:

You can think of a universal joint as a mechanism consisting of three bodies; the parent body P, an intermediate cross-shaped body I, and the child body B. In a real universal joint, usually body I has a much smaller mass than P or B. This universal joint model corresponds to the mathematical limit of having a body I of negligible mass. With this mental picture of the mechanism, we naturally define the orientation of B in P as follows using a body fixed sequence. A first rotation of θ₁ about Fx defines the orientation R_FI of the intermediate frame I (notice that by definition Ix = Fx at all times). A second rotation of θ₂ about Iy defines the orientation R_IM of frame M rigidly affixed to P (notice that by definition My = Iy at all times). Therefore, the orientation of frame M in F is given by:

  R_FM(q) = R_FI(θ₁) * R_IM(θ₂)

Actually, probably best if that doc lives in the class's documentation and this here could reference that. Consider placing in the class's doc instead.

---

multibody/tree/universal_joint.h, line 105 at r1 (raw file):

  /// rotation of amount θ₁ about the x-axis of frame F and `R_IM(θ₂)` defines
  /// the orientation of M in I as an elemental rotation of amount θ₂ about the
  /// y-axis of frame I (also the y-axis of frame M).

I believe the text above with this "mental picture" of the mass less body I already says this. Consider refactoring as appropriate.

---

multibody/tree/universal_joint.h, line 162 at r1 (raw file):

  /// Sets the default angles of this joint.
  /// If the parent tree has been finalized and the underlying mobilizer is

"mobilizer" is an internal detail. Nothing would prevent us in the future on modeling this joint as a constraint for instance. Thus the API docs should not leak how we actually model the joint. That is, the word "mobilizer" in these docs is a defect. Simply remove this second paragraph.

---

multibody/tree/universal_joint.h, line 211 at r1 (raw file):

  void DoAddInDamping(const systems::Context<T>& context,
                      MultibodyForces<T>* forces) const override {
    Eigen::VectorBlock<Eigen::Ref<VectorX<T>>> t_BMo_F =

this monogram notation only applies to 3D vectors representing a torque.
This is not a vector, but an array containing the generalized forces tau (or tau_B if you wan to emphasize the sign convention as applied on B).
Thus, t_BMo_F --> tau.

---

multibody/tree/universal_joint.h, line 251 at r1 (raw file):

  // Friend class to facilitate testing.
  friend class JointTester;

not used?

---

multibody/tree/universal_joint.cc, line 4 at r1 (raw file):

#include <memory>
#include <stdexcept>

nit, not used?

---

multibody/tree/test/universal_joint_test.cc, line 27 at r1 (raw file):

constexpr double kDamping = 3;
constexpr double kPositionNonZeroDefault =
    (kPositionLowerLimit + kPositionUpperLimit) / 2;

nit, you might want to type in the number by hand. Any reason to take the average? unless you want to call it something specific like kMeanPositionLimit for some reason?

---

multibody/tree/test/universal_joint_test.cc, line 196 at r1 (raw file):

  EXPECT_THROW(mutable_joint_->set_velocity_limits(Vector2d::Constant(2),
                                                   Vector2d::Constant(0)),
               std::runtime_error);

nit, have you seen the macro DRAKE_EXPECT_THROWS_MESSAGE()? it'd be helpful in these tests to verify you throw for the right reasons.

[mpetersen94]

Reviewable status: 8 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri)

---

bindings/pydrake/multibody/tree_py.cc, line 341 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, none of these bindings are unit tested.

Done.

---

multibody/tree/universal_joint.h, line 17 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, model --> models.

Done.

---

multibody/tree/universal_joint.h, line 24 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

specifies --> specify

Done.

---

multibody/tree/universal_joint.h, line 83 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

A few border line ambiguous things here. It is not quite clear from the symbols whether ω is a 3D angular velocity or actual rates (though you do say it) and whether τ is a 3D torque vector or not.
I'd just rephrase to something like:

Viscous damping is modeled as

τᵢ = -damping ⋅  ωᵢ,  i = 1, 2

with ωᵢ the angular rate about the i-th axis and τᵢ is the torque on body B about the same i-th axis.

Done.

---

multibody/tree/universal_joint.h, line 95 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

"The x-axis of F and I are aligned and the axes are offset by the rotation, θ₁, about their shared x-axis." doesn't parse quite well.

I'd probably have a text saying:

You can think of a universal joint as a mechanism consisting of three bodies; the parent body P, an intermediate cross-shaped body I, and the child body B. In a real universal joint, usually body I has a much smaller mass than P or B. This universal joint model corresponds to the mathematical limit of having a body I of negligible mass. With this mental picture of the mechanism, we naturally define the orientation of B in P as follows using a body fixed sequence. A first rotation of θ₁ about Fx defines the orientation R_FI of the intermediate frame I (notice that by definition Ix = Fx at all times). A second rotation of θ₂ about Iy defines the orientation R_IM of frame M rigidly affixed to P (notice that by definition My = Iy at all times). Therefore, the orientation of frame M in F is given by:

  R_FM(q) = R_FI(θ₁) * R_IM(θ₂)

Actually, probably best if that doc lives in the class's documentation and this here could reference that. Consider placing in the class's doc instead.

Done.

---

multibody/tree/universal_joint.h, line 105 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I believe the text above with this "mental picture" of the mass less body I already says this. Consider refactoring as appropriate.

Done.

---

multibody/tree/universal_joint.h, line 162 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

"mobilizer" is an internal detail. Nothing would prevent us in the future on modeling this joint as a constraint for instance. Thus the API docs should not leak how we actually model the joint. That is, the word "mobilizer" in these docs is a defect. Simply remove this second paragraph.

Done.

---

multibody/tree/universal_joint.h, line 211 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

this monogram notation only applies to 3D vectors representing a torque.
This is not a vector, but an array containing the generalized forces tau (or tau_B if you wan to emphasize the sign convention as applied on B).
Thus, t_BMo_F --> tau.

Done.

---

multibody/tree/universal_joint.h, line 251 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

not used?

Then why does every joint type declare this line?

---

multibody/tree/universal_joint.cc, line 4 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, not used?

Done.

---

multibody/tree/test/universal_joint_test.cc, line 27 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, you might want to type in the number by hand. Any reason to take the average? unless you want to call it something specific like kMeanPositionLimit for some reason?

Done.

---

multibody/tree/test/universal_joint_test.cc, line 196 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit, have you seen the macro DRAKE_EXPECT_THROWS_MESSAGE()? it'd be helpful in these tests to verify you throw for the right reasons.

I tried implementing using the macro but I can't get the macro to match the error messages (see commented out tests). Any idea what's going wrong?

[sherm1]

Reviewable status: 8 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri and @mpetersen94)

---

multibody/tree/test/universal_joint_test.cc, line 196 at r1 (raw file):

Previously, mpetersen94 (Mark Petersen) wrote…

I tried implementing using the macro but I can't get the macro to match the error messages (see commented out tests). Any idea what's going wrong?

The problem is likely to be regular expression metacharacters in the string. Usually parentheses are the worst offenders. Easiest just to .* those, but you can include them literally if you throw in enough backslashes (takes two), e.g. \\(\\) will match literal "()".

[mpetersen94]

Reviewable status: 7 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri)

---

multibody/tree/test/universal_joint_test.cc, line 196 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

The problem is likely to be regular expression metacharacters in the string. Usually parentheses are the worst offenders. Easiest just to .* those, but you can include them literally if you throw in enough backslashes (takes two), e.g. \\(\\) will match literal "()".

That fixed it! Thanks!

[amcastro-tri]

Reviewed 4 of 5 files at r2.
Reviewable status: 3 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri and @mpetersen94)

a discussion (no related file):
Thank you @mpetersen94. Great job.

I am worried about that exception in the python test. I'll see to reproduce today to hopefully get an informative message and evaluate the problem.

---

---

bindings/pydrake/multibody/test/plant_test.py, line 1117 at r2 (raw file):

            if joint.name() == "ball_rpy":
                # TODO(mpetersen94): Figure out why creating a context throws
                # an error and implement this test

this seems real bad. Is when you create the context that you get the exception

---

multibody/tree/universal_joint.h, line 251 at r1 (raw file):

Previously, mpetersen94 (Mark Petersen) wrote…

Then why does every joint type declare this line?

ah.... good catch, unfortunately copy/pasta, a defect if not used. I only see the one for RevoluteJoint in use. Feel free to remove them all in this PR, otherwise I can submit a separate PR removing them.

For this one PR at least, adding an unused friend is a defect though.

---

multibody/tree/universal_joint.h, line 40 at r2 (raw file):

/// Zero θ₁, θ₂ angles corresponds to frames F, I, and M being coincident.
/// Angles (θ₁, θ₂) are defined to be positive according to the
/// right-hand-rule with the thumb aligned in the direction of their

btw, thanks! I'm glad you rewrote my bad phrasing :-)

[mpetersen94]

Reviewable status: 3 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri and @mpetersen94)

a discussion (no related file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Thank you @mpetersen94. Great job.

I am worried about that exception in the python test. I'll see to reproduce today to hopefully get an informative message and evaluate the problem.

So it comes from the fact that SpaceXYZMobilizer claims to be a floating mobilizer but isn't because it constrains translation. I put the todo in because I wasn't sure how much this bad assumption permeated the code base and wanted time to focus on it. I can look into it tomorrow.

---

---

bindings/pydrake/multibody/test/plant_test.py, line 1117 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

this seems real bad. Is when you create the context that you get the exception

Yes. See above.

---

multibody/tree/universal_joint.h, line 251 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

ah.... good catch, unfortunately copy/pasta, a defect if not used. I only see the one for RevoluteJoint in use. Feel free to remove them all in this PR, otherwise I can submit a separate PR removing them.

For this one PR at least, adding an unused friend is a defect though.

Will remove them all then.

---

multibody/tree/universal_joint.h, line 40 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

btw, thanks! I'm glad you rewrote my bad phrasing :-)

No problem!

[amcastro-tri]

Reviewable status: 3 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri and @mpetersen94)

a discussion (no related file):

Previously, mpetersen94 (Mark Petersen) wrote…

So it comes from the fact that SpaceXYZMobilizer claims to be a floating mobilizer but isn't because it constrains translation. I put the todo in because I wasn't sure how much this bad assumption permeated the code base and wanted time to focus on it. I can look into it tomorrow.

oh wow, that's a bug! I can push a fix

---

[mpetersen94]

Reviewable status: 3 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri)

a discussion (no related file):

Previously, amcastro-tri (Alejandro Castro) wrote…

oh wow, that's a bug! I can push a fix

I've tried removing it from the mobilizer and the only place it has caused an issue is in free_rotating_body_test.cc. I'm not sure what that test is verifying and therefore how to repair the test. If you can explain what this test is doing, I might be able to repair it and save you the time.

---

---

multibody/tree/universal_joint.h, line 251 at r1 (raw file):

Previously, mpetersen94 (Mark Petersen) wrote…

Will remove them all then.

Done.

[amcastro-tri]

Reviewable status: 3 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri and @mpetersen94)

a discussion (no related file):

Previously, mpetersen94 (Mark Petersen) wrote…

I've tried removing it from the mobilizer and the only place it has caused an issue is in free_rotating_body_test.cc. I'm not sure what that test is verifying and therefore how to repair the test. If you can explain what this test is doing, I might be able to repair it and save you the time.

Thank you for checking. That test is wrong. It should be EXPECT_FALSE(free_body_plant.body().is_floating());. That model is for a body free to rotate, but it is not a 6dof body.
Therefore if you do update SpaceXYZMobilizer::is_floating() to return false and this particular test, it should fix your problem.

Thank you for offering, I was swamped today.

---

[mpetersen94]

Reviewable status: 3 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri)

a discussion (no related file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Thank you for checking. That test is wrong. It should be EXPECT_FALSE(free_body_plant.body().is_floating());. That model is for a body free to rotate, but it is not a 6dof body.
Therefore if you do update SpaceXYZMobilizer::is_floating() to return false and this particular test, it should fix your problem.

Thank you for offering, I was swamped today.

Done.

---

[amcastro-tri]

Thanks @mpetersen94. You also fixed two bugs!

Reviewed 7 of 7 files at r3.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee ggould-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @ggould-tri and @mpetersen94)

a discussion (no related file):
It was meant to be @ggould-tri. Above I assigned you wrong (forgot the +). But today is your platform review day, so here it goes again. +@ggould-tri for platform please.

Notice that with @mpetersen94 we found two bugs in master and he kindly offered to fix them. That is why you will see, in addition to the new UniversalJoint:

1. SpaceXYZMobilizer::is_floating() override was erroneously returning true
2. Removal for friend class JointTester; from other joints since it was not used.

---

[ggould-tri]

This is a jumbo PR and so it will be late in my priority list. I'll get to it today for sure though.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee ggould-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mpetersen94)

a discussion (no related file):

Previously, amcastro-tri (Alejandro Castro) wrote…

It was meant to be @ggould-tri. Above I assigned you wrong (forgot the +). But today is your platform review day, so here it goes again. +@ggould-tri for platform please.

Notice that with @mpetersen94 we found two bugs in master and he kindly offered to fix them. That is why you will see, in addition to the new UniversalJoint:

1. SpaceXYZMobilizer::is_floating() override was erroneously returning true
2. Removal for friend class JointTester; from other joints since it was not used.

minor: PR message should mention those changes, since git blame will be confusing otherwise.

---

[mpetersen94]

No worries!

Reviewable status: 1 unresolved discussion, LGTM missing from assignee ggould-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri)

a discussion (no related file):

Previously, ggould-tri wrote…

minor: PR message should mention those changes, since git blame will be confusing otherwise.

I have added a mention of these in the PR description. In addition the commits are properly curated (I can't add the tag for that though...) so we can merge with no squashing.

---

[ggould-tri]

; trivial typos below.

Reviewable status: 3 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @amcastro-tri and @mpetersen94)

---

multibody/tree/test/universal_joint_test.cc, line 245 at r4 (raw file):

  EXPECT_EQ(joint_->get_default_angles(), default_angles);

  // Setting a new default angle should propogate so that `get_default_angle()`

minor: typo "propogate" should be "propagate".

---

multibody/tree/test/universal_joint_test.cc, line 251 at r4 (raw file):

  // Setting the default angle out of the bounds of the position limits
  // should NOT throw an exception

minor: Please use a period on this complete sentence.

[ggould-tri]

Reviewable status: 3 unresolved discussions (waiting on @amcastro-tri and @mpetersen94)

a discussion (no related file):

Previously, mpetersen94 (Mark Petersen) wrote…

I have added a mention of these in the PR description. In addition the commits are properly curated (I can't add the tag for that though...) so we can merge with no squashing.

+(status: commits are properly curated)

---

[mpetersen94]

Reviewable status: complete! all discussions resolved, LGTM from assignees ggould-tri(platform),amcastro-tri (waiting on @amcastro-tri)

---

multibody/tree/test/universal_joint_test.cc, line 245 at r4 (raw file):

Previously, ggould-tri wrote…

minor: typo "propogate" should be "propagate".

Done.

---

multibody/tree/test/universal_joint_test.cc, line 251 at r4 (raw file):

Previously, ggould-tri wrote…

minor: Please use a period on this complete sentence.

Done.

[ggould-tri]

Reviewed 2 of 7 files at r3, 1 of 1 files at r5.
Reviewable status: complete! all discussions resolved, LGTM from assignees ggould-tri(platform),amcastro-tri