Drake
trajectory_car.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <algorithm>
4 #include <memory>
5 #include <stdexcept>
6 #include <utility>
7 #include <vector>
8 
9 #include <Eigen/Geometry>
10 
13 #include "drake/automotive/gen/simple_car_state.h"
14 #include "drake/automotive/gen/trajectory_car_params.h"
15 #include "drake/automotive/gen/trajectory_car_state.h"
21 
22 namespace drake {
23 namespace automotive {
24 
25 /// TrajectoryCar models a car that follows a pre-established trajectory. Note
26 /// that TrajectoryCar can move forward (up to a given "soft" speed limit) but
27 /// cannot travel in reverse.
28 ///
29 /// parameters:
30 /// * uses systems::Parameters wrapping a TrajectoryCarParams
31 ///
32 /// state vector:
33 /// * A TrajectoryCarState, consisting of a position and speed along the given
34 /// curve, provided as the constructor parameter.
35 ///
36 /// input vector:
37 /// * desired acceleration, a systems::BasicVector of size 1 (optional input).
38 /// If left unconnected, the trajectory car will travel at a constant speed
39 /// specified in the TrajectoryCarState.
40 ///
41 /// output port 0:
42 /// * position: x, y, heading;
43 /// heading is 0 rad when pointed +x, pi/2 rad when pointed +y;
44 /// heading is defined around the +z axis, so positive-turn-left
45 /// * velocity
46 /// (OutputPort getter: raw_pose_output())
47 ///
48 /// output port 1: A PoseVector containing X_WC, where C is the car frame.
49 /// (OutputPort getter: pose_output())
50 ///
51 /// output port 2: A FrameVelocity containing Xdot_WC, where C is the car frame.
52 /// (OutputPort getter: velocity_output())
53 ///
54 /// Instantiated templates for the following kinds of T's are provided:
55 /// - double
56 /// - drake::AutoDiffXd
57 ///
58 /// They are already available to link against in the containing library.
59 ///
60 /// @ingroup automotive_plants
61 template <typename T>
62 class TrajectoryCar final : public systems::LeafSystem<T> {
63  public:
64  typedef typename Curve2<T>::Point2T Point2;
65 
67 
68  /// Constructs a TrajectoryCar system that traces a given two-dimensional @p
69  /// curve. Throws an error if the curve is empty (has a zero @p path_length).
70  explicit TrajectoryCar(const Curve2<double>& curve)
71  : systems::LeafSystem<T>(
72  systems::SystemTypeTag<automotive::TrajectoryCar>{}),
73  curve_(curve.waypoints()) {
74  if (curve_.path_length() == 0.0) {
75  throw std::invalid_argument{"empty curve"};
76  }
77  this->DeclareInputPort(systems::kVectorValued, 1 /* single-valued input */);
81  this->DeclareContinuousState(TrajectoryCarState<T>());
82  this->DeclareNumericParameter(TrajectoryCarParams<T>());
83  }
84 
85  /// Scalar-converting copy constructor. See @ref system_scalar_conversion.
86  template <typename U>
87  explicit TrajectoryCar(const TrajectoryCar<U>& other)
88  : TrajectoryCar<T>(Curve2<double>(other.curve_.waypoints())) {}
89 
90  /// The command input port (optional).
92  return this->get_input_port(0);
93  }
94  /// See class description for details about the following ports.
95  /// @{
97  return this->get_output_port(0);
98  }
100  return this->get_output_port(1);
101  }
103  return this->get_output_port(2);
104  }
105  /// @}
106 
107  protected:
108  /// Data structure returned by CalcRawPose containing raw pose information.
110  Point2 position = Point2::Zero();
111  T heading{0.};
112  };
113 
115  SimpleCarState<T>* output_vector) const {
116  const TrajectoryCarState<T>& state = GetState(context);
117  const auto raw_pose = CalcRawPose(state);
118  ImplCalcOutput(raw_pose, state, output_vector);
119  }
120 
121  void CalcPoseOutput(const systems::Context<T>& context,
123  const auto raw_pose = CalcRawPose(GetState(context));
124  ImplCalcPose(raw_pose, pose);
125  }
126 
128  const systems::Context<T>& context,
130  const TrajectoryCarState<T>& state = GetState(context);
131  const auto raw_pose = CalcRawPose(state);
132  ImplCalcVelocity(raw_pose, state, velocity);
133  }
134 
136  const systems::Context<T>& context,
137  systems::ContinuousState<T>* derivatives) const override {
138  // Obtain the parameters.
139  const TrajectoryCarParams<T>& params =
140  this->template GetNumericParameter<TrajectoryCarParams>(context, 0);
141 
142  // Obtain the state.
143  const TrajectoryCarState<T>* const state =
144  dynamic_cast<const TrajectoryCarState<T>*>(
145  &context.get_continuous_state_vector());
146  DRAKE_ASSERT(state);
147 
148  // Obtain the input.
149  const systems::BasicVector<T>* input =
150  this->template EvalVectorInput<systems::BasicVector>(context, 0);
151 
152  // If the input is null, then apply a default acceleration of zero.
153  const auto default_input = systems::BasicVector<T>::Make(0.);
154  if (input == nullptr) {
155  input = default_input.get();
156  }
157  DRAKE_ASSERT(input->size() == 1); // Expect the input to have only a single
158  // acceleration value.
159 
160  // Obtain the result structure.
161  DRAKE_ASSERT(derivatives != nullptr);
162  systems::VectorBase<T>& vector_derivatives =
163  derivatives->get_mutable_vector();
164  TrajectoryCarState<T>* const rates =
165  dynamic_cast<TrajectoryCarState<T>*>(&vector_derivatives);
166  DRAKE_ASSERT(rates);
167 
168  ImplCalcTimeDerivatives(params, *state, *input, rates);
169  }
170 
171  private:
172  // Allow different specializations to access each other's private data.
173  template <typename> friend class TrajectoryCar;
174 
175  void ImplCalcOutput(const PositionHeading& raw_pose,
176  const TrajectoryCarState<T>& state,
177  SimpleCarState<T>* output) const {
178  // Convert raw pose to output type.
179  output->set_x(raw_pose.position[0]);
180  output->set_y(raw_pose.position[1]);
181  output->set_heading(raw_pose.heading);
182  output->set_velocity(state.speed());
183  }
184 
185  void ImplCalcPose(const PositionHeading& raw_pose,
187  // Convert the raw pose into a pose vector.
188  pose->set_translation(Eigen::Translation<T, 3>(raw_pose.position[0],
189  raw_pose.position[1], 0));
190  const Vector3<T> z_axis{0.0, 0.0, 1.0};
191  const Eigen::AngleAxis<T> rotation(raw_pose.heading, z_axis);
192  pose->set_rotation(Eigen::Quaternion<T>(rotation));
193  }
194 
195  void ImplCalcVelocity(const PositionHeading& raw_pose,
196  const TrajectoryCarState<T>& state,
198  using std::cos;
199  using std::sin;
200 
201  // Convert the state derivatives into a spatial velocity.
203  output.translational().x() = state.speed() * cos(raw_pose.heading);
204  output.translational().y() = state.speed() * sin(raw_pose.heading);
205  output.translational().z() = T(0);
206  output.rotational().x() = T(0);
207  output.rotational().y() = T(0);
208  // N.B. The instantaneous rotation rate is always zero, as the Curve2 is
209  // constructed from line segments.
210  output.rotational().z() = T(0);
211  velocity->set_velocity(output);
212  }
213 
214  void ImplCalcTimeDerivatives(const TrajectoryCarParams<T>& params,
215  const TrajectoryCarState<T>& state,
216  const systems::BasicVector<T>& input,
217  TrajectoryCarState<T>* rates) const {
218  using std::max;
219 
220  // Create an acceleration profile that caps the maximum speed of the vehicle
221  // as it approaches or exceeds the `params.max_speed()` limit, passing the
222  // input acceleration through when away from the limit. Note that
223  // accelerations of zero are passed through unaffected.
224  const T desired_acceleration = input.GetAtIndex(0);
225  const T smooth_acceleration =
226  calc_smooth_acceleration(desired_acceleration, params.max_speed(),
227  params.speed_limit_kp(), state.speed());
228 
229  // Don't allow small negative velocities to affect position.
230  const T nonneg_velocity = max(T(0), state.speed());
231 
232  rates->set_position(nonneg_velocity);
233  rates->set_speed(smooth_acceleration);
234  }
235 
236  // Extract the appropriately-typed state from the context.
237  const TrajectoryCarState<T>& GetState(
238  const systems::Context<T>& context) const {
239  auto state = dynamic_cast<const TrajectoryCarState<T>*>(
240  &context.get_continuous_state_vector());
241  DRAKE_DEMAND(state != nullptr);
242  return *state;
243  }
244 
245  // Computes the PositionHeading of the trajectory car based on the car's
246  // current position along the curve.
247  const PositionHeading CalcRawPose(const TrajectoryCarState<T>& state) const {
248  using std::atan2;
249 
251 
252  // Compute the curve at the current longitudinal (along-curve) position.
253  const typename Curve2<T>::PositionResult pose =
254  curve_.GetPosition(state.position());
255  // TODO(jadecastro): Now that the curve is a function of position rather
256  // than time, we are not acting on a `trajectory` anymore. Rename this
257  // System to PathFollowingCar or something similar.
258  DRAKE_ASSERT(pose.position_dot.norm() > 0.0);
259 
260  result.position = pose.position;
261  result.heading = atan2(pose.position_dot[1], pose.position_dot[0]);
262  return result;
263  }
264 
265  const Curve2<T> curve_;
266 };
267 
268 } // namespace automotive
269 
270 namespace systems {
271 namespace scalar_conversion {
272 // Disable symbolic support, because we use ExtractDoubleOrThrow.
273 template <>
274 struct Traits<automotive::TrajectoryCar> : public NonSymbolicTraits {};
275 } // namespace scalar_conversion
276 } // namespace systems
277 
278 } // namespace drake
This class is used to represent a spatial velocity (also called a twist) that combines rotational (an...
Definition: spatial_force.h:14
int DeclareNumericParameter(const BasicVector< T > &model_vector)
Declares a numeric parameter using the given model_vector.
Definition: leaf_system.h:520
int size() const override
Returns the number of elements in the vector.
Definition: basic_vector.h:62
Point2T position_dot
Definition: curve2.h:57
Definition: automotive_demo.cc:88
Curve2 represents a path through two-dimensional Cartesian space.
Definition: curve2.h:31
const systems::OutputPort< T > & raw_pose_output() const
See class description for details about the following ports.
Definition: trajectory_car.h:96
Expression sin(const Expression &e)
Definition: symbolic_expression.cc:599
Expression atan2(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:652
const systems::InputPortDescriptor< T > & command_input() const
The command input port (optional).
Definition: trajectory_car.h:91
const Vector3< T > & rotational() const
Const access to the rotational component of this spatial vector.
Definition: spatial_vector.h:92
const VectorBase< T > & get_continuous_state_vector() const
Returns a reference to the continuous state vector, devoid of second-order structure.
Definition: context.h:124
Context is an abstract base class template that represents all the inputs to a System: time...
Definition: query_handle.h:10
static std::unique_ptr< BasicVector< T > > Make(const std::initializer_list< T > &data)
Constructs a BasicVector whose elements are the elements of data.
Definition: basic_vector.h:41
VectorBase is an abstract base class that real-valued signals between Systems and real-valued System ...
Definition: vector_base.h:27
Curve2< T >::Point2T Point2
Definition: trajectory_car.h:64
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
Point2T position
Definition: curve2.h:56
void DeclareContinuousState(int num_state_variables)
Declares that this System should reserve continuous state with num_state_variables state variables...
Definition: leaf_system.h:661
T heading
Definition: trajectory_car.h:111
const systems::OutputPort< T > & velocity_output() const
Definition: trajectory_car.h:102
void CalcPoseOutput(const systems::Context< T > &context, systems::rendering::PoseVector< T > *pose) const
Definition: trajectory_car.h:121
VectorBase< T > & get_mutable_vector()
Returns a mutable reference to the entire continuous state vector.
Definition: continuous_state.h:111
InputPortDescriptor is a notation for specifying the kind of input a System accepts, on a given port.
Definition: input_port_descriptor.h:21
An OutputPort belongs to a System and represents the properties of one of that System&#39;s output ports...
Definition: output_port.h:67
std::vector< Number > result
Definition: ipopt_solver.cc:151
Eigen::Matrix< T, 2, 1, Eigen::DontAlign > Point2T
Definition: curve2.h:37
const OutputPort< T > & DeclareVectorOutputPort(const BasicVectorSubtype &model_vector, void(MySystem::*calc)(const Context< T > &, BasicVectorSubtype *) const)
Declares a vector-valued output port by specifying (1) a model vector of type BasicVectorSubtype deri...
Definition: leaf_system.h:827
const InputPortDescriptor< T > & DeclareInputPort(PortDataType type, int size, optional< RandomDistribution > random_type=nullopt)
Adds a port with the specified type and size to the input topology.
Definition: system.h:1459
A result type for the GetPosition method.
Definition: curve2.h:55
#define DRAKE_ASSERT(condition)
DRAKE_ASSERT(condition) is similar to the built-in assert(condition) from the C++ system header <cas...
Definition: drake_assert.h:37
Definition: framework_common.h:25
Eigen::Matrix< Scalar, 3, 1 > Vector3
A column vector of size 3, templated on scalar type.
Definition: eigen_types.h:34
void set_velocity(const multibody::SpatialVelocity< T > &velocity)
Assigns the entire spatial velocity Xdot_WA.
Definition: frame_velocity.cc:39
void CalcStateOutput(const systems::Context< T > &context, SimpleCarState< T > *output_vector) const
Definition: trajectory_car.h:114
const T & GetAtIndex(int index) const override
Returns the element at the given index in the vector.
Definition: basic_vector.h:87
Expression max(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:697
void DoCalcTimeDerivatives(const systems::Context< T > &context, systems::ContinuousState< T > *derivatives) const override
Definition: trajectory_car.h:135
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
friend class TrajectoryCar
Definition: trajectory_car.h:173
Data structure returned by CalcRawPose containing raw pose information.
Definition: trajectory_car.h:109
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:82
A templated traits class for whether an S<T> can be converted into an S<U>; the default value is true...
Definition: scalar_conversion_traits.h:30
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
BasicVector is a semantics-free wrapper around an Eigen vector that satisfies VectorBase.
Definition: basic_vector.h:25
LeafSystem()
Default constructor that declares no inputs, outputs, state, parameters, events, nor scalar-type conv...
Definition: leaf_system.h:267
void CalcVelocityOutput(const systems::Context< T > &context, systems::rendering::FrameVelocity< T > *velocity) const
Definition: trajectory_car.h:127
TrajectoryCar(const TrajectoryCar< U > &other)
Scalar-converting copy constructor. See System Scalar Conversion.
Definition: trajectory_car.h:87
void set_translation(const Eigen::Translation< T, 3 > &q)
Assigns the translation p_WA.
Definition: pose_vector.cc:42
A concrete traits class providing sugar to disable support for symbolic evaluation (i...
Definition: scalar_conversion_traits.h:52
Point2 position
Definition: trajectory_car.h:110
Expression cos(const Expression &e)
Definition: symbolic_expression.cc:607
const Vector3< T > & translational() const
Const access to the translational component of this spatial vector.
Definition: spatial_vector.h:106
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
ContinuousState is a container for all the continuous state variables xc.
Definition: continuous_state.h:44
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
void set_rotation(const Eigen::Quaternion< T > &q)
Assigns the rotation R_WA.
Definition: pose_vector.cc:49
Vector6< double > velocity
Definition: pose_smoother.cc:29
TrajectoryCar models a car that follows a pre-established trajectory.
Definition: trajectory_car.h:62
T calc_smooth_acceleration(const T &desired_acceleration, const T &max_velocity, const T &velocity_limit_kp, const T &current_velocity)
Computes and returns an acceleration command that results in a smooth acceleration profile...
Definition: calc_smooth_acceleration.cc:16
const systems::OutputPort< T > & pose_output() const
Definition: trajectory_car.h:99
Provides careful macros to selectively enable or disable the special member functions for copy-constr...