Make joint store default value (#13105)

This is a PR to have the Joint class store a default value and to pass the value to its mobilizer implementation upon construction of the joint implementation.

Solves the issue of mobilizers storing default joint positions (#13065)

multibody/tree/ball_rpy_joint.cc

@@ -56,9 +56,10 @@ template <typename T>
 std::unique_ptr<typename Joint<T>::BluePrint>
 BallRpyJoint<T>::MakeImplementationBlueprint() const {
   auto blue_print = std::make_unique<typename Joint<T>::BluePrint>();
-  blue_print->mobilizers_.push_back(
-      std::make_unique<internal::SpaceXYZMobilizer<T>>(this->frame_on_parent(),
-                                                       this->frame_on_child()));
+  auto ballrpy_mobilizer = std::make_unique<internal::SpaceXYZMobilizer<T>>(
+      this->frame_on_parent(), this->frame_on_child());
+  ballrpy_mobilizer->set_default_position(this->default_positions());
+  blue_print->mobilizers_.push_back(std::move(ballrpy_mobilizer));
   return std::move(blue_print);
 }
 

multibody/tree/ball_rpy_joint.h

@@ -132,12 +132,6 @@ class BallRpyJoint final : public Joint<T> {
     return *this;
   }
 
-  /// Sets the default angles of this joint.  See get_angles() for details on
-  /// the angle representation.
-  void set_default_angles(const Vector3<double>& angles) {
-    get_mutable_mobilizer()->set_default_position(angles);
-  }
-
   /// Sets the random distribution that angles of this joint will be randomly
   /// sampled from. See get_angles() for details on the angle representation.
   void set_random_angles_distribution(
@@ -176,6 +170,25 @@ class BallRpyJoint final : public Joint<T> {
 
   /// @}
 
+  /// Gets the default angles for `this` joint. Wrapper for the more general
+  /// `Joint::default_positions()`.
+  /// @returns The default angles of `this` stored in `default_positions_`
+  Vector3<double> get_default_angles() const {
+    return this->default_positions();
+  }
+
+  /// Sets the default angles of this joint.
+  /// If the parent tree has been finalized and the underlying mobilizer is
+  /// valid, this method sets the default positions of that mobilizer.
+  /// @param[in] angles
+  ///   The desired default angles of the joint
+  void set_default_angles(const Vector3<double>& angles) {
+    this->set_default_positions(angles);
+    if (this->has_implementation()) {
+      get_mutable_mobilizer()->set_default_position(this->default_positions());
+    }
+  }
+
  protected:
   /// Joint<T> override called through public NVI, Joint::AddInForce().
   /// Adding forces per-dof makes no physical sense. Therefore, this method

multibody/tree/joint.h

@@ -69,7 +69,7 @@ namespace multibody {
 ///
 /// @tparam_default_scalar
 template <typename T>
-class Joint : public MultibodyElement<Joint, T, JointIndex>  {
+class Joint : public MultibodyElement<Joint, T, JointIndex> {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Joint)
 
@@ -118,7 +118,7 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
         const VectorX<double>& acc_lower_limits,
         const VectorX<double>& acc_upper_limits)
       : MultibodyElement<Joint, T, JointIndex>(
-            frame_on_child.model_instance()),
+        frame_on_child.model_instance()),
         name_(name),
         frame_on_parent_(frame_on_parent),
         frame_on_child_(frame_on_child),
@@ -139,6 +139,12 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
 
     DRAKE_DEMAND(acc_lower_limits.size() == acc_upper_limits.size());
     DRAKE_DEMAND((acc_lower_limits.array() <= acc_upper_limits.array()).all());
+
+    // N.B. We cannot use `num_positions()` here because it is virtual.
+    const int num_positions = pos_lower_limits.size();
+
+    // intialize the default positions.
+    default_positions_ = VectorX<double>::Zero(num_positions);
   }
 
   virtual ~Joint() {}
@@ -263,7 +269,7 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
   ///   to which this joint belongs.
   // NVI to DoAddInOneForce().
   void AddInDamping(
-      const systems::Context<T> &context, MultibodyForces<T>* forces) const {
+      const systems::Context<T>& context, MultibodyForces<T>* forces) const {
     DRAKE_DEMAND(forces != nullptr);
     DRAKE_DEMAND(forces->CheckHasRightSizeForModel(this->get_parent_tree()));
     DoAddInDamping(context, forces);
@@ -305,11 +311,19 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
     return acc_upper_limits_;
   }
 
+  /// Returns the default positions.
+  const VectorX<double>& default_positions() const {
+    return default_positions_;
+  }
+
   /// Sets the position limits to @p lower_limits and @p upper_limits.
   /// @throws std::exception if the dimension of @p lower_limits or
   /// @p upper_limits does not match num_positions().
   /// @throws std::exception if any of @p lower_limits is larger than the
   /// corresponding term in @p upper_limits.
+  /// @note Setting the position limits does not affect the
+  /// `default_positions()`, regardless of whether the current
+  /// `default_positions()` satisfy the new position limits.
   void set_position_limits(const VectorX<double>& lower_limits,
                            const VectorX<double>& upper_limits) {
     DRAKE_THROW_UNLESS(lower_limits.size() == upper_limits.size());
@@ -346,6 +360,16 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
     acc_lower_limits_ = lower_limits;
     acc_upper_limits_ = upper_limits;
   }
+
+  /// Sets the default positions to @p default_positions.
+  /// @throws std::exception if the dimension of @p default_positions does not
+  /// match num_positions().
+  /// @note The values in @p default_positions are NOT constrained to be within
+  /// `position_lower_limits()` and `position_upper_limits()`.
+  void set_default_positions(const VectorX<double>& default_positions) {
+    DRAKE_THROW_UNLESS(default_positions.size() == num_positions());
+    default_positions_ = default_positions;
+  }
   /// @}
 
   // Hide the following section from Doxygen.
@@ -527,10 +551,15 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
     return *implementation_;
   }
 
+  /// Returns whether `this` joint owns a particular implementation.
+  /// If the MultibodyTree has been finalized, this will return true.
+  bool has_implementation() const { return implementation_ != nullptr; }
+
  private:
   // Make all other Joint<U> objects a friend of Joint<T> so they can make
   // Joint<ToScalar>::JointImplementation from CloneToScalar<ToScalar>().
-  template <typename> friend class Joint;
+  template <typename>
+  friend class Joint;
 
   // JointImplementationBuilder is a friend so that it can access the
   // Joint<T>::BluePrint and protected method MakeImplementationBlueprint().
@@ -562,6 +591,9 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
   VectorX<double> acc_lower_limits_;
   VectorX<double> acc_upper_limits_;
 
+  // Joint default position. This vector has zero size for joints with no state.
+  VectorX<double> default_positions_;
+
   // The Joint<T> implementation:
   std::unique_ptr<JointImplementation> implementation_;
 };

multibody/tree/prismatic_joint.h

@@ -30,7 +30,7 @@ class PrismaticJoint final : public Joint<T> {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(PrismaticJoint)
 
-  template<typename Scalar>
+  template <typename Scalar>
   using Context = systems::Context<Scalar>;
 
   static const char kTypeName[];
@@ -187,8 +187,24 @@ class PrismaticJoint final : public Joint<T> {
 
   /// @}
 
+  /// Gets the default translation. Wrapper for the more general
+  /// `Joint::default_positions()`.
+  /// @returns The default translation of `this` stored in `default_positions_`.
+  double get_default_translation() const {
+    return this->default_positions()[0];
+  }
+
+  /// Sets the `default_positions` of this joint (in this case a single
+  /// translation)
+  /// If the parent tree has been finalized and the underlying mobilizer is
+  /// valid, this method sets the default position of that mobilizer.
+  /// @param[in] translation
+  ///   The desired default translation of the joint
   void set_default_translation(double translation) {
-    get_mutable_mobilizer()->set_default_position(Vector1d{translation});
+    this->set_default_positions(Vector1d{translation});
+    if (this->has_implementation()) {
+      get_mutable_mobilizer()->set_default_position(this->default_positions());
+    }
   }
 
   void set_random_translation_distribution(
@@ -273,9 +289,11 @@ class PrismaticJoint final : public Joint<T> {
   std::unique_ptr<typename Joint<T>::BluePrint>
   MakeImplementationBlueprint() const final {
     auto blue_print = std::make_unique<typename Joint<T>::BluePrint>();
-    blue_print->mobilizers_.push_back(
+    auto prismatic_mobilizer =
         std::make_unique<internal::PrismaticMobilizer<T>>(
-            this->frame_on_parent(), this->frame_on_child(), axis_));
+            this->frame_on_parent(), this->frame_on_child(), axis_);
+    prismatic_mobilizer->set_default_position(this->default_positions());
+    blue_print->mobilizers_.push_back(std::move(prismatic_mobilizer));
     return std::move(blue_print);
   }
 

multibody/tree/revolute_joint.cc

@@ -10,8 +10,7 @@ namespace multibody {
 
 template <typename T>
 template <typename ToScalar>
-std::unique_ptr<Joint<ToScalar>>
-RevoluteJoint<T>::TemplatedDoCloneToScalar(
+std::unique_ptr<Joint<ToScalar>> RevoluteJoint<T>::TemplatedDoCloneToScalar(
     const internal::MultibodyTree<ToScalar>& tree_clone) const {
   const Frame<ToScalar>& frame_on_parent_body_clone =
       tree_clone.get_variant(this->frame_on_parent());
@@ -56,9 +55,10 @@ template <typename T>
 std::unique_ptr<typename Joint<T>::BluePrint>
 RevoluteJoint<T>::MakeImplementationBlueprint() const {
   auto blue_print = std::make_unique<typename Joint<T>::BluePrint>();
-  blue_print->mobilizers_.push_back(
-      std::make_unique<internal::RevoluteMobilizer<T>>(
-          this->frame_on_parent(), this->frame_on_child(), axis_));
+  auto revolute_mobilizer = std::make_unique<internal::RevoluteMobilizer<T>>(
+      this->frame_on_parent(), this->frame_on_child(), axis_);
+  revolute_mobilizer->set_default_position(this->default_positions());
+  blue_print->mobilizers_.push_back(std::move(revolute_mobilizer));
   return std::move(blue_print);
 }
 

multibody/tree/revolute_joint.h

@@ -31,7 +31,7 @@ class RevoluteJoint final : public Joint<T> {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(RevoluteJoint)
 
-  template<typename Scalar>
+  template <typename Scalar>
   using Context = systems::Context<Scalar>;
 
   static const char kTypeName[];
@@ -191,10 +191,6 @@ class RevoluteJoint final : public Joint<T> {
     return *this;
   }
 
-  void set_default_angle(double angle) {
-    get_mutable_mobilizer()->set_default_position(Vector1d{angle});
-  }
-
   void set_random_angle_distribution(const symbolic::Expression& angle) {
     get_mutable_mobilizer()->set_random_position_distribution(
         Vector1<symbolic::Expression>{angle});
@@ -227,6 +223,23 @@ class RevoluteJoint final : public Joint<T> {
 
   /// @}
 
+  /// Gets the default rotation angle. Wrapper for the more general
+  /// `Joint::default_positions()`.
+  /// @returns The default angle of `this` stored in `default_positions_`
+  double get_default_angle() const { return this->default_positions()[0]; }
+
+  /// Sets the `default_positions` of this joint (in this case a single angle).
+  /// If the parent tree has been finalized and the underlying mobilizer is
+  /// valid, this method sets the default position of that mobilizer.
+  /// @param[in] angle
+  ///   The desired default angle of the joint
+  void set_default_angle(double angle) {
+    this->set_default_positions(Vector1d{angle});
+    if (this->has_implementation()) {
+      get_mutable_mobilizer()->set_default_position(this->default_positions());
+    }
+  }
+
   /// Adds into `forces` a given `torque` for `this` joint that is to be applied
   /// about the joint's axis. The torque is defined to be positive according to
   /// the right-hand-rule with the thumb aligned in the direction of `this`

multibody/tree/test/ball_rpy_joint_test.cc

@@ -23,6 +23,8 @@ constexpr double kVelocityUpperLimit = 1.6;
 constexpr double kAccelerationLowerLimit = -1.2;
 constexpr double kAccelerationUpperLimit = 1.7;
 constexpr double kDamping = 3;
+constexpr double kPositionNonZeroDefault =
+    (kPositionLowerLimit + kPositionUpperLimit) / 2;
 
 class BallRpyJointTest : public ::testing::Test {
  public:
@@ -198,6 +200,36 @@ TEST_F(BallRpyJointTest, SetVelocityAndAccelerationLimits) {
                std::runtime_error);
 }
 
+TEST_F(BallRpyJointTest, DefaultAngles) {
+  const Vector3d lower_limit_angles = Vector3d::Constant(kPositionLowerLimit);
+  const Vector3d upper_limit_angles = Vector3d::Constant(kPositionUpperLimit);
+
+  const Vector3d default_angles = Vector3d::Zero();
+
+  const Vector3d new_default_angles =
+      Vector3d::Constant(kPositionNonZeroDefault);
+
+  const Vector3d out_of_bounds_low_angles =
+      lower_limit_angles - Vector3d::Constant(1);
+  const Vector3d out_of_bounds_high_angles =
+      upper_limit_angles + Vector3d::Constant(1);
+
+  // Constructor should set the default angle to Vector3d::Zero()
+  EXPECT_EQ(joint_->get_default_angles(), default_angles);
+
+  // Setting a new default angle should propogate so that `get_default_angle()`
+  // remains correct.
+  mutable_joint_->set_default_angles(new_default_angles);
+  EXPECT_EQ(joint_->get_default_angles(), new_default_angles);
+
+  // Setting the default angle out of the bounds of the position limits
+  // should NOT throw an exception
+  EXPECT_NO_THROW(
+      mutable_joint_->set_default_angles(out_of_bounds_low_angles));
+  EXPECT_NO_THROW(
+      mutable_joint_->set_default_angles(out_of_bounds_high_angles));
+}
+
 }  // namespace
 }  // namespace multibody
 }  // namespace drake

multibody/tree/test/prismatic_joint_test.cc

@@ -186,6 +186,31 @@ TEST_F(PrismaticJointTest, RandomTranslationTest) {
   EXPECT_GE(kPositionUpperLimit, joint1_->get_translation(*context_));
 }
 
+TEST_F(PrismaticJointTest, DefaultTranslation) {
+  const double default_translation = 0.0;
+
+  const double new_default_translation =
+      0.5 * kPositionLowerLimit + 0.5 * kPositionUpperLimit;
+
+  const double out_of_bounds_low_translation = kPositionLowerLimit - 1;
+  const double out_of_bounds_high_translation = kPositionUpperLimit + 1;
+
+  // Constructor should set the default tranlation to 0.0
+  EXPECT_EQ(joint1_->get_default_translation(), default_translation);
+
+  // Setting a new default translation should propogate so that
+  // `get_default_translation()` remains correct.
+  mutable_joint1_->set_default_translation(new_default_translation);
+  EXPECT_EQ(joint1_->get_default_translation(), new_default_translation);
+
+  // Setting the default angle out of the bounds of the position limits
+  // should NOT throw an exception
+  EXPECT_NO_THROW(
+      mutable_joint1_->set_default_translation(out_of_bounds_low_translation));
+  EXPECT_NO_THROW(
+      mutable_joint1_->set_default_translation(out_of_bounds_high_translation));
+}
+
 }  // namespace
 }  // namespace multibody
 }  // namespace drake

multibody/tree/test/revolute_joint_test.cc

@@ -211,6 +211,29 @@ TEST_F(RevoluteJointTest, SetVelocityAndAccelerationLimits) {
                std::runtime_error);
 }
 
+TEST_F(RevoluteJointTest, DefaultAngle) {
+  const double default_angle = 0.0;
+
+  const double new_default_angle =
+      0.5 * kPositionLowerLimit + 0.5 * kPositionUpperLimit;
+
+  const double out_of_bounds_low_angle = kPositionLowerLimit - 1;
+  const double out_of_bounds_high_angle = kPositionUpperLimit + 1;
+
+  // Constructor should set the default angle to 0.0
+  EXPECT_EQ(joint1_->get_default_angle(), default_angle);
+
+  // Setting a new default angle should propogate so that `get_default_angle()`
+  // remains correct.
+  mutable_joint1_->set_default_angle(new_default_angle);
+  EXPECT_EQ(joint1_->get_default_angle(), new_default_angle);
+
+  // Setting the default angle out of the bounds of the position limits
+  // should NOT throw an exception
+  EXPECT_NO_THROW(mutable_joint1_->set_default_angle(out_of_bounds_low_angle));
+  EXPECT_NO_THROW(mutable_joint1_->set_default_angle(out_of_bounds_high_angle));
+}
+
 }  // namespace
 }  // namespace multibody
 }  // namespace drake