Drake
accelerometer.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <string>
5 
15 
16 namespace drake {
17 namespace systems {
18 namespace sensors {
19 
20 /// A simulated ideal accelerometer that measures the linear acceleration of a
21 /// frame associated with a RigidBodyPlant.
22 ///
23 /// The math implemented by this sensor is as follows. Let:
24 /// - `v` be the RigidBodyPlant's generalized velocity state vector.
25 /// - `A` be the accelerometer's frame with origin point `Ao`.
26 /// - `W` be the world frame with origin point `Wo`.
27 /// - `v_WAo_W` be `Ao`'s translational velocity in `W` expressed in `W`.
28 /// - `J_WA` be the Jacobian matrix that relates `v_WAo_W` to `v`.
29 /// - `R_AW` be the rotation matrix from `W` to `A`.
30 ///
31 /// The equation for `v_WAo_W` is as follows:
32 ///
33 /// <pre>
34 /// v_WAo_W = J_WA v
35 /// </pre>
36 ///
37 /// Let `a_WAo_W` be the `Ao`'s translational acceleration in `W` expressed in
38 /// `W`. `a_WAo_W` can be computed by taking the time derivative of `v_WAo_W`,
39 /// which is derived as follows using the chain rule:
40 ///
41 /// <pre>
42 /// a_WAo_W = d/dt v_WAo_W = J_WA vdot_WAo_W + Jdot_WA v_WAo_W
43 /// </pre>
44 ///
45 /// `a_WAo_W` is then re-expressed in `A` using the rotation matrix `R_AW`:
46 ///
47 /// <pre>
48 /// a_WAo_A = R_AW a_WAo_W
49 /// </pre>
50 ///
51 /// Note that `a_WA_A` does not include gravity. The constructor includes
52 /// boolean input parameter `include_gravity`, which allows the user to specify
53 /// whether the accelerometer should also sense the acceleration due to gravity.
54 /// If gravity is to be included, this acceleration performs the following
55 /// additional math:
56 ///
57 /// Let:
58 /// - `g_W` be the acceleration due to gravity in `W`, as specified by
59 /// RigidBodyTree::a_grav.
60 /// - `g_A` be the acceleration due to gravity in `A`.
61 ///
62 /// `g_A` is computed as follows:
63 ///
64 /// <pre>
65 /// g_A = R_AW * g_W
66 /// </pre>
67 ///
68 /// `g_A` is then added to `a_WAo_A`:
69 ///
70 /// <pre>
71 /// a_WAo_A = a_WAo_A + g_A
72 /// </pre>
73 ///
74 /// This concludes the discussion of what is computed by this accelerometer.
75 ///
76 /// <B>%System Input Ports:</B>
77 ///
78 /// There are two input ports that are accessible via the following accessors:
79 ///
80 /// - get_plant_state_input_port(): Contains the state (i.e., position and
81 /// velocity) vector, `x`, of the RigidBodyPlant is being sensed by this
82 /// sensor.
83 ///
84 /// - get_plant_state_derivative_input_port(): Contains the derivative of the
85 /// state vector, `xdot`, of the RigidBodyPlant being sensed by this sensor.
86 ///
87 /// <B>%System Output Ports:</B>
88 ///
89 /// This system has one output port that is accessible via the following
90 /// accessor method:
91 ///
92 /// - get_output_port(): Contains the sensed acceleration data in this sensor's
93 /// frame.
94 ///
95 /// @ingroup sensor_systems
96 ///
97 class Accelerometer : public systems::LeafSystem<double> {
98  public:
100 
101  /// A constructor that initializes an Accelerometer.
102  ///
103  /// @param[in] name The name of the accelerometer. This can be any value.
104  ///
105  /// @param[in] frame The frame `A` to which this accelerometer is attached
106  /// (see class documentation for definition of `A`). This is the frame in
107  /// which this accelerometer's output is given. This reference must remain
108  /// valid for the lifetime of this class's instance.
109  ///
110  /// @param[in] tree The RigidBodyTree that belongs to the RigidBodyPlant being
111  /// sensed by this sensor. This should be a reference to the same
112  /// RigidBodyTree that is being used by the RigidBodyPlant whose outputs are
113  /// fed into this sensor. This parameter's lifespan must exceed that of this
114  /// class's instance.
115  ///
116  /// @param[in] include_gravity Whether to include the acceleration due to
117  /// gravity in the sensor's readings. See this class's description for more
118  /// details about the meaning of this parameter.
119  ///
120  Accelerometer(const std::string& name, const RigidBodyFrame<double>& frame,
121  const RigidBodyTree<double>& tree, bool include_gravity = true);
122 
123  /// Instantiates and attaches an Accelerometer to a RigidBodyPlant. It
124  /// connects the Accelerometer's plant state input port. <b>It does not
125  /// connect the plant state derivative input port.</b>
126  ///
127  /// @param[in] name The name of the sensor. This can be any value.
128  ///
129  /// @param[in] frame The frame to which the Accelerometer is attached.
130  ///
131  /// @param[in] plant The plant that the newly instantiated Accelerometer must
132  /// sense. The provided plant must be in the provided builder.
133  ///
134  /// @param[in] include_gravity Whether to include the acceleration due to
135  /// gravity in the sensor's readings.
136  ///
137  /// @param[out] builder A pointer to the DiagramBuilder to which the newly
138  /// instantiated Accelerometer is added. This must not be `nullptr`.
139  ///
140  /// @return A pointer to the newly instantiated and added accelerometer. The
141  /// accelerometer is initially owned by the builder. Ownership will
142  /// subsequently be transferred to the Diagram that is built by the builder.
144  const std::string& name,
145  const RigidBodyFrame<double>& frame,
146  const RigidBodyPlant<double>& plant,
147  bool include_gravity,
148  DiagramBuilder<double>* builder);
149 
150  /// Returns whether gravity is included in this sensor's measurements.
151  bool get_include_gravity() const { return include_gravity_; }
152 
153  /// Returns the RigidBodyTree that this sensor is sensing.
154  const RigidBodyTree<double>& get_tree() const { return tree_; }
155 
156  /// Returns this sensor's frame, which specifies its location and orientation
157  /// in the RigidBodyTree.
158  ///
159  /// @see get_tree()
160  const RigidBodyFrame<double>& get_frame() const { return frame_; }
161 
162  /// Returns the input port that should contain the generalized position and
163  /// velocity vector of the RigidBodyPlant that this sensor is sensing.
165  return System<double>::get_input_port(plant_state_input_port_index_);
166  }
167 
168  /// Returns the input port that should contain the derivative of the
169  /// generalized position and velocity vector of the RigidBodyPlant that this
170  /// sensor is sensing.
172  const {
174  plant_state_derivative_input_port_index_);
175  }
176 
177  /// Returns the state output port, which contains the sensor's
178  /// sensed values.
180  return System<double>::get_output_port(output_port_index_);
181  }
182 
183  private:
184  // Computes the "sensed" linear acceleration.
185  void CalcAccelerationOutput(const Context<double>& context,
186  AccelerometerOutput<double>* output_vector) const;
187 
188  const std::string name_;
189  const RigidBodyFrame<double> frame_;
190  const RigidBodyTree<double>& tree_;
191  const bool include_gravity_{true};
192 
193  int plant_state_input_port_index_{};
194  int plant_state_derivative_input_port_index_{};
195  int output_port_index_{};
196 };
197 
198 } // namespace sensors
199 } // namespace systems
200 } // namespace drake
const RigidBodyTree< double > & get_tree() const
Returns the RigidBodyTree that this sensor is sensing.
Definition: accelerometer.h:154
const RigidBodyFrame< double > & get_frame() const
Returns this sensor&#39;s frame, which specifies its location and orientation in the RigidBodyTree.
Definition: accelerometer.h:160
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
static Accelerometer * AttachAccelerometer(const std::string &name, const RigidBodyFrame< double > &frame, const RigidBodyPlant< double > &plant, bool include_gravity, DiagramBuilder< double > *builder)
Instantiates and attaches an Accelerometer to a RigidBodyPlant.
Definition: accelerometer.cc:44
Definition: automotive_demo.cc:105
const InputPort< double > & get_plant_state_input_port() const
Returns the input port that should contain the generalized position and velocity vector of the RigidB...
Definition: accelerometer.h:164
const InputPort< T > & get_input_port(int port_index) const
Returns the typed input port at index port_index.
Definition: system.h:1083
const InputPort< double > & get_plant_state_derivative_input_port() const
Returns the input port that should contain the derivative of the generalized position and velocity ve...
Definition: accelerometer.h:171
Accelerometer(const Accelerometer &)=delete
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:84
A simulated ideal accelerometer that measures the linear acceleration of a frame associated with a Ri...
Definition: accelerometer.h:97
bool get_include_gravity() const
Returns whether gravity is included in this sensor&#39;s measurements.
Definition: accelerometer.h:151
Specializes BasicVector with accessors and setters that are useful for consumers of Accelerometer&#39;s o...
Definition: accelerometer_output.h:19
const OutputPort< T > & get_output_port(int port_index) const
Returns the typed output port at index port_index.
Definition: system.h:1090
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
const OutputPort< double > & get_output_port() const
Returns the state output port, which contains the sensor&#39;s sensed values.
Definition: accelerometer.h:179
Provides careful macros to selectively enable or disable the special member functions for copy-constr...