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 /// * position: x, y, heading;
23 /// heading is 0 rad when pointed +x, pi/2 rad when pointed +y;
24 /// heading is defined around the +z axis, positive-left-turn.
25 /// * speed: s = √{ẋ² + ẏ²}
26 /// (OutputPort getter: state_output())
27 ///
28 /// output port 1: A PoseVector containing X_WA, where A is the agent's
29 /// reference frame.
30 /// (OutputPort getter: pose_output())
31 ///
32 /// output port 2: A FrameVelocity containing the spatial velocity V_WA, where A
33 /// is the agent's reference frame.
34 /// (OutputPort getter: velocity_output())
35 ///
36 /// Instantiated templates for the following kinds of T's are provided:
37 /// - double
38 /// - drake::AutoDiffXd
39 /// - symbolic::Expression
40 ///
41 /// They are already available to link against in the containing library.
42 ///
43 /// @ingroup automotive_plants
44 template <typename T>
45 class TrajectoryFollower final : public systems::LeafSystem<T> {
46  public:
48 
49  /// Constructs a TrajectoryFollower system that traces a given Trajectory.
50  ///
51  /// @param trajectory a Trajectory containing the trajectory.
52  /// @param sampling_time_sec the requested sampling time (in sec) for this
53  /// system. @default 0.01.
54  TrajectoryFollower(const Trajectory& trajectory,
55  double sampling_time_sec = 0.01);
56 
57  /// Scalar-converting copy constructor. See @ref system_scalar_conversion.
58  template <typename U>
60  : TrajectoryFollower<T>(other.trajectory_) {}
61 
62  /// @name Accessors for the outputs, as enumerated in the class documentation.
63  /// @{
65  return this->get_output_port(0);
66  }
68  return this->get_output_port(1);
69  }
71  return this->get_output_port(2);
72  }
73  /// @}
74 
75  private:
76  // Converts a PoseVelocity, evaluated at the current time, into a
77  // SimpleCarState output.
78  void CalcStateOutput(const systems::Context<T>& context,
79  SimpleCarState<T>* output_vector) const;
80 
81  // Converts a PoseVelocity, evaluated at the current time, into a PoseVector
82  // output.
83  void CalcPoseOutput(const systems::Context<T>& context,
85 
86  // Converts a PoseVelocity, evaluated at the current time, into a
87  // FrameVelocity output.
88  void CalcVelocityOutput(const systems::Context<T>& context,
90 
91  // Extracts the PoseVelocity value at the current time, as provided in
92  // Context.
93  PoseVelocity GetValues(const systems::Context<T>& context) const;
94 
95  // Allow different specializations to access each other's private data.
96  template <typename>
97  friend class TrajectoryFollower;
98 
99  const Trajectory trajectory_;
100 };
101 
102 } // namespace automotive
103 } // namespace drake
Definition: automotive_demo.cc:105
friend class TrajectoryFollower
Definition: trajectory_follower.h:97
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:67
An OutputPort belongs to a System and represents the properties of one of that System&#39;s output ports...
Definition: output_port.h:71
TrajectoryFollower simply moves along a pre-established trajectory.
Definition: trajectory_follower.h:45
const systems::OutputPort< T > & state_output() const
Definition: trajectory_follower.h:64
const systems::OutputPort< T > & velocity_output() const
Definition: trajectory_follower.h:70
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:84
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
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
TrajectoryFollower(const TrajectoryFollower< U > &other)
Scalar-converting copy constructor. See System Scalar Conversion.
Definition: trajectory_follower.h:59
const OutputPort< T > & get_output_port(int port_index) const
Returns the typed output port at index port_index.
Definition: system.h:1090
#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...