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