Drake
gyroscope.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_tree.h"
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 ///
26 /// - `G` be the gyroscope's frame.
27 /// - `W` be the world frame.
28 /// - `ω_G_W` be `G`'s rotational velocity expressed in `W`.
29 /// - `R_GW` be the rotation matrix from `W` to `G`.
30 /// - `ω_G_G` be `G`'s rotational velocity expressed in `G`. This is the output
31 /// of the gyroscope.
32 ///
33 /// The math implemented by this sensor is as follows:
34 ///
35 /// <pre>
36 /// ω_G_G = R_GW * ω_G_W
37 /// </pre>
38 ///
39 /// <B>System Input Ports:</B>
40 ///
41 /// This system has one input port that is accessible via the following
42 /// accessor:
43 ///
44 /// - get_input_port(): Contains the state (i.e., position and velocity)
45 /// vector, `x`, of the RigidBodyPlant being sensed by this sensor.
46 ///
47 /// <B>System Output Ports:</B>
48 ///
49 /// This system has one output port that is accessible via the following
50 /// accessor method:
51 ///
52 /// - get_output_port(): Contains the sensed angular velocity data in this
53 /// sensor's frame. See GyroscopeOutput.
54 ///
55 /// @ingroup sensor_systems
56 ///
57 class Gyroscope : public systems::LeafSystem<double> {
58  public:
60 
61  /// A constructor.
62  ///
63  /// @param[in] name The name of the gyroscope. This can be any value, but
64  /// should typically be unique among all sensors attached to a particular
65  /// model instance within @p tree.
66  ///
67  /// @param[in] frame The frame to which this gyroscope is attached. This
68  /// is the frame in which this sensor's output is given. It need not be in the
69  /// provided `tree`, but must reference a body in the `tree`.
70  ///
71  /// @param[in] tree The RigidBodyTree that belongs to the RigidBodyPlant being
72  /// sensed by this sensor. This should be a reference to the same
73  /// RigidBodyTree that is being used by the RigidBodyPlant whose outputs are
74  /// fed into this sensor. This parameter's lifespan must exceed that of this
75  /// class's instance.
76  ///
77  Gyroscope(const std::string& name, const RigidBodyFrame<double>& frame,
78  const RigidBodyTree<double>& tree);
79 
80  /// Returns the RigidBodyTree that this sensor is sensing.
81  const RigidBodyTree<double>& get_tree() const { return tree_; }
82 
83  /// Returns this sensor's frame, which specifies its location and orientation
84  /// in the RigidBodyTree.
85  ///
86  /// @see get_tree()
87  const RigidBodyFrame<double>& get_frame() const { return frame_; }
88 
89  /// Returns the input port that should contain the generalized (i.e., linear
90  /// and rotational) position and velocity state of the 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 OutputPort< T > & get_output_port(int port_index) const
Returns the typed output port at index port_index.
Definition: system.h:1107
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Definition: bullet_model.cc:22
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
const InputPort< T > & get_input_port(int port_index) const
Returns the typed input port at index port_index.
Definition: system.h:1085
const RigidBodyFrame< double > & get_frame() const
Returns this sensor&#39;s frame, which specifies its location and orientation in the RigidBodyTree.
Definition: gyroscope.h:87
A simulated ideal gyroscope.
Definition: gyroscope.h:57
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:81
Gyroscope(const Gyroscope &)=delete
const InputPort< double > & get_input_port() const
Returns the input port that should contain the generalized (i.e., linear and rotational) position and...
Definition: gyroscope.h:91
const RigidBodyTree< double > & get_tree() const
Returns the RigidBodyTree that this sensor is sensing.
Definition: gyroscope.h:81
Specializes BasicVector with accessors and setters that are useful for consumers of Gyroscope&#39;s outpu...
Definition: gyroscope_output.h:20
#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...