Drake
trajectory_follower.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <Eigen/Geometry>
4 
5 #include "drake/automotive/gen/simple_car_state.h"
12 
13 namespace drake {
14 namespace automotive {
15 
16 /// TrajectoryFollower simply moves along a pre-established trajectory.
17 ///
18 /// Note that, when T = AutoDiffXd, the AutoDiffXd derivatives for each element
19 /// of the the outputs are empty.
20 ///
21 /// output port 0: A SimpleCarState containing:
22 ///
23 /// * position: x, y, heading;
24 /// heading is 0 rad when pointed +x, pi/2 rad when pointed +y;
25 /// heading is defined around the +z axis, positive-left-turn.
26 /// * speed: s = √{ẋ² + ẏ²}
27 /// (OutputPort getter: state_output())
28 ///
29 /// output port 1: A PoseVector containing X_WA, where A is the agent's
30 /// reference frame.
31 /// (OutputPort getter: pose_output())
32 ///
33 /// output port 2: A FrameVelocity containing the spatial velocity V_WA, where A
34 /// is the agent's reference frame.
35 /// (OutputPort getter: velocity_output())
36 ///
37 /// Instantiated templates for the following kinds of T's are provided:
38 ///
39 /// - double
40 /// - drake::AutoDiffXd
41 /// - symbolic::Expression
42 ///
43 /// They are already available to link against in the containing library.
44 ///
45 /// @ingroup automotive_plants
46 template <typename T>
47 class TrajectoryFollower final : public systems::LeafSystem<T> {
48  public:
50 
51  /// Constructs a TrajectoryFollower system that traces a given Trajectory.
52  ///
53  /// @param trajectory a Trajectory containing the trajectory.
54  /// @param sampling_time_sec the requested sampling time (in sec) for this
55  /// system. @default 0.01.
56  TrajectoryFollower(const Trajectory& trajectory,
57  double sampling_time_sec = 0.01);
58 
59  /// Scalar-converting copy constructor. See @ref system_scalar_conversion.
60  template <typename U>
62  : TrajectoryFollower<T>(other.trajectory_) {}
63 
64  /// @name Accessors for the outputs, as enumerated in the class documentation.
65  /// @{
67  return this->get_output_port(0);
68  }
70  return this->get_output_port(1);
71  }
73  return this->get_output_port(2);
74  }
75  /// @}
76 
77  private:
78  // Converts a PoseVelocity, evaluated at the current time, into a
79  // SimpleCarState output.
80  void CalcStateOutput(const systems::Context<T>& context,
81  SimpleCarState<T>* output_vector) const;
82 
83  // Converts a PoseVelocity, evaluated at the current time, into a PoseVector
84  // output.
85  void CalcPoseOutput(const systems::Context<T>& context,
87 
88  // Converts a PoseVelocity, evaluated at the current time, into a
89  // FrameVelocity output.
90  void CalcVelocityOutput(const systems::Context<T>& context,
92 
93  // Extracts the PoseVelocity value at the current time, as provided in
94  // Context.
95  PoseVelocity GetValues(const systems::Context<T>& context) const;
96 
97  // Allow different specializations to access each other's private data.
98  template <typename>
99  friend class TrajectoryFollower;
100 
101  const Trajectory trajectory_;
102 };
103 
104 } // namespace automotive
105 } // 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
Definition: bullet_model.cc:22
friend class TrajectoryFollower
Definition: trajectory_follower.h:99
Context is an abstract class template that represents all the typed values that are used in a System&#39;...
Definition: context.h:40
const systems::OutputPort< T > & pose_output() const
Definition: trajectory_follower.h:69
An OutputPort belongs to a System and represents the properties of one of that System&#39;s output ports...
Definition: output_port.h:73
TrajectoryFollower simply moves along a pre-established trajectory.
Definition: trajectory_follower.h:47
const systems::OutputPort< T > & velocity_output() const
Definition: trajectory_follower.h:72
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:81
A 7-vector representing the transform of frame A in the world frame, X_WA, in the form {p_WA...
Definition: pose_vector.h:19
const systems::OutputPort< T > & state_output() const
Definition: trajectory_follower.h:66
A class that wraps a piecewise trajectory instantiated from pose data.
Definition: trajectory.h:85
Wraps the raw data contained in a trajectory expressed in terms of pose and velocity values...
Definition: trajectory.h:23
Isometry3< double > pose
Definition: pose_smoother.cc:28
A 6-vector representing the derivatives of the position transform of frame A in the world frame...
Definition: frame_velocity.h:22
boolean< T > T
Definition: drake_bool_deprecated.h:26
TrajectoryFollower(const TrajectoryFollower< U > &other)
Scalar-converting copy constructor. See System Scalar Conversion.
Definition: trajectory_follower.h:61
#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
Vector6< double > velocity
Definition: pose_smoother.cc:29
Provides careful macros to selectively enable or disable the special member functions for copy-constr...