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