diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index c2e06e998917..22cceded5692 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -3895,8 +3895,8 @@ void MultibodyPlant::CalcReactionForces( // for the discrete solvers we have today, though it might change for future // solvers that prefer an implicit evaluation of these terms. // TODO(amcastro-tri): Consider having a - // DiscreteUpdateManager::EvalReactionForces() to ensure the manager performs - // this computation consistently with its discrete update. + // DiscreteUpdateManager::EvalReactionForces() to ensure the manager performs + // this computation consistently with its discrete update. const VectorX& vdot = this->EvalForwardDynamics(context).get_vdot(); std::vector> A_WB_vector(num_bodies()); std::vector> F_BMo_W_vector(num_bodies()); @@ -3908,9 +3908,9 @@ void MultibodyPlant::CalcReactionForces( // Since vdot is the result of Fapplied and tau_applied we expect the result // from inverse dynamics to be zero. // TODO(amcastro-tri): find a better estimation for this bound. For instance, - // we can make an estimation based on the trace of the mass matrix (Jain - // 2011, Eq. 4.21). For now we only ASSERT though with a better estimation we - // could promote this to a DEMAND. + // we can make an estimation based on the trace of the mass matrix (Jain 2011, + // Eq. 4.21). For now we only ASSERT though with a better estimation we could + // promote this to a DEMAND. // TODO(amcastro-tri) Uncomment this line once issue #12473 is resolved. // DRAKE_ASSERT(tau_id.norm() < // 100 * num_velocities() * @@ -3924,49 +3924,61 @@ void MultibodyPlant::CalcReactionForces( internal_tree().get_joint_mobilizer(joint_index); const internal::Mobilizer& mobilizer = internal_tree().get_mobilizer(mobilizer_index); - - // Reversed means the joint's parent(child) body is the outboard(inboard) - // body for the mobilizer. - const bool is_reversed = mobilizer.mobod().is_reversed(); + const internal::MobodIndex mobod_index = mobilizer.mobod().index(); // F_BMo_W is the mobilizer reaction force on mobilized body B at the origin // Mo of the mobilizer's outboard frame M, expressed in the world frame W. - const SpatialForce& F_BMo_W = F_BMo_W_vector[mobilizer_index]; - - // But the quantity of interest, F_CJc_Jc, is the joint's reaction force on - // the joint's child body C at the joint's child frame Jc, expressed in Jc. - SpatialForce& F_CJc_Jc = output->at(joint.ordinal()); + const SpatialForce& F_BMo_W = F_BMo_W_vector[mobod_index]; // Frames of interest: + const Frame& frame_Jp = joint.frame_on_parent(); const Frame& frame_Jc = joint.frame_on_child(); - const Frame& frame_M = mobilizer.outboard_frame(); + const FrameIndex F_index = mobilizer.inboard_frame().index(); + const FrameIndex M_index = mobilizer.outboard_frame().index(); + const FrameIndex Jp_index = frame_Jp.index(); + const FrameIndex Jc_index = frame_Jc.index(); + + // In Drake we must have either: + // - Jp == F and Jc == M (typical case) + // - Jp == M and Jc == F (mobilizer is reversed from joint) + DRAKE_DEMAND((Jp_index == F_index && Jc_index == M_index) || + (Jp_index == M_index && Jc_index == F_index)); + + // Mobilizer is reversed if the joint's parent frame Jp is the mobilizer's + // outboard frame M. + const bool is_reversed = (Jp_index == M_index); // We'll need this in both cases below since we're required to report // the reaction force expressed in the joint's child frame Jc. const RotationMatrix R_JcW = frame_Jc.CalcRotationMatrixInWorld(context).inverse(); - if (&frame_M == &frame_Jc) { - // This is the easy case. Just need to re-express. - F_CJc_Jc = R_JcW * F_BMo_W; - continue; - } + // The quantity of interest, F_CJc_Jc, is the joint's reaction force on the + // joint's child body C at the joint's child frame Jc, expressed in Jc. + SpatialForce& F_CJc_Jc = output->at(joint.ordinal()); + if (!is_reversed) { + F_CJc_Jc = R_JcW * F_BMo_W; // The typical case: Mo==Jc and B==C. + } else { + // For this reversed case, F_BMo_W is the reaction on the joint's _parent_ + // body at Jp, expressed in W. + const SpatialForce& F_PJp_W = F_BMo_W; // Reversed: Mo==Jp and B==P. - // If the mobilizer is reversed, Newton's 3ʳᵈ law (action/reaction) (and - // knowing Drake's joints are massless) says the force on the child at M - // is equal and opposite to the force on the parent at M. - const SpatialForce F_CMo_W = is_reversed ? -F_BMo_W : F_BMo_W; - const SpatialForce F_CMo_Jc = R_JcW * F_CMo_W; // Reexpress in Jc. + // Newton's 3ʳᵈ law (action/reaction) (and knowing Drake's joints are + // massless) says the force on the child _at Jp_ is equal and opposite to + // the force on the parent at Jp. + const SpatialForce F_CJp_W = -F_PJp_W; + const SpatialForce F_CJp_Jc = R_JcW * F_CJp_W; // Reexpress in Jc. - // However, the reaction force we want to report on the child is at Jc, - // not M. We need to shift the application point from Mo to Jco. + // However, the reaction force we want to report on the child is at Jc, + // not Jp. We need to shift the application point from Jp to Jc. - // Find the shift vector p_MoJco_Jc (= -p_JcoMo_Jc). - const RigidTransform X_JcM = frame_M.CalcPose(context, frame_Jc); - const Vector3 p_MoJco_Jc = -X_JcM.translation(); + // Find the shift vector p_JpJc_Jc (= -p_JcJp_Jc). + const RigidTransform X_JcJp = frame_Jp.CalcPose(context, frame_Jc); + const Vector3 p_JpJc_Jc = -X_JcJp.translation(); - // Perform the M->Jc shift. - F_CJc_Jc = F_CMo_Jc.Shift(p_MoJco_Jc); + // Perform the Jp->Jc shift. + F_CJc_Jc = F_CJp_Jc.Shift(p_JpJc_Jc); + } } } diff --git a/multibody/plant/test/sap_driver_multidof_joints_test.cc b/multibody/plant/test/sap_driver_multidof_joints_test.cc index 25e8ca11752a..14ba01104a6f 100644 --- a/multibody/plant/test/sap_driver_multidof_joints_test.cc +++ b/multibody/plant/test/sap_driver_multidof_joints_test.cc @@ -92,15 +92,13 @@ class MultiDofJointWithLimits final : public Joint { int do_get_position_start() const override { return 0; } std::unique_ptr> MakeMobilizerForJoint( - const SpanningForest::Mobod& mobod, - MultibodyTree* tree) const override { - DRAKE_DEMAND(tree != nullptr); // Just a sanity check; we don't need it. + const SpanningForest::Mobod& mobod) const override { const auto [inboard_frame, outboard_frame] = this->tree_frames(mobod.is_reversed()); // TODO(sherm1) The mobilizer needs to be reversed, not just the frames. - // 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(). + // 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>( mobod, *inboard_frame, *outboard_frame); return revolute_mobilizer; diff --git a/multibody/rational/rational_forward_kinematics.cc b/multibody/rational/rational_forward_kinematics.cc index 15f95dc5f7ff..208a1a7f9759 100644 --- a/multibody/rational/rational_forward_kinematics.cc +++ b/multibody/rational/rational_forward_kinematics.cc @@ -179,11 +179,10 @@ RationalForwardKinematics::Pose RationalForwardKinematics:: template RationalForwardKinematics::Pose RationalForwardKinematics::CalcWeldJointChildBodyPose( - const math::RigidTransformd& X_PF, const math::RigidTransformd& X_MC, - const Pose& X_AP) const { - // X_FM is always identity for a Weld mobilizer. - const Matrix3 R_FM = Matrix3::Identity(); - const Vector3 p_FM = Vector3::Zero(); + const math::RigidTransformd& X_FM, const math::RigidTransformd& X_PF, + const math::RigidTransformd& X_MC, const Pose& X_AP) const { + const Matrix3 R_FM = X_FM.rotation().matrix(); + const Vector3 p_FM = X_FM.translation(); const Matrix3& R_AP = X_AP.rotation; const Vector3& p_AP = X_AP.position; return CalcChildPose(R_AP, p_AP, X_PF, X_MC, R_FM, p_FM); @@ -288,9 +287,17 @@ RationalForwardKinematics::CalcChildBodyPoseAsMultilinearPolynomial( return CalcPrismaticJointChildLinkPose(axis_F, X_PF, X_MC, X_AP, q_star(s_index), s_[s_index]); } else if (IsWeld(*mobilizer)) { - return CalcWeldJointChildBodyPose(X_PF, X_MC, X_AP); + const internal::WeldMobilizer* weld_mobilizer = + static_cast*>(mobilizer); + math::RigidTransformd X_FM; + if (!is_order_reversed) { + X_FM = weld_mobilizer->get_X_FM(); + } else { + X_FM = weld_mobilizer->get_X_FM().inverse(); + } + return CalcWeldJointChildBodyPose(X_FM, X_PF, X_MC, X_AP); } - // Successful construction guarantees that all supported mobilizers are + // Successful construction guarantess that all supported mobilizers are // handled. DRAKE_UNREACHABLE(); } diff --git a/multibody/rational/rational_forward_kinematics.h b/multibody/rational/rational_forward_kinematics.h index 2598fc80eea0..89bd66179b4b 100644 --- a/multibody/rational/rational_forward_kinematics.h +++ b/multibody/rational/rational_forward_kinematics.h @@ -233,7 +233,8 @@ class RationalForwardKinematics { // Computes the pose of the body, connected to its parent body through a // weld joint. template - Pose CalcWeldJointChildBodyPose(const math::RigidTransformd& X_PF, + Pose CalcWeldJointChildBodyPose(const math::RigidTransformd& X_FM, + const math::RigidTransformd& X_PF, const math::RigidTransformd& X_MC, const Pose& X_AP) const; diff --git a/multibody/tree/ball_rpy_joint.cc b/multibody/tree/ball_rpy_joint.cc index 32d003d062b9..746f69253f40 100644 --- a/multibody/tree/ball_rpy_joint.cc +++ b/multibody/tree/ball_rpy_joint.cc @@ -71,8 +71,7 @@ std::unique_ptr> BallRpyJoint::DoShallowClone() const { // in the header file. template std::unique_ptr> BallRpyJoint::MakeMobilizerForJoint( - const internal::SpanningForest::Mobod& mobod, - internal::MultibodyTree*) const { + const internal::SpanningForest::Mobod& mobod) const { const auto [inboard_frame, outboard_frame] = this->tree_frames(mobod.is_reversed()); // TODO(sherm1) The mobilizer needs to be reversed, not just the frames. diff --git a/multibody/tree/ball_rpy_joint.h b/multibody/tree/ball_rpy_joint.h index 685878d1d0c4..f54387c89de3 100644 --- a/multibody/tree/ball_rpy_joint.h +++ b/multibody/tree/ball_rpy_joint.h @@ -71,9 +71,9 @@ class BallRpyJoint final : public Joint { DRAKE_THROW_UNLESS(damping >= 0); } - ~BallRpyJoint() final; + ~BallRpyJoint() override; - const std::string& type_name() const final; + const std::string& type_name() const override; /// Returns `this` joint's default damping constant in N⋅m⋅s. The damping /// torque (in N⋅m) is modeled as `τ = -damping⋅ω`, i.e. opposing motion, with @@ -191,7 +191,7 @@ class BallRpyJoint final : public Joint { /// Adding forces per-dof makes no physical sense. Therefore, this method /// throws an exception if invoked. void DoAddInOneForce(const systems::Context&, int, const T&, - MultibodyForces*) const final { + MultibodyForces*) const override { throw std::logic_error( "Ball RPY joints do not allow applying forces to individual degrees of " "freedom."); @@ -203,7 +203,7 @@ class BallRpyJoint final : public Joint { /// viscous law `τ = -d⋅ω`, with d the damping coefficient (see /// default_damping()). void DoAddInDamping(const systems::Context& context, - MultibodyForces* forces) const final { + MultibodyForces* forces) const override { Eigen::Ref> t_BMo_F = get_mobilizer().get_mutable_generalized_forces_from_array( &forces->mutable_generalized_forces()); @@ -212,28 +212,28 @@ class BallRpyJoint final : public Joint { } private: - int do_get_velocity_start() const final { + int do_get_velocity_start() const override { return get_mobilizer().velocity_start_in_v(); } - int do_get_num_velocities() const final { return 3; } + int do_get_num_velocities() const override { return 3; } - int do_get_position_start() const final { + int do_get_position_start() const override { return get_mobilizer().position_start_in_q(); } - int do_get_num_positions() const final { return 3; } + int do_get_num_positions() const override { return 3; } - std::string do_get_position_suffix(int index) const final { + std::string do_get_position_suffix(int index) const override { return get_mobilizer().position_suffix(index); } - std::string do_get_velocity_suffix(int index) const final { + std::string do_get_velocity_suffix(int index) const override { return get_mobilizer().velocity_suffix(index); } void do_set_default_positions( - const VectorX& default_positions) final { + const VectorX& default_positions) override { if (this->has_mobilizer()) { get_mutable_mobilizer().set_default_position(default_positions); } @@ -241,19 +241,18 @@ class BallRpyJoint final : public Joint { // Joint overrides: std::unique_ptr> MakeMobilizerForJoint( - const internal::SpanningForest::Mobod& mobod, - internal::MultibodyTree* tree) const final; + const internal::SpanningForest::Mobod& mobod) const override; std::unique_ptr> DoCloneToScalar( - const internal::MultibodyTree& tree_clone) const final; + const internal::MultibodyTree& tree_clone) const override; std::unique_ptr> DoCloneToScalar( - const internal::MultibodyTree& tree_clone) const final; + const internal::MultibodyTree& tree_clone) const override; std::unique_ptr> DoCloneToScalar( - const internal::MultibodyTree&) const final; + const internal::MultibodyTree&) const override; - std::unique_ptr> DoShallowClone() const final; + std::unique_ptr> DoShallowClone() const override; // Make BallRpyJoint templated on every other scalar type a friend of // BallRpyJoint so that CloneToScalar() can access diff --git a/multibody/tree/curvilinear_joint.cc b/multibody/tree/curvilinear_joint.cc index e1e98076cf40..8d6d9ce30c08 100644 --- a/multibody/tree/curvilinear_joint.cc +++ b/multibody/tree/curvilinear_joint.cc @@ -101,8 +101,7 @@ std::unique_ptr> CurvilinearJoint::DoShallowClone() const { template std::unique_ptr> CurvilinearJoint::MakeMobilizerForJoint( - const internal::SpanningForest::Mobod& mobod, - internal::MultibodyTree*) const { + const internal::SpanningForest::Mobod& mobod) const { const auto [inboard_frame, outboard_frame] = this->tree_frames(mobod.is_reversed()); // TODO(sherm1) The mobilizer needs to be reversed, not just the frames. diff --git a/multibody/tree/curvilinear_joint.h b/multibody/tree/curvilinear_joint.h index 65ba6ea504e3..e5323001343a 100644 --- a/multibody/tree/curvilinear_joint.h +++ b/multibody/tree/curvilinear_joint.h @@ -121,9 +121,9 @@ class CurvilinearJoint final : public Joint { curvilinear_path, double pos_lower_limit, double pos_upper_limit, double damping = 0); - ~CurvilinearJoint() final; + ~CurvilinearJoint() override; - const std::string& type_name() const final; + const std::string& type_name() const override; /** Returns `this` joint's default damping constant in N⋅s/m. */ double default_damping() const { return this->default_damping_vector()[0]; } @@ -269,7 +269,7 @@ class CurvilinearJoint final : public Joint { @see The public NVI AddInOneForce() for details. */ void DoAddInOneForce(const systems::Context&, int joint_dof, const T& joint_tau, - MultibodyForces* forces) const final { + MultibodyForces* forces) const override { DRAKE_DEMAND(joint_dof == 0); Eigen::Ref> tau_mob = get_mobilizer().get_mutable_generalized_forces_from_array( @@ -288,63 +288,62 @@ class CurvilinearJoint final : public Joint { @param[out] forces The MultibodyForces object to which the damping force is added. */ void DoAddInDamping(const systems::Context& context, - MultibodyForces* forces) const final { + MultibodyForces* forces) const override { const T damping_force = -this->GetDamping(context) * get_tangential_velocity(context); AddInForce(context, damping_force, forces); } private: - int do_get_velocity_start() const final { + int do_get_velocity_start() const override { return get_mobilizer().velocity_start_in_v(); } - int do_get_num_velocities() const final { return 1; } + int do_get_num_velocities() const override { return 1; } - int do_get_position_start() const final { + int do_get_position_start() const override { return get_mobilizer().position_start_in_q(); } - int do_get_num_positions() const final { return 1; } + int do_get_num_positions() const override { return 1; } - std::string do_get_position_suffix(int index) const final { + std::string do_get_position_suffix(int index) const override { return get_mobilizer().position_suffix(index); } - std::string do_get_velocity_suffix(int index) const final { + std::string do_get_velocity_suffix(int index) const override { return get_mobilizer().velocity_suffix(index); } void do_set_default_positions( - const VectorX& default_positions) final { + const VectorX& default_positions) override { if (this->has_mobilizer()) { get_mutable_mobilizer().set_default_position(default_positions); } } - const T& DoGetOnePosition(const systems::Context& context) const final { + const T& DoGetOnePosition(const systems::Context& context) const override { return get_distance(context); } - const T& DoGetOneVelocity(const systems::Context& context) const final { + const T& DoGetOneVelocity(const systems::Context& context) const override { return get_tangential_velocity(context); } // Joint overrides: std::unique_ptr> MakeMobilizerForJoint( - const internal::SpanningForest::Mobod& mobod, - internal::MultibodyTree* tree) const final; + const internal::SpanningForest::Mobod& mobod) const override; std::unique_ptr> DoCloneToScalar( - const internal::MultibodyTree& tree_clone) const final; + const internal::MultibodyTree& tree_clone) const override; std::unique_ptr> DoCloneToScalar( - const internal::MultibodyTree& tree_clone) const final; + const internal::MultibodyTree& tree_clone) const override; std::unique_ptr> DoCloneToScalar( - const internal::MultibodyTree&) const final; + const internal::MultibodyTree&) const override; - std::unique_ptr> DoShallowClone() const final; + std::unique_ptr> DoShallowClone() const override; // Make CurvilinearJoint templated on every other scalar type a friend of // CurvilinearJoint so that CloneToScalar() can access diff --git a/multibody/tree/joint.cc b/multibody/tree/joint.cc index 5a55746e2cd4..01c34467b0f5 100644 --- a/multibody/tree/joint.cc +++ b/multibody/tree/joint.cc @@ -73,11 +73,9 @@ Eigen::Ref> Joint::GetVelocities( template std::unique_ptr> Joint::Build( - const internal::SpanningForest::Mobod& mobod, - internal::MultibodyTree* tree) { - DRAKE_DEMAND(tree != nullptr); + const internal::SpanningForest::Mobod& mobod) { std::unique_ptr> owned_mobilizer = - MakeMobilizerForJoint(mobod, tree); + MakeMobilizerForJoint(mobod); mobilizer_ = owned_mobilizer.get(); return owned_mobilizer; } diff --git a/multibody/tree/joint.h b/multibody/tree/joint.h index 9f4e8c35c85f..45b682bb8a24 100644 --- a/multibody/tree/joint.h +++ b/multibody/tree/joint.h @@ -753,10 +753,8 @@ class Joint : public MultibodyElement { // Hide the following section from Doxygen. #ifndef DRAKE_DOXYGEN_CXX // (Internal use only) Model this joint using the appropriate Mobilizer. - // `tree` must be non-null. std::unique_ptr> Build( - const internal::SpanningForest::Mobod& mobod, - internal::MultibodyTree* tree); + const internal::SpanningForest::Mobod& mobod); // NVI to DoCloneToScalar() templated on the scalar type of the new clone to // be created. This method is intended to be called by @@ -976,45 +974,10 @@ class Joint : public MultibodyElement { template friend class Joint; - /* This method must be implemented by derived Joint classes in order to create - a Mobilizer as the Joint's internal representation. Starting with the - user's joint frames Jp (on parent) and Jc (on child) we must create an - inboard frame F and outboard frame M suitable for an available Mobilizer. - For example, if a revolute Mobilizer can only rotate around its z axis, - while the revolute Joint specifies an arbitrary axis â, we'll need to - calculate frames such that Fz and Mz are aligned with â, and the other - axes chosen so that the joint coordinate q has the same meaning as it would - when rotating about â. We also must decide whether inboard/outboard is - reversed from parent/child. Normally we need X_JpF and X_JcM but when reversed - we need X_JcF and X_JpM. (We're ignoring reversal in the discussion below.) - - In the case of revolute, prismatic, and screw joints we have an axis â - whose measure numbers are the same in Jp and Jc. However, for maximum - speed, the available mobilizers are specialized to rotate only about the - +z axis. - TODO(sherm1) Make that happen. - Consequently, we want new frames F and M with Fz=Mz=â, Fo=Jpo, Mo=Jco. We - also want F==M when Jp==Jc, i.e. at the joint zero position so that the - coordinate q will mean the same thing using F and M as it would have using - Jp, Jc, and â. We need to calculate R_JpF and R_JcM so that we can create - appropriate offset frames: - R_JpF = MakeFromOneVector(â_Jp, 2) ("2" means "z axis") - R_JcM = R_JcJp(0) * R_JpF * R_FM(0) (at q=0) - But in the zero configurations we have R_JcJp(0)=I and we want R_FM(0)=I - also, so R_JcM = R_JpF. - - For a weld joint, we have Jp, Jc, and a fixed X_JpJc. We want to pick F - and M so F=M at all times. We can choose M=Jc and create a new offset frame - for F that is colocated with Jp: - X_JpF = X_JpJc - This yields X_FM = X_FJc = X_JpF⁻¹ * X_JpJc = Identity. - - In order to permit auxiliary frames to be created, we provide mutable - access to the MultibodyTree here. Don't use that to add anything other - than frames. We promise that tree will be non-null. */ + // This method must be implemented by derived Joint classes in order to + // create a Mobilizer as the Joint's internal representation. virtual std::unique_ptr> MakeMobilizerForJoint( - const internal::SpanningForest::Mobod& mobod, - internal::MultibodyTree* tree) const = 0; + const internal::SpanningForest::Mobod& mobod) const = 0; // Helper method to be called within Joint::CloneToScalar() to locate the // cloned Mobilizer corresponding to this Joint's Mobilizer. @@ -1050,8 +1013,8 @@ class Joint : public MultibodyElement { const char* func) const; std::string name_; - const Frame& frame_on_parent_; // Frame Jp. - const Frame& frame_on_child_; // Frame Jc. + const Frame& frame_on_parent_; + const Frame& frame_on_child_; VectorX damping_; @@ -1073,7 +1036,7 @@ class Joint : public MultibodyElement { // Joint default position. This vector has zero size for joints with no state. VectorX default_positions_; - // The Joint implementation. + // The Joint implementation: internal::Mobilizer* mobilizer_{}; // System parameter indices. diff --git a/multibody/tree/multibody_tree-inl.h b/multibody/tree/multibody_tree-inl.h index f1c5e7498b92..12899c8d4a00 100644 --- a/multibody/tree/multibody_tree-inl.h +++ b/multibody/tree/multibody_tree-inl.h @@ -99,8 +99,8 @@ const MobilizerType& MultibodyTree::AddMobilizer( // to this tree. This is a pathological case, but in theory nothing // (but this test) stops a user from adding frames to a tree1 and attempting // later to define mobilizers between those frames in a second tree2. - mobilizer->inboard_frame().HasThisParentTreeOrThrow(this); // F frame - mobilizer->outboard_frame().HasThisParentTreeOrThrow(this); // M frame + mobilizer->inboard_frame().HasThisParentTreeOrThrow(this); + mobilizer->outboard_frame().HasThisParentTreeOrThrow(this); MobodIndex mobilizer_index = topology_.add_mobilizer( mobilizer->mobod(), mobilizer->inboard_frame().index(), mobilizer->outboard_frame().index()); @@ -239,9 +239,9 @@ template template