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 */);
78  this->DeclareVectorOutputPort(&TrajectoryCar::CalcStateOutput);
79  this->DeclareVectorOutputPort(&TrajectoryCar::CalcPoseOutput);
80  this->DeclareVectorOutputPort(&TrajectoryCar::CalcVelocityOutput);
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  private:
108  /// Data structure returned by CalcRawPose containing raw pose information.
109  struct PositionHeading {
110  Point2 position = Point2::Zero();
111  T heading{0.};
112  };
113 
114  void CalcStateOutput(const systems::Context<T>& context,
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 
127  void CalcVelocityOutput(
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 
135  void DoCalcTimeDerivatives(
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  // Allow different specializations to access each other's private data.
172  template <typename> friend class TrajectoryCar;
173 
174  void ImplCalcOutput(const PositionHeading& raw_pose,
175  const TrajectoryCarState<T>& state,
176  SimpleCarState<T>* output) const {
177  // Convert raw pose to output type.
178  output->set_x(raw_pose.position[0]);
179  output->set_y(raw_pose.position[1]);
180  output->set_heading(raw_pose.heading);
181  output->set_velocity(state.speed());
182  }
183 
184  void ImplCalcPose(const PositionHeading& raw_pose,
185  systems::rendering::PoseVector<T>* pose) const {
186  // Convert the raw pose into a pose vector.
187  pose->set_translation(Eigen::Translation<T, 3>(raw_pose.position[0],
188  raw_pose.position[1], 0));
189  const Vector3<T> z_axis{0.0, 0.0, 1.0};
190  const Eigen::AngleAxis<T> rotation(raw_pose.heading, z_axis);
191  pose->set_rotation(Eigen::Quaternion<T>(rotation));
192  }
193 
194  void ImplCalcVelocity(const PositionHeading& raw_pose,
195  const TrajectoryCarState<T>& state,
196  systems::rendering::FrameVelocity<T>* velocity) const {
197  using std::cos;
198  using std::sin;
199 
200  // Convert the state derivatives into a spatial velocity.
202  output.translational().x() = state.speed() * cos(raw_pose.heading);
203  output.translational().y() = state.speed() * sin(raw_pose.heading);
204  output.translational().z() = T(0);
205  output.rotational().x() = T(0);
206  output.rotational().y() = T(0);
207  // N.B. The instantaneous rotation rate is always zero, as the Curve2 is
208  // constructed from line segments.
209  output.rotational().z() = T(0);
210  velocity->set_velocity(output);
211  }
212 
213  void ImplCalcTimeDerivatives(const TrajectoryCarParams<T>& params,
214  const TrajectoryCarState<T>& state,
215  const systems::BasicVector<T>& input,
216  TrajectoryCarState<T>* rates) const {
217  using std::max;
218 
219  // Create an acceleration profile that caps the maximum speed of the vehicle
220  // as it approaches or exceeds the `params.max_speed()` limit, passing the
221  // input acceleration through when away from the limit. Note that
222  // accelerations of zero are passed through unaffected.
223  const T desired_acceleration = input.GetAtIndex(0);
224  const T smooth_acceleration =
225  calc_smooth_acceleration(desired_acceleration, params.max_speed(),
226  params.speed_limit_kp(), state.speed());
227 
228  // Don't allow small negative velocities to affect position.
229  const T nonneg_velocity = max(T(0), state.speed());
230 
231  rates->set_position(nonneg_velocity);
232  rates->set_speed(smooth_acceleration);
233  }
234 
235  // Extract the appropriately-typed state from the context.
236  const TrajectoryCarState<T>& GetState(
237  const systems::Context<T>& context) const {
238  auto state = dynamic_cast<const TrajectoryCarState<T>*>(
239  &context.get_continuous_state_vector());
240  DRAKE_DEMAND(state != nullptr);
241  return *state;
242  }
243 
244  // Computes the PositionHeading of the trajectory car based on the car's
245  // current position along the curve.
246  const PositionHeading CalcRawPose(const TrajectoryCarState<T>& state) const {
247  using std::atan2;
248 
249  PositionHeading result;
250 
251  // Compute the curve at the current longitudinal (along-curve) position.
252  const typename Curve2<T>::PositionResult pose =
253  curve_.GetPosition(state.position());
254  // TODO(jadecastro): Now that the curve is a function of position rather
255  // than time, we are not acting on a `trajectory` anymore. Rename this
256  // System to PathFollowingCar or something similar.
257  DRAKE_ASSERT(pose.position_dot.norm() > 0.0);
258 
259  result.position = pose.position;
260  result.heading = atan2(pose.position_dot[1], pose.position_dot[0]);
261  return result;
262  }
263 
264  const Curve2<T> curve_;
265 };
266 
267 } // namespace automotive
268 
269 namespace systems {
270 namespace scalar_conversion {
271 // Disable symbolic support, because we use ExtractDoubleOrThrow.
272 template <>
273 struct Traits<automotive::TrajectoryCar> : public NonSymbolicTraits {};
274 } // namespace scalar_conversion
275 } // namespace systems
276 
277 } // 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:564
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:90
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:613
const double position
Definition: robot_plan_interpolator_test.cc:65
Expression atan2(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:666
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:141
Context is an abstract class template that represents all the typed values that are used in a System&#39;...
Definition: context.h:40
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 typed 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:715
const systems::OutputPort< T > & velocity_output() const
Definition: trajectory_car.h:102
VectorBase< T > & get_mutable_vector()
Returns a mutable reference to the entire continuous state vector.
Definition: continuous_state.h:166
This extends InputPortBase with some scalar type-dependent methods.
Definition: input_port_descriptor.h:19
An OutputPort belongs to a System and represents the properties of one of that System&#39;s output ports...
Definition: output_port.h:69
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:1033
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:1438
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:65
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 V_WA.
Definition: frame_velocity.cc:45
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:711
#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:172
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:83
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:37
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:287
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:49
A concrete traits class providing sugar to disable support for symbolic evaluation (i...
Definition: scalar_conversion_traits.h:59
Expression cos(const Expression &e)
Definition: symbolic_expression.cc:621
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 view of, and optionally a container for, all the continuous state variables xc o...
Definition: continuous_state.h:76
const OutputPort< T > & get_output_port(int port_index) const
Returns the typed output port at index port_index.
Definition: system.h:979
#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:56
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...