From 402fac29aae85d62c396dbecaebd2f295f50d414 Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Mon, 29 Jan 2024 19:07:34 -0800 Subject: [PATCH] Rename spaceXYZ mobilizers to more closely match joint terminology (#20827) Also added missing coordinate suffixes to rpy_floating mobilizer --- .../test/compliant_contact_manager_test.cc | 7 - .../test/sap_driver_multidof_joints_test.cc | 4 +- .../rational/rational_forward_kinematics.cc | 2 - multibody/tree/BUILD.bazel | 12 +- multibody/tree/ball_rpy_joint.cc | 2 +- multibody/tree/ball_rpy_joint.h | 12 +- .../tree/quaternion_floating_mobilizer.h | 6 - multibody/tree/rigid_body.h | 5 +- ...xyz_mobilizer.cc => rpy_ball_mobilizer.cc} | 54 ++-- ...e_xyz_mobilizer.h => rpy_ball_mobilizer.h} | 199 +++++++-------- ...mobilizer.cc => rpy_floating_mobilizer.cc} | 133 ++++++---- ...g_mobilizer.h => rpy_floating_mobilizer.h} | 240 +++++++++--------- .../tree/test/free_rotating_body_plant.cc | 2 +- .../tree/test/free_rotating_body_plant.h | 2 +- .../tree/test/free_rotating_body_test.cc | 2 +- ...zer_test.cc => rpy_ball_mobilizer_test.cc} | 26 +- ...test.cc => rpy_floating_mobilizer_test.cc} | 52 ++-- 17 files changed, 398 insertions(+), 362 deletions(-) rename multibody/tree/{space_xyz_mobilizer.cc => rpy_ball_mobilizer.cc} (89%) rename multibody/tree/{space_xyz_mobilizer.h => rpy_ball_mobilizer.h} (50%) rename multibody/tree/{space_xyz_floating_mobilizer.cc => rpy_floating_mobilizer.cc} (79%) rename multibody/tree/{space_xyz_floating_mobilizer.h => rpy_floating_mobilizer.h} (50%) rename multibody/tree/test/{space_xyz_mobilizer_test.cc => rpy_ball_mobilizer_test.cc} (88%) rename multibody/tree/test/{space_xyz_floating_mobilizer_test.cc => rpy_floating_mobilizer_test.cc} (83%) diff --git a/multibody/plant/test/compliant_contact_manager_test.cc b/multibody/plant/test/compliant_contact_manager_test.cc index e339d1dac077..a4f0ae388b3a 100644 --- a/multibody/plant/test/compliant_contact_manager_test.cc +++ b/multibody/plant/test/compliant_contact_manager_test.cc @@ -1,7 +1,6 @@ #include "drake/multibody/plant/compliant_contact_manager.h" #include -#include #include #include @@ -16,17 +15,11 @@ #include "drake/multibody/contact_solvers/sap/sap_contact_problem.h" #include "drake/multibody/contact_solvers/sap/sap_solver.h" #include "drake/multibody/contact_solvers/sap/sap_solver_results.h" -#include "drake/multibody/parsing/parser.h" #include "drake/multibody/plant/multibody_plant.h" -#include "drake/multibody/plant/multibody_plant_config_functions.h" #include "drake/multibody/plant/sap_driver.h" #include "drake/multibody/plant/test/compliant_contact_manager_tester.h" #include "drake/multibody/plant/test/spheres_stack.h" #include "drake/multibody/plant/test_utilities/rigid_body_on_compliant_ground.h" -#include "drake/multibody/tree/joint_actuator.h" -#include "drake/multibody/tree/prismatic_joint.h" -#include "drake/multibody/tree/revolute_joint.h" -#include "drake/multibody/tree/space_xyz_mobilizer.h" using drake::geometry::GeometryId; using drake::geometry::GeometryInstance; diff --git a/multibody/plant/test/sap_driver_multidof_joints_test.cc b/multibody/plant/test/sap_driver_multidof_joints_test.cc index 24f270ccfe3a..a777a8039419 100644 --- a/multibody/plant/test/sap_driver_multidof_joints_test.cc +++ b/multibody/plant/test/sap_driver_multidof_joints_test.cc @@ -7,7 +7,7 @@ #include "drake/multibody/plant/multibody_plant.h" #include "drake/multibody/plant/sap_driver.h" #include "drake/multibody/plant/test/compliant_contact_manager_tester.h" -#include "drake/multibody/tree/space_xyz_mobilizer.h" +#include "drake/multibody/tree/rpy_ball_mobilizer.h" /* @file This file provides testing for the SapDriver's limited support for joint limits on joints with multiple degrees of freedom. @@ -97,7 +97,7 @@ class MultiDofJointWithLimits final : public Joint { // The only restriction here relevant for these tests is that we provide a // mobilizer with kNumDofs positions and velocities, so that indexes are // consistent during MultibodyPlant::Finalize(). - auto revolute_mobilizer = std::make_unique>( + auto revolute_mobilizer = std::make_unique>( this->frame_on_parent(), this->frame_on_child()); blue_print->mobilizer = std::move(revolute_mobilizer); return blue_print; diff --git a/multibody/rational/rational_forward_kinematics.cc b/multibody/rational/rational_forward_kinematics.cc index b9c9c9856c77..4095a483bd3c 100644 --- a/multibody/rational/rational_forward_kinematics.cc +++ b/multibody/rational/rational_forward_kinematics.cc @@ -1,6 +1,5 @@ #include "drake/multibody/rational/rational_forward_kinematics.h" -#include #include #include "drake/common/drake_assert.h" @@ -8,7 +7,6 @@ #include "drake/multibody/tree/multibody_tree_topology.h" #include "drake/multibody/tree/prismatic_mobilizer.h" #include "drake/multibody/tree/revolute_mobilizer.h" -#include "drake/multibody/tree/space_xyz_mobilizer.h" #include "drake/multibody/tree/weld_mobilizer.h" namespace drake { diff --git a/multibody/tree/BUILD.bazel b/multibody/tree/BUILD.bazel index 34580fc24b33..0b76989d5de3 100644 --- a/multibody/tree/BUILD.bazel +++ b/multibody/tree/BUILD.bazel @@ -116,10 +116,10 @@ drake_cc_library( "revolute_mobilizer.cc", "revolute_spring.cc", "rigid_body.cc", + "rpy_ball_mobilizer.cc", + "rpy_floating_mobilizer.cc", "screw_joint.cc", "screw_mobilizer.cc", - "space_xyz_floating_mobilizer.cc", - "space_xyz_mobilizer.cc", "uniform_gravity_field_element.cc", "universal_joint.cc", "universal_mobilizer.cc", @@ -162,10 +162,10 @@ drake_cc_library( "revolute_mobilizer.h", "revolute_spring.h", "rigid_body.h", + "rpy_ball_mobilizer.h", + "rpy_floating_mobilizer.h", "screw_joint.h", "screw_mobilizer.h", - "space_xyz_floating_mobilizer.h", - "space_xyz_mobilizer.h", "uniform_gravity_field_element.h", "universal_joint.h", "universal_mobilizer.h", @@ -499,7 +499,7 @@ drake_cc_googletest( ) drake_cc_googletest( - name = "space_xyz_mobilizer_test", + name = "rpy_ball_mobilizer_test", deps = [ ":mobilizer_tester", ":tree", @@ -509,7 +509,7 @@ drake_cc_googletest( ) drake_cc_googletest( - name = "space_xyz_floating_mobilizer_test", + name = "rpy_floating_mobilizer_test", deps = [ ":mobilizer_tester", ":tree", diff --git a/multibody/tree/ball_rpy_joint.cc b/multibody/tree/ball_rpy_joint.cc index 6aee507695b2..f3d0c2ce1ec4 100644 --- a/multibody/tree/ball_rpy_joint.cc +++ b/multibody/tree/ball_rpy_joint.cc @@ -63,7 +63,7 @@ template std::unique_ptr::BluePrint> BallRpyJoint::MakeImplementationBlueprint() const { auto blue_print = std::make_unique::BluePrint>(); - auto ballrpy_mobilizer = std::make_unique>( + auto ballrpy_mobilizer = std::make_unique>( this->frame_on_parent(), this->frame_on_child()); ballrpy_mobilizer->set_default_position(this->default_positions()); blue_print->mobilizer = std::move(ballrpy_mobilizer); diff --git a/multibody/tree/ball_rpy_joint.h b/multibody/tree/ball_rpy_joint.h index deaa99f907d3..70f49f77ca2a 100644 --- a/multibody/tree/ball_rpy_joint.h +++ b/multibody/tree/ball_rpy_joint.h @@ -9,7 +9,7 @@ #include "drake/common/drake_copyable.h" #include "drake/multibody/tree/joint.h" #include "drake/multibody/tree/multibody_forces.h" -#include "drake/multibody/tree/space_xyz_mobilizer.h" +#include "drake/multibody/tree/rpy_ball_mobilizer.h" namespace drake { namespace multibody { @@ -258,18 +258,18 @@ class BallRpyJoint final : public Joint { // Returns the mobilizer implementing this joint. // The internal implementation of this joint could change in a future version. // However its public API should remain intact. - const internal::SpaceXYZMobilizer* get_mobilizer() const { + const internal::RpyBallMobilizer* get_mobilizer() const { DRAKE_DEMAND(this->get_implementation().has_mobilizer()); - const internal::SpaceXYZMobilizer* mobilizer = - dynamic_cast*>( + const internal::RpyBallMobilizer* mobilizer = + dynamic_cast*>( this->get_implementation().mobilizer); DRAKE_DEMAND(mobilizer != nullptr); return mobilizer; } - internal::SpaceXYZMobilizer* get_mutable_mobilizer() { + internal::RpyBallMobilizer* get_mutable_mobilizer() { DRAKE_DEMAND(this->get_implementation().has_mobilizer()); - auto* mobilizer = dynamic_cast*>( + auto* mobilizer = dynamic_cast*>( this->get_implementation().mobilizer); DRAKE_DEMAND(mobilizer != nullptr); return mobilizer; diff --git a/multibody/tree/quaternion_floating_mobilizer.h b/multibody/tree/quaternion_floating_mobilizer.h index 51b2595d3839..70476775a456 100644 --- a/multibody/tree/quaternion_floating_mobilizer.h +++ b/multibody/tree/quaternion_floating_mobilizer.h @@ -132,12 +132,6 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl { // @param[in] R_FM // The rotation matrix relating the orientation of frame F and frame M. // @returns a constant reference to `this` mobilizer. - // @note: To create a RotationMatrix R_FM (which is inherently orthonormal) - // from a non-orthonormal Matrix3 m (e.g., m is approximate data), use - // R_FM = math::RotationMatrix::ProjectToRotationMatrix( m ). - // Alternatively, set this mobilizer's orientation with the two statements: - // const Eigen::Quaternion q_FM = RotationMatrix::ToQuaternion( m ); - // set_quaternion(context, q_FM); const QuaternionFloatingMobilizer& SetFromRotationMatrix( systems::Context* context, const math::RotationMatrix& R_FM) const { const Eigen::Quaternion q_FM = R_FM.ToQuaternion(); diff --git a/multibody/tree/rigid_body.h b/multibody/tree/rigid_body.h index fc0a666a77d5..0f015d27a731 100644 --- a/multibody/tree/rigid_body.h +++ b/multibody/tree/rigid_body.h @@ -288,8 +288,9 @@ class RigidBody : public MultibodyElement { /// (Advanced) Returns `true` if this body is granted 6-dofs by a Mobilizer /// and the parent body of this body's associated 6-dof joint is `world`. /// @note A floating body is not necessarily modeled with a quaternion - /// mobilizer, see has_quaternion_dofs(). Alternative options include a space - /// XYZ parametrization of rotations, see SpaceXYZMobilizer. + /// mobilizer, see has_quaternion_dofs(). Alternative options include a + /// roll-pitch-yaw (rpy) parametrization of rotations, see + /// RpyFloatingMobilizer. /// @throws std::exception if called pre-finalize, /// @see MultibodyPlant::Finalize() bool is_floating() const { diff --git a/multibody/tree/space_xyz_mobilizer.cc b/multibody/tree/rpy_ball_mobilizer.cc similarity index 89% rename from multibody/tree/space_xyz_mobilizer.cc rename to multibody/tree/rpy_ball_mobilizer.cc index c6e735efaa04..be7454154389 100644 --- a/multibody/tree/space_xyz_mobilizer.cc +++ b/multibody/tree/rpy_ball_mobilizer.cc @@ -1,4 +1,4 @@ -#include "drake/multibody/tree/space_xyz_mobilizer.h" +#include "drake/multibody/tree/rpy_ball_mobilizer.h" #include #include @@ -15,7 +15,7 @@ namespace multibody { namespace internal { template -std::string SpaceXYZMobilizer::position_suffix( +std::string RpyBallMobilizer::position_suffix( int position_index_in_mobilizer) const { switch (position_index_in_mobilizer) { case 0: @@ -25,11 +25,11 @@ std::string SpaceXYZMobilizer::position_suffix( case 2: return "qz"; } - throw std::runtime_error("SpaceXYZMobilizer has only 3 positions."); + throw std::runtime_error("RpyBallMobilizer has only 3 positions."); } template -std::string SpaceXYZMobilizer::velocity_suffix( +std::string RpyBallMobilizer::velocity_suffix( int velocity_index_in_mobilizer) const { switch (velocity_index_in_mobilizer) { case 0: @@ -40,17 +40,17 @@ std::string SpaceXYZMobilizer::velocity_suffix( return "wz"; } throw std::runtime_error( - "SpaceXYZMobilizer has only 3 velocities."); + "RpyBallMobilizer has only 3 velocities."); } template -Vector3 SpaceXYZMobilizer::get_angles( +Vector3 RpyBallMobilizer::get_angles( const systems::Context& context) const { return this->get_positions(context); } template -const SpaceXYZMobilizer& SpaceXYZMobilizer::set_angles( +const RpyBallMobilizer& RpyBallMobilizer::set_angles( systems::Context* context, const Vector3& angles) const { auto q = this->GetMutablePositions(context); q = angles; @@ -58,7 +58,7 @@ const SpaceXYZMobilizer& SpaceXYZMobilizer::set_angles( } template -const SpaceXYZMobilizer& SpaceXYZMobilizer::SetFromRotationMatrix( +const RpyBallMobilizer& RpyBallMobilizer::SetFromRotationMatrix( systems::Context* context, const math::RotationMatrix& R_FM) const { auto q = this->GetMutablePositions(context); DRAKE_ASSERT(q.size() == kNq); @@ -67,19 +67,19 @@ const SpaceXYZMobilizer& SpaceXYZMobilizer::SetFromRotationMatrix( } template -Vector3 SpaceXYZMobilizer::get_angular_velocity( +Vector3 RpyBallMobilizer::get_angular_velocity( const systems::Context& context) const { return this->get_velocities(context); } template -const SpaceXYZMobilizer& SpaceXYZMobilizer::set_angular_velocity( +const RpyBallMobilizer& RpyBallMobilizer::set_angular_velocity( systems::Context* context, const Vector3& w_FM) const { return set_angular_velocity(*context, w_FM, &context->get_mutable_state()); } template -const SpaceXYZMobilizer& SpaceXYZMobilizer::set_angular_velocity( +const RpyBallMobilizer& RpyBallMobilizer::set_angular_velocity( const systems::Context&, const Vector3& w_FM, systems::State* state) const { auto v = this->get_mutable_velocities(state); @@ -89,7 +89,7 @@ const SpaceXYZMobilizer& SpaceXYZMobilizer::set_angular_velocity( } template -math::RigidTransform SpaceXYZMobilizer::CalcAcrossMobilizerTransform( +math::RigidTransform RpyBallMobilizer::CalcAcrossMobilizerTransform( const systems::Context& context) const { const Eigen::Matrix& rpy = this->get_positions(context); DRAKE_ASSERT(rpy.size() == kNq); @@ -99,7 +99,7 @@ math::RigidTransform SpaceXYZMobilizer::CalcAcrossMobilizerTransform( } template -SpatialVelocity SpaceXYZMobilizer::CalcAcrossMobilizerSpatialVelocity( +SpatialVelocity RpyBallMobilizer::CalcAcrossMobilizerSpatialVelocity( const systems::Context&, const Eigen::Ref>& v) const { DRAKE_ASSERT(v.size() == kNv); @@ -108,7 +108,7 @@ SpatialVelocity SpaceXYZMobilizer::CalcAcrossMobilizerSpatialVelocity( template SpatialAcceleration -SpaceXYZMobilizer::CalcAcrossMobilizerSpatialAcceleration( +RpyBallMobilizer::CalcAcrossMobilizerSpatialAcceleration( const systems::Context&, const Eigen::Ref>& vdot) const { DRAKE_ASSERT(vdot.size() == kNv); @@ -116,7 +116,7 @@ SpaceXYZMobilizer::CalcAcrossMobilizerSpatialAcceleration( } template -void SpaceXYZMobilizer::ProjectSpatialForce( +void RpyBallMobilizer::ProjectSpatialForce( const systems::Context&, const SpatialForce& F_Mo_F, Eigen::Ref> tau) const { @@ -125,7 +125,7 @@ void SpaceXYZMobilizer::ProjectSpatialForce( } template -void SpaceXYZMobilizer::DoCalcNMatrix( +void RpyBallMobilizer::DoCalcNMatrix( const systems::Context& context, EigenPtr> N) const { using std::sin; using std::cos; @@ -151,7 +151,7 @@ void SpaceXYZMobilizer::DoCalcNMatrix( // singular. if (abs(cp) < 1.0e-3) { throw std::runtime_error(fmt::format( - "The SpaceXYZMobilizer (likely associated with a BallRpyJoint) between " + "The RpyBallMobilizer (implementing a BallRpyJoint) between " "body {} and body {} has reached a singularity. This occurs when the " "pitch angle takes values near π/2 + kπ, ∀ k ∈ ℤ. At the current " "configuration, we have pitch = {}. Drake does not yet support a " @@ -179,7 +179,7 @@ void SpaceXYZMobilizer::DoCalcNMatrix( } template -void SpaceXYZMobilizer::DoCalcNplusMatrix( +void RpyBallMobilizer::DoCalcNplusMatrix( const systems::Context& context, EigenPtr> Nplus) const { // The linear map between q̇ and v is given by matrix E_F(q) defined by: // [ cos(y) * cos(p), -sin(y), 0] @@ -208,7 +208,7 @@ void SpaceXYZMobilizer::DoCalcNplusMatrix( } template -void SpaceXYZMobilizer::MapVelocityToQDot( +void RpyBallMobilizer::MapVelocityToQDot( const systems::Context& context, const Eigen::Ref>& v, EigenPtr> qdot) const { @@ -271,7 +271,7 @@ void SpaceXYZMobilizer::MapVelocityToQDot( const T cp = cos(angles[1]); if (abs(cp) < 1.0e-3) { throw std::runtime_error(fmt::format( - "The SpaceXYZMobilizer (likely associated with a BallRpyJoint) between " + "The RpyBallMobilizer (implementing a BallRpyJoint) between " "body {} and body {} has reached a singularity. This occurs when the " "pitch angle takes values near π/2 + kπ, ∀ k ∈ ℤ. At the current " "configuration, we have pitch = {}. Drake does not yet support a " @@ -303,7 +303,7 @@ void SpaceXYZMobilizer::MapVelocityToQDot( } template -void SpaceXYZMobilizer::MapQDotToVelocity( +void RpyBallMobilizer::MapQDotToVelocity( const systems::Context& context, const Eigen::Ref>& qdot, EigenPtr> v) const { @@ -370,32 +370,32 @@ void SpaceXYZMobilizer::MapQDotToVelocity( template template std::unique_ptr> -SpaceXYZMobilizer::TemplatedDoCloneToScalar( +RpyBallMobilizer::TemplatedDoCloneToScalar( const MultibodyTree& tree_clone) const { const Frame& inboard_frame_clone = tree_clone.get_variant(this->inboard_frame()); const Frame& outboard_frame_clone = tree_clone.get_variant(this->outboard_frame()); - return std::make_unique>( + return std::make_unique>( inboard_frame_clone, outboard_frame_clone); } template -std::unique_ptr> SpaceXYZMobilizer::DoCloneToScalar( +std::unique_ptr> RpyBallMobilizer::DoCloneToScalar( const MultibodyTree& tree_clone) const { return TemplatedDoCloneToScalar(tree_clone); } template std::unique_ptr> -SpaceXYZMobilizer::DoCloneToScalar( +RpyBallMobilizer::DoCloneToScalar( const MultibodyTree& tree_clone) const { return TemplatedDoCloneToScalar(tree_clone); } template std::unique_ptr> -SpaceXYZMobilizer::DoCloneToScalar( +RpyBallMobilizer::DoCloneToScalar( const MultibodyTree& tree_clone) const { return TemplatedDoCloneToScalar(tree_clone); } @@ -405,4 +405,4 @@ SpaceXYZMobilizer::DoCloneToScalar( } // namespace drake DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class ::drake::multibody::internal::SpaceXYZMobilizer) + class ::drake::multibody::internal::RpyBallMobilizer) diff --git a/multibody/tree/space_xyz_mobilizer.h b/multibody/tree/rpy_ball_mobilizer.h similarity index 50% rename from multibody/tree/space_xyz_mobilizer.h rename to multibody/tree/rpy_ball_mobilizer.h index 2f570b015633..2934d5456de1 100644 --- a/multibody/tree/space_xyz_mobilizer.h +++ b/multibody/tree/rpy_ball_mobilizer.h @@ -16,61 +16,58 @@ namespace drake { namespace multibody { namespace internal { -// This mobilizer models a gimbal joint between an inboard frame F and an -// outboard frame M that allows frame M to rotate freely with respect to F ( -// though a gimbal joint provides arbitrary orientation like a ball joint but -// with some restrictions, discussed below). No translational motion of M in F -// is allowed and the inboard frame origin `Fo` and the outboard frame origin -// `Mo` are coincident at all times. +// This mobilizer models a 3 dof gimbal joint between an inboard frame F and an +// outboard frame M that allows frame M to rotate freely with respect to F. A +// gimbal joint provides arbitrary orientation like a ball joint (and in fact we +// call it a BallRpyJoint in Drake) but with some restrictions, discussed +// below). No translational motion of M in F is allowed and the inboard frame +// origin Fo and the outboard frame origin Mo are coincident at all times. // -// The orientation `R_FM` of the outboard frame M in F is parameterized with -// space `x-y-z` Euler angles (also known as extrinsic angles). That is, the -// generalized coordinates for this mobilizer correspond to angles -// θ₁, θ₂, θ₃, for a sequence of rotations about the x̂, ŷ, ẑ axes solidary with -// frame F, respectively. Mathematically, rotation `R_FM` is given in terms of -// angles θ₁, θ₂, θ₃ by: 
-//   R_FM(q) = Rz(θ₃) * Ry(θ₂) * Rx(θ₁)
-//
-// where `Rx(θ)`, `Ry(θ)` and `Rz(θ)` correspond to the elemental rotations in -// amount of θ about the x, y and z axes respectively. -// Zero θ₁, θ₂, θ₃ angles define the "zero configuration" which corresponds -// to frames F and M being coincident, see set_zero_state(). -// Angles θ₁, θ₂, θ₃ are defined to be positive according to the -// right-hand-rule with the thumb aligned in the direction of their respective -// axes. +// The orientation R_FM of the outboard frame M in F is parameterized with +// roll-pitch-yaw angles (also known as space-fixed x-y-z Euler angles or +// extrinsic angles). That is, the generalized coordinates q for this mobilizer +// correspond to angles q = [θ₀ θ₁ θ₂], for a sequence of rotations about the +// Fx, Fy, and Fz axes, respectively. Mathematically, rotation R_FM is given in +// terms of angles θ₀, θ₁, θ₂ by: // -// The generalized velocities for this mobilizer correspond to the angular -// velocity `w_FM` of frame M in F, expressed in frame F. -// MapVelocityToQDot() maps the angular velocity `w_FM` to Euler angles's rates -// while MapQDotToVelocity() maps Euler angles's rates to angular velocity -// `w_FM`. -// While the mapping MapVelocityToQDot() always exists, its inverse -// MapQDotToVelocity() is singular for values of θ₂ (many times referred to as -// the pitch angle) such that `θ₂ = π/2 + kπ, ∀ k ∈ ℤ`. +// R_FM(q) = Rz(θ₂) * Ry(θ₁) * Rx(θ₀) +// +// where Rx(θ), Ry(θ) and Rz(θ) correspond to the elemental rotations in amount +// of θ about the Fx, Fy and Fz axes respectively. Zero θ₀, θ₁, θ₂ angles define +// the "zero configuration" which corresponds to frames F and M being +// coincident, see set_zero_state(). Angles θ₀, θ₁, θ₂ are defined to be +// positive according to the right-hand-rule with the thumb aligned in the +// direction of their respective axes. // -// @note Space `x-y-z` angles (extrinsic) are equivalent to Body `z-y-x` angles -// (intrinsic). +// The generalized velocities v for this mobilizer correspond to the angular +// velocity w_FM of frame M in F, expressed in frame F. Note that this is _not_ +// the same as the time derivative q̇ the rotation angles; you must use +// q̇ = MapVelocityToQDot(w_FM) to get the angle time derivatives. +// +// MapVelocityToQDot() maps the angular velocity w_FM to Euler angle rates +// while MapQDotToVelocity() maps Euler angles's rates to angular velocity +// w_FM. While the mapping MapVelocityToQDot() always exists, its inverse +// MapQDotToVelocity() is singular for values of θ₁ (many times referred to as +// the pitch angle) such that θ₁ = π/2 + kπ, ∀ k ∈ ℤ. // -// @note This particular choice of generalized coordinates θ₁, θ₂, θ₃ for this -// mobilizer is many times referred to as the roll, pitch and yaw angles by -// many dynamicists. They are also known as the Tait-Bryan angles or Cardan -// angles. +// @note Roll-pitch-yaw (space x-y-z) angles (extrinsic, about Fx Fy Fz) are +// equivalent to body-fixed z-y-x angles (intrinsic, about Mz M'y M''x where +// the primes indicate new axis directions after each rotation). // -// @note The mapping from angular velocity to Euler angle's rates is singular -// for angle `θ₂` (many times referred to as the pitch angle) such that -// `θ₂ = π/2 + kπ, ∀ k ∈ ℤ`. +// @note The roll-pitch-yaw (space x-y-z) Euler sequence is also known as the +// Tait-Bryan angles or Cardan angles. // // @tparam_default_scalar template -class SpaceXYZMobilizer final : public MobilizerImpl { +class RpyBallMobilizer final : public MobilizerImpl { public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SpaceXYZMobilizer) + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RpyBallMobilizer) - // Constructor for a %SpaceXYZMobilizer between an inboard frame F - // `inboard_frame_F` and an outboard frame M `outboard_frame_M` granting - // three rotational degree of freedom corresponding to angles θ₁, θ₂, θ₃ as + // Constructor for an RpyBallMobilizer between an inboard frame F + // inboard_frame_F and an outboard frame M outboard_frame_M granting + // three rotational degree of freedom corresponding to angles θ₀, θ₁, θ₂ as // described in this class's documentation. - SpaceXYZMobilizer(const Frame& inboard_frame_F, + RpyBallMobilizer(const Frame& inboard_frame_F, const Frame& outboard_frame_M) : MobilizerBase(inboard_frame_F, outboard_frame_M) {} @@ -84,49 +81,45 @@ class SpaceXYZMobilizer final : public MobilizerImpl { bool can_rotate() const final { return true; } bool can_translate() const final { return false; } - // Retrieves from `context` the three space x-y-z angles θ₁, θ₂, θ₃ which - // describe the state for `this` mobilizer as documented in this class's + // Retrieves from context the three roll-pitch-yaw angles θ₀, θ₁, θ₂ which + // describe the state for this mobilizer as documented in this class's // documentation. // // @param[in] context // The context of the MultibodyTree this mobilizer belongs to. // @retval angles - // The three space x-y-z angles θ₁, θ₂, θ₃, associated with the sequence of - // rotations about the space fixed axes x̂, ŷ, ẑ, respectively packed and - // returned as a Vector3 with entries `angles(0) = θ₁`, `angles(1) = θ₂`, - // `angles(2) = θ₃`. + // The three roll-pitch-yaw angles θ₀, θ₁, θ₂, associated with the sequence + // of rotations about the space fixed axes x̂, ŷ, ẑ, respectively packed and + // returned as a Vector3 with entries angles(0) = θ₀, angles(1) = θ₁, + // angles(2) = θ₂. Vector3 get_angles(const systems::Context& context) const; - // Sets in `context` the state for `this` mobilizer to have the space x-y-z - // angles θ₁, θ₂, θ₃, provided in the input argument `angles`, which stores - // the with the format `angles = [θ₁, θ₂, θ₃]`. + // Sets in context the state for this mobilizer to have the roll-pitch-yaw + // angles θ₀, θ₁, θ₂, provided in the input argument angles, which stores + // them with the format angles = [θ₀, θ₁, θ₂]. // // @param[out] context // The context of the MultibodyTree this mobilizer belongs to. // @param[in] angles - // A Vector3 which must pack values for the space x-y-z angles θ₁, θ₂, θ₃, - // described in this class's documentation, at entries `angles(0)`, - // `angles(1)` and `angles(2)`, respectively. - // @returns a constant reference to `this` mobilizer. - const SpaceXYZMobilizer& set_angles( + // A Vector3 which must pack values for the roll-pitch-yaw angles θ₀, θ₁, + // θ₂, described in this class's documentation, at entries angles(0), + // angles(1) and angles(2), respectively. + // @returns a constant reference to this mobilizer. + const RpyBallMobilizer& set_angles( systems::Context* context, const Vector3& angles) const; - // Sets `context` so this mobilizer's generalized coordinates (space x-y-z - // angles θ₁, θ₂, θ₃) are consistent with the given `R_FM` rotation matrix. + // Sets context so this mobilizer's generalized coordinates (roll-pitch-yaw + // angles θ₀, θ₁, θ₂) are consistent with the given R_FM rotation matrix. // @param[in] context // The context of the MultibodyTree that this mobilizer belongs to. // @param[in] R_FM // The rotation matrix relating the orientation of frame F and frame M. - // @returns a constant reference to `this` mobilizer. - // @note: To create a RotationMatrix R_FM (which is inherently orthonormal) - // from a non-orthonormal Matrix3 m (e.g., m is approximate data), use - // R_FM = math::RotationMatrix::ProjectToRotationMatrix( m ). - // See @ref RotationMatrix::ProjectToRotationMatrix(). - const SpaceXYZMobilizer& SetFromRotationMatrix( + // @returns a constant reference to this mobilizer. + const RpyBallMobilizer& SetFromRotationMatrix( systems::Context* context, const math::RotationMatrix& R_FM) const; - // Retrieves from `context` the angular velocity `w_FM` of the outboard frame + // Retrieves from context the angular velocity w_FM of the outboard frame // M in the inboard frame F, expressed in F. // // @param[in] context @@ -137,58 +130,58 @@ class SpaceXYZMobilizer final : public MobilizerImpl { // // @note Many dynamicists follow the convention of expressing angular // velocity in the outboard frame M; we return it expressed in the inboard - // frame F. That is, this method returns `W_FM_F`. + // frame F. That is, this method returns W_FM_F. Vector3 get_angular_velocity(const systems::Context& context) const; - // Sets in `context` the state for `this` mobilizer so that the angular - // velocity of the outboard frame M in the inboard frame F is `w_FM`. + // Sets in context the state for this mobilizer so that the angular + // velocity of the outboard frame M in the inboard frame F is w_FM. // @param[out] context // The context of the MultibodyTree this mobilizer belongs to. // @param[in] w_FM // A vector in ℝ³ with the desired angular velocity of the outboard frame M // in the inboard frame F, expressed in F. - // @returns a constant reference to `this` mobilizer. - const SpaceXYZMobilizer& set_angular_velocity( + // @returns a constant reference to this mobilizer. + const RpyBallMobilizer& set_angular_velocity( systems::Context* context, const Vector3& w_FM) const; - // Stores in `state` the angular velocity `w_FM` of the outboard frame - // M in the inboard frame F corresponding to `this` mobilizer. + // Stores in state the angular velocity w_FM of the outboard frame + // M in the inboard frame F corresponding to this mobilizer. // // @param[in] context // The context of the MultibodyTree this mobilizer belongs to. // @param[out] state - // On return, `state` will store the angular velocity `w_FM` of frame F in + // On return, state will store the angular velocity w_FM of frame F in // frame M. // @param[in] w_FM // A vector in ℝ³ with the desired angular velocity of the outboard frame M // in the inboard frame F, expressed in F. - // @returns a constant reference to `this` mobilizer. - const SpaceXYZMobilizer& set_angular_velocity( + // @returns a constant reference to this mobilizer. + const RpyBallMobilizer& set_angular_velocity( const systems::Context& context, const Vector3& w_FM, systems::State* state) const; - // Computes the across-mobilizer transform `X_FM(q)` between the inboard - // frame F and the outboard frame M as a function of the space x-y-z angles - // θ₁, θ₂, θ₃ stored in `context`. + // Computes the across-mobilizer transform X_FM(q) between the inboard + // frame F and the outboard frame M as a function of the roll-pitch-yaw angles + // θ₀, θ₁, θ₂ stored in context. math::RigidTransform CalcAcrossMobilizerTransform( const systems::Context& context) const override; - // Computes the across-mobilizer velocity `V_FM(q, v)` of the outboard frame - // M measured and expressed in frame F as a function of the space x-y-z - // angles θ₁, θ₂, θ₃ stored in `context` and of the input generalized - // velocity v which contains the components of the angular velocity `w_FM` + // Computes the across-mobilizer velocity V_FM(q, v) of the outboard frame + // M measured and expressed in frame F as a function of the roll-pitch-yaw + // angles θ₀, θ₁, θ₂ stored in context and of the input generalized + // velocity v which contains the components of the angular velocity w_FM // expressed in frame F. SpatialVelocity CalcAcrossMobilizerSpatialVelocity( const systems::Context& context, const Eigen::Ref>& v) const override; - // Computes the across-mobilizer acceleration `A_FM(q, v, v̇)` of the + // Computes the across-mobilizer acceleration A_FM(q, v, v̇) of the // outboard frame M in the inboard frame F. - // The acceleration `A_FM` will be a function of the generalized positions q - // (space x-y-z angles) stored in `context`, of the generalized velocities - // v (angular velocity `w_FM`) also stored in `context` and of the supplied - // generalized accelerations `vdot`, which in this case correspond to angular - // acceleration of M in F `alpha_FM = Dt_F(w_FM)` (see + // The acceleration A_FM will be a function of the generalized positions q + // (roll-pitch-yaw angles) stored in context, of the generalized velocities + // v (angular velocity w_FM) also stored in context and of the supplied + // generalized accelerations vdot, which in this case correspond to angular + // acceleration of M in F alpha_FM = Dt_F(w_FM) (see // @ref Dt_multibody_quantities for our notation of time derivatives in // different reference frames). SpatialAcceleration CalcAcrossMobilizerSpatialAcceleration( @@ -203,40 +196,40 @@ class SpaceXYZMobilizer final : public MobilizerImpl { bool is_velocity_equal_to_qdot() const override { return false; } // Maps the generalized velocity v, which corresponds to the angular velocity - // `w_FM`, to time derivatives of space x-y-z angles θ₁, θ₂, θ₃ in `qdot`. + // w_FM, to time derivatives of roll-pitch-yaw angles θ₀, θ₁, θ₂ in qdot. // // @param[in] context // The context of the MultibodyTree this mobilizer belongs to. // @param[in] v // A vector of generalized velocities for this Mobilizer which should - // correspond to a vector in ℝ³ for an angular velocity `w_FM` of M in F. + // correspond to a vector in ℝ³ for an angular velocity w_FM of M in F. // @param[out] qdot - // A Vector3 packing of the time derivatives of the space x-y-z angles θ₁, - // θ₂, θ₃ in `qdot(0)`, `qdot(1)` and `qdot(2)`, respectively. + // A Vector3 packing of the time derivatives of the roll-pitch-yaw angles + // θ₀, θ₁, θ₂ in qdot(0), qdot(1) and qdot(2), respectively. // // @warning The mapping from Euler angle's rates to angular velocity is - // singular for angle `θ₂` such that `θ₂ = π/2 + kπ, ∀ k ∈ ℤ`. + // singular for angle θ₁ such that θ₁ = π/2 + kπ, ∀ k ∈ ℤ. // To avoid working close to this singularity (which could potentially result - // in large errors for `qdot`), this method aborts when the absolute value of - // the cosine of θ₂ is smaller than 10⁻³, a number arbitrarily chosen to this + // in large errors for qdot), this method aborts when the absolute value of + // the cosine of θ₁ is smaller than 10⁻³, a number arbitrarily chosen to this // end. void MapVelocityToQDot( const systems::Context& context, const Eigen::Ref>& v, EigenPtr> qdot) const override; - // Maps time derivatives of the space x-y-z angles θ₁, θ₂, θ₃ in `qdot` to + // Maps time derivatives of the roll-pitch-yaw angles θ₀, θ₁, θ₂ in qdot to // the generalized velocity v, which corresponds to the angular velocity - // `w_FM`. + // w_FM. // // @param[in] context // The context of the MultibodyTree this mobilizer belongs to. // @param[in] qdot - // A vector containing the time derivatives of the space x-y-z angles - // θ₁, θ₂, θ₃ in `qdot(0)`, `qdot(1)` and `qdot(2)`, respectively. + // A vector containing the time derivatives of the roll-pitch-yaw angles + // θ₀, θ₁, θ₂ in qdot(0), qdot(1) and qdot(2), respectively. // @param[out] v // A vector of generalized velocities for this Mobilizer which should - // correspond to a vector in ℝ³ for an angular velocity `w_FM` of M in F. + // correspond to a vector in ℝ³ for an angular velocity w_FM of M in F. void MapQDotToVelocity( const systems::Context& context, const Eigen::Ref>& qdot, @@ -282,4 +275,4 @@ class SpaceXYZMobilizer final : public MobilizerImpl { } // namespace drake DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class ::drake::multibody::internal::SpaceXYZMobilizer) + class ::drake::multibody::internal::RpyBallMobilizer) diff --git a/multibody/tree/space_xyz_floating_mobilizer.cc b/multibody/tree/rpy_floating_mobilizer.cc similarity index 79% rename from multibody/tree/space_xyz_floating_mobilizer.cc rename to multibody/tree/rpy_floating_mobilizer.cc index cb50d1c47dcb..d79d9374d32b 100644 --- a/multibody/tree/space_xyz_floating_mobilizer.cc +++ b/multibody/tree/rpy_floating_mobilizer.cc @@ -1,7 +1,8 @@ -#include "drake/multibody/tree/space_xyz_floating_mobilizer.h" +#include "drake/multibody/tree/rpy_floating_mobilizer.h" #include #include +#include #include "drake/common/drake_assert.h" #include "drake/common/eigen_types.h" @@ -14,43 +15,85 @@ namespace multibody { namespace internal { template -Vector6 SpaceXYZFloatingMobilizer::get_generalized_positions( +std::string RpyFloatingMobilizer::position_suffix( + int position_index_in_mobilizer) const { + switch (position_index_in_mobilizer) { + case 0: + return "qx"; + case 1: + return "qy"; + case 2: + return "qz"; + case 3: + return "x"; + case 4: + return "y"; + case 5: + return "z"; + } + throw std::runtime_error( + "RpyFloatingMobilizer has only 6 positions."); +} + +template +std::string RpyFloatingMobilizer::velocity_suffix( + int velocity_index_in_mobilizer) const { + switch (velocity_index_in_mobilizer) { + case 0: + return "wx"; + case 1: + return "wy"; + case 2: + return "wz"; + case 3: + return "vx"; + case 4: + return "vy"; + case 5: + return "vz"; + } + throw std::runtime_error( + "RpyFloatingMobilizer has only 6 velocities."); +} + +template +Vector6 RpyFloatingMobilizer::get_generalized_positions( const systems::Context& context) const { return this->get_positions(context); } template -Vector6 SpaceXYZFloatingMobilizer::get_generalized_velocities( +Vector6 RpyFloatingMobilizer::get_generalized_velocities( const systems::Context& context) const { return this->get_velocities(context); } template -Vector3 SpaceXYZFloatingMobilizer::get_angles( +Vector3 RpyFloatingMobilizer::get_angles( const systems::Context& context) const { return this->get_positions(context).template head<3>(); } template -Vector3 SpaceXYZFloatingMobilizer::get_translation( +Vector3 RpyFloatingMobilizer::get_translation( const systems::Context& context) const { return this->get_positions(context).template tail<3>(); } template -Vector3 SpaceXYZFloatingMobilizer::get_angular_velocity( +Vector3 RpyFloatingMobilizer::get_angular_velocity( const systems::Context& context) const { return this->get_velocities(context).template head<3>(); } template -Vector3 SpaceXYZFloatingMobilizer::get_translational_velocity( +Vector3 RpyFloatingMobilizer::get_translational_velocity( const systems::Context& context) const { return this->get_velocities(context).template tail<3>(); } template -const SpaceXYZFloatingMobilizer& SpaceXYZFloatingMobilizer::set_angles( +const RpyFloatingMobilizer& RpyFloatingMobilizer::set_angles( systems::Context* context, const Vector3& angles) const { auto q = this->GetMutablePositions(context).template head<3>(); q = angles; @@ -58,17 +101,17 @@ const SpaceXYZFloatingMobilizer& SpaceXYZFloatingMobilizer::set_angles( } template -const SpaceXYZFloatingMobilizer& -SpaceXYZFloatingMobilizer::set_translation(systems::Context* context, - const Vector3& p_FM) const { +const RpyFloatingMobilizer& +RpyFloatingMobilizer::set_translation(systems::Context* context, + const Vector3& p_FM) const { auto q = this->GetMutablePositions(context).template tail<3>(); q = p_FM; return *this; } template -const SpaceXYZFloatingMobilizer& -SpaceXYZFloatingMobilizer::set_angular_velocity( +const RpyFloatingMobilizer& +RpyFloatingMobilizer::set_angular_velocity( systems::Context* context, const Vector3& w_FM) const { auto v = this->GetMutableVelocities(context).template head<3>(); v = w_FM; @@ -76,8 +119,8 @@ SpaceXYZFloatingMobilizer::set_angular_velocity( } template -const SpaceXYZFloatingMobilizer& -SpaceXYZFloatingMobilizer::set_translational_velocity( +const RpyFloatingMobilizer& +RpyFloatingMobilizer::set_translational_velocity( systems::Context* context, const Vector3& v_FM) const { auto v = this->GetMutableVelocities(context).template tail<3>(); v = v_FM; @@ -85,8 +128,8 @@ SpaceXYZFloatingMobilizer::set_translational_velocity( } template -const SpaceXYZFloatingMobilizer& -SpaceXYZFloatingMobilizer::SetFromRigidTransform( +const RpyFloatingMobilizer& +RpyFloatingMobilizer::SetFromRigidTransform( systems::Context* context, const math::RigidTransform& X_FM) const { set_angles(context, math::RollPitchYaw(X_FM.rotation()).vector()); set_translation(context, X_FM.translation()); @@ -95,7 +138,7 @@ SpaceXYZFloatingMobilizer::SetFromRigidTransform( template math::RigidTransform -SpaceXYZFloatingMobilizer::CalcAcrossMobilizerTransform( +RpyFloatingMobilizer::CalcAcrossMobilizerTransform( const systems::Context& context) const { const auto rpy = this->get_angles(context); const auto p_FM = this->get_translation(context); @@ -105,7 +148,7 @@ SpaceXYZFloatingMobilizer::CalcAcrossMobilizerTransform( template SpatialVelocity -SpaceXYZFloatingMobilizer::CalcAcrossMobilizerSpatialVelocity( +RpyFloatingMobilizer::CalcAcrossMobilizerSpatialVelocity( const systems::Context&, const Eigen::Ref>& v) const { DRAKE_ASSERT(v.size() == kNv); return SpatialVelocity(v); @@ -113,7 +156,7 @@ SpaceXYZFloatingMobilizer::CalcAcrossMobilizerSpatialVelocity( template SpatialAcceleration -SpaceXYZFloatingMobilizer::CalcAcrossMobilizerSpatialAcceleration( +RpyFloatingMobilizer::CalcAcrossMobilizerSpatialAcceleration( const systems::Context&, const Eigen::Ref>& vdot) const { DRAKE_ASSERT(vdot.size() == kNv); @@ -121,7 +164,7 @@ SpaceXYZFloatingMobilizer::CalcAcrossMobilizerSpatialAcceleration( } template -void SpaceXYZFloatingMobilizer::ProjectSpatialForce( +void RpyFloatingMobilizer::ProjectSpatialForce( const systems::Context&, const SpatialForce& F_Mo_F, Eigen::Ref> tau) const { DRAKE_ASSERT(tau.size() == kNv); @@ -129,7 +172,7 @@ void SpaceXYZFloatingMobilizer::ProjectSpatialForce( } template -void SpaceXYZFloatingMobilizer::DoCalcNMatrix( +void RpyFloatingMobilizer::DoCalcNMatrix( const systems::Context& context, EigenPtr> N) const { using std::abs; using std::cos; @@ -154,13 +197,12 @@ void SpaceXYZFloatingMobilizer::DoCalcNMatrix( // Demand for the computation to be away from a state for which Einv_F is // singular. if (abs(cp) < 1.0e-3) { - // TODO(russt): Add a "likely associated with SpaceXYZFloatingJoint" to - // match the error in space_xyz_mobilizer pending #19065. throw std::runtime_error(fmt::format( - "The SpaceXYZFloatingMobilizer between body {} and body {} has reached " - "a singularity. This occurs when the pitch angle takes values near π/2 " - "+ kπ, ∀ k ∈ ℤ. At the current configuration, we have pitch = {}. " - "Consider using a QuaternionFloatingJoint instead.", + "The RpyFloatingMobilizer (implementing an RpyFloatingJoint) between " + "body {} and body {} has reached a singularity. This occurs when the " + "pitch angle takes values near π/2 + kπ, ∀ k ∈ ℤ. At the current " + "configuration, we have pitch = {}. Consider using a " + "QuaternionFloatingJoint instead.", this->inboard_body().name(), this->outboard_body().name(), angles[1])); } @@ -185,7 +227,7 @@ void SpaceXYZFloatingMobilizer::DoCalcNMatrix( } template -void SpaceXYZFloatingMobilizer::DoCalcNplusMatrix( +void RpyFloatingMobilizer::DoCalcNplusMatrix( const systems::Context& context, EigenPtr> Nplus) const { // The linear map between q̇ and v is given by matrix E_F(q) defined by: // [ cos(y) * cos(p), -sin(y), 0] @@ -220,7 +262,7 @@ void SpaceXYZFloatingMobilizer::DoCalcNplusMatrix( } template -void SpaceXYZFloatingMobilizer::MapVelocityToQDot( +void RpyFloatingMobilizer::MapVelocityToQDot( const systems::Context& context, const Eigen::Ref>& v, EigenPtr> qdot) const { DRAKE_ASSERT(v.size() == kNv); @@ -235,7 +277,7 @@ void SpaceXYZFloatingMobilizer::MapVelocityToQDot( // w_FM = E_F(q) * q̇; q̇ = [ṙ, ṗ, ẏ]ᵀ // // Here, following a convention used by many dynamicists, we are calling the - // angles θ₁, θ₂, θ₃ as roll (r), pitch (p) and yaw (y), respectively. + // angles θ₀, θ₁, θ₂ as roll (r), pitch (p) and yaw (y), respectively. // // The linear map from v to q̇ is given by the inverse of E_F(q): // [ cos(y) / cos(p), sin(y) / cos(p), 0] @@ -281,13 +323,12 @@ void SpaceXYZFloatingMobilizer::MapVelocityToQDot( const Vector3 angles = get_angles(context); const T cp = cos(angles[1]); if (abs(cp) < 1.0e-3) { - // TODO(russt): Add a "likely associated with SpaceXYZFloatingJoint" to - // match the error in space_xyz_mobilizer pending #19065. throw std::runtime_error(fmt::format( - "The SpaceXYZFloatingMobilizer between body {} and body {} has reached " - "a singularity. This occurs when the pitch angle takes values near π/2 " - "+ kπ, ∀ k ∈ ℤ. At the current configuration, we have pitch = {}. " - "Consider using a QuaternionFloatingJoint instead.", + "The RpyFloatingMobilizer (implementing an RpyFloatingJoint) between " + "body {} and body {} has reached a singularity. This occurs when the " + "pitch angle takes values near π/2 + kπ, ∀ k ∈ ℤ. At the current " + "configuration, we have pitch = {}. Consider using a " + "QuaternionFloatingJoint instead.", this->inboard_body().name(), this->outboard_body().name(), angles[1])); } @@ -301,7 +342,7 @@ void SpaceXYZFloatingMobilizer::MapVelocityToQDot( const T cpi = 1.0 / cp; // Although the linear equations relating v to q̇ can be used to explicitly - // solve the equation w_FM = E_F(q) * q̇ for q̇, a more computational + // solve the equation w_FM = E_F(q) * q̇ for q̇, a more computationally // efficient solution results by implicit solution of those linear equations. // Namely, the first two equations in w_FM = E_F(q) * q̇ are used to solve for // ṙ and ṗ, then the third equation is used to solve for ẏ in terms of just @@ -315,7 +356,7 @@ void SpaceXYZFloatingMobilizer::MapVelocityToQDot( } template -void SpaceXYZFloatingMobilizer::MapQDotToVelocity( +void RpyFloatingMobilizer::MapQDotToVelocity( const systems::Context& context, const Eigen::Ref>& qdot, EigenPtr> v) const { DRAKE_ASSERT(qdot.size() == kNq); @@ -332,7 +373,7 @@ void SpaceXYZFloatingMobilizer::MapQDotToVelocity( // w_FM = E_F(q) * q̇; q̇ = [ṙ, ṗ, ẏ]ᵀ // // Here, following a convention used by many dynamicists, we are calling the - // angles θ₁, θ₂, θ₃ as roll (r), pitch (p) and yaw (y), respectively. + // angles θ₀, θ₁, θ₂ as roll (r), pitch (p) and yaw (y), respectively. // // Note to developers: // Matrix E_F(q) is obtained by computing w_FM as the composition of the @@ -384,33 +425,33 @@ void SpaceXYZFloatingMobilizer::MapQDotToVelocity( template template std::unique_ptr> -SpaceXYZFloatingMobilizer::TemplatedDoCloneToScalar( +RpyFloatingMobilizer::TemplatedDoCloneToScalar( const MultibodyTree& tree_clone) const { const Frame& inboard_frame_clone = tree_clone.get_variant(this->inboard_frame()); const Frame& outboard_frame_clone = tree_clone.get_variant(this->outboard_frame()); - return std::make_unique>( + return std::make_unique>( inboard_frame_clone, outboard_frame_clone); } template std::unique_ptr> -SpaceXYZFloatingMobilizer::DoCloneToScalar( +RpyFloatingMobilizer::DoCloneToScalar( const MultibodyTree& tree_clone) const { return TemplatedDoCloneToScalar(tree_clone); } template std::unique_ptr> -SpaceXYZFloatingMobilizer::DoCloneToScalar( +RpyFloatingMobilizer::DoCloneToScalar( const MultibodyTree& tree_clone) const { return TemplatedDoCloneToScalar(tree_clone); } template std::unique_ptr> -SpaceXYZFloatingMobilizer::DoCloneToScalar( +RpyFloatingMobilizer::DoCloneToScalar( const MultibodyTree& tree_clone) const { return TemplatedDoCloneToScalar(tree_clone); } @@ -420,4 +461,4 @@ SpaceXYZFloatingMobilizer::DoCloneToScalar( } // namespace drake DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class ::drake::multibody::internal::SpaceXYZFloatingMobilizer) + class ::drake::multibody::internal::RpyFloatingMobilizer) diff --git a/multibody/tree/space_xyz_floating_mobilizer.h b/multibody/tree/rpy_floating_mobilizer.h similarity index 50% rename from multibody/tree/space_xyz_floating_mobilizer.h rename to multibody/tree/rpy_floating_mobilizer.h index fa12236ce401..77ecb582801f 100644 --- a/multibody/tree/space_xyz_floating_mobilizer.h +++ b/multibody/tree/rpy_floating_mobilizer.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include "drake/common/default_scalars.h" #include "drake/common/drake_copyable.h" @@ -16,101 +17,102 @@ namespace internal { // This mobilizer introduces six degrees of freedom to model arbitrary rigid // body motions of an outboard frame M relative to an inboard frame F. -// Translational motions of M in F are parametrized by the position `p_FM` of -// frame M in F, measured in frame F. -// As hinted by the name of this class, the orientation `R_FM` of the outboard -// frame M in F is parameterized with space `x-y-z` Euler angles (also known as -// extrinsic angles). That is, the generalized coordinates for this mobilizer -// correspond to angles θ₁, θ₂, θ₃, for a sequence of rotations about the x̂, -// ŷ, ẑ axes solidary with frame F, respectively. Mathematically, rotation -// `R_FM` is given in terms of angles θ₁, θ₂, θ₃ by: 
-//   R_FM(q) = Rz(θ₃) * Ry(θ₂) * Rx(θ₁)
-//
-// where `Rx(θ)`, `Ry(θ)` and `Rz(θ)` correspond to the elemental rotations in -// amount of θ about the x, y and z axes respectively. Refer to -// math::RollPitchYaw for further details on this representation. -// Zero θ₁, θ₂, θ₃ angles and zero position `p_FM` define the "zero -// configuration" which corresponds to frames F and M being coincident, see -// set_zero_state(). Angles θ₁, θ₂, θ₃ are defined to be positive according to -// the right-hand-rule with the thumb aligned in the direction of their -// respective axes. +// Translational motions of M in F are parametrized by the position p_FM of +// frame M in F, measured in frame F. As hinted by the name of this class, the +// orientation R_FM of the outboard frame M in F is parameterized with +// roll-pitch-yaw angles (also known as space-fixed x-y-z Euler angles or +// extrinsic angles). That is, the first three generalized coordinates q for +// this mobilizer correspond to angles q=[θ₀ θ₁ θ₂], for a sequence of rotations +// about the Fx, Fy, and Fz axes, respectively. Mathematically, rotation R_FM is +// given in terms of angles θ₀, θ₁, θ₂ by: // -// The generalized velocities for this mobilizer correspond to the angular -// velocity `w_FM` of frame M in F and to the translational velocity `v_FM` of M -// in F, both expressed in the inboard frame F. +// R_FM(q) = Rz(θ₂) * Ry(θ₁) * Rx(θ₀) // -// While the mapping MapQDotToVelocity() always exists, its inverse -// MapVelocityToQDot() is singular for values of θ₂ (many times referred to as -// the pitch angle) such that `θ₂ = π/2 + kπ, ∀ k ∈ ℤ`. -// In a related matter, there are no range limits on the allowed values of the -// space x-y-z angles θ₁, θ₂, θ₃. All operations performed by this mobilizer are -// valid for θᵢ ∈ ℝ. The only operations that are not well defined at -// `θ₂ = π/2 + kπ, ∀ k ∈ ℤ` are those related with the kinematic mapping from -// angular velocity `w_FM` to Euler angles' rates, namely MapVelocityToQDot() -// and CalcNMatrix(). +// where Rx(θ), Ry(θ) and Rz(θ) correspond to the elemental rotations in +// amount of θ about the Fx, Fy and Fz axes respectively. Refer to +// math::RollPitchYaw for further details on this representation. Zero θ₀, θ₁, +// θ₂ angles and zero position p_FM define the "zero configuration" which +// corresponds to frames F and M being coincident, see set_zero_state(). Angles +// θ₀, θ₁, θ₂ are defined to be positive according to the right-hand-rule with +// the thumb aligned in the direction of their respective axes. +// +// The generalized velocities v for this mobilizer correspond to the angular +// velocity w_FM of frame M in F followed by the translational velocity v_FM of +// M in F, both expressed in the inboard frame F. Note that this is _not_ the +// same as the time derivative q̇ the rotation angles; you must use +// q̇ = MapVelocityToQDot(w_FM) to get the angle time derivatives. // -// @note Space `x-y-z` angles (extrinsic) are equivalent to Body `z-y-x` angles -// (intrinsic). +// While the mapping MapQDotToVelocity() always exists, its inverse +// MapVelocityToQDot() is singular for values of θ₁ (many times referred to as +// the pitch angle) such that θ₁ = π/2 + kπ, ∀ k ∈ ℤ. In a related matter, there +// are no range limits on the allowed values of the r-p-y angles θ₀, θ₁, θ₂. All +// operations performed by this mobilizer are valid for θᵢ ∈ ℝ. The only +// operations that are not well defined at θ₁ = π/2 + kπ, ∀ k ∈ ℤ are those +// related to the kinematic mapping from angular velocity w_FM to Euler angle +// rates, namely MapVelocityToQDot() and CalcNMatrix(). // -// @note This particular choice of generalized coordinates θ₁, θ₂, θ₃ for this -// mobilizer is many times referred to as the roll, pitch and yaw angles by -// many dynamicists. They are also known as the Tait-Bryan angles or Cardan -// angles. +// @note Roll-pitch-yaw (space x-y-z) angles (extrinsic, about Fx Fy Fz) are +// equivalent to body-fixed z-y-x angles (intrinsic, about Mz M'y M''x where +// the primes indicate new axis directions after each rotation). // -// @note The mapping from angular velocity to Euler angle's rates is singular -// for angle `θ₂` (many times referred to as the pitch angle) such that -// `θ₂ = π/2 + kπ, ∀ k ∈ ℤ`. +// @note The roll-pitch-yaw (space x-y-z) Euler sequence is also known as the +// Tait-Bryan angles or Cardan angles. // // @tparam_default_scalar template -class SpaceXYZFloatingMobilizer final : public MobilizerImpl { +class RpyFloatingMobilizer final : public MobilizerImpl { public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SpaceXYZFloatingMobilizer) + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RpyFloatingMobilizer) - // Constructor for a SpaceXYZFloatingMobilizer between an inboard frame F - // `inboard_frame_F` and an outboard frame M `outboard_frame_M`. - SpaceXYZFloatingMobilizer(const Frame& inboard_frame_F, - const Frame& outboard_frame_M) + // Constructor for an RpyFloatingMobilizer between an inboard frame F + // inboard_frame_F and an outboard frame M outboard_frame_M. + RpyFloatingMobilizer(const Frame& inboard_frame_F, + const Frame& outboard_frame_M) : MobilizerBase(inboard_frame_F, outboard_frame_M) {} - bool is_floating() const override { return true; } + bool is_floating() const final { return true; } + + bool has_quaternion_dofs() const final { return false; } - bool has_quaternion_dofs() const override { return false; } + // Overloads to define the suffix names for the position and velocity + // elements. + std::string position_suffix(int position_index_in_mobilizer) const final; + std::string velocity_suffix(int velocity_index_in_mobilizer) const final; bool can_rotate() const final { return true; } bool can_translate() const final { return true; } - // Returns the generalized positions for this mobilizer stored in `context`. + // Returns the generalized positions for this mobilizer stored in context. // Generalized positions q for this mobilizer are packed in exactly the - // following order: `q = [θ₁, θ₂, θ₃, px_FM, py_FM, pz_FM]` that is, rpy + // following order: q = [θ₀, θ₁, θ₂, px_FM, py_FM, pz_FM] that is, rpy // angles are stored in the first three entries of the configuration vector, // followed by the three translational coordinates. Vector6 get_generalized_positions( const systems::Context& context) const; - // Returns the generalized velocities for this mobilizer stored in `context`. + // Returns the generalized velocities for this mobilizer stored in context. // Generalized velocities v for this mobilizer are packed in exactly the - // following order: `v = [wx_FM, wy_FM, wz_FM, vx_FM, vy_FM, vz_FM]` that is, - // the components of the angular velocity `w_FM` are stored in the first three + // following order: v = [wx_FM, wy_FM, wz_FM, vx_FM, vy_FM, vz_FM] that is, + // the components of the angular velocity w_FM are stored in the first three // entries of the generalized velocities vector, followed by the three - // components of the translational velocity `v_FM`. + // components of the translational velocity v_FM. Vector6 get_generalized_velocities( const systems::Context& context) const; - // Retrieves the three space x-y-z angles θ₁, θ₂, θ₃ stored in `context`, - // which describe the state for `this` mobilizer as documented in this class's + // Retrieves the three roll-pitch-yaw angles θ₀, θ₁, θ₂ stored in context, + // which describe the state for this mobilizer as documented in this class's // documentation. // // @param[in] context // The context of the model this mobilizer belongs to. // @retval angles - // The three space x-y-z angles θ₁, θ₂, θ₃, associated with the sequence of - // rotations about the space fixed axes x̂, ŷ, ẑ, respectively packed and - // returned as a Vector3 with entries `angles(0) = θ₁`, `angles(1) = θ₂`, - // `angles(2) = θ₃`. There are no range limits for the angular values. + // The three roll-pitch-yaw angles θ₀, θ₁, θ₂, associated with the sequence + // of rotations about the space fixed axes x̂, ŷ, ẑ, respectively packed and + // returned as a Vector3 with entries angles(0) = θ₀, angles(1) = θ₁, + // angles(2) = θ₂. There are no range limits for the angular values. Vector3 get_angles(const systems::Context& context) const; - // Retrieves the position `p_FM` stored in `context`. + // Retrieves the position p_FM stored in context. // // @param[in] context // The context of the model this mobilizer belongs to. @@ -118,7 +120,7 @@ class SpaceXYZFloatingMobilizer final : public MobilizerImpl { // The position of M in F. Vector3 get_translation(const systems::Context& context) const; - // Retrieves from `context` the angular velocity `w_FM` of the outboard frame + // Retrieves from context the angular velocity w_FM of the outboard frame // M in the inboard frame F, expressed in F. // // @param[in] context @@ -128,7 +130,7 @@ class SpaceXYZFloatingMobilizer final : public MobilizerImpl { // inboard frame F, expressed in F. Vector3 get_angular_velocity(const systems::Context& context) const; - // Retrieves from `context` the translational velocity `v_FM` of the outboard + // Retrieves from context the translational velocity v_FM of the outboard // frame M in the inboard frame F, expressed in F. // // @param[in] context @@ -139,103 +141,103 @@ class SpaceXYZFloatingMobilizer final : public MobilizerImpl { Vector3 get_translational_velocity( const systems::Context& context) const; - // Stores in `context` the space x-y-z angles θ₁, θ₂, θ₃, provided in the - // input argument `angles`, which stores the with the format `angles = [θ₁, - // θ₂, θ₃]`. + // Stores in context the roll-pitch-yaw angles θ₀, θ₁, θ₂, provided in the + // input argument angles, which stores the with the format angles = [θ₀, + // θ₁, θ₂]. // // @param[in,out] context // The context of the model this mobilizer belongs to. // @param[in] angles - // A Vector3 which must pack values for the space x-y-z angles θ₁, θ₂, θ₃, - // described in this class's documentation, at entries `angles(0)`, - // `angles(1)` and `angles(2)`, respectively. - // @returns a constant reference to `this` mobilizer. - const SpaceXYZFloatingMobilizer& set_angles( + // A Vector3 which must pack values for the roll-pitch-yaw angles + // θ₀, θ₁, θ₂, described in this class's documentation, at entries + // angles(0), angles(1) and angles(2), respectively. + // @returns a constant reference to this mobilizer. + const RpyFloatingMobilizer& set_angles( systems::Context* context, const Vector3& angles) const; - // Stores in `context` the position `p_FM` of M in F. + // Stores in context the position p_FM of M in F. // // @param[in,out] context // The context of the model this mobilizer belongs to. // @param[in] p_FM // Position of F in M. - // @returns a constant reference to `this` mobilizer. - const SpaceXYZFloatingMobilizer& set_translation( + // @returns a constant reference to this mobilizer. + const RpyFloatingMobilizer& set_translation( systems::Context* context, const Vector3& p_FM) const; - // Sets in `context` the state for `this` mobilizer so that the angular - // velocity of the outboard frame M in the inboard frame F is `w_FM`. + // Sets in context the state for this mobilizer so that the angular + // velocity of the outboard frame M in the inboard frame F is w_FM. // @param[in,out] context // The context of the model this mobilizer belongs to. // @param[in] w_FM // A vector in ℝ³ with the desired angular velocity of the outboard frame M // in the inboard frame F, expressed in F. - // @returns a constant reference to `this` mobilizer. - const SpaceXYZFloatingMobilizer& set_angular_velocity( + // @returns a constant reference to this mobilizer. + const RpyFloatingMobilizer& set_angular_velocity( systems::Context* context, const Vector3& w_FM) const; - // Stores in `context` the translational velocity `v_FM` of M in F. + // Stores in context the translational velocity v_FM of M in F. // // @param[in,out] context // The context of the model this mobilizer belongs to. // @param[in] v_FM // Translational velocity of F in M. - // @returns a constant reference to `this` mobilizer. - const SpaceXYZFloatingMobilizer& set_translational_velocity( + // @returns a constant reference to this mobilizer. + const RpyFloatingMobilizer& set_translational_velocity( systems::Context* context, const Vector3& v_FM) const; - // Sets `context` so this mobilizer's generalized coordinates (space x-y-z - // angles θ₁, θ₂, θ₃ and position p_FM) represent the given rigid - // transform `X_FM`. + // Sets context so this mobilizer's generalized coordinates (roll-pitch-yaw + // angles θ₀, θ₁, θ₂ and position p_FM) represent the given rigid + // transform X_FM. // @param[in,out] context // The context of the model that this mobilizer belongs to. // @param[in] X_FM // Pose of M in F. - // @returns a constant reference to `this` mobilizer. - // @note Even though there is no range limit for the space x-y-z angles θ₁, - // θ₂, θ₃, this specific method will generate roll-pitch-yaw angles in the - // range `-π <= θ₁ <= π`, `-π/2 <= θ₂ <= π/2`, `-π <= θ₃ <= π`. - const SpaceXYZFloatingMobilizer& SetFromRigidTransform( + // @returns a constant reference to this mobilizer. + // @note Even though there is no range limit for the angles θ₀, + // θ₁, θ₂, this specific method will generate roll-pitch-yaw angles in the + // range -π <= θ₀ <= π, -π/2 <= θ₁ <= π/2, -π <= θ₂ <= π. + const RpyFloatingMobilizer& SetFromRigidTransform( systems::Context* context, const math::RigidTransform& X_FM) const; - // Computes the across-mobilizer transform `X_FM(q)` between the inboard + // Computes the across-mobilizer transform X_FM(q) between the inboard // frame F and the outboard frame M as a function of the configuration q - // stored in `context`. + // stored in context. math::RigidTransform CalcAcrossMobilizerTransform( - const systems::Context& context) const override; + const systems::Context& context) const final; - // Computes the across-mobilizer velocity `V_FM(q, v)` of the outboard frame M + // Computes the across-mobilizer velocity V_FM(q, v) of the outboard frame M // measured and expressed in frame F as a function of the configuration stored - // in `context` and of the input generalized velocity v, packed as documented + // in context and of the input generalized velocity v, packed as documented // in get_generalized_velocities(). SpatialVelocity CalcAcrossMobilizerSpatialVelocity( const systems::Context& context, - const Eigen::Ref>& v) const override; + const Eigen::Ref>& v) const final; - // Computes the across-mobilizer acceleration `A_FM(q, v, v̇)` of the + // Computes the across-mobilizer acceleration A_FM(q, v, v̇) of the // outboard frame M in the inboard frame F. - // The acceleration `A_FM` will be a function of the generalized positions q - // and generalized velocities v stored in `context` and of the supplied - // generalized accelerations `vdot`. `vdot` must contain the rates of change + // The acceleration A_FM will be a function of the generalized positions q + // and generalized velocities v stored in context and of the supplied + // generalized accelerations vdot. vdot must contain the rates of change // 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 - // `a_FM` the angular and translational accelerations of M in F, respectively + // corresponds to vdot = [alpha_FM; a_FM], with alpha_FM = Dt_F(w_FM) and + // a_FM the angular and translational accelerations of M in F, respectively // (see @ref Dt_multibody_quantities for our notation of time derivatives in // different reference frames.) SpatialAcceleration CalcAcrossMobilizerSpatialAcceleration( const systems::Context& context, - const Eigen::Ref>& vdot) const override; + const Eigen::Ref>& vdot) const final; // See Mobilizer::ProjectSpatialForce() for details. void ProjectSpatialForce(const systems::Context& context, const SpatialForce& F_Mo_F, - Eigen::Ref> tau) const override; + Eigen::Ref> tau) const final; - bool is_velocity_equal_to_qdot() const override { return false; } + bool is_velocity_equal_to_qdot() const final { return false; } // Maps the generalized velocity v to time derivatives of configuration - // `qdot`. + // qdot. // // @param[in] context // The context of the model this mobilizer belongs to. @@ -247,15 +249,15 @@ class SpaceXYZFloatingMobilizer final : public MobilizerImpl { // get_generalized_positions(). // // @warning The mapping from angular velocity to Euler angle's rates is - // singular for angle `θ₂` such that `θ₂ = π/2 + kπ, ∀ k ∈ ℤ`. To avoid + // singular for angle θ₁ such that θ₁ = π/2 + kπ, ∀ k ∈ ℤ. To avoid // working close to this singularity (which could potentially result in large - // errors for `qdot`), this method aborts when the absolute value of the - // cosine of θ₂ is smaller than 10⁻³, a number arbitrarily chosen to this end. + // errors for qdot), this method aborts when the absolute value of the + // cosine of θ₁ is smaller than 10⁻³, a number arbitrarily chosen to this end. void MapVelocityToQDot(const systems::Context& context, const Eigen::Ref>& v, - EigenPtr> qdot) const override; + EigenPtr> qdot) const final; - // Maps time derivatives of the configuration in `qdot` to + // Maps time derivatives of the configuration in qdot to // the generalized velocities v. // // @param[in] context @@ -268,30 +270,30 @@ class SpaceXYZFloatingMobilizer final : public MobilizerImpl { // documented in get_generalized_velocities(). void MapQDotToVelocity(const systems::Context& context, const Eigen::Ref>& qdot, - EigenPtr> v) const override; + EigenPtr> v) const final; protected: // Implements Mobilizer's NVI, see Mobilizer::CalcNMatrix() for details. // @warning The mapping from angular velocity to Euler angle's rates is - // singular for angle `θ₂` such that `θ₂ = π/2 + kπ, ∀ k ∈ ℤ`. To avoid + // singular for angle θ₁ such that θ₁ = π/2 + kπ, ∀ k ∈ ℤ. To avoid // working close to this singularity (which could potentially result in large - // errors for `qdot`), this method aborts when the absolute value of the - // cosine of θ₂ is smaller than 10⁻³, a number arbitrarily chosen to this end. + // errors for qdot), this method aborts when the absolute value of the + // cosine of θ₁ is smaller than 10⁻³, a number arbitrarily chosen to this end. void DoCalcNMatrix(const systems::Context& context, - EigenPtr> N) const override; + EigenPtr> N) const final; // Implements Mobilizer's NVI, see Mobilizer::DoCalcNplusMatrix() for details. void DoCalcNplusMatrix(const systems::Context& context, - EigenPtr> Nplus) const override; + EigenPtr> Nplus) const final; std::unique_ptr> DoCloneToScalar( - const MultibodyTree& tree_clone) const override; + const MultibodyTree& tree_clone) const final; std::unique_ptr> DoCloneToScalar( - const MultibodyTree& tree_clone) const override; + const MultibodyTree& tree_clone) const final; std::unique_ptr> DoCloneToScalar( - const MultibodyTree& tree_clone) const override; + const MultibodyTree& tree_clone) const final; private: typedef MobilizerImpl MobilizerBase; @@ -315,4 +317,4 @@ class SpaceXYZFloatingMobilizer final : public MobilizerImpl { } // namespace drake DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class ::drake::multibody::internal::SpaceXYZFloatingMobilizer) + class ::drake::multibody::internal::RpyFloatingMobilizer) diff --git a/multibody/tree/test/free_rotating_body_plant.cc b/multibody/tree/test/free_rotating_body_plant.cc index 19603fa4d172..7dff2126249a 100644 --- a/multibody/tree/test/free_rotating_body_plant.cc +++ b/multibody/tree/test/free_rotating_body_plant.cc @@ -54,7 +54,7 @@ void FreeRotatingBodyPlant::SetDefaultState( const internal::Mobilizer& mobilizer = joint_->GetMobilizerInUse(); const auto* xyz_mobilizer = - dynamic_cast*>(&mobilizer); + dynamic_cast*>(&mobilizer); DRAKE_DEMAND(xyz_mobilizer != nullptr); xyz_mobilizer->set_angular_velocity( diff --git a/multibody/tree/test/free_rotating_body_plant.h b/multibody/tree/test/free_rotating_body_plant.h index d97db17acc2a..4339550f010e 100644 --- a/multibody/tree/test/free_rotating_body_plant.h +++ b/multibody/tree/test/free_rotating_body_plant.h @@ -7,7 +7,7 @@ #include "drake/multibody/tree/multibody_tree.h" #include "drake/multibody/tree/multibody_tree_system.h" #include "drake/multibody/tree/rigid_body.h" -#include "drake/multibody/tree/space_xyz_mobilizer.h" +#include "drake/multibody/tree/rpy_ball_mobilizer.h" #include "drake/systems/framework/basic_vector.h" #include "drake/systems/framework/leaf_system.h" diff --git a/multibody/tree/test/free_rotating_body_test.cc b/multibody/tree/test/free_rotating_body_test.cc index 378ff8020206..213a7eed7609 100644 --- a/multibody/tree/test/free_rotating_body_test.cc +++ b/multibody/tree/test/free_rotating_body_test.cc @@ -50,7 +50,7 @@ GTEST_TEST(RollPitchYawTest, TimeDerivatives) { // The body in this model is not a floating body but is free to rotate. The // rotation is not modeled using a quaternion mobilizer (it uses a - // SpaceXYZMobilizer). + // RpyBallMobilizer). EXPECT_FALSE(free_body_plant.body().is_floating()); EXPECT_FALSE(free_body_plant.body().has_quaternion_dofs()); diff --git a/multibody/tree/test/space_xyz_mobilizer_test.cc b/multibody/tree/test/rpy_ball_mobilizer_test.cc similarity index 88% rename from multibody/tree/test/space_xyz_mobilizer_test.cc rename to multibody/tree/test/rpy_ball_mobilizer_test.cc index de24f6690492..72df17554f41 100644 --- a/multibody/tree/test/space_xyz_mobilizer_test.cc +++ b/multibody/tree/test/rpy_ball_mobilizer_test.cc @@ -1,4 +1,4 @@ -#include "drake/multibody/tree/space_xyz_mobilizer.h" +#include "drake/multibody/tree/rpy_ball_mobilizer.h" #include @@ -29,27 +29,27 @@ using systems::Context; constexpr double kTolerance = 10 * std::numeric_limits::epsilon(); // Fixture to setup a simple MBT model containing a space xyz mobilizer. -class SpaceXYZMobilizerTest : public MobilizerTester { +class RpyBallMobilizerTest : public MobilizerTester { public: // Creates a simple model consisting of a single body with a space xyz // mobilizer connecting it to the world. void SetUp() override { - mobilizer_ = &AddJointAndFinalize( + mobilizer_ = &AddJointAndFinalize( std::make_unique>("joint0", tree().world_body().body_frame(), body_->body_frame())); } protected: - const SpaceXYZMobilizer* mobilizer_{nullptr}; + const RpyBallMobilizer* mobilizer_{nullptr}; }; -TEST_F(SpaceXYZMobilizerTest, CanRotateOrTranslate) { +TEST_F(RpyBallMobilizerTest, CanRotateOrTranslate) { EXPECT_TRUE(mobilizer_->can_rotate()); EXPECT_FALSE(mobilizer_->can_translate()); } // Verifies methods to mutate and access the context. -TEST_F(SpaceXYZMobilizerTest, StateAccess) { +TEST_F(RpyBallMobilizerTest, StateAccess) { const Vector3d rpy_value(M_PI / 3, -M_PI / 3, M_PI / 5); mobilizer_->set_angles(context_.get(), rpy_value); EXPECT_EQ(mobilizer_->get_angles(*context_), rpy_value); @@ -63,7 +63,7 @@ TEST_F(SpaceXYZMobilizerTest, StateAccess) { kTolerance, MatrixCompareType::relative)); } -TEST_F(SpaceXYZMobilizerTest, ZeroState) { +TEST_F(RpyBallMobilizerTest, ZeroState) { // Set an arbitrary "non-zero" state. const Vector3d rpy_value(M_PI / 3, -M_PI / 3, M_PI / 5); mobilizer_->set_angles(context_.get(), rpy_value); @@ -79,7 +79,7 @@ TEST_F(SpaceXYZMobilizerTest, ZeroState) { // For an arbitrary state verify that the computed Nplus(q) matrix is the // inverse of N(q). -TEST_F(SpaceXYZMobilizerTest, KinematicMapping) { +TEST_F(RpyBallMobilizerTest, KinematicMapping) { const Vector3d rpy(M_PI / 3, -M_PI / 3, M_PI / 5); mobilizer_->set_angles(context_.get(), rpy); @@ -106,7 +106,7 @@ TEST_F(SpaceXYZMobilizerTest, KinematicMapping) { kTolerance, MatrixCompareType::relative)); } -TEST_F(SpaceXYZMobilizerTest, MapUsesN) { +TEST_F(RpyBallMobilizerTest, MapUsesN) { // Set an arbitrary "non-zero" state. const Vector3d rpy_value(M_PI / 3, -M_PI / 3, M_PI / 5); mobilizer_->set_angles(context_.get(), rpy_value); @@ -122,12 +122,12 @@ TEST_F(SpaceXYZMobilizerTest, MapUsesN) { MatrixX N(3, 3); mobilizer_->CalcNMatrix(*context_, &N); - // Ensure N(q) is used in `q̇ = N(q)⋅v` + // Ensure N(q) is used in q̇ = N(q)⋅v EXPECT_TRUE( CompareMatrices(qdot, N * v, kTolerance, MatrixCompareType::relative)); } -TEST_F(SpaceXYZMobilizerTest, MapUsesNplus) { +TEST_F(RpyBallMobilizerTest, MapUsesNplus) { // Set an arbitrary "non-zero" state. const Vector3d rpy_value(M_PI / 3, -M_PI / 3, M_PI / 5); mobilizer_->set_angles(context_.get(), rpy_value); @@ -141,12 +141,12 @@ TEST_F(SpaceXYZMobilizerTest, MapUsesNplus) { MatrixX Nplus(3, 3); mobilizer_->CalcNplusMatrix(*context_, &Nplus); - // Ensure N⁺(q) is used in `v = N⁺(q)⋅q̇` + // Ensure N⁺(q) is used in v = N⁺(q)⋅q̇ EXPECT_TRUE(CompareMatrices(v, Nplus * qdot, kTolerance, MatrixCompareType::relative)); } -TEST_F(SpaceXYZMobilizerTest, SingularityError) { +TEST_F(RpyBallMobilizerTest, SingularityError) { // Set state in singularity const Vector3d rpy_value(M_PI / 3, M_PI / 2, M_PI / 5); mobilizer_->set_angles(context_.get(), rpy_value); diff --git a/multibody/tree/test/space_xyz_floating_mobilizer_test.cc b/multibody/tree/test/rpy_floating_mobilizer_test.cc similarity index 83% rename from multibody/tree/test/space_xyz_floating_mobilizer_test.cc rename to multibody/tree/test/rpy_floating_mobilizer_test.cc index d901dbcfa973..0abd0251ffcf 100644 --- a/multibody/tree/test/space_xyz_floating_mobilizer_test.cc +++ b/multibody/tree/test/rpy_floating_mobilizer_test.cc @@ -1,4 +1,4 @@ -#include "drake/multibody/tree/space_xyz_floating_mobilizer.h" +#include "drake/multibody/tree/rpy_floating_mobilizer.h" #include @@ -10,7 +10,6 @@ #include "drake/multibody/tree/multibody_tree-inl.h" #include "drake/multibody/tree/multibody_tree_system.h" #include "drake/multibody/tree/test/mobilizer_tester.h" -#include "drake/systems/framework/context.h" namespace drake { namespace multibody { @@ -25,13 +24,13 @@ using math::RotationMatrixd; constexpr double kTolerance = 10 * std::numeric_limits::epsilon(); // Fixture to setup a simple model containing a space xyz floating mobilizer. -class SpaceXYZFloatingMobilizerTest : public MobilizerTester { +class RpyFloatingMobilizerTest : public MobilizerTester { public: // Creates a simple model consisting of a single body with a space xyz // floating mobilizer connecting it to the world. void SetUp() override { mobilizer_ = &AddMobilizerAndFinalize( - std::make_unique>( + std::make_unique>( tree().world_body().body_frame(), body_->body_frame())); } @@ -48,22 +47,37 @@ class SpaceXYZFloatingMobilizerTest : public MobilizerTester { Vector3d arbitrary_translation() const { return Vector3d(1.0, 2.0, 3.0); } protected: - const SpaceXYZFloatingMobilizer* mobilizer_{nullptr}; + const RpyFloatingMobilizer* mobilizer_{nullptr}; }; -TEST_F(SpaceXYZFloatingMobilizerTest, CanRotateOrTranslate) { +TEST_F(RpyFloatingMobilizerTest, CanRotateOrTranslate) { EXPECT_TRUE(mobilizer_->can_rotate()); EXPECT_TRUE(mobilizer_->can_translate()); } // Verifies methods to mutate and access the context. -TEST_F(SpaceXYZFloatingMobilizerTest, BasicIntrospection) { +TEST_F(RpyFloatingMobilizerTest, BasicIntrospection) { EXPECT_TRUE(mobilizer_->is_floating()); EXPECT_FALSE(mobilizer_->has_quaternion_dofs()); } +TEST_F(RpyFloatingMobilizerTest, NameSuffix) { + EXPECT_EQ(mobilizer_->position_suffix(0), "qx"); + EXPECT_EQ(mobilizer_->position_suffix(1), "qy"); + EXPECT_EQ(mobilizer_->position_suffix(2), "qz"); + EXPECT_EQ(mobilizer_->position_suffix(3), "x"); + EXPECT_EQ(mobilizer_->position_suffix(4), "y"); + EXPECT_EQ(mobilizer_->position_suffix(5), "z"); + EXPECT_EQ(mobilizer_->velocity_suffix(0), "wx"); + EXPECT_EQ(mobilizer_->velocity_suffix(1), "wy"); + EXPECT_EQ(mobilizer_->velocity_suffix(2), "wz"); + EXPECT_EQ(mobilizer_->velocity_suffix(3), "vx"); + EXPECT_EQ(mobilizer_->velocity_suffix(4), "vy"); + EXPECT_EQ(mobilizer_->velocity_suffix(5), "vz"); +} + // Verifies methods to mutate and access the context. -TEST_F(SpaceXYZFloatingMobilizerTest, ConfigurationAccessAndMutation) { +TEST_F(RpyFloatingMobilizerTest, ConfigurationAccessAndMutation) { SetArbitraryNonZeroState(); EXPECT_EQ(mobilizer_->get_angles(*context_), arbitrary_rpy().vector()); EXPECT_EQ(mobilizer_->get_translation(*context_), arbitrary_translation()); @@ -72,7 +86,7 @@ TEST_F(SpaceXYZFloatingMobilizerTest, ConfigurationAccessAndMutation) { EXPECT_EQ(mobilizer_->get_generalized_positions(*context_), q); } -TEST_F(SpaceXYZFloatingMobilizerTest, SetFromRigidTransform) { +TEST_F(RpyFloatingMobilizerTest, SetFromRigidTransform) { SetArbitraryNonZeroState(); const RigidTransformd X_WB(arbitrary_rpy(), arbitrary_translation()); mobilizer_->SetFromRigidTransform(context_.get(), X_WB); @@ -82,7 +96,7 @@ TEST_F(SpaceXYZFloatingMobilizerTest, SetFromRigidTransform) { EXPECT_EQ(mobilizer_->get_translation(*context_), arbitrary_translation()); } -TEST_F(SpaceXYZFloatingMobilizerTest, VelocityAccessAndMutation) { +TEST_F(RpyFloatingMobilizerTest, VelocityAccessAndMutation) { const Vector3d w_FM(M_PI / 3, -M_PI / 3, M_PI / 5); mobilizer_->set_angular_velocity(context_.get(), w_FM); EXPECT_EQ(mobilizer_->get_angular_velocity(*context_), w_FM); @@ -95,7 +109,7 @@ TEST_F(SpaceXYZFloatingMobilizerTest, VelocityAccessAndMutation) { EXPECT_EQ(mobilizer_->get_generalized_velocities(*context_), v); } -TEST_F(SpaceXYZFloatingMobilizerTest, ZeroState) { +TEST_F(RpyFloatingMobilizerTest, ZeroState) { SetArbitraryNonZeroState(); // The non-zero state is not the identity transform as expected. @@ -110,7 +124,7 @@ TEST_F(SpaceXYZFloatingMobilizerTest, ZeroState) { } // Verify the correctness of across-mobilizer quantities; X_FM, V_FM, A_FM. -TEST_F(SpaceXYZFloatingMobilizerTest, CalcAcrossMobilizer) { +TEST_F(RpyFloatingMobilizerTest, CalcAcrossMobilizer) { SetArbitraryNonZeroState(); const RigidTransformd X_FM_expected(arbitrary_rpy(), arbitrary_translation()); @@ -146,7 +160,7 @@ TEST_F(SpaceXYZFloatingMobilizerTest, CalcAcrossMobilizer) { MatrixCompareType::relative)); } -TEST_F(SpaceXYZFloatingMobilizerTest, MapVelocityToQdotAndBack) { +TEST_F(RpyFloatingMobilizerTest, MapVelocityToQdotAndBack) { EXPECT_FALSE(mobilizer_->is_velocity_equal_to_qdot()); SetArbitraryNonZeroState(); @@ -161,7 +175,7 @@ TEST_F(SpaceXYZFloatingMobilizerTest, MapVelocityToQdotAndBack) { // For an arbitrary state verify that the computed Nplus(q) matrix is the // inverse of N(q). -TEST_F(SpaceXYZFloatingMobilizerTest, KinematicMapping) { +TEST_F(RpyFloatingMobilizerTest, KinematicMapping) { RollPitchYawd rpy(M_PI / 3, -M_PI / 3, M_PI / 5); mobilizer_->set_angles(context_.get(), rpy.vector()); @@ -186,7 +200,7 @@ TEST_F(SpaceXYZFloatingMobilizerTest, KinematicMapping) { kTolerance, MatrixCompareType::relative)); } -TEST_F(SpaceXYZFloatingMobilizerTest, MapUsesN) { +TEST_F(RpyFloatingMobilizerTest, MapUsesN) { SetArbitraryNonZeroState(); const Vector6 v = (Vector6() << 1, 2, 3, 4, 5, 6).finished(); Vector6 qdot; @@ -196,12 +210,12 @@ TEST_F(SpaceXYZFloatingMobilizerTest, MapUsesN) { Matrix6 N; mobilizer_->CalcNMatrix(*context_, &N); - // Ensure N(q) is used in `q̇ = N(q)⋅v` + // Ensure N(q) is used in q̇ = N(q)⋅v EXPECT_TRUE( CompareMatrices(qdot, N * v, kTolerance, MatrixCompareType::relative)); } -TEST_F(SpaceXYZFloatingMobilizerTest, MapUsesNplus) { +TEST_F(RpyFloatingMobilizerTest, MapUsesNplus) { SetArbitraryNonZeroState(); const Vector6 qdot = (Vector6() << 1, 2, 3, 4, 5, 6).finished(); @@ -212,12 +226,12 @@ TEST_F(SpaceXYZFloatingMobilizerTest, MapUsesNplus) { Matrix6 Nplus; mobilizer_->CalcNplusMatrix(*context_, &Nplus); - // Ensure N⁺(q) is used in `v = N⁺(q)⋅q̇` + // Ensure N⁺(q) is used in v = N⁺(q)⋅q̇ EXPECT_TRUE(CompareMatrices(v, Nplus * qdot, kTolerance, MatrixCompareType::relative)); } -TEST_F(SpaceXYZFloatingMobilizerTest, SingularityError) { +TEST_F(RpyFloatingMobilizerTest, SingularityError) { // Set state in singularity const Vector3d rpy_value(M_PI / 3, M_PI / 2, M_PI / 5); mobilizer_->set_angles(context_.get(), rpy_value);