Drake
gyroscope.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <string>
5 
14 
15 namespace drake {
16 namespace systems {
17 namespace sensors {
18 
19 /// A simulated ideal gyroscope. It measures angular velocity along three
20 /// dimensions.
21 ///
22 /// <B>The Gyroscope Math:</B>
23 ///
24 /// Let:
25 /// - `G` be the gyroscope's frame.
26 /// - `W` be the world frame.
27 /// - `ω_G_W` be `G`'s rotational velocity expressed in `W`.
28 /// - `R_GW` be the rotation matrix from `W` to `G`.
29 /// - `ω_G_G` be `G`'s rotational velocity expressed in `G`. This is the output
30 /// of the gyroscope.
31 ///
32 /// The math implemented by this sensor is as follows:
33 ///
34 /// <pre>
35 /// ω_G_G = R_GW * ω_G_W
36 /// </pre>
37 ///
38 /// <B>System Input Ports:</B>
39 ///
40 /// This system has one input port that is accessible via the following
41 /// accessor:
42 ///
43 /// - get_input_port(): Contains the state (i.e., position and velocity)
44 /// vector, `x`, of the RigidBodyPlant being sensed by this sensor.
45 ///
46 /// <B>System Output Ports:</B>
47 ///
48 /// This system has one output port that is accessible via the following
49 /// accessor method:
50 ///
51 /// - get_output_port(): Contains the sensed angular velocity data in this
52 /// sensor's frame. See GyroscopeOutput.
53 ///
54 /// @ingroup sensor_systems
55 ///
56 class Gyroscope : public systems::LeafSystem<double> {
57  public:
59 
60  /// A constructor.
61  ///
62  /// @param[in] name The name of the gyroscope. This can be any value, but
63  /// should typically be unique among all sensors attached to a particular
64  /// model instance within @p tree.
65  ///
66  /// @param[in] frame The frame to which this gyroscope is attached. This
67  /// is the frame in which this sensor's output is given. It need not be in the
68  /// provided `tree`, but must reference a body in the `tree`.
69  ///
70  /// @param[in] tree The RigidBodyTree that belongs to the RigidBodyPlant being
71  /// sensed by this sensor. This should be a reference to the same
72  /// RigidBodyTree that is being used by the RigidBodyPlant whose outputs are
73  /// fed into this sensor. This parameter's lifespan must exceed that of this
74  /// class's instance.
75  ///
76  Gyroscope(const std::string& name, const RigidBodyFrame<double>& frame,
77  const RigidBodyTree<double>& tree);
78 
79  /// Returns the RigidBodyTree that this sensor is sensing.
80  const RigidBodyTree<double>& get_tree() const { return tree_; }
81 
82  /// Returns this sensor's frame, which specifies its location and orientation
83  /// in the RigidBodyTree.
84  ///
85  /// @see get_tree()
86  const RigidBodyFrame<double>& get_frame() const { return frame_; }
87 
88  /// Returns a descriptor of the input port that should contain the generalized
89  /// (i.e., linear and rotational) position and velocity state of the
90  /// RigidBodyTree DOFs.
92  return System<double>::get_input_port(input_port_index_);
93  }
94 
95  /// Returns a the state output port, which contains the sensor's
96  /// sensed values.
98  return System<double>::get_output_port(output_port_index_);
99  }
100 
101  private:
102  // Computes the angular velocity as sensed by this sensor.
103  void CalcAngularVelocity(const Context<double>& context,
104  GyroscopeOutput<double>* output_vector) const;
105 
106  const std::string name_;
107  const RigidBodyFrame<double> frame_;
108  const RigidBodyTree<double>& tree_;
109 
110  int input_port_index_{};
111  int output_port_index_{};
112 };
113 
114 } // namespace sensors
115 } // namespace systems
116 } // namespace drake
const RigidBodyFrame< double > & get_frame() const
Returns this sensor&#39;s frame, which specifies its location and orientation in the RigidBodyTree.
Definition: gyroscope.h:86
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Definition: automotive_demo.cc:88
const InputPortDescriptor< double > & get_input_port() const
Returns a descriptor of the input port that should contain the generalized (i.e., linear and rotation...
Definition: gyroscope.h:91
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
A simulated ideal gyroscope.
Definition: gyroscope.h:56
const OutputPort< double > & get_output_port() const
Returns a the state output port, which contains the sensor&#39;s sensed values.
Definition: gyroscope.h:97
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:82
Gyroscope(const Gyroscope &)=delete
Specializes BasicVector with accessors and setters that are useful for consumers of Gyroscope&#39;s outpu...
Definition: gyroscope_output.h:20
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 RigidBodyTree< double > & get_tree() const
Returns the RigidBodyTree that this sensor is sensing.
Definition: gyroscope.h:80
Provides careful macros to selectively enable or disable the special member functions for copy-constr...