From d46d62fc7b086b53cfc769e0ae9a0d246fe2c3ab Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Wed, 11 Jan 2023 18:09:02 -0800 Subject: [PATCH 1/4] Initial renaming of Mobilizer to MobilizedBody; no functionality changes. Includes cleanup of fallout like lines too long, and some comment cleanup. --- .../optimization/dev/cspace_free_polytope.cc | 5 +- .../test/multibody_plant_free_body_test.cc | 2 +- multibody/plant/multibody_plant.cc | 4 +- multibody/plant/multibody_plant.h | 8 +- .../rational/rational_forward_kinematics.cc | 12 +- .../rational/rational_forward_kinematics.h | 14 +- .../rational_forward_kinematics_internal.cc | 11 +- .../rational_forward_kinematics_internal.h | 2 +- ...tional_forward_kinematics_test_utilities.h | 4 +- multibody/tree/BUILD.bazel | 6 +- multibody/tree/ball_rpy_joint.cc | 3 - multibody/tree/body_node.cc | 2 +- multibody/tree/body_node.h | 31 +- multibody/tree/body_node_impl.h | 10 +- multibody/tree/force_element.h | 2 +- multibody/tree/joint.cc | 8 +- multibody/tree/joint.h | 16 +- .../tree/{mobilizer.h => mobilized_body.h} | 424 +++++++++--------- ...bilizer_impl.cc => mobilized_body_impl.cc} | 27 +- ...mobilizer_impl.h => mobilized_body_impl.h} | 46 +- multibody/tree/model_instance.cc | 8 +- multibody/tree/model_instance.h | 8 +- multibody/tree/multibody_tree-inl.h | 20 +- multibody/tree/multibody_tree.cc | 24 +- multibody/tree/multibody_tree.h | 82 ++-- multibody/tree/multibody_tree_indexes.h | 4 +- multibody/tree/multibody_tree_system.cc | 2 +- multibody/tree/multibody_tree_topology.h | 47 +- multibody/tree/planar_joint.cc | 3 - multibody/tree/planar_mobilizer.cc | 8 +- multibody/tree/planar_mobilizer.h | 20 +- multibody/tree/position_kinematics_cache.h | 2 +- multibody/tree/prismatic_mobilizer.cc | 9 +- multibody/tree/prismatic_mobilizer.h | 22 +- multibody/tree/quaternion_floating_joint.cc | 3 - .../tree/quaternion_floating_mobilizer.cc | 8 +- .../tree/quaternion_floating_mobilizer.h | 24 +- multibody/tree/revolute_joint.cc | 3 - multibody/tree/revolute_mobilizer.cc | 9 +- multibody/tree/revolute_mobilizer.h | 20 +- multibody/tree/rigid_body.h | 2 +- multibody/tree/screw_joint.cc | 3 - multibody/tree/screw_mobilizer.cc | 8 +- multibody/tree/screw_mobilizer.h | 20 +- .../tree/space_xyz_floating_mobilizer.cc | 8 +- multibody/tree/space_xyz_floating_mobilizer.h | 28 +- multibody/tree/space_xyz_mobilizer.cc | 8 +- multibody/tree/space_xyz_mobilizer.h | 24 +- .../test/articulated_body_algorithm_test.cc | 16 +- multibody/tree/test/frames_test.cc | 2 +- multibody/tree/test/mobilizer_tester.h | 2 +- .../tree/test/multibody_tree_creation_test.cc | 16 +- .../tree/test/tree_from_mobilizers_test.cc | 10 +- multibody/tree/universal_joint.cc | 3 - multibody/tree/universal_mobilizer.cc | 11 +- multibody/tree/universal_mobilizer.h | 22 +- multibody/tree/weld_joint.cc | 3 - multibody/tree/weld_mobilizer.cc | 8 +- multibody/tree/weld_mobilizer.h | 20 +- 59 files changed, 563 insertions(+), 614 deletions(-) rename multibody/tree/{mobilizer.h => mobilized_body.h} (67%) rename multibody/tree/{mobilizer_impl.cc => mobilized_body_impl.cc} (63%) rename multibody/tree/{mobilizer_impl.h => mobilized_body_impl.h} (87%) diff --git a/geometry/optimization/dev/cspace_free_polytope.cc b/geometry/optimization/dev/cspace_free_polytope.cc index 52591aa3b850..9fe0195eda44 100644 --- a/geometry/optimization/dev/cspace_free_polytope.cc +++ b/geometry/optimization/dev/cspace_free_polytope.cc @@ -33,7 +33,7 @@ namespace { [[nodiscard]] bool ChainIsWeld(const multibody::MultibodyPlant& plant, multibody::BodyIndex start, multibody::BodyIndex end) { - const std::vector mobilizers = + const std::vector mobilizers = multibody::internal::FindMobilizersOnPath(plant, start, end); if (mobilizers.size() == 0) { return true; @@ -124,7 +124,8 @@ void FindMonomialBasisArray( std::array, 4>* monomial_basis_array) { auto it = map_body_to_monomial_basis_array->find(body_pair); if (it == map_body_to_monomial_basis_array->end()) { - const std::vector mobilizer_indices = + const std::vector + mobilizer_indices = multibody::internal::FindMobilizersOnPath(rational_forward_kin.plant(), body_pair.first(), body_pair.second()); diff --git a/multibody/benchmarks/free_body/test/multibody_plant_free_body_test.cc b/multibody/benchmarks/free_body/test/multibody_plant_free_body_test.cc index 6de5764c2b60..d5bb48c7f00b 100644 --- a/multibody/benchmarks/free_body/test/multibody_plant_free_body_test.cc +++ b/multibody/benchmarks/free_body/test/multibody_plant_free_body_test.cc @@ -17,7 +17,7 @@ #include "drake/multibody/benchmarks/free_body/free_body.h" #include "drake/multibody/test_utilities/floating_body_plant.h" #include "drake/multibody/tree/body.h" -#include "drake/multibody/tree/mobilizer.h" +#include "drake/multibody/tree/mobilized_body.h" #include "drake/multibody/tree/multibody_tree.h" #include "drake/systems/analysis/runge_kutta3_integrator.h" #include "drake/systems/analysis/simulator.h" diff --git a/multibody/plant/multibody_plant.cc b/multibody/plant/multibody_plant.cc index 075b5c75d154..228a0e2d055e 100644 --- a/multibody/plant/multibody_plant.cc +++ b/multibody/plant/multibody_plant.cc @@ -2988,9 +2988,9 @@ void MultibodyPlant::CalcReactionForces( // necessary frame conversions. for (JointIndex joint_index(0); joint_index < num_joints(); ++joint_index) { const Joint& joint = get_joint(joint_index); - const internal::MobilizerIndex mobilizer_index = + const internal::MobilizedBodyIndex mobilizer_index = internal_tree().get_joint_mobilizer(joint_index); - const internal::Mobilizer& mobilizer = + const internal::MobilizedBody& mobilizer = internal_tree().get_mobilizer(mobilizer_index); const internal::BodyNodeIndex body_node_index = mobilizer.get_topology().body_node; diff --git a/multibody/plant/multibody_plant.h b/multibody/plant/multibody_plant.h index 39e4c1b6752f..4419690cfdf4 100644 --- a/multibody/plant/multibody_plant.h +++ b/multibody/plant/multibody_plant.h @@ -2456,8 +2456,8 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// A %MultibodyPlant user adds sets of Body and Joint objects to `this` plant /// to build a physical representation of a mechanical model. /// At Finalize(), %MultibodyPlant builds a mathematical representation of - /// such system, consisting of a tree representation. In this - /// representation each body is assigned a Mobilizer, which grants a certain + /// such system, consisting of a tree representation. In this representation + /// each body is assigned a MobilizedBody, which grants a certain /// number of degrees of freedom in accordance to the physical specification. /// In this regard, the modeling representation can be seen as a forest of /// tree structures each of which contains a single body at the root of the @@ -3199,7 +3199,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// or if it is not of size num_positions(). /// /// @see MapQDotToVelocity() - /// @see Mobilizer::MapVelocityToQDot() + /// @see MobilizedBody::MapVelocityToQDot() void MapVelocityToQDot( const systems::Context& context, const Eigen::Ref>& v, @@ -3233,7 +3233,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem { /// is not of size num_velocities(). /// /// @see MapVelocityToQDot() - /// @see Mobilizer::MapQDotToVelocity() + /// @see MobilizedBody::MapQDotToVelocity() void MapQDotToVelocity( const systems::Context& context, const Eigen::Ref>& qdot, diff --git a/multibody/rational/rational_forward_kinematics.cc b/multibody/rational/rational_forward_kinematics.cc index 60dbba140e42..565ca3464207 100644 --- a/multibody/rational/rational_forward_kinematics.cc +++ b/multibody/rational/rational_forward_kinematics.cc @@ -69,7 +69,7 @@ RationalForwardKinematics::RationalForwardKinematics( ++body_index) { const internal::BodyTopology& body_topology = tree.get_topology().get_body(body_index); - const internal::Mobilizer* mobilizer = + const internal::MobilizedBody* mobilizer = &(tree.get_mobilizer(body_topology.inboard_mobilizer)); if (IsRevolute(*mobilizer)) { const symbolic::Variable s_angle(fmt::format("s[{}]", s_.size())); @@ -235,7 +235,7 @@ RationalForwardKinematics::CalcChildBodyPoseAsMultilinearPolynomial( tree.get_topology().get_body(parent); const internal::BodyTopology& child_topology = tree.get_topology().get_body(child); - internal::MobilizerIndex mobilizer_index; + internal::MobilizedBodyIndex mobilizer_index; bool is_order_reversed; if (parent_topology.parent_body.is_valid() && parent_topology.parent_body == child) { @@ -251,7 +251,7 @@ RationalForwardKinematics::CalcChildBodyPoseAsMultilinearPolynomial( "({}, {}) do not have a parent-child relationship.", parent, child)); } - const internal::Mobilizer* mobilizer = + const internal::MobilizedBody* mobilizer = &(tree.get_mobilizer(mobilizer_index)); math::RigidTransformd X_PF, X_MC; if (!is_order_reversed) { @@ -310,7 +310,7 @@ RationalForwardKinematics::CalcChildBodyPoseAsMultilinearPolynomial( // TODO(hongkai.dai): determine the joint type through a Reifier. bool RationalForwardKinematics::IsRevolute( - const internal::Mobilizer& mobilizer) const { + const internal::MobilizedBody& mobilizer) const { const bool is_revolute = (mobilizer.num_positions() == 1 && mobilizer.num_velocities() == 1 && mobilizer.can_rotate() && !mobilizer.can_translate()); @@ -323,7 +323,7 @@ bool RationalForwardKinematics::IsRevolute( // TODO(hongkai.dai): determine the joint type through a Reifier. bool RationalForwardKinematics::IsWeld( - const internal::Mobilizer& mobilizer) const { + const internal::MobilizedBody& mobilizer) const { const bool is_weld = (mobilizer.num_positions() == 0 && mobilizer.num_velocities() == 0 && !mobilizer.can_rotate() && !mobilizer.can_translate()); @@ -336,7 +336,7 @@ bool RationalForwardKinematics::IsWeld( // TODO(hongkai.dai): determine the joint type through a Reifier. bool RationalForwardKinematics::IsPrismatic( - const internal::Mobilizer& mobilizer) const { + const internal::MobilizedBody& mobilizer) const { const bool is_prismatic = (mobilizer.num_positions() == 1 && mobilizer.num_velocities() == 1 && !mobilizer.can_rotate() && mobilizer.can_translate()); diff --git a/multibody/rational/rational_forward_kinematics.h b/multibody/rational/rational_forward_kinematics.h index 80ee4aefb204..de15f295f9bd 100644 --- a/multibody/rational/rational_forward_kinematics.h +++ b/multibody/rational/rational_forward_kinematics.h @@ -8,7 +8,7 @@ #include "drake/common/symbolic/rational_function.h" #include "drake/common/symbolic/trigonometric_polynomial.h" #include "drake/multibody/plant/multibody_plant.h" -#include "drake/multibody/tree/mobilizer.h" +#include "drake/multibody/tree/mobilized_body.h" #include "drake/multibody/tree/multibody_tree.h" namespace drake { @@ -114,7 +114,7 @@ class RationalForwardKinematics { const Eigen::Ref& q_star_val) const { VectorX s_val(s_.size()); for (int i = 0; i < s_val.size(); ++i) { - const internal::Mobilizer& mobilizer = + const internal::MobilizedBody& mobilizer = GetInternalTree(plant_).get_mobilizer( map_s_to_mobilizer_.at(s_[i].get_id())); // the mobilizer cannot be a weld joint since weld joint doesn't introduce @@ -145,7 +145,7 @@ class RationalForwardKinematics { const Eigen::Ref& q_star_val) const { VectorX q_val(s_.size()); for (int i = 0; i < s_val.size(); ++i) { - const internal::Mobilizer& mobilizer = + const internal::MobilizedBody& mobilizer = GetInternalTree(plant_).get_mobilizer( map_s_to_mobilizer_.at(s_[i].get_id())); // the mobilizer cannot be a weld joint since weld joint doesn't introduce @@ -255,20 +255,20 @@ class RationalForwardKinematics { BodyIndex child, const Pose& X_AP) const; /* Determines whether the current joint is revolute. */ - bool IsRevolute(const internal::Mobilizer& mobilizer) const; + bool IsRevolute(const internal::MobilizedBody& mobilizer) const; /* Determines whether the current joint is a weld. */ - bool IsWeld(const internal::Mobilizer& mobilizer) const; + bool IsWeld(const internal::MobilizedBody& mobilizer) const; /* Determines whether the current joint is prismatic. */ - bool IsPrismatic(const internal::Mobilizer& mobilizer) const; + bool IsPrismatic(const internal::MobilizedBody& mobilizer) const; const MultibodyPlant& plant_; // The variables used in computing the pose as rational functions. s_ are the // indeterminates in the rational functions. std::vector s_; // Each s(i) is associated with a mobilizer. - std::unordered_map + std::unordered_map map_s_to_mobilizer_; // map_mobilizer_to_s_index_[mobilizer_index] returns the starting index of // the mobilizer's variable in s_ (the variable will be contiguous in s for diff --git a/multibody/rational/rational_forward_kinematics_internal.cc b/multibody/rational/rational_forward_kinematics_internal.cc index f8a3bff14b11..574949c10aa1 100644 --- a/multibody/rational/rational_forward_kinematics_internal.cc +++ b/multibody/rational/rational_forward_kinematics_internal.cc @@ -64,10 +64,10 @@ std::vector FindPath(const MultibodyPlant& plant, return path; } -std::vector FindMobilizersOnPath( +std::vector FindMobilizersOnPath( const MultibodyPlant& plant, BodyIndex start, BodyIndex end) { const std::vector path = FindPath(plant, start, end); - std::vector mobilizers_on_path; + std::vector mobilizers_on_path; mobilizers_on_path.reserve(path.size() - 1); const MultibodyTree& tree = GetInternalTree(plant); for (int i = 0; i < static_cast(path.size()) - 1; ++i) { @@ -96,11 +96,12 @@ BodyIndex FindBodyInTheMiddleOfChain(const MultibodyPlant& plant, path_not_weld.reserve(path.size()); path_not_weld.push_back(start); const MultibodyTree& tree = GetInternalTree(plant); - const std::vector mobilizer_indices = + const std::vector mobilizer_indices = FindMobilizersOnPath(plant, path[0], path.back()); for (int i = 0; i < static_cast(mobilizer_indices.size()); ++i) { - const MobilizerIndex mobilizer_index = mobilizer_indices[i]; - const Mobilizer& mobilizer = tree.get_mobilizer(mobilizer_index); + const MobilizedBodyIndex mobilizer_index = mobilizer_indices[i]; + const MobilizedBody& mobilizer = + tree.get_mobilizer(mobilizer_index); if (mobilizer.num_positions() != 0) { path_not_weld.push_back(path[i + 1]); } diff --git a/multibody/rational/rational_forward_kinematics_internal.h b/multibody/rational/rational_forward_kinematics_internal.h index c9f936e9c809..9fa1026ce493 100644 --- a/multibody/rational/rational_forward_kinematics_internal.h +++ b/multibody/rational/rational_forward_kinematics_internal.h @@ -30,7 +30,7 @@ std::vector FindPath(const MultibodyPlant& plant, /* * Finds all the mobilizer on the path from start to the end. */ -std::vector FindMobilizersOnPath( +std::vector FindMobilizersOnPath( const MultibodyPlant& plant, BodyIndex start, BodyIndex end); /* diff --git a/multibody/rational/test/rational_forward_kinematics_test_utilities.h b/multibody/rational/test/rational_forward_kinematics_test_utilities.h index de5ba4f4e026..34908ab62dad 100644 --- a/multibody/rational/test/rational_forward_kinematics_test_utilities.h +++ b/multibody/rational/test/rational_forward_kinematics_test_utilities.h @@ -51,7 +51,7 @@ class IiwaTest : public ::testing::Test { const internal::MultibodyTree& iiwa_tree_; const BodyIndex world_; std::array iiwa_link_; - std::array iiwa_joint_; + std::array iiwa_joint_; }; /* @@ -66,7 +66,7 @@ class FinalizedIiwaTest : public ::testing::Test { const internal::MultibodyTree& iiwa_tree_; const BodyIndex world_; std::array iiwa_link_; - std::array iiwa_joint_; + std::array iiwa_joint_; }; /* diff --git a/multibody/tree/BUILD.bazel b/multibody/tree/BUILD.bazel index 356e78f1f95b..2814ceff0dad 100644 --- a/multibody/tree/BUILD.bazel +++ b/multibody/tree/BUILD.bazel @@ -112,7 +112,7 @@ drake_cc_library( "joint_actuator.cc", "linear_bushing_roll_pitch_yaw.cc", "linear_spring_damper.cc", - "mobilizer_impl.cc", + "mobilized_body_impl.cc", "model_instance.cc", "multibody_element.cc", "multibody_forces.cc", @@ -154,8 +154,8 @@ drake_cc_library( "joint_actuator.h", "linear_bushing_roll_pitch_yaw.h", "linear_spring_damper.h", - "mobilizer.h", - "mobilizer_impl.h", + "mobilized_body.h", + "mobilized_body_impl.h", "model_instance.h", "multibody_element.h", "multibody_forces.h", diff --git a/multibody/tree/ball_rpy_joint.cc b/multibody/tree/ball_rpy_joint.cc index 3804e93e1814..ecc831549e8f 100644 --- a/multibody/tree/ball_rpy_joint.cc +++ b/multibody/tree/ball_rpy_joint.cc @@ -56,9 +56,6 @@ std::unique_ptr> BallRpyJoint::DoCloneToScalar( return TemplatedDoCloneToScalar(tree_clone); } -// N.B. Due to esoteric linking errors on Mac (see #9345) involving -// `MobilizerImpl`, we must place this implementation in the source file, not -// in the header file. template std::unique_ptr::BluePrint> BallRpyJoint::MakeImplementationBlueprint() const { diff --git a/multibody/tree/body_node.cc b/multibody/tree/body_node.cc index 7a13017a746d..c2d695579872 100644 --- a/multibody/tree/body_node.cc +++ b/multibody/tree/body_node.cc @@ -50,7 +50,7 @@ void BodyNode::CalcArticulatedBodyHingeInertiaMatrixFactorization( // positive definite, we _know_ D_B is near singular. if (llt_D_B->eigen_linear_solver().info() != Eigen::Success) { // Create a meaningful message that helps the user as much as possible. - const Mobilizer& mobilizer = get_mobilizer(); + const MobilizedBody& mobilizer = get_mobilizer(); const Body& inboard_body = mobilizer.inboard_body(); const Body& outboard_body = mobilizer.outboard_body(); const std::string& inboard_body_name = inboard_body.name(); diff --git a/multibody/tree/body_node.h b/multibody/tree/body_node.h index 6debcdea3252..56c316a4a58e 100644 --- a/multibody/tree/body_node.h +++ b/multibody/tree/body_node.h @@ -13,7 +13,7 @@ #include "drake/multibody/tree/articulated_body_force_cache.h" #include "drake/multibody/tree/articulated_body_inertia_cache.h" #include "drake/multibody/tree/body.h" -#include "drake/multibody/tree/mobilizer.h" +#include "drake/multibody/tree/mobilized_body.h" #include "drake/multibody/tree/multibody_element.h" #include "drake/multibody/tree/multibody_tree_indexes.h" #include "drake/multibody/tree/multibody_tree_topology.h" @@ -25,13 +25,16 @@ namespace drake { namespace multibody { namespace internal { +// TODO(sherm1) Get rid of this class by moving its functionality to +// MobilizedBody. + // For internal use only of the MultibodyTree implementation. // This is a base class representing a **node** in the tree structure of a // MultibodyTree. %BodyNode provides implementations for convenience methods to // be used in MultibodyTree recursive algorithms but that however should not -// leak into the public API for the Mobilizer class. In this regard, %BodyNode -// provides an additional separation layer between implementation internals and -// user facing API. +// leak into the public API for the MobilizedBody class. In this regard, +// %BodyNode provides an additional separation layer between implementation +// internals and user facing API. // //

Tree Structure

// @@ -65,7 +68,7 @@ namespace internal { // //

Associated State

// -// In the same way a Mobilizer and a Body have a number of generalized +// In the same way a MobilizedBody and a Body have a number of generalized // positions associated with them, a %BodyNode is associated with the // generalized positions of body B and of its inboard mobilizer. // @@ -98,7 +101,7 @@ class BodyNode : public MultibodyElement { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(BodyNode) - // A node encompasses a Body in a MultibodyTree and the inboard Mobilizer + // A node encompasses a Body in a MultibodyTree and the inboard MobilizedBody // that connects this body to the rest of tree. Given a body and its inboard // mobilizer in a MultibodyTree this constructor creates the corresponding // %BodyNode. See this class' documentation for details on how a %BodyNode is @@ -116,7 +119,7 @@ class BodyNode : public MultibodyElement { // @note %BodyNode keeps a reference to the parent body, body and mobilizer // for this node, which must outlive `this` BodyNode. BodyNode(const BodyNode* parent_node, - const Body* body, const Mobilizer* mobilizer) + const Body* body, const MobilizedBody* mobilizer) : MultibodyElement(body->model_instance()), parent_node_(parent_node), body_(body), @@ -165,7 +168,7 @@ class BodyNode : public MultibodyElement { // Returns a constant reference to the mobilizer associated with this node. // Aborts if called on the root node corresponding to the _world_ body, for // which there is no mobilizer. - const Mobilizer& get_mobilizer() const { + const MobilizedBody& get_mobilizer() const { DRAKE_DEMAND(mobilizer_ != nullptr); return *mobilizer_; } @@ -173,14 +176,14 @@ class BodyNode : public MultibodyElement { // @name Methods to retrieve BodyNode sizes //@{ - // Returns the number of generalized positions for the Mobilizer in `this` + // Returns the number of generalized positions for the MobilizedBody in `this` // node. int get_num_mobilizer_positions() const { return topology_.num_mobilizer_positions; } - // Returns the number of generalized velocities for the Mobilizer in `this` - // node. + // Returns the number of generalized velocities for the MobilizedBody in + // `this` node. int get_num_mobilizer_velocities() const { return topology_.num_mobilizer_velocities; } @@ -241,7 +244,7 @@ class BodyNode : public MultibodyElement { // TODO(amcastro-tri): // With H_FM(qm) already in the cache (computed by - // Mobilizer::UpdatePositionKinematicsCache()) update the cache + // MobilizedBody::UpdatePositionKinematicsCache()) update the cache // entries for H_PB_W, the hinge matrix for the SpatialVelocity jump between // body B and its parent body P expressed in the world frame W. } @@ -464,7 +467,7 @@ class BodyNode : public MultibodyElement { // A_PB_W = R_WF * A_FM.Shift(p_MB_F, w_FM) (4) // where R_WF is the rotation matrix from F to W and A_FM expressed in the // inboard frame F is the direct result from - // Mobilizer::CalcAcrossMobilizerAcceleration(). + // MobilizedBody::CalcAcrossMobilizerAcceleration(). // // * Note: // The rigid body assumption is made in Eq. (3) in two places: @@ -1903,7 +1906,7 @@ class BodyNode : public MultibodyElement { // Pointers for fast access. const Body* body_; - const Mobilizer* mobilizer_{nullptr}; + const MobilizedBody* mobilizer_{nullptr}; }; } // namespace internal diff --git a/multibody/tree/body_node_impl.h b/multibody/tree/body_node_impl.h index fa72885aa4b8..279458854c13 100644 --- a/multibody/tree/body_node_impl.h +++ b/multibody/tree/body_node_impl.h @@ -5,7 +5,7 @@ #include "drake/common/drake_assert.h" #include "drake/common/eigen_types.h" #include "drake/multibody/tree/body_node.h" -#include "drake/multibody/tree/mobilizer.h" +#include "drake/multibody/tree/mobilized_body.h" #include "drake/multibody/tree/multibody_tree_topology.h" namespace drake { @@ -38,11 +38,11 @@ class BodyNodeImpl : public BodyNode { // the owning MultibodyTree. It can be a `nullptr` only when `body` **is** // the **world** body, otherwise the parent class constructor will abort. // @param[in] body The body B associated with `this` node. - // @param[in] mobilizer The mobilizer associated with this `node`. It can - // only be a `nullptr` for the **world** body. + // @param[in] mobilized_body The mobilizer associated with this `node`. It can + // only be a `nullptr` for the **world** body. BodyNodeImpl(const internal::BodyNode* parent_node, - const Body* body, const Mobilizer* mobilizer) : - BodyNode(parent_node, body, mobilizer) {} + const Body* body, const MobilizedBody* mobilized_body) : + BodyNode(parent_node, body, mobilized_body) {} // TODO(amcastro-tri): Implement methods for computing velocity kinematics // using fixed-size Eigen matrices. diff --git a/multibody/tree/force_element.h b/multibody/tree/force_element.h index 3d2eb132f0ea..36e5c7064df5 100644 --- a/multibody/tree/force_element.h +++ b/multibody/tree/force_element.h @@ -191,7 +191,7 @@ class ForceElement : public MultibodyElement { /// since there is no external mechanism to ensure the argument `tree_clone` /// is in a valid stage of cloning. In contrast, /// MultibodyTree::CloneToScalar() guarantees that by when - /// ForceElement::CloneToScalar() is called, all Body, Frame and Mobilizer + /// ForceElement::CloneToScalar() is called, all Body, Frame and MobilizedBody /// objects in the original tree (templated on T) to which `this` /// %ForceElement belongs, have a corresponding clone in the cloned tree /// (argument `tree_clone` for these methods). Therefore, implementations of diff --git a/multibody/tree/joint.cc b/multibody/tree/joint.cc index 0128ef4ca9e9..09a9aae74610 100644 --- a/multibody/tree/joint.cc +++ b/multibody/tree/joint.cc @@ -5,9 +5,9 @@ namespace multibody { template bool Joint::can_rotate() const { - const std::vector*>& mobilizers = + const std::vector*>& mobilizers = get_implementation().mobilizers_; - for (const internal::Mobilizer* mobilizer : mobilizers) { + for (const internal::MobilizedBody* mobilizer : mobilizers) { if (mobilizer->can_rotate()) return true; } return false; @@ -15,9 +15,9 @@ bool Joint::can_rotate() const { template bool Joint::can_translate() const { - const std::vector*>& mobilizers = + const std::vector*>& mobilizers = get_implementation().mobilizers_; - for (const internal::Mobilizer* mobilizer : mobilizers) { + for (const internal::MobilizedBody* mobilizer : mobilizers) { if (mobilizer->can_translate()) return true; } return false; diff --git a/multibody/tree/joint.h b/multibody/tree/joint.h index ff99c2b7d02e..3f2a9b8e40d4 100644 --- a/multibody/tree/joint.h +++ b/multibody/tree/joint.h @@ -9,7 +9,7 @@ #include "drake/common/drake_copyable.h" #include "drake/multibody/tree/fixed_offset_frame.h" -#include "drake/multibody/tree/mobilizer.h" +#include "drake/multibody/tree/mobilized_body.h" #include "drake/multibody/tree/multibody_forces.h" #include "drake/multibody/tree/multibody_tree_indexes.h" #include "drake/multibody/tree/rigid_body.h" @@ -345,7 +345,7 @@ class Joint : public MultibodyElement { // Joint locking is only supported for discrete mode. // TODO(sherm1): extend the design to support continuous-mode systems. DRAKE_THROW_UNLESS(this->get_parent_tree().is_state_discrete()); - for (internal::Mobilizer* mobilizer : implementation_->mobilizers_) { + for (internal::MobilizedBody* mobilizer : implementation_->mobilizers_) { mobilizer->Lock(context); } } @@ -357,7 +357,7 @@ class Joint : public MultibodyElement { // Joint locking is only supported for discrete mode. // TODO(sherm1): extend the design to support continuous-mode systems. DRAKE_THROW_UNLESS(this->get_parent_tree().is_state_discrete()); - for (internal::Mobilizer* mobilizer : implementation_->mobilizers_) { + for (internal::MobilizedBody* mobilizer : implementation_->mobilizers_) { mobilizer->Unlock(context); } } @@ -365,7 +365,7 @@ class Joint : public MultibodyElement { /// @return true if the joint is locked, false otherwise. bool is_locked(const systems::Context& context) const { bool locked = false; - for (internal::Mobilizer* mobilizer : implementation_->mobilizers_) { + for (internal::MobilizedBody* mobilizer : implementation_->mobilizers_) { locked |= mobilizer->is_locked(context); } return locked; @@ -522,7 +522,7 @@ class Joint : public MultibodyElement { /// %Joint creates a BluePrint of its implementation with MakeModelBlueprint() /// so that MultibodyTree can build an implementation for it. struct BluePrint { - std::vector>> mobilizers_; + std::vector>> mobilizers_; // TODO(amcastro-tri): add force elements, constraints, bodies. }; @@ -561,8 +561,8 @@ class Joint : public MultibodyElement { CloneToScalar(internal::MultibodyTree* tree_clone) const { auto implementation_clone = std::make_unique::JointImplementation>(); - for (const internal::Mobilizer* mobilizer : mobilizers_) { - internal::Mobilizer* mobilizer_clone = + for (const internal::MobilizedBody* mobilizer : mobilizers_) { + internal::MobilizedBody* mobilizer_clone = &tree_clone->get_mutable_variant(*mobilizer); implementation_clone->mobilizers_.push_back(mobilizer_clone); } @@ -573,7 +573,7 @@ class Joint : public MultibodyElement { /// References (raw pointers) to the mobilizers that make part of this /// implementation. - std::vector*> mobilizers_; + std::vector*> mobilizers_; // TODO(amcastro-tri): add force elements, constraints, bodies, etc. }; diff --git a/multibody/tree/mobilizer.h b/multibody/tree/mobilized_body.h similarity index 67% rename from multibody/tree/mobilizer.h rename to multibody/tree/mobilized_body.h index 085027c8e698..1d70556caa7d 100644 --- a/multibody/tree/mobilizer.h +++ b/multibody/tree/mobilized_body.h @@ -25,202 +25,207 @@ namespace internal { template class BodyNode; -// %Mobilizer is a fundamental object within Drake's multibody engine used to -// specify the allowed motions between two Frame objects within a -// MultibodyTree. Specifying the allowed motions between two Frame objects -// effectively also specifies a kinematic relationship between the two bodies -// associated with those two frames. Consider the following example to build a -// simple pendulum system: -// -// @code -// MultibodyTree model; -// // ... Code here to setup quantities below as mass, com, X_BP, etc. ... -// const Body& pendulum = -// model.AddBody(SpatialInertia(mass, com, unit_inertia)); -// // We will connect the pendulum body to the world frame using a -// // RevoluteMobilizer. To do so we define a pin frame P rigidly attached to -// // the pendulum body. -// FixedOffsetFrame& pin_frame = -// model.AddFrame( -// pendulum.body_frame(), -// X_BP /* pose of pin frame P in body frame B */); -// // The mobilizer connects the world frame and the pin frame effectively -// // adding the single degree of freedom describing this system. In this -// // regard, the role of a mobilizer is equivalent but conceptually -// // different than a set of constraints that effectively remove all degrees -// // of freedom but the one permitting rotation about the z-axis. -// const RevoluteMobilizer& revolute_mobilizer = -// model.AddMobilizer( -// model.world_frame(), /* inboard frame */ -// pin_frame, /* outboard frame */ -// Vector3d::UnitZ() /* revolute axis in this case */)); -// @endcode -// -//

Tree Structure

-// -// A %Mobilizer induces a tree structure within a MultibodyTree -// model, connecting an inboard (topologically closer to the world) frame to an -// outboard (topologically further from the world) frame. Every time a -// %Mobilizer is added to a MultibodyTree (using the -// MultibodyTree::AddMobilizer() method), a number of degrees of -// freedom associated with the particular type of %Mobilizer are added to the -// multibody system. In the example above for the single pendulum, adding a -// RevoluteMobilizer has two purposes: -// -// - It defines the tree structure of the model. World is the inboard body -// while "pendulum" is the outboard body in the MultibodyTree. -// - It informs the MultibodyTree of the degrees of freedom granted by the -// revolute mobilizer between the two frames it connects. -// - It defines a permissible motion space spanned by the generalized -// coordinates introduced by the mobilizer. -// -//

Mathematical Description of a %Mobilizer

-// -// A %Mobilizer describes the kinematics relationship between an inboard frame -// F and an outboard frame M, introducing an nq-dimensional vector of -// generalized coordinates q and an nv-dimensional vector of generalized -// velocities v. Notice that in general `nq != nv`, though `nq == nv` is a very -// common case. The kinematic relationships introduced by a %Mobilizer are -// fully specified by, [Seth 2010]. The monogram notation used below for X_FM, -// V_FM, F_Mo_F, etc., are described in @ref multibody_frames_and_bodies. -// -// - X_FM(q): -// The outboard frame M's pose as measured and expressed in the inboard -// frame F, as a function of the mobilizer's generalized positions `q`. -// This pose is computed by CalcAcrossMobilizerTransform(). -// - H_FM(q): -// The `6 x nv` mobilizer hinge matrix `H_FM` relates `V_FM` (outboard -// frame M's spatial velocity in its inboard frame F, expressed in F) to -// the mobilizer's `nv` generalized velocities (or mobilities) `v` as -// `V_FM = H_FM * v`. The method CalcAcrossMobilizerSpatialVelocity() -// calculates `V_FM`. Be aware that Drake's spatial velocities are not the -// Plücker vectors defined in [Featherstone 2008, Ch. 2]. -// Note: `H_FM` is only a function of the `nq` generalized positions `q`. -// - H_FMᵀ(q): -// H_FMᵀ is the `nv x 6` matrix transpose of `H_FM`. It relates the `nv` -// generalized forces `tau` to `F_Mo_F` (the spatial force on frame M at -// point Mo, expressed in F) as `tau = H_FMᵀ ⋅ F_Mo_F` -// The %Mobilizer method ProjectSpatialForce() calculates `tau`. -// Be aware that Drake's spatial forces are not the Plücker vectors defined -// in [Featherstone 2008, Ch. 2]. -// - Hdot_FM(q, v): -// The time derivative of the mobilizer hinge matrix `H_FM` is used in the -// calculation of `A_FM(q, v, v̇)` (outboard frame M's spatial acceleration -// in its inboard frame F, expressed in F) as -// `A_FM(q, v, v̇) = H_FM(q) * v̇ + Ḣ_FM(q, v) * v`. The %Mobilizer method -// CalcAcrossMobilizerSpatialAcceleration() calculates `A_FM`. -// - N(q): -// This `nq x nv` kinematic coupling matrix relates q̇ (the time-derivative -// of the nq mobilizer's generalized positions) to `v` (the mobilizer's -// generalized velocities) as `q̇ = N(q) * v`, [Seth 2010]. -// The %Mobilizer method MapVelocityToQDot() calculates `N(q)`. -// - N⁺(q): -// The left pseudo-inverse of `N(q)`. `N⁺(q)` can be used to invert the -// relationship `q̇ = N(q) * v` without residual error, provided that `q̇` is -// in the range space of `N(q)` (that is, if it *could* have been produced -// as `q̇ = N(q) * v` for some `v`). The application `v = N⁺(q) * q̇` is -// implemented in MapQDotToVelocity(). -// -// In general, `nv != nq`. As an example, consider a quaternion mobilizer that -// would allow frame M to move freely with respect to frame F. For such a -// mobilizer the generalized positions vector might contain a quaternion to -// describe rotations plus a position vector to describe translations. However, -// we might choose the angular velocity `w_FM` and the linear velocity `v_FM` -// as the generalized velocities (or more generally, the spatial velocity -// `V_FM`.) In such a case `nq = 7` (4 dofs for a quaternion plus 3 dofs for a -// position vector) and `nv = 6` (3 dofs for an angular velocity and 3 dofs for -// a linear velocity). -// -// For a detailed discussion on the concept of a mobilizer please refer to -// [Seth 2010]. The mobilizer "hinge" matrix `H_FM(q)` is introduced in -// [Jain 2010], though be aware that what [Jain 2010] calls the hinge matrix is -// the transpose of the mobilizer hinge matrix H_FM matrix here in Drake. -// For details in the monogram notation used above please refer to -// @ref multibody_spatial_algebra. -// -// %Mobilizer is an abstract base class defining the minimum functionality that -// derived %Mobilizer objects must implement in order to fully define the -// kinematic relationship between the two frames they connect. -// -//

Relation between hinge matrix and Jacobians

-// -// The relationship between the across-mobilizer spatial velocity `V_FM` and -// the time derivative of the across-mobilizer transform `X_FM` is similar to -// the relationship between the rigid transform Jacobian Jq_X_VM (partial -// derivatives of rigid transform X_FM with respect to generalized positions q) -// and the Drake mobilizer hinge matrix `H_FM` (partial derivatives of -// across-mobilizer q̇ with respect to generalized velocities v). -// -// The translational velocity v_FM component of the spatial velocity `V_FM` is -// defined as the time derivative of the position vector p_FM in `X_FM`. 
-//   v_FM = dp_FM/dt = ∂p_FM/∂q * q̇ = ∂p_FM/∂q * N(q) * v = Hv_FM * v
-//
-// where `Hv_FM = ∂p_FM/∂q * N(q)` is the last three rows in `H_FM`. -// -// The angular velocity w_FM component of the spatial velocity `V_FM` can be -// related to the time derivative of the rotation matrix R_FM in `X_FM`. This -// complicated relationship can be written in terms of the skew symmetric -// angular velocity matrix [w_FM] as 
-//  [w_FM] = d(R_FM)/dt * (R_FM)ᵀ
-//
-// The ordinary time-derivative of the rotation matrix R_FM is 
-//   d(R_FM)/dt = ∂R/∂q * q̇ = ∂R/∂q * N(q) * v
-//
-// Combining the previous two equations leads to 
-//  [w_FM] = ∂R/∂q * N(q) * v * (R_FM)ᵀ
-//
-// Post-multiplying both sides of the previous equation by R_FM gives 
-//  [w_FM] * R_FM = ∂R/∂q * N(q) * v
-//
-// `Hw_FM` is the first three rows in `H_FM`, defined by context as 
-//  Hw_FM * R_FM = ∂R/∂q * N(q)
-//
-// -//

Active forces and power

-// -// The power generated by a mobilizer can be computed in two equivalent ways. -// That is, the power can be computed in terms of the spatial force `F_Mo` and -// the spatial velocity `V_FM` as: 
-//   P = F_Moᵀ * V_FM
-//
-// or in terms of the generalized forces `tau = H_FMᵀ(q) ⋅ F_Mo` and the -// generalized velocities v as: 
-//   P = tauᵀ * v
-//
-// Notice that spatial forces in the null space of `H_FM(q)` do not perform any -// work. Since the result from the previous two expressions must be equal, the -// mobilizer hinge matrix `H_FM(q)` and its transpose `H_FMᵀ(q)` are -// constrained by: 
-//   (H_FMᵀ(q) * F) * v = Fᵀ * (H_FM(q) * v), ∀ v ∈ ℝⁿᵛ ∧ `F ∈ F⁶`
-//
-// Therefore, this enforces a relationship to the operations implemented by -// CalcAcrossMobilizerSpatialVelocity() and ProjectSpatialForce() for any -// %Mobilizer object. -// -// - [Jain 2010] Jain, A., 2010. Robot and multibody dynamics: analysis and -// algorithms. Springer Science & Business Media. -// - [Seth 2010] Seth, A., Sherman, M., Eastman, P. and Delp, S., 2010. -// Minimal formulation of joint motion for biomechanisms. -// Nonlinear dynamics, 62(1), pp.291-303. -// - [Sciavicco 2000] Sciavicco, L. and Siciliano, B., 2000. Modelling and -// control of robot manipulators, 2nd Edn. Springer. -// - [Featherstone 2008] Featherstone, R., 2008. Rigid body dynamics -// algorithms. Springer. -// -// @tparam_default_scalar +/* %MobilizedBody represents the mobility of an outboard body B with respect to +its inboard (parent) body P in the multibody system's spanning tree. It +consists of computationally-relevant information about a MultibodyPlant Body, +and a Mobilizer that is the computational representation of one of the plant's +Joints and provides mobility (degrees of freedom) for the Body. + +A Mobilizer connects a frame F ("fixed") on the inboard body to a frame M +("moving") on the outboard body. Consider the following example to build a +simple pendulum system: + +@code +MultibodyTree model; +// ... Code here to setup quantities below as mass, com, X_BM, etc. ... +const Body& pendulum = + model.AddBody(SpatialInertia(mass, com, unit_inertia)); +// We will connect the pendulum (outboard) body B to the world frame W +// (world is the inboard body here) using a RevoluteMobilizer. To do so we +// define a pin frame M rigidly attached to the pendulum body. +FixedOffsetFrame& pin_frame = + model.AddFrame( + pendulum.body_frame(), + X_BM // pose of pin frame M in body frame B + ); +// The mobilizer connects the world frame and the pin frame effectively +// adding the single degree of freedom describing this system. In this +// regard, the role of a mobilizer is equivalent but conceptually +// different than a set of constraints that effectively remove all degrees +// of freedom but the one permitting rotation about the z-axis. +const RevoluteMobilizer& revolute_mobilizer = + model.AddMobilizedBody( + model.world_frame(), // inboard frame F (= W) + pin_frame, // outboard frame M + Vector3d::UnitZ() // revolute axis in this case + )); +@endcode + +

Tree Structure

+ +A %MobilizedBody induces a tree structure within a MultibodyTree +model, connecting an inboard (topologically closer to the world) frame to an +outboard (topologically further from the world) frame. Every time a +%MobilizedBody is added to a MultibodyTree (using the +MultibodyTree::AddMobilizedBody() method), a number of degrees of +freedom associated with the particular type of %MobilizedBody are added to +the multibody system. In the example above for the single pendulum, adding a +RevoluteMobilizer has two purposes: + +- It defines the tree structure of the model. World is the inboard body + while "pendulum" is the outboard body in the MultibodyTree. +- It informs the MultibodyTree of the degrees of freedom granted by the + revolute mobilizer between the two frames it connects. +- It defines a permissible motion space spanned by the generalized + coordinates introduced by the mobilizer. + +

Mathematical Description of a %MobilizedBody

+ +A %MobilizedBody describes the kinematics relationship between an inboard +frame F and an outboard frame M, introducing an nq-dimensional vector of +generalized coordinates q and an nv-dimensional vector of generalized +velocities v. Notice that in general `nq != nv`, though `nq == nv` is a very +common case. The kinematic relationships introduced by a %MobilizedBody are +fully specified by [Seth 2010]. The monogram notation used below for X_FM, +V_FM, F_Mo_F, etc., are described in @ref multibody_frames_and_bodies. + +- X_FM(q): + The outboard frame M's pose as measured and expressed in the inboard + frame F, as a function of the mobilizer's generalized positions `q`. + This pose is computed by CalcAcrossMobilizerTransform(). +- H_FM(q): + The `6 x nv` mobilizer hinge matrix `H_FM` relates `V_FM` (outboard + frame M's spatial velocity in its inboard frame F, expressed in F) to + the mobilizer's `nv` generalized velocities (or mobilities) `v` as + `V_FM = H_FM * v`. The method CalcAcrossMobilizerSpatialVelocity() + calculates `V_FM`. Be aware that Drake's spatial velocities are not the + Plücker vectors defined in [Featherstone 2008, Ch. 2]. + Note: `H_FM` is only a function of the `nq` generalized positions `q`. +- H_FMᵀ(q): + H_FMᵀ is the `nv x 6` matrix transpose of `H_FM`. It relates the `nv` + generalized forces `tau` to `F_Mo_F` (the spatial force on frame M at + point Mo, expressed in F) as `tau = H_FMᵀ ⋅ F_Mo_F` + The %MobilizedBody method ProjectSpatialForce() calculates `tau`. + Be aware that Drake's spatial forces are not the Plücker vectors defined + in [Featherstone 2008, Ch. 2]. +- Hdot_FM(q, v): + The time derivative of the mobilizer hinge matrix `H_FM` is used in the + calculation of `A_FM(q, v, v̇)` (outboard frame M's spatial acceleration + in its inboard frame F, expressed in F) as + `A_FM(q, v, v̇) = H_FM(q) * v̇ + Ḣ_FM(q, v) * v`. The %MobilizedBody method + CalcAcrossMobilizerSpatialAcceleration() calculates `A_FM`. +- N(q): + This `nq x nv` kinematic coupling matrix relates q̇ (the time-derivative + of the nq mobilizer's generalized positions) to `v` (the mobilizer's + generalized velocities) as `q̇ = N(q) * v`, [Seth 2010]. + The %MobilizedBody method MapVelocityToQDot() performs this computation. +- N⁺(q): + The left pseudo-inverse of `N(q)`. `N⁺(q)` can be used to invert the + relationship `q̇ = N(q) * v` without residual error, provided that `q̇` is + in the range space of `N(q)` (that is, if it _could_ have been produced + as `q̇ = N(q) * v` for some `v`). The application `v = N⁺(q) * q̇` is + implemented in MapQDotToVelocity(). + +In general, `nv != nq`. As an example, consider a quaternion mobilizer that +would allow frame M to move freely with respect to frame F. For such a +mobilizer the generalized positions vector might contain a quaternion to +describe rotations plus a position vector to describe translations. However, +we might choose the angular velocity `w_FM` and the linear velocity `v_FM` +as the generalized velocities (or more generally, the spatial velocity +`V_FM`.) In such a case `nq = 7` (4 dofs for a quaternion plus 3 dofs for a +position vector) and `nv = 6` (3 dofs for an angular velocity and 3 dofs for +a linear velocity). + +For a detailed discussion on the concept of a mobilizer please refer to +[Seth 2010]. The mobilizer "hinge" matrix `H_FM(q)` is introduced in +[Jain 2010], though be aware that what [Jain 2010] calls the hinge matrix is +the transpose of the mobilizer hinge matrix H_FM matrix here in Drake. +For details in the monogram notation used above please refer to +@ref multibody_spatial_algebra. + +%MobilizedBody is an abstract base class defining the minimum functionality that +derived %MobilizedBody objects must implement in order to fully define the +kinematic relationship between the two frames they connect. + +

Relation between hinge matrix and Jacobians

+ +The relationship between the across-mobilizer spatial velocity `V_FM` and +the time derivative of the across-mobilizer transform `X_FM` is similar to +the relationship between the rigid transform Jacobian Jq_X_VM (partial +derivatives of rigid transform X_FM with respect to generalized positions q) +and the Drake mobilizer hinge matrix `H_FM` (partial derivatives of +across-mobilizer q̇ with respect to generalized velocities v). + +The translational velocity v_FM component of the spatial velocity `V_FM` is +defined as the time derivative of the position vector p_FM in `X_FM`. 
+  v_FM = dp_FM/dt = ∂p_FM/∂q * q̇ = ∂p_FM/∂q * N(q) * v = Hv_FM * v
+
+where `Hv_FM = ∂p_FM/∂q * N(q)` is the last three rows in `H_FM`. + +The angular velocity w_FM component of the spatial velocity `V_FM` can be +related to the time derivative of the rotation matrix R_FM in `X_FM`. This +complicated relationship can be written in terms of the skew symmetric +angular velocity matrix [w_FM] as 
+ [w_FM] = d(R_FM)/dt * (R_FM)ᵀ
+
+The ordinary time-derivative of the rotation matrix R_FM is 
+  d(R_FM)/dt = ∂R/∂q * q̇ = ∂R/∂q * N(q) * v
+
+Combining the previous two equations leads to 
+ [w_FM] = ∂R/∂q * N(q) * v * (R_FM)ᵀ
+
+Post-multiplying both sides of the previous equation by R_FM gives 
+ [w_FM] * R_FM = ∂R/∂q * N(q) * v
+
+`Hw_FM` is the first three rows in `H_FM`, defined by context as 
+ Hw_FM * R_FM = ∂R/∂q * N(q)
+
+ +

Active forces and power

+ +The power generated by a mobilizer can be computed in two equivalent ways. +That is, the power can be computed in terms of the spatial force `F_Mo` and +the spatial velocity `V_FM` as: 
+  P = F_Moᵀ * V_FM
+
+or in terms of the generalized forces `tau = H_FMᵀ(q) ⋅ F_Mo` and the +generalized velocities v as: 
+  P = tauᵀ * v
+
+Notice that spatial forces in the null space of `H_FM(q)` do not perform any +work. Since the result from the previous two expressions must be equal, the +mobilizer hinge matrix `H_FM(q)` and its transpose `H_FMᵀ(q)` are +constrained by: 
+  (H_FMᵀ(q) * F) * v = Fᵀ * (H_FM(q) * v), ∀ v ∈ ℝⁿᵛ ∧ `F ∈ F⁶`
+
+Therefore, this enforces a relationship to the operations implemented by +CalcAcrossMobilizerSpatialVelocity() and ProjectSpatialForce() for any +%MobilizedBody object. + +- [Jain 2010] Jain, A., 2010. Robot and multibody dynamics: analysis and + algorithms. Springer Science & Business Media. +- [Seth 2010] Seth, A., Sherman, M., Eastman, P. and Delp, S., 2010. + Minimal formulation of joint motion for biomechanisms. + Nonlinear dynamics, 62(1), pp.291-303. +- [Sciavicco 2000] Sciavicco, L. and Siciliano, B., 2000. Modelling and + control of robot manipulators, 2nd Edn. Springer. +- [Featherstone 2008] Featherstone, R., 2008. Rigid body dynamics + algorithms. Springer. + +@tparam_default_scalar */ template -class Mobilizer : public MultibodyElement { +class MobilizedBody : public MultibodyElement { public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Mobilizer) + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MobilizedBody) - // The minimum amount of information that we need to define a %Mobilizer is - // the knowledge of the inboard and outboard frames it connects. - // Subclasses of %Mobilizer are therefore required to provide this + // The minimum amount of information that we need to define a %MobilizedBody + // is the inboard and outboard frames it connects. + // Subclasses of %MobilizedBody are therefore required to provide this // information in their respective constructors. // @throws std::exception if `inboard_frame` and `outboard_frame` // reference the same frame object. - Mobilizer(const Frame& inboard_frame, - const Frame& outboard_frame) : + MobilizedBody(const Frame& inboard_frame, + const Frame& outboard_frame) : inboard_frame_(inboard_frame), outboard_frame_(outboard_frame) { // Verify they are not the same frame. if (&inboard_frame == &outboard_frame) { @@ -229,9 +234,11 @@ class Mobilizer : public MultibodyElement { } } + virtual ~MobilizedBody() = default; + /// Returns this element's unique index. - MobilizerIndex index() const { - return this->template index_impl(); + MobilizedBodyIndex index() const { + return this->template index_impl(); } // Returns the number of generalized coordinates granted by this mobilizer. @@ -336,7 +343,7 @@ class Mobilizer : public MultibodyElement { // bookkeeping detail. const MobilizerTopology& get_topology() const { return topology_; } - // @name Methods that define a %Mobilizer + // @name Methods that define a %MobilizedBody // @{ // Sets the `state` to what will be considered to be the _zero_ state @@ -373,28 +380,27 @@ class Mobilizer : public MultibodyElement { // the zero state (it is where our joint angles measure zero). But we also // support a default state (perhaps a more comfortable initial configuration // of the IIWA), which need not be the zero state, that describes a state of - // the Mobilizer to be used in e.g. MultibodyPlant::SetDefaultContext(). + // the MobilizedBody to be used in e.g. MultibodyPlant::SetDefaultContext(). virtual void set_default_state(const systems::Context& context, systems::State* state) const = 0; // Sets the `state` to a (potentially) random position and velocity, by // evaluating any random distributions that were declared (via e.g. - // MobilizerImpl::set_random_position_distribution() and/or - // MobilizerImpl::set_random_velocity_distribution(), or calling + // MobilizedBodyImpl::set_random_position_distribution() and/or + // MobilizedBodyImpl::set_random_velocity_distribution(), or calling // set_zero_state() if none have been declared. Note that the intended // caller of this method is `MultibodyTree::SetRandomState()` which treats - // the independent samples returned from this sample as an initial guess, - // but may change the value in order to "project" it onto a constraint - // manifold. + // the independent samples returned from this sample as an initial guess, but + // may change the value in order to "project" it onto a constraint manifold. virtual void set_random_state(const systems::Context& context, systems::State* state, RandomGenerator* generator) const = 0; // Computes the across-mobilizer transform `X_FM(q)` between the inboard // frame F and the outboard frame M as a function of the vector of - // generalized positions `q`. - // %Mobilizer subclasses implementing this method can retrieve the fixed-size - // vector of generalized positions for `this` mobilizer from `context` with: + // generalized positions `q`. %MobilizedBody subclasses implementing this + // method can retrieve the fixed-size vector of generalized positions for + // `this` mobilizer from `context` with: // // @code // auto q = this->get_positions(context); @@ -628,7 +634,7 @@ class Mobilizer : public MultibodyElement { // needed. // @sa MultibodyTree::CloneToScalar() template - std::unique_ptr> CloneToScalar( + std::unique_ptr> CloneToScalar( const MultibodyTree& cloned_tree) const { return DoCloneToScalar(cloned_tree); } @@ -636,7 +642,7 @@ class Mobilizer : public MultibodyElement { // For MultibodyTree internal use only. virtual std::unique_ptr> CreateBodyNode( const internal::BodyNode* parent_node, - const Body* body, const Mobilizer* mobilizer) const = 0; + const Body* body, const MobilizedBody* mobilizer) const = 0; /// Lock the mobilizer. Its generalized velocities will be 0 until it is /// unlocked. Locking is not yet supported for continuous-mode systems. @@ -689,21 +695,21 @@ class Mobilizer : public MultibodyElement { // inboard and outboard frames of the mobilizer being cloned. // @{ - // Clones this %Mobilizer (templated on T) to a mobilizer templated on + // Clones this %MobilizedBody (templated on T) to a mobilizer templated on // `double`. // @pre Inboard and outboard frames for this mobilizer already have a clone // in `tree_clone`. - virtual std::unique_ptr> DoCloneToScalar( + virtual std::unique_ptr> DoCloneToScalar( const MultibodyTree& tree_clone) const = 0; - // Clones this %Mobilizer (templated on T) to a mobilizer templated on + // Clones this %MobilizedBody (templated on T) to a mobilizer templated on // AutoDiffXd. // @pre Inboard and outboard frames for this mobilizer already have a clone // in `tree_clone`. - virtual std::unique_ptr> DoCloneToScalar( + virtual std::unique_ptr> DoCloneToScalar( const MultibodyTree& tree_clone) const = 0; - virtual std::unique_ptr> DoCloneToScalar( + virtual std::unique_ptr> DoCloneToScalar( const MultibodyTree& tree_clone) const = 0; // @} diff --git a/multibody/tree/mobilizer_impl.cc b/multibody/tree/mobilized_body_impl.cc similarity index 63% rename from multibody/tree/mobilizer_impl.cc rename to multibody/tree/mobilized_body_impl.cc index 60a2e17bc0b0..5e4e9f2aee0e 100644 --- a/multibody/tree/mobilizer_impl.cc +++ b/multibody/tree/mobilized_body_impl.cc @@ -1,4 +1,4 @@ -#include "drake/multibody/tree/mobilizer_impl.h" +#include "drake/multibody/tree/mobilized_body_impl.h" #include @@ -8,29 +8,30 @@ namespace drake { namespace multibody { namespace internal { -template -std::unique_ptr> MobilizerImpl::CreateBodyNode( - const internal::BodyNode* parent_node, - const Body* body, const Mobilizer* mobilizer) const { - return std::make_unique>(parent_node, - body, mobilizer); +template +std::unique_ptr> +MobilizedBodyImpl::CreateBodyNode( + const internal::BodyNode* parent_node, const Body* body, + const MobilizedBody* mobilizer) const { + return std::make_unique>(parent_node, body, + mobilizer); } // Helper classes to aid the explicit instantiation with macro // DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(), which // expects a template class as an argument. template -class MobilizerImpl0x0 : public MobilizerImpl {}; +class MobilizerImpl0x0 : public MobilizedBodyImpl {}; template -class MobilizerImpl1x1 : public MobilizerImpl {}; +class MobilizerImpl1x1 : public MobilizedBodyImpl {}; template -class MobilizerImpl2x2 : public MobilizerImpl {}; +class MobilizerImpl2x2 : public MobilizedBodyImpl {}; template -class MobilizerImpl3x3 : public MobilizerImpl {}; +class MobilizerImpl3x3 : public MobilizedBodyImpl {}; template -class MobilizerImpl6x6 : public MobilizerImpl {}; +class MobilizerImpl6x6 : public MobilizedBodyImpl {}; template -class MobilizerImpl7x6 : public MobilizerImpl {}; +class MobilizerImpl7x6 : public MobilizedBodyImpl {}; } // namespace internal } // namespace multibody diff --git a/multibody/tree/mobilizer_impl.h b/multibody/tree/mobilized_body_impl.h similarity index 87% rename from multibody/tree/mobilizer_impl.h rename to multibody/tree/mobilized_body_impl.h index c00a2e36137c..328447e81eb8 100644 --- a/multibody/tree/mobilizer_impl.h +++ b/multibody/tree/mobilized_body_impl.h @@ -10,7 +10,7 @@ #include "drake/common/eigen_types.h" #include "drake/common/random.h" #include "drake/multibody/tree/frame.h" -#include "drake/multibody/tree/mobilizer.h" +#include "drake/multibody/tree/mobilized_body.h" #include "drake/multibody/tree/multibody_element.h" #include "drake/multibody/tree/multibody_tree_indexes.h" #include "drake/multibody/tree/multibody_tree_topology.h" @@ -20,34 +20,34 @@ namespace drake { namespace multibody { namespace internal { -// Base class for specific Mobilizer implementations with the number of +// Base class for specific MobilizedBody implementations with the number of // generalized positions and velocities resolved at compile time as template // parameters. This allows specific mobilizer implementations to only work on // fixed-size Eigen expressions therefore allowing for optimized operations on // fixed-size matrices. In addition, this layer discourages the proliferation // of dynamic-sized Eigen matrices that would otherwise lead to run-time // dynamic memory allocations. -// %MobilizerImpl also provides a number of size specific methods to retrieve -// multibody quantities of interest from caching structures. These are common -// to all mobilizer implementations and therefore they live in this class. -// Users should not need to interact with this class directly unless they need -// to implement a custom Mobilizer class. +// %MobilizedBodyImpl also provides a number of size specific methods to +// retrieve multibody quantities of interest from caching structures. These are +// common to all mobilizer implementations and therefore they live in this +// class. Users should not need to interact with this class directly unless they +// need to implement a custom MobilizedBody class. // // @tparam_default_scalar template -class MobilizerImpl : public Mobilizer { +class MobilizedBodyImpl : public MobilizedBody { public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MobilizerImpl) + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MobilizedBodyImpl) - // As with Mobilizer this the only constructor available for this base class. - // The minimum amount of information that we need to define a mobilizer is - // the knowledge of the inboard and outboard frames it connects. - // Subclasses of %MobilizerImpl are therefore forced to provide this + // As with MobilizedBody this the only constructor available for this base + // class. The minimum amount of information that we need to define a mobilizer + // is the inboard and outboard frames it connects. + // Subclasses of %MobilizedBodyImpl are therefore forced to provide this // information in their respective constructors. - MobilizerImpl(const Frame& inboard_frame, - const Frame& outboard_frame) : - Mobilizer(inboard_frame, outboard_frame) {} + MobilizedBodyImpl(const Frame& inboard_frame, + const Frame& outboard_frame) : + MobilizedBody(inboard_frame, outboard_frame) {} // Returns the number of generalized coordinates granted by this mobilizer. int num_positions() const final { return kNq;} @@ -55,30 +55,30 @@ class MobilizerImpl : public Mobilizer { // Returns the number of generalized velocities granted by this mobilizer. int num_velocities() const final { return kNv;} - // Sets the elements of the `state` associated with this Mobilizer to the - // _zero_ state. See Mobilizer::set_zero_state(). + // Sets the elements of the `state` associated with this MobilizedBody to the + // _zero_ state. See MobilizedBody::set_zero_state(). void set_zero_state(const systems::Context&, systems::State* state) const final { get_mutable_positions(state) = get_zero_position(); get_mutable_velocities(state).setZero(); }; - // Sets the elements of the `state` associated with this Mobilizer to the - // _default_ state. See Mobilizer::set_default_state(). + // Sets the elements of the `state` associated with this MobilizedBody to the + // _default_ state. See MobilizedBody::set_default_state(). void set_default_state(const systems::Context&, systems::State* state) const final { get_mutable_positions(&*state) = get_default_position(); get_mutable_velocities(&*state).setZero(); }; - // Sets the default position of this Mobilizer to be used in subsequent + // Sets the default position of this MobilizedBody to be used in subsequent // calls to set_default_state(). void set_default_position(const Eigen::Ref>& position) { default_position_.emplace(position); } - // Sets the elements of the `state` associated with this Mobilizer to a + // Sets the elements of the `state` associated with this MobilizedBody to a // _random_ state. If no random distribution has been set, then `state` is // set to the _default_ state. void set_random_state(const systems::Context& context, @@ -129,7 +129,7 @@ class MobilizerImpl : public Mobilizer { // For MultibodyTree internal use only. std::unique_ptr> CreateBodyNode( const internal::BodyNode* parent_node, - const Body* body, const Mobilizer* mobilizer) const final; + const Body* body, const MobilizedBody* mobilizer) const final; protected: // Handy enum to grant specific implementations compile time sizes. diff --git a/multibody/tree/model_instance.cc b/multibody/tree/model_instance.cc index 911c33b6436b..69acd2dffae8 100644 --- a/multibody/tree/model_instance.cc +++ b/multibody/tree/model_instance.cc @@ -64,7 +64,7 @@ void ModelInstance::GetPositionsFromArray( if (q_out->size() != num_positions_) throw std::logic_error("Output array is not properly sized."); int position_offset = 0; - for (const Mobilizer* mobilizer : mobilizers_) { + for (const MobilizedBody* mobilizer : mobilizers_) { const int mobilizer_positions = mobilizer->num_positions(); q_out->segment(position_offset, mobilizer_positions) = mobilizer->get_positions_from_array(q); @@ -83,7 +83,7 @@ void ModelInstance::SetPositionsInArray( throw std::logic_error("Passed in array(s) is not properly sized."); } int position_offset = 0; - for (const Mobilizer* mobilizer : mobilizers_) { + for (const MobilizedBody* mobilizer : mobilizers_) { const int mobilizer_positions = mobilizer->num_positions(); q_array->segment(mobilizer->position_start_in_q(), mobilizer_positions) = model_q.segment(position_offset, mobilizer_positions); @@ -111,7 +111,7 @@ void ModelInstance::GetVelocitiesFromArray( if (v_out->size() != num_velocities_) throw std::logic_error("Output array is not properly sized."); int velocity_offset = 0; - for (const Mobilizer* mobilizer : mobilizers_) { + for (const MobilizedBody* mobilizer : mobilizers_) { const int mobilizer_velocities = mobilizer->num_velocities(); v_out->segment(velocity_offset, mobilizer_velocities) = mobilizer->get_velocities_from_array(v); @@ -128,7 +128,7 @@ void ModelInstance::SetVelocitiesInArray( DRAKE_DEMAND(v_array->size() == this->get_parent_tree().num_velocities()); DRAKE_DEMAND(model_v.size() == num_velocities()); int velocity_offset = 0; - for (const Mobilizer* mobilizer : mobilizers_) { + for (const MobilizedBody* mobilizer : mobilizers_) { const int mobilizer_velocities = mobilizer->num_velocities(); v_array->segment(mobilizer->velocity_start_in_v(), mobilizer_velocities) = model_v.segment(velocity_offset, mobilizer_velocities); diff --git a/multibody/tree/model_instance.h b/multibody/tree/model_instance.h index d7de7f5316d3..9d0664a9c461 100644 --- a/multibody/tree/model_instance.h +++ b/multibody/tree/model_instance.h @@ -6,7 +6,7 @@ #include "drake/common/drake_copyable.h" #include "drake/common/eigen_types.h" #include "drake/multibody/tree/joint_actuator.h" -#include "drake/multibody/tree/mobilizer.h" +#include "drake/multibody/tree/mobilized_body.h" #include "drake/multibody/tree/multibody_element.h" #include "drake/multibody/tree/multibody_tree_indexes.h" @@ -43,7 +43,7 @@ namespace multibody { /// * Frame: Same as the associated Body /// * Joint: Same as the child frame /// * JointActuator: Same as the associated Joint -/// * Mobilizer: +/// * MobilizedBody: /// * Same as the joint (if created from a joint) /// * Same as the body (for free floating bodies) /// * When creating mobilizers for other reasons (e.g. for unit tests) the @@ -72,7 +72,7 @@ class ModelInstance : public MultibodyElement { int num_states() const { return num_positions_ + num_velocities_; } int num_actuated_dofs() const { return num_actuated_dofs_; } - void add_mobilizer(const Mobilizer* mobilizer) { + void add_mobilizer(const MobilizedBody* mobilizer) { num_positions_ += mobilizer->num_positions(); num_velocities_ += mobilizer->num_velocities(); mobilizers_.push_back(mobilizer); @@ -178,7 +178,7 @@ class ModelInstance : public MultibodyElement { int num_velocities_{0}; int num_actuated_dofs_{0}; - std::vector*> mobilizers_; + std::vector*> mobilizers_; std::vector*> joint_actuators_; }; diff --git a/multibody/tree/multibody_tree-inl.h b/multibody/tree/multibody_tree-inl.h index d364361be2d9..9135a8f1c673 100644 --- a/multibody/tree/multibody_tree-inl.h +++ b/multibody/tree/multibody_tree-inl.h @@ -23,7 +23,7 @@ #include "drake/multibody/tree/frame.h" #include "drake/multibody/tree/joint.h" #include "drake/multibody/tree/joint_actuator.h" -#include "drake/multibody/tree/mobilizer.h" +#include "drake/multibody/tree/mobilized_body.h" #include "drake/multibody/tree/model_instance.h" #include "drake/multibody/tree/multibody_forces.h" #include "drake/multibody/tree/multibody_tree.h" @@ -167,8 +167,8 @@ template template class MobilizerType> const MobilizerType& MultibodyTree::AddMobilizer( std::unique_ptr> mobilizer) { - static_assert(std::is_convertible_v*, Mobilizer*>, - "MobilizerType must be a sub-class of mobilizer."); + static_assert(std::is_convertible_v*, MobilizedBody*>, + "MobilizerType must be a sub-class of MobilizedBody."); if (topology_is_valid()) { throw std::logic_error("This MultibodyTree is finalized already. " "Therefore adding more mobilizers is not allowed. " @@ -185,7 +185,7 @@ const MobilizerType& MultibodyTree::AddMobilizer( mobilizer->outboard_frame().HasThisParentTreeOrThrow(this); const int num_positions = mobilizer->num_positions(); const int num_velocities = mobilizer->num_velocities(); - MobilizerIndex mobilizer_index = topology_.add_mobilizer( + MobilizedBodyIndex mobilizer_index = topology_.add_mobilizer( mobilizer->inboard_frame().index(), mobilizer->outboard_frame().index(), num_positions, num_velocities); @@ -225,8 +225,8 @@ const MobilizerType& MultibodyTree::AddMobilizer( template template class MobilizerType, typename... Args> const MobilizerType& MultibodyTree::AddMobilizer(Args&&... args) { - static_assert(std::is_base_of_v, MobilizerType>, - "MobilizerType must be a sub-class of Mobilizer."); + static_assert(std::is_base_of_v, MobilizerType>, + "MobilizerType must be a sub-class of MobilizedBody."); return AddMobilizer( std::make_unique>(std::forward(args)...)); } @@ -446,13 +446,13 @@ Body* MultibodyTree::CloneBodyAndAdd(const Body& body) { template template -Mobilizer* MultibodyTree::CloneMobilizerAndAdd( - const Mobilizer& mobilizer) { - MobilizerIndex mobilizer_index = mobilizer.index(); +MobilizedBody* MultibodyTree::CloneMobilizerAndAdd( + const MobilizedBody& mobilizer) { + MobilizedBodyIndex mobilizer_index = mobilizer.index(); auto mobilizer_clone = mobilizer.CloneToScalar(*this); mobilizer_clone->set_parent_tree(this, mobilizer_index); mobilizer_clone->set_model_instance(mobilizer.model_instance()); - Mobilizer* raw_mobilizer_clone_ptr = mobilizer_clone.get(); + MobilizedBody* raw_mobilizer_clone_ptr = mobilizer_clone.get(); owned_mobilizers_.push_back(std::move(mobilizer_clone)); return raw_mobilizer_clone_ptr; } diff --git a/multibody/tree/multibody_tree.cc b/multibody/tree/multibody_tree.cc index a39021534c8b..c1850805d460 100644 --- a/multibody/tree/multibody_tree.cc +++ b/multibody/tree/multibody_tree.cc @@ -44,9 +44,9 @@ template class JointImplementationBuilder { public: JointImplementationBuilder() = delete; - static std::vector*> Build( + static std::vector*> Build( Joint* joint, MultibodyTree* tree) { - std::vector*> mobilizers; + std::vector*> mobilizers; std::unique_ptr blue_print = joint->MakeImplementationBlueprint(); auto implementation = std::make_unique(*blue_print); @@ -544,7 +544,7 @@ std::vector MultibodyTree::GetBodiesKinematicallyAffectedBy( // For each joint, get its mobilizer collect the corresponding outboard body. std::vector bodies; for (const JointIndex& joint : joint_indexes) { - const MobilizerIndex mobilizer = get_joint_mobilizer(joint); + const MobilizedBodyIndex mobilizer = get_joint_mobilizer(joint); DRAKE_THROW_UNLESS(mobilizer.is_valid()); bodies.emplace_back(get_mobilizer(mobilizer).outboard_body().index()); } @@ -618,7 +618,7 @@ template void MultibodyTree::CreateJointImplementations() { DRAKE_DEMAND(!topology_is_valid()); // Create Joint objects' implementation. Joints are implemented using a - // combination of MultibodyTree's building blocks such as Body, Mobilizer, + // combination of MultibodyTree's building blocks such as Body, MobilizedBody, // ForceElement and Constraint. For a same physical Joint, several // implementations could be created (for instance, a Constraint instead of a // Mobilizer). The decision on what implementation to create is performed by @@ -633,13 +633,13 @@ void MultibodyTree::CreateJointImplementations() { joint_to_mobilizer_.resize(num_joints_pre_floating_joints); for (int i = 0; i < num_joints_pre_floating_joints; ++i) { auto& joint = owned_joints_[i]; - std::vector*> mobilizers = + std::vector*> mobilizers = internal::JointImplementationBuilder::Build(joint.get(), this); // Below we assume a single mobilizer per joint, which is true // for all joint types currently implemented. This may change in the future // when closed topologies are supported. DRAKE_DEMAND(mobilizers.size() == 1); - for (Mobilizer* mobilizer : mobilizers) { + for (MobilizedBody* mobilizer : mobilizers) { mobilizer->set_model_instance(joint->model_instance()); // Record the joint to mobilizer map. joint_to_mobilizer_[joint->index()] = mobilizer->index(); @@ -665,13 +665,13 @@ void MultibodyTree::CreateJointImplementations() { joint_to_mobilizer_.resize(num_joints()); for (int i = num_joints_pre_floating_joints; i < num_joints(); ++i) { auto& joint = owned_joints_[i]; - std::vector*> mobilizers = + std::vector*> mobilizers = internal::JointImplementationBuilder::Build(joint.get(), this); // Below we assume a single mobilizer per joint, which is true // for all joint types currently implemented. This may change in the future // when closed topologies are supported. DRAKE_DEMAND(mobilizers.size() == 1); - for (Mobilizer* mobilizer : mobilizers) { + for (MobilizedBody* mobilizer : mobilizers) { mobilizer->set_model_instance(joint->model_instance()); // Record the joint to mobilizer map. joint_to_mobilizer_[joint->index()] = mobilizer->index(); @@ -778,7 +778,7 @@ void MultibodyTree::CreateBodyNode(BodyNodeIndex body_node_index) { } else { // The mobilizer should be valid if not at the root (the world). DRAKE_ASSERT(node_topology.mobilizer.is_valid()); - const Mobilizer* mobilizer = + const MobilizedBody* mobilizer = owned_mobilizers_[node_topology.mobilizer].get(); BodyNode* parent_node = @@ -2670,7 +2670,7 @@ void MultibodyTree::CalcJacobianAngularAndOrTranslationalVelocityInWorld( const BodyNodeIndex body_node_index = path_to_world[ilevel]; const BodyNode& node = *body_nodes_[body_node_index]; const BodyNodeTopology& node_topology = node.get_topology(); - const Mobilizer& mobilizer = node.get_mobilizer(); + const MobilizedBody& mobilizer = node.get_mobilizer(); const int start_index_in_v = node_topology.mobilizer_velocities_start_in_v; const int start_index_in_q = node_topology.mobilizer_positions_start; const int mobilizer_num_velocities = @@ -3032,9 +3032,9 @@ void MultibodyTree::ThrowDefaultMassInertiaError() const { const BodyIndex parent_body_index = *welded_body.begin(); const BodyTopology& parent_body_topology = tree_topology.get_body(parent_body_index); - const MobilizerIndex& parent_mobilizer_index = + const MobilizedBodyIndex& parent_mobilizer_index = parent_body_topology.inboard_mobilizer; - const Mobilizer& parent_mobilizer = + const MobilizedBody& parent_mobilizer = get_mobilizer(parent_mobilizer_index); // Check previous assumptions. diff --git a/multibody/tree/multibody_tree.h b/multibody/tree/multibody_tree.h index ce6683da5019..b7c4648047c3 100644 --- a/multibody/tree/multibody_tree.h +++ b/multibody/tree/multibody_tree.h @@ -66,7 +66,7 @@ namespace internal { template class BodyNode; template class ModelInstance; -template class Mobilizer; +template class MobilizedBody; template class QuaternionFloatingMobilizer; // %MultibodyTree provides a representation for a physical system consisting of @@ -304,8 +304,8 @@ class MultibodyTree { // Vector3d::UnitZ() /*revolute axis*/)); // @endcode // - // A %Mobilizer effectively connects the two bodies to which the inboard and - // outboard frames belong. + // A %MobilizedBody effectively connects the two bodies to which the inboard + // and outboard frames belong. // // @throws std::exception if `mobilizer` is a nullptr. // @throws std::exception if Finalize() was already called on `this` tree. @@ -325,8 +325,8 @@ class MultibodyTree { // mobilizer. This reference which will remain valid for the // lifetime of `this` %MultibodyTree. // - // @tparam MobilizerType The type of the specific sub-class of Mobilizer to - // add. The template needs to be specialized on the + // @tparam MobilizerType The type of the specific sub-class of MobilizedBody + // to add. The template needs to be specialized on the // same scalar type T of this %MultibodyTree. template class MobilizerType> const MobilizerType& AddMobilizer( @@ -359,16 +359,16 @@ class MultibodyTree { // @throws std::exception if attempting to connect two bodies with more // than one mobilizer between them. // - // @param[in] args The arguments needed to construct a valid Mobilizer of + // @param[in] args The arguments needed to construct a valid MobilizedBody of // type `MobilizerType`. `MobilizerType` must provide a // public constructor that takes these arguments. // @returns A constant reference of type `MobilizerType` to the created // mobilizer. This reference which will remain valid for the // lifetime of `this` %MultibodyTree. // - // @tparam MobilizerType A template for the type of Mobilizer to construct. - // The template will be specialized on the scalar type - // T of `this` %MultibodyTree. + // @tparam MobilizerType A template for the type of MobilizedBody to + // construct. The template will be specialized on the + // scalar type T of `this` %MultibodyTree. template class MobilizerType, typename... Args> const MobilizerType& AddMobilizer(Args&&... args); @@ -619,7 +619,7 @@ class MultibodyTree { // That is, the number of bodies in the longest kinematic path between the // world and any other leaf body. For a model that only contains the _world_ // body, the height of the tree is one. - // Kinematic paths are created by Mobilizer objects connecting a chain of + // Kinematic paths are created by MobilizedBody objects connecting a chain of // frames. Therefore, this method does not count kinematic cycles, which // could only be considered in the model using constraints. int tree_height() const { @@ -688,12 +688,13 @@ class MultibodyTree { } // See MultibodyPlant method. - const Mobilizer& get_mobilizer(MobilizerIndex mobilizer_index) const { + const MobilizedBody& get_mobilizer( + MobilizedBodyIndex mobilizer_index) const { DRAKE_THROW_UNLESS(mobilizer_index < num_mobilizers()); return *owned_mobilizers_[mobilizer_index]; } - Mobilizer& get_mutable_mobilizer(MobilizerIndex mobilizer_index) { + MobilizedBody& get_mutable_mobilizer(MobilizedBodyIndex mobilizer_index) { DRAKE_THROW_UNLESS(mobilizer_index < num_mobilizers()); return *owned_mobilizers_[mobilizer_index]; } @@ -901,7 +902,7 @@ class MultibodyTree { // Returns the mobilizer model for joint with index `joint_index`. The index // is invalid if the joint is not modeled with a mobilizer. - MobilizerIndex get_joint_mobilizer(JointIndex joint_index) const { + MobilizedBodyIndex get_joint_mobilizer(JointIndex joint_index) const { DRAKE_DEMAND(joint_index < num_joints()); return joint_to_mobilizer_[joint_index]; } @@ -1484,7 +1485,7 @@ class MultibodyTree { // (Advanced) Given the state of `this` %MultibodyTree in `context` and a // known vector of generalized accelerations `vdot`, this method computes the // set of generalized forces `tau` that would need to be applied at each - // Mobilizer in order to attain the specified generalized accelerations. + // MobilizedBody in order to attain the specified generalized accelerations. // Mathematically, this method computes: 
   //   tau = M(q)v̇ + C(q, v)v - tau_app - ∑ J_WBᵀ(q) Fapp_Bo_W
   //
@@ -1516,9 +1517,9 @@ class MultibodyTree { // `context`. // @param[in] known_vdot // A vector with the known generalized accelerations `vdot` for the full - // %MultibodyTree model. Use Mobilizer::get_accelerations_from_array() to - // access entries into this array for a particular Mobilizer. You can use - // the mutable version of this method to write into this array. + // %MultibodyTree model. Use MobilizedBody::get_accelerations_from_array() + // to access entries into this array for a particular MobilizedBody. You + // can use the mutable version of this method to write into this array. // @param[in] Fapplied_Bo_W_array // A vector containing the spatial force `Fapplied_Bo_W` applied on each // body at the body's frame origin `Bo` and expressed in the world frame W. @@ -1532,8 +1533,8 @@ class MultibodyTree { // @param[in] tau_applied_array // An array of applied generalized forces for the entire model. For a // given mobilizer, entries in this array can be accessed using the method - // Mobilizer::get_generalized_forces_from_array() while its mutable - // counterpart, Mobilizer::get_mutable_generalized_forces_from_array(), + // MobilizedBody::get_generalized_forces_from_array() while its mutable + // counterpart, MobilizedBody::get_mutable_generalized_forces_from_array(), // allows writing into this array. // `tau_applied_array` can have zero size, which means there are no applied // forces. To apply non-zero forces, `tau_applied_array` must be of size @@ -1565,8 +1566,8 @@ class MultibodyTree { // applied in order to achieve the desired generalized accelerations given // by the input argument `known_vdot`. It must not be nullptr and it // must be of size MultibodyTree::num_velocities(). Generalized forces - // for each Mobilizer can be accessed with - // Mobilizer::get_generalized_forces_from_array(). + // for each MobilizedBody can be accessed with + // MobilizedBody::get_generalized_forces_from_array(). // // @warning There is no mechanism to assert that either `A_WB_array` nor // `F_BMo_W_array` are ordered by BodyNodeIndex. You can use @@ -1598,7 +1599,7 @@ class MultibodyTree { // Given the state stored in `context` and a // known vector of generalized accelerations `vdot`, this method computes the // set of generalized forces `tau_id` that would need to be applied at each - // Mobilizer in order to attain the specified generalized accelerations. + // MobilizedBody in order to attain the specified generalized accelerations. // Mathematically, this method computes: 
   //   tau_id = M(q)v̇ + C(q, v)v - tau_app - ∑ J_WBᵀ(q) Fapp_Bo_W
   //
@@ -2147,17 +2148,17 @@ class MultibodyTree { return get_body_variant(element); } - // SFINAE overload for Mobilizer elements. + // SFINAE overload for MobilizedBody elements. template