Add gyroscope sensor (RobotLocomotion#13604)

Also fixes minor typos in accelerometer sensor.

systems/sensors/BUILD.bazel

@@ -28,6 +28,7 @@ drake_cc_package_library(
         ":beam_model_params",
         ":camera_info",
         ":color_palette",
+        ":gyroscope",
         ":image",
         ":image_to_lcm_image_array_t",
         ":image_writer",
@@ -44,7 +45,6 @@ drake_cc_library(
     name = "accelerometer",
     srcs = ["accelerometer.cc"],
     hdrs = ["accelerometer_sensor.h"],
-    data = ["//examples/pendulum:prod_models"],
     deps = [
         "//math:geometric_transform",
         "//multibody/math",
@@ -83,6 +83,19 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "gyroscope",
+    srcs = ["gyroscope.cc"],
+    hdrs = ["gyroscope_sensor.h"],
+    deps = [
+        "//math:geometric_transform",
+        "//multibody/math",
+        "//multibody/plant",
+        "//multibody/tree:multibody_tree_indexes",
+        "//systems/framework",
+    ],
+)
+
 drake_cc_library(
     name = "image",
     srcs = [
@@ -248,11 +261,13 @@ drake_cc_library(
 
 drake_cc_googletest(
     name = "accelerometer_test",
+    data = ["//examples/pendulum:prod_models"],
     deps = [
         ":accelerometer",
         "//common/test_utilities:eigen_matrix_compare",
         "//multibody/parsing",
         "//multibody/plant",
+        "//systems/framework/test_utilities",
     ],
 )
 
@@ -277,6 +292,18 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "gyroscope_test",
+    data = ["//examples/pendulum:prod_models"],
+    deps = [
+        ":gyroscope",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//multibody/parsing",
+        "//multibody/plant",
+        "//systems/framework/test_utilities",
+    ],
+)
+
 drake_cc_googletest(
     name = "image_test",
     deps = [

systems/sensors/accelerometer.cc

@@ -14,36 +14,44 @@ using multibody::SpatialAcceleration;
 using multibody::SpatialVelocity;
 
 template <typename T>
-Accelerometer<T>::Accelerometer(multibody::BodyIndex index,
-                                const RigidTransform<double>& X_BC,
+Accelerometer<T>::Accelerometer(const multibody::Body<T>& body,
+                                const RigidTransform<double>& X_BS,
                                 const Eigen::Vector3d& gravity_vector)
-    : body_index_(index),
-      X_BS_(X_BC),
+    : Accelerometer(body.index(), X_BS, gravity_vector) {}
+
+template <typename T>
+Accelerometer<T>::Accelerometer(const multibody::BodyIndex& body_index,
+                                const RigidTransform<double>& X_BS,
+                                const Eigen::Vector3d& gravity_vector)
+    : LeafSystem<T>(SystemTypeTag<Accelerometer>{}),
+      body_index_(body_index),
+      X_BS_(X_BS),
       gravity_vector_(gravity_vector) {
   // Declare measurement output port.
-  this->DeclareVectorOutputPort("measured_acceleration", BasicVector<T>(3),
-                                &Accelerometer<T>::CalcOutput);
+  measurement_output_port_ = &this->DeclareVectorOutputPort(
+      "measurement", BasicVector<T>(3), &Accelerometer<T>::CalcOutput);
 
   body_poses_input_port_ = &this->DeclareAbstractInputPort(
       "body_poses", Value<std::vector<RigidTransform<T>>>());
   body_velocities_input_port_ = &this->DeclareAbstractInputPort(
-      "body_velocities", Value<std::vector<SpatialVelocity<T>>>());
+      "body_spatial_velocities", Value<std::vector<SpatialVelocity<T>>>());
   body_accelerations_input_port_ = &this->DeclareAbstractInputPort(
-      "body_accelerations", Value<std::vector<SpatialAcceleration<T>>>());
+      "body_spatial_accelerations",
+      Value<std::vector<SpatialAcceleration<T>>>());
 }
 
 template <typename T>
 void Accelerometer<T>::CalcOutput(const Context<T>& context,
                                   BasicVector<T>* output) const {
-  const auto& X_WB = get_body_poses_input_port()
-                          .template Eval<std::vector<RigidTransform<T>>>(
-                              context)[body_index_];
-  const auto& V_WB = get_body_velocities_input_port()
-                          .template Eval<std::vector<SpatialVelocity<T>>>(
-                              context)[body_index_];
+  const auto& X_WB =
+      get_body_poses_input_port().template Eval<std::vector<RigidTransform<T>>>(
+          context)[body_index_];
+  const auto& V_WB =
+      get_body_velocities_input_port()
+          .template Eval<std::vector<SpatialVelocity<T>>>(context)[body_index_];
   const auto& A_WB = get_body_accelerations_input_port()
-                          .template Eval<std::vector<SpatialAcceleration<T>>>(
-                              context)[body_index_];
+                         .template Eval<std::vector<SpatialAcceleration<T>>>(
+                             context)[body_index_];
 
   // Kinematic term expressed in world coordinates unless specified
   // Sensor frame position and orientation.
@@ -66,14 +74,12 @@ void Accelerometer<T>::CalcOutput(const Context<T>& context,
 
 template <typename T>
 const Accelerometer<T>& Accelerometer<T>::AddToDiagram(
-    multibody::BodyIndex body_index,
-    const RigidTransform<double>& X_BS,
+    const multibody::Body<T>& body, const RigidTransform<double>& X_BS,
     const Eigen::Vector3d& gravity_vector,
-    const multibody::MultibodyPlant<T>& plant,
-    DiagramBuilder<T>* builder) {
-  const auto& accelerometer =
-      *builder->template AddSystem<Accelerometer<T>>(body_index, X_BS,
-          gravity_vector);
+    const multibody::MultibodyPlant<T>& plant, DiagramBuilder<T>* builder) {
+  const auto& accelerometer = *builder->template AddSystem<Accelerometer<T>>(
+      body, X_BS, gravity_vector);
+
   builder->Connect(plant.get_body_poses_output_port(),
                    accelerometer.get_body_poses_input_port());
   builder->Connect(plant.get_body_spatial_velocities_output_port(),
@@ -83,6 +89,11 @@ const Accelerometer<T>& Accelerometer<T>::AddToDiagram(
   return accelerometer;
 }
 
+template <typename T>
+template <typename U>
+Accelerometer<T>::Accelerometer(const Accelerometer<U>& other)
+    : Accelerometer(other.body_index(), other.pose(), other.gravity_vector()) {}
+
 DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
     class ::drake::systems::sensors::Accelerometer)
 

systems/sensors/accelerometer_sensor.h

@@ -36,27 +36,27 @@ namespace sensors {
 ///      (e.g. plant.get_body_spatial_accelerations_output_port())
 ///
 /// This class is therefore defined by:
-///   1. The BodyIndex of the body to which this sensor is rigidly affixed.
+///   1. The body to which this sensor is rigidly affixed.
 ///   2. A rigid transform from the body frame to the sensor frame.
 ///
 /// @system{Accelerometer,
 ///    @input_port{body_poses}
 ///    @input_port{body_spatial_velocities}
 ///    @input_port{body_spatial_accelerations},
-///    @output_port{measured_acceleration}
+///    @output_port{measurement}
 /// }
 /// @ingroup sensor_systems
 template <typename T>
-class Accelerometer : public LeafSystem<T> {
+class Accelerometer final : public LeafSystem<T> {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Accelerometer)
 
-  /// @param body_index The index of body B
-  /// @param X_BS the transform from body B to the accelerometer frame S
+  /// @param body the body B to which the sensor is affixed
+  /// @param X_BS the pose of sensor frame S in body B
   /// @param gravity_vector the constant acceleration due to gravity
   ///    expressed in world coordinates
   Accelerometer(
-      multibody::BodyIndex body_index, const math::RigidTransform<double>& X_BS,
+      const multibody::Body<T>& body, const math::RigidTransform<double>& X_BS,
       const Eigen::Vector3d& gravity_vector = Eigen::Vector3d::Zero());
 
   /// Scalar-converting copy constructor.  See @ref system_scalar_conversion.
@@ -75,6 +75,21 @@ class Accelerometer : public LeafSystem<T> {
     return *body_accelerations_input_port_;
   }
 
+  const OutputPort<T>& get_measurement_output_port() const {
+    return *measurement_output_port_;
+  }
+
+  /// Returns the index of the Body that was supplied in the constructor.
+  const multibody::BodyIndex& body_index() const { return body_index_; }
+
+  /// Returns the gravity vector supplied in the constructor, or zero if none.
+  const Eigen::Vector3d& gravity_vector() const { return gravity_vector_; }
+
+  /// Gets X_BS, the pose of sensor frame S in body B
+  const math::RigidTransform<double>& pose() const {
+    return X_BS_;
+  }
+
   /// Static factory method that creates an Accelerometer object and connects
   /// it to the given plant. Modifies a Diagram by connecting the input ports
   /// of the new Accelerometer to the appropriate output ports of a
@@ -87,32 +102,33 @@ class Accelerometer : public LeafSystem<T> {
   ///        this.get_body_velocities_input_port()
   /// 3. plant.get_body_spatial_accelerations_output_port() to
   ///        this.get_body_spatial_accelerations_output_port()
-  /// @param body_index the index of body B
-  /// @param X_BS the transform from body B to the accelerometer frame S
+  /// @param body the body B to which the sensor is affixed
+  /// @param X_BS the pose of sensor frame S in body B
   /// @param gravity_vector the constant acceleration due to gravity
   ///    expressed in world coordinates
+  /// @param plant the plant to which the sensor will be connected
   /// @param builder a pointer to the DiagramBuilder
   static const Accelerometer& AddToDiagram(
-      multibody::BodyIndex body_index, const math::RigidTransform<double>& X_BS,
+      const multibody::Body<T>& body, const math::RigidTransform<double>& X_BS,
       const Eigen::Vector3d& gravity_vector,
       const multibody::MultibodyPlant<T>& plant, DiagramBuilder<T>* builder);
 
  private:
-  // Allow different specializations to access each other's private data.
-  template <typename>
-  friend class Accelerometer;
+  Accelerometer(
+      const multibody::BodyIndex& body_index,
+      const math::RigidTransform<double>& X_BS,
+      const Eigen::Vector3d& gravity_vector = Eigen::Vector3d::Zero());
 
   // Outputs the transformed signal.
- private:
   void CalcOutput(const Context<T>& context, BasicVector<T>* output) const;
 
-  // Index into the input body velocities/accelerations
   const multibody::BodyIndex body_index_;
   const math::RigidTransform<double> X_BS_;
   const Eigen::Vector3d gravity_vector_;
   const InputPort<T>* body_poses_input_port_{nullptr};
   const InputPort<T>* body_velocities_input_port_{nullptr};
   const InputPort<T>* body_accelerations_input_port_{nullptr};
+  const OutputPort<T>* measurement_output_port_{nullptr};
 };
 
 }  // namespace sensors

systems/sensors/gyroscope.cc

@@ -0,0 +1,76 @@
+/* clang-format off to disable clang-format-includes */
+#include "drake/systems/sensors/gyroscope_sensor.h"
+/* clang-format on */
+#include "drake/multibody/math/spatial_velocity.h"
+
+namespace drake {
+namespace systems {
+namespace sensors {
+
+using math::RigidTransform;
+using multibody::SpatialVelocity;
+
+template <typename T>
+Gyroscope<T>::Gyroscope(const multibody::Body<T>& body,
+                        const RigidTransform<double>& X_BS)
+    : Gyroscope(body.index(), X_BS) {}
+
+template <typename T>
+Gyroscope<T>::Gyroscope(const multibody::BodyIndex& body_index,
+                        const RigidTransform<double>& X_BS)
+    : LeafSystem<T>(SystemTypeTag<Gyroscope>{}),
+      body_index_(body_index),
+      X_BS_(X_BS) {
+  // Declare measurement output port.
+  measurement_output_port_ = &this->DeclareVectorOutputPort(
+      "measurement", BasicVector<T>(3), &Gyroscope<T>::CalcOutput);
+
+  body_poses_input_port_ = &this->DeclareAbstractInputPort(
+      "body_poses", Value<std::vector<RigidTransform<T>>>());
+  body_velocities_input_port_ = &this->DeclareAbstractInputPort(
+      "body_spatial_velocities", Value<std::vector<SpatialVelocity<T>>>());
+}
+
+template <typename T>
+void Gyroscope<T>::CalcOutput(const Context<T>& context,
+                              BasicVector<T>* output) const {
+  const auto& X_WB =
+      get_body_poses_input_port().template Eval<std::vector<RigidTransform<T>>>(
+          context)[body_index_];
+  const auto& V_WB =
+      get_body_velocities_input_port()
+          .template Eval<std::vector<SpatialVelocity<T>>>(context)[body_index_];
+
+  // Calculate rotation from world to gyroscope: R_SW = R_SB * R_BW.
+  const auto R_SB = X_BS_.rotation().matrix().template cast<T>().transpose();
+  const auto R_BW = X_WB.rotation().matrix().transpose();
+  const auto R_SW = R_SB * R_BW;
+
+  // Re-express in local frame and return.
+  output->SetFromVector(R_SW * V_WB.rotational());
+}
+
+template <typename T>
+const Gyroscope<T>& Gyroscope<T>::AddToDiagram(
+    const multibody::Body<T>& body, const RigidTransform<double>& X_BS,
+    const multibody::MultibodyPlant<T>& plant, DiagramBuilder<T>* builder) {
+  const auto& gyroscope =
+      *builder->template AddSystem<Gyroscope<T>>(body, X_BS);
+  builder->Connect(plant.get_body_poses_output_port(),
+                   gyroscope.get_body_poses_input_port());
+  builder->Connect(plant.get_body_spatial_velocities_output_port(),
+                   gyroscope.get_body_velocities_input_port());
+  return gyroscope;
+}
+
+template <typename T>
+template <typename U>
+Gyroscope<T>::Gyroscope(const Gyroscope<U>& other)
+    : Gyroscope(other.body_index(), other.pose()) {}
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
+    class ::drake::systems::sensors::Gyroscope)
+
+}  // namespace sensors
+}  // namespace systems
+}  // namespace drake