default joint position implementation

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)
 
@@ -117,8 +117,7 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
         const VectorX<double>& vel_upper_limits,
         const VectorX<double>& acc_lower_limits,
         const VectorX<double>& acc_upper_limits)
-      : MultibodyElement<Joint, T, JointIndex>(
-            frame_on_child.model_instance()),
+      : MultibodyElement<Joint, T, JointIndex>(frame_on_child.model_instance()),
         name_(name),
         frame_on_parent_(frame_on_parent),
         frame_on_child_(frame_on_child),
@@ -139,6 +138,9 @@ 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());
+
+    // initialize the default positions to 0.
+    default_positions_ = VectorX<double>::Zero(pos_lower_limits.size());
   }
 
   virtual ~Joint() {}
@@ -147,24 +149,16 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
   const std::string& name() const { return name_; }
 
   /// Returns a const reference to the parent body P.
-  const Body<T>& parent_body() const {
-    return frame_on_parent_.body();
-  }
+  const Body<T>& parent_body() const { return frame_on_parent_.body(); }
 
   /// Returns a const reference to the child body B.
-  const Body<T>& child_body() const {
-    return frame_on_child_.body();
-  }
+  const Body<T>& child_body() const { return frame_on_child_.body(); }
 
   /// Returns a const reference to the frame F attached on the parent body P.
-  const Frame<T>& frame_on_parent() const {
-    return frame_on_parent_;
-  }
+  const Frame<T>& frame_on_parent() const { return frame_on_parent_; }
 
   /// Returns a const reference to the frame M attached on the child body B.
-  const Frame<T>& frame_on_child() const {
-    return frame_on_child_;
-  }
+  const Frame<T>& frame_on_child() const { return frame_on_child_; }
 
   /// Returns a string identifying the type of `this` joint, such as "revolute"
   /// or "prismatic".
@@ -173,9 +167,7 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
   /// Returns the index to the first generalized velocity for this joint
   /// within the vector v of generalized velocities for the full multibody
   /// system.
-  int velocity_start() const {
-    return do_get_velocity_start();
-  }
+  int velocity_start() const { return do_get_velocity_start(); }
 
   /// Returns the number of generalized velocities describing this joint.
   int num_velocities() const {
@@ -186,9 +178,7 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
   /// Returns the index to the first generalized position for this joint
   /// within the vector q of generalized positions for the full multibody
   /// system.
-  int position_start() const {
-    return do_get_position_start();
-  }
+  int position_start() const { return do_get_position_start(); }
 
   /// Returns the number of generalized positions describing this joint.
   int num_positions() const {
@@ -240,11 +230,8 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
   ///   `forces` is `nullptr` or if `forces` doest not have the right sizes to
   ///   accommodate a set of forces for the model to which this joint belongs.
   // NVI to DoAddInOneForce().
-  void AddInOneForce(
-      const systems::Context<T>& context,
-      int joint_dof,
-      const T& joint_tau,
-      MultibodyForces<T>* forces) const {
+  void AddInOneForce(const systems::Context<T>& context, int joint_dof,
+                     const T& joint_tau, MultibodyForces<T>* forces) const {
     DRAKE_DEMAND(forces != nullptr);
     DRAKE_DEMAND(0 <= joint_dof && joint_dof < num_velocities());
     DRAKE_DEMAND(forces->CheckHasRightSizeForModel(this->get_parent_tree()));
@@ -262,8 +249,8 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
   ///   not have the right sizes to accommodate a set of forces for the model
   ///   to which this joint belongs.
   // NVI to DoAddInOneForce().
-  void AddInDamping(
-      const systems::Context<T> &context, MultibodyForces<T>* forces) const {
+  void AddInDamping(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,6 +292,11 @@ 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().
@@ -346,6 +338,22 @@ 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().
+  /// @throws std::exception if any of @p default_positions is smaller than
+  /// the corresponding term in `position_lower_limits()`.
+  /// @throws std::exception if any of @p default_positions is larger than
+  /// the corresponding term in `position_upper_limits()`.
+  void set_default_positions(const VectorX<double>& default_positions) {
+    DRAKE_THROW_UNLESS(default_positions.size() == num_positions());
+    DRAKE_THROW_UNLESS(
+        (position_lower_limits().array() <= default_positions.array()).all());
+    DRAKE_THROW_UNLESS(
+        (default_positions.array() <= position_upper_limits().array()).all());
+    default_positions_ = default_positions;
+  }
   /// @}
 
   // Hide the following section from Doxygen.
@@ -360,7 +368,8 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
 
     std::unique_ptr<typename Joint<ToScalar>::JointImplementation>
         implementation_clone =
-        this->get_implementation().template CloneToScalar<ToScalar>(tree_clone);
+            this->get_implementation().template CloneToScalar<ToScalar>(
+                tree_clone);
     joint_clone->OwnImplementation(std::move(implementation_clone));
 
     return std::move(joint_clone);
@@ -400,9 +409,7 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
     }
 
     /// Returns the number of mobilizers in this implementation.
-    int num_mobilizers() const {
-      return static_cast<int>(mobilizers_.size());
-    }
+    int num_mobilizers() const { return static_cast<int>(mobilizers_.size()); }
 
     // Hide the following section from Doxygen.
 #ifndef DRAKE_DOXYGEN_CXX
@@ -479,20 +486,18 @@ class Joint : public MultibodyElement<Joint, T, JointIndex>  {
   /// This method is only called by the public NVI AddInOneForce() and therefore
   /// input arguments were checked to be valid.
   /// @see The public NVI AddInOneForce() for details.
-  virtual void DoAddInOneForce(
-      const systems::Context<T>& context,
-      int joint_dof,
-      const T& joint_tau,
-      MultibodyForces<T>* forces) const = 0;
+  virtual void DoAddInOneForce(const systems::Context<T>& context,
+                               int joint_dof, const T& joint_tau,
+                               MultibodyForces<T>* forces) const = 0;
 
   /// Adds into MultibodyForces the forces due to damping within `this` joint.
   /// How forces are added to a MultibodyTree model depends on the underlying
   /// implementation of a particular joint (for instance, mobilizer vs.
   /// constraint) and therefore specific %Joint subclasses must provide a
   /// definition for this method.
   /// The default implementation is a no-op for joints with no damping.
-  virtual void DoAddInDamping(
-      const systems::Context<T>&, MultibodyForces<T>*) const {}
+  virtual void DoAddInDamping(const systems::Context<T>&,
+                              MultibodyForces<T>*) const {}
 
   // Implements MultibodyElement::DoSetTopology(). Joints have no topology
   // though we could require them to have one in the future.
@@ -527,10 +532,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 +572,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.cc

@@ -9,8 +9,7 @@ namespace multibody {
 
 template <typename T>
 template <typename ToScalar>
-std::unique_ptr<Joint<ToScalar>>
-PrismaticJoint<T>::TemplatedDoCloneToScalar(
+std::unique_ptr<Joint<ToScalar>> PrismaticJoint<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());