[multibody] Implements MultibodyTreeSystem::SetDefaultParameters (Rob…

…otLocomotion#20307)

Sets up a framework within MultibodyTree/MultibodyPlant to have
all MultibodyElements fill in their default model parameter values
upon default context creation. This change means that model default
values can now change post-Finalize, and those changes will be
reflected in new default contexts.

multibody/plant/test/multibody_plant_reflected_inertia_test.cc

@@ -597,6 +597,57 @@ TEST_F(MultibodyPlantReflectedInertiaTests, ScalarConversion) {
                               MatrixCompareType::relative));
 }
 
+// Test that default parameters can be changed after finalize and end up in new
+// default contexts.
+TEST_F(MultibodyPlantReflectedInertiaTests, DefaultParameters) {
+  // Arbitrary reflected inertia values.
+  VectorX<double> rotor_inertias(kNumJoints);
+  VectorX<double> gear_ratios(kNumJoints);
+  rotor_inertias << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9;
+  gear_ratios << 1, 2, 4, 8, 16, 32, 64, 128, 256;
+
+  // Load the models.
+  LoadBothModelsSetStateAndFinalize(rotor_inertias, gear_ratios);
+
+  for (JointActuatorIndex index(0); index < plant_ri_.num_actuators();
+       ++index) {
+    JointActuator<double>& joint_actuator =
+        dynamic_cast<JointActuator<double>&>(
+            plant_ri_.get_mutable_joint_actuator(JointActuatorIndex(index)));
+
+    // Default gear ratio and rotor inertia end up in the context.
+    EXPECT_EQ(joint_actuator.default_gear_ratio(),
+              joint_actuator.gear_ratio(*context_ri_));
+    EXPECT_EQ(joint_actuator.default_rotor_inertia(),
+              joint_actuator.rotor_inertia(*context_ri_));
+  }
+
+  for (JointActuatorIndex index(0); index < plant_ri_.num_actuators();
+       ++index) {
+    JointActuator<double>& joint_actuator =
+        dynamic_cast<JointActuator<double>&>(
+            plant_ri_.get_mutable_joint_actuator(JointActuatorIndex(index)));
+    // Set the model parameters to something different.
+    joint_actuator.set_default_gear_ratio(99);
+    joint_actuator.set_default_rotor_inertia(100);
+  }
+
+  // Create a new default context.
+  auto context = plant_ri_.CreateDefaultContext();
+
+  for (JointActuatorIndex index(0); index < plant_ri_.num_actuators();
+       ++index) {
+    JointActuator<double>& joint_actuator =
+        dynamic_cast<JointActuator<double>&>(
+            plant_ri_.get_mutable_joint_actuator(JointActuatorIndex(index)));
+    // New default values should propagate to a new default context.
+    EXPECT_EQ(joint_actuator.default_gear_ratio(),
+              joint_actuator.gear_ratio(*context));
+    EXPECT_EQ(joint_actuator.default_rotor_inertia(),
+              joint_actuator.rotor_inertia(*context));
+  }
+}
+
 }  // namespace
 }  // namespace multibody
 }  // namespace drake

multibody/tree/body.h

@@ -478,6 +478,14 @@ class Body : public MultibodyElement<T> {
         name_(internal::DeprecateWhenEmptyName(name, "Body")),
         body_frame_(*this) {}
 
+  /// Called by DoDeclareParameters(). Derived classes may choose to override
+  /// to declare their sub-class specific parameters.
+  virtual void DoDeclareBodyParameters(internal::MultibodyTreeSystem<T>*) {}
+
+  /// Called by DoSetDefaultParameters(). Derived classes may choose to override
+  /// to set their sub-class specific parameters.
+  virtual void DoSetDefaultBodyParameters(systems::Parameters<T>*) const {}
+
   /// @name Methods to make a clone templated on different scalar types.
   ///
   /// These methods are meant to be called by MultibodyTree::CloneToScalar()
@@ -533,6 +541,17 @@ class Body : public MultibodyElement<T> {
     body_frame_.SetTopology(tree_topology);
   }
 
+  // Implementation for MultibodyElement::DoDeclareParameters().
+  void DoDeclareParameters(
+      internal::MultibodyTreeSystem<T>* tree_system) final {
+    DoDeclareBodyParameters(tree_system);
+  }
+
+  // Implementation for MultibodyElement::DoSetDefaultParameters().
+  void DoSetDefaultParameters(systems::Parameters<T>* parameters) const final {
+    DoSetDefaultBodyParameters(parameters);
+  }
+
   // MultibodyTree has access to the mutable BodyFrame through BodyAttorney.
   BodyFrame<T>& get_mutable_body_frame() {
     return body_frame_;

multibody/tree/fixed_offset_frame.h

@@ -147,16 +147,26 @@ class FixedOffsetFrame final : public Frame<T> {
   /// @}
 
  private:
-  // Implementation for MultibodyElement::DoDeclareParameters().
+  // Implementation for Frame::DoDeclareFrameParameters().
   // FixedOffsetFrame declares a single parameter for its RigidTransform.
-  void DoDeclareParameters(
+  void DoDeclareFrameParameters(
       internal::MultibodyTreeSystem<T>* tree_system) final {
-    // Declare parent classes' parameters
-    Frame<T>::DoDeclareParameters(tree_system);
-    X_PF_parameter_index_ = this->DeclareNumericParameter(
-        tree_system,
-        systems::BasicVector<T>(Eigen::Map<const VectorX<T>>(
-            X_PF_.template cast<T>().GetAsMatrix34().data(), 12, 1)));
+    // Model value of this transform is set to a dummy value to indicate that
+    // the model value is not used. This class stores the default value and
+    // sets it in DoSetDefaultFrameParameters().
+    X_PF_parameter_index_ =
+        this->DeclareNumericParameter(tree_system, systems::BasicVector<T>(12));
+  }
+
+  // Implementation for Frame::DoSetDefaultFrameParameters().
+  // FixedOffsetFrame sets a single parameter for its RigidTransform.
+  void DoSetDefaultFrameParameters(
+      systems::Parameters<T>* parameters) const final {
+    // Set default rigid transform between P and F.
+    systems::BasicVector<T>& X_PF_parameter =
+        parameters->get_mutable_numeric_parameter(X_PF_parameter_index_);
+    X_PF_parameter.set_value(Eigen::Map<const VectorX<T>>(
+        X_PF_.template cast<T>().GetAsMatrix34().data(), 12, 1));
   }
 
   // Helper method to make a clone templated on ToScalar.

multibody/tree/force_element.h

@@ -183,6 +183,16 @@ class ForceElement : public MultibodyElement<T> {
       const internal::VelocityKinematicsCache<T>& vc,
       MultibodyForces<T>* forces) const = 0;
 
+  /// Called by DoDeclareParameters(). Derived classes may choose to override
+  /// to declare their sub-class specific parameters.
+  virtual void DoDeclareForceElementParameters(
+      internal::MultibodyTreeSystem<T>*) {}
+
+  /// Called by DoSetDefaultParameters(). Derived classes may choose to override
+  /// to set their sub-class specific parameters.
+  virtual void DoSetDefaultForceElementParameters(
+      systems::Parameters<T>*) const {}
+
   /// @name Methods to make a clone templated on different scalar types.
   ///
   /// Specific force element subclasses must implement these to support scalar
@@ -252,6 +262,17 @@ class ForceElement : public MultibodyElement<T> {
   // At MultibodyTree::Finalize() time, each force element retrieves its
   // topology from the parent MultibodyTree.
   void DoSetTopology(const internal::MultibodyTreeTopology&) final {}
+
+  // Implementation for MultibodyElement::DoDeclareParameters().
+  void DoDeclareParameters(
+      internal::MultibodyTreeSystem<T>* tree_system) final {
+    DoDeclareForceElementParameters(tree_system);
+  }
+
+  // Implementation for MultibodyElement::DoSetDefaultParameters().
+  void DoSetDefaultParameters(systems::Parameters<T>* parameters) const final {
+    DoSetDefaultForceElementParameters(parameters);
+  }
 };
 
 }  // namespace multibody

multibody/tree/frame.h

@@ -601,6 +601,14 @@ class Frame : public FrameBase<T> {
         name_(internal::DeprecateWhenEmptyName(name, "Frame")),
         body_(body) {}
 
+  /// Called by DoDeclareParameters(). Derived classes may choose to override
+  /// to declare their sub-class specific parameters.
+  virtual void DoDeclareFrameParameters(internal::MultibodyTreeSystem<T>*) {}
+
+  /// Called by DoSetDefaultParameters(). Derived classes may choose to override
+  /// to set their sub-class specific parameters.
+  virtual void DoSetDefaultFrameParameters(systems::Parameters<T>*) const {}
+
   /// @name Methods to make a clone templated on different scalar types.
   ///
   /// These methods are meant to be called by MultibodyTree::CloneToScalar()
@@ -633,6 +641,17 @@ class Frame : public FrameBase<T> {
     DRAKE_ASSERT(topology_.index == this->index());
   }
 
+  // Implementation for MultibodyElement::DoDeclareParameters().
+  void DoDeclareParameters(
+      internal::MultibodyTreeSystem<T>* tree_system) final {
+    DoDeclareFrameParameters(tree_system);
+  }
+
+  // Implementation for MultibodyElement::DoSetDefaultParameters().
+  void DoSetDefaultParameters(systems::Parameters<T>* parameters) const final {
+    DoSetDefaultFrameParameters(parameters);
+  }
+
   const std::string name_;
 
   // The body associated with this frame.

multibody/tree/joint_actuator.h

@@ -348,13 +348,25 @@ class JointActuator final : public MultibodyElement<T> {
   // Implementation for MultibodyElement::DoDeclareParameters().
   void DoDeclareParameters(
       internal::MultibodyTreeSystem<T>* tree_system) final {
-    // Declare parent classes' parameters
-    MultibodyElement<T>::DoDeclareParameters(tree_system);
-    rotor_inertia_parameter_index_ = this->DeclareNumericParameter(
-        tree_system,
-        systems::BasicVector<T>(Vector1<T>(default_rotor_inertia_)));
-    gear_ratio_parameter_index_ = this->DeclareNumericParameter(
-        tree_system, systems::BasicVector<T>(Vector1<T>(default_gear_ratio_)));
+    // Sets model values to dummy values to indicate that the model values are
+    // not used. This class stores the the default values of the parameters.
+    rotor_inertia_parameter_index_ =
+        this->DeclareNumericParameter(tree_system, systems::BasicVector<T>(1));
+    gear_ratio_parameter_index_ =
+        this->DeclareNumericParameter(tree_system, systems::BasicVector<T>(1));
+  }
+
+  // Implementation for MultibodyElement::DoSetDefaultParameters().
+  void DoSetDefaultParameters(systems::Parameters<T>* parameters) const final {
+    // Set the default rotor inertia.
+    systems::BasicVector<T>& rotor_inertia_parameter =
+        parameters->get_mutable_numeric_parameter(
+            rotor_inertia_parameter_index_);
+    rotor_inertia_parameter.set_value(Vector1<T>(default_rotor_inertia_));
+    // Set the default gear ratio.
+    systems::BasicVector<T>& gear_ratio_parameter =
+        parameters->get_mutable_numeric_parameter(gear_ratio_parameter_index_);
+    gear_ratio_parameter.set_value(Vector1<T>(default_gear_ratio_));
   }
 
   // The actuator's unique name in the MultibodyTree model

multibody/tree/linear_bushing_roll_pitch_yaw.h

@@ -503,31 +503,49 @@ class LinearBushingRollPitchYaw final : public ForceElement<T> {
   SpatialForce<T> CalcBushingSpatialForceOnFrameC(
       const systems::Context<T>& context) const;
 
- protected:
-  // Implementation for MultibodyElement::DoDeclareParameters().
-  void DoDeclareParameters(
-      internal::MultibodyTreeSystem<T>* tree_system) override {
-    // Declare parent classes' parameters
-    ForceElement<T>::DoDeclareParameters(tree_system);
-
-    torque_stiffness_parameter_index_ = this->DeclareNumericParameter(
-        tree_system, systems::BasicVector<T>(
-                         torque_stiffness_constants_.template cast<T>()));
-
-    torque_damping_parameter_index_ = this->DeclareNumericParameter(
-        tree_system,
-        systems::BasicVector<T>(torque_damping_constants_.template cast<T>()));
-
-    force_stiffness_parameter_index_ = this->DeclareNumericParameter(
-        tree_system,
-        systems::BasicVector<T>(force_stiffness_constants_.template cast<T>()));
-
-    force_damping_parameter_index_ = this->DeclareNumericParameter(
-        tree_system,
-        systems::BasicVector<T>(force_damping_constants_.template cast<T>()));
+ private:
+  // Implementation for ForceElement::DoDeclareForceElementParameters().
+  void DoDeclareForceElementParameters(
+      internal::MultibodyTreeSystem<T>* tree_system) final {
+    // Sets model values to dummy values to indicate that the model values are
+    // not used. This class stores the the default values of the parameters.
+    torque_stiffness_parameter_index_ =
+        this->DeclareNumericParameter(tree_system, systems::BasicVector<T>(3));
+    torque_damping_parameter_index_ =
+        this->DeclareNumericParameter(tree_system, systems::BasicVector<T>(3));
+    force_stiffness_parameter_index_ =
+        this->DeclareNumericParameter(tree_system, systems::BasicVector<T>(3));
+    force_damping_parameter_index_ =
+        this->DeclareNumericParameter(tree_system, systems::BasicVector<T>(3));
+  }
+
+  // Implementation for ForceElement::DoSetDefaultForceElementParameters().
+  void DoSetDefaultForceElementParameters(
+      systems::Parameters<T>* parameters) const final {
+    // Set the default stiffness and damping parameters.
+    systems::BasicVector<T>& torque_stiffness_parameter =
+        parameters->get_mutable_numeric_parameter(
+            torque_stiffness_parameter_index_);
+    systems::BasicVector<T>& torque_damping_parameter_ =
+        parameters->get_mutable_numeric_parameter(
+            torque_damping_parameter_index_);
+    systems::BasicVector<T>& force_stiffness_parameter_ =
+        parameters->get_mutable_numeric_parameter(
+            force_stiffness_parameter_index_);
+    systems::BasicVector<T>& force_damping_parameter_ =
+        parameters->get_mutable_numeric_parameter(
+            force_damping_parameter_index_);
+
+    torque_stiffness_parameter.set_value(
+        torque_stiffness_constants_.template cast<T>());
+    torque_damping_parameter_.set_value(
+        torque_damping_constants_.template cast<T>());
+    force_stiffness_parameter_.set_value(
+        force_stiffness_constants_.template cast<T>());
+    force_damping_parameter_.set_value(
+        force_damping_constants_.template cast<T>());
   }
 
- private:
   // Friend class for accessing protected/private internals of this class.
   friend class BushingTester;
 

multibody/tree/mobilizer.h

@@ -704,14 +704,18 @@ class Mobilizer : public MultibodyElement<T> {
 
   // Implementation for MultibodyElement::DoDeclareParameters().
   void DoDeclareParameters(
-      internal::MultibodyTreeSystem<T>* tree_system) override {
-    // Declare parent class's parameters
-    MultibodyElement<T>::DoDeclareParameters(tree_system);
-
+      internal::MultibodyTreeSystem<T>* tree_system) final {
     is_locked_parameter_index_ =
         this->DeclareAbstractParameter(tree_system, Value<bool>(false));
   }
 
+  // Implementation for MultibodyElement::DoDeclareParameters().
+  void DoSetDefaultParameters(systems::Parameters<T>* parameters) const final {
+    // TODO(joemasterjonh): Consider exposing a default locked model value.
+    parameters->template get_mutable_abstract_parameter<bool>(
+        is_locked_parameter_index_) = false;
+  }
+
  private:
   // Implementation for MultibodyElement::DoSetTopology().
   // At MultibodyTree::Finalize() time, each mobilizer retrieves its topology

multibody/tree/multibody_element.cc

@@ -4,6 +4,7 @@ namespace drake {
 namespace multibody {
 
 using internal::MultibodyTreeSystem;
+using systems::Parameters;
 
 template <typename T>
 MultibodyElement<T>::~MultibodyElement() {}
@@ -15,6 +16,13 @@ void MultibodyElement<T>::DeclareParameters(
   DoDeclareParameters(tree_system);
 }
 
+template <typename T>
+void MultibodyElement<T>::SetDefaultParameters(
+    systems::Parameters<T>* parameters) const {
+  DRAKE_DEMAND(parameters != nullptr);
+  DoSetDefaultParameters(parameters);
+}
+
 template <typename T>
 MultibodyElement<T>::MultibodyElement() {}
 
@@ -25,6 +33,9 @@ MultibodyElement<T>::MultibodyElement(ModelInstanceIndex model_instance)
 template <typename T>
 void MultibodyElement<T>::DoDeclareParameters(MultibodyTreeSystem<T>*) {}
 
+template <typename T>
+void MultibodyElement<T>::DoSetDefaultParameters(Parameters<T>*) const {}
+
 template <typename T>
 systems::NumericParameterIndex MultibodyElement<T>::DeclareNumericParameter(
     MultibodyTreeSystem<T>* tree_system,

multibody/tree/multibody_element.h

@@ -68,6 +68,12 @@ class MultibodyElement {
   /// returned from GetParentTreeSystem()).
   void DeclareParameters(internal::MultibodyTreeSystem<T>* tree_system);
 
+  /// Sets default values of parameters belonging to each %MultibodyElement in
+  /// `parameters` at a call to MultibodyTreeSystem::SetDefaultParameters().
+  /// @param[out] parameters A mutable collections of parameters in a context.
+  /// @pre parameters != nullptr
+  void SetDefaultParameters(systems::Parameters<T>* parameters) const;
+
  protected:
   /// Default constructor made protected so that sub-classes can still declare
   /// their default constructors if they need to.
@@ -109,11 +115,13 @@ class MultibodyElement {
   virtual void DoSetTopology(const internal::MultibodyTreeTopology& tree) = 0;
 
   /// Implementation of the NVI DeclareParameters(). MultibodyElement-derived
-  /// objects must override to declare their specific parameters. If an object
-  /// is not a direct descendent of MultibodyElement, it must invoke its parent
-  /// classes' DoDeclareParameters() before declaring its own parameters.
+  /// objects may override to declare their specific parameters.
   virtual void DoDeclareParameters(internal::MultibodyTreeSystem<T>*);
 
+  /// Implementation of the NVI SetDefaultParameters(). MultibodyElement-derived
+  /// objects may override to set default values of their specific parameters.
+  virtual void DoSetDefaultParameters(systems::Parameters<T>*) const;
+
   /// To be used by MultibodyElement-derived objects when declaring parameters
   /// in their implementation of DoDeclareParameters(). For an example, see
   /// RigidBody::DoDeclareParameters().