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 
22 
23 namespace drake {
24 namespace automotive {
25 
62 template <typename T>
63 class TrajectoryCar final : public systems::LeafSystem<T> {
64  public:
65  typedef typename Curve2<double>::Point2 Point2d;
66 
68 
69 
70  explicit TrajectoryCar(Curve2<double> curve)
72  : systems::LeafSystem<T>(
73  systems::SystemTypeTag<automotive::TrajectoryCar>{}),
74  curve_(std::move(curve)) {
75  if (curve_.path_length() == 0.0) {
76  throw std::invalid_argument{"empty curve"};
77  }
78  this->DeclareInputPort(systems::kVectorValued, 1 /* single-valued input */);
84  }
85 
87  template <typename U>
88  explicit TrajectoryCar(const TrajectoryCar<U>& other)
89  : TrajectoryCar<T>(other.curve_) {}
90 
93  return this->get_input_port(0);
94  }
98  return this->get_output_port(0);
99  }
101  return this->get_output_port(1);
102  }
104  return this->get_output_port(2);
105  }
107 
108  protected:
111  Point2d position = Point2d(Point2d::Zero());
112  double heading{0.};
113  };
114 
116  SimpleCarState<T>* output_vector) const {
117  const TrajectoryCarState<T>& state = GetState(context);
118  const auto raw_pose = CalcRawPose(state);
119  ImplCalcOutput(raw_pose, state, output_vector);
120  }
121 
122  void CalcPoseOutput(const systems::Context<T>& context,
124  const auto raw_pose = CalcRawPose(GetState(context));
125  ImplCalcPose(raw_pose, pose);
126  }
127 
129  const systems::Context<T>& context,
131  const TrajectoryCarState<T>& state = GetState(context);
132  const auto raw_pose = CalcRawPose(state);
133  ImplCalcVelocity(raw_pose, state, velocity);
134  }
135 
137  const systems::Context<T>& context,
138  systems::ContinuousState<T>* derivatives) const override {
139  // Obtain the parameters.
140  const TrajectoryCarParams<T>& params =
141  this->template GetNumericParameter<TrajectoryCarParams>(context, 0);
142 
143  // Obtain the state.
144  const TrajectoryCarState<T>* const state =
145  dynamic_cast<const TrajectoryCarState<T>*>(
146  &context.get_continuous_state_vector());
147  DRAKE_ASSERT(state);
148 
149  // Obtain the input.
150  const systems::BasicVector<T>* input =
151  this->template EvalVectorInput<systems::BasicVector>(context, 0);
152 
153  // If the input is null, then apply a default acceleration of zero.
154  const auto default_input = systems::BasicVector<T>::Make(0.);
155  if (input == nullptr) {
156  input = default_input.get();
157  }
158  DRAKE_ASSERT(input->size() == 1); // Expect the input to have only a single
159  // acceleration value.
160 
161  // Obtain the result structure.
162  DRAKE_ASSERT(derivatives != nullptr);
163  systems::VectorBase<T>* const vector_derivatives =
164  derivatives->get_mutable_vector();
165  DRAKE_ASSERT(vector_derivatives);
166  TrajectoryCarState<T>* const rates =
167  dynamic_cast<TrajectoryCarState<T>*>(vector_derivatives);
168  DRAKE_ASSERT(rates);
169 
170  ImplCalcTimeDerivatives(params, *state, *input, rates);
171  }
172 
173  private:
174  // Allow different specializations to access each other's private data.
175  template <typename> friend class TrajectoryCar;
176 
177  void ImplCalcOutput(const PositionHeading& raw_pose,
178  const TrajectoryCarState<T>& state,
179  SimpleCarState<T>* output) const {
180  // Convert raw pose to output type.
181  output->set_x(raw_pose.position[0]);
182  output->set_y(raw_pose.position[1]);
183  output->set_heading(raw_pose.heading);
184  output->set_velocity(state.speed());
185  }
186 
187  void ImplCalcPose(const PositionHeading& raw_pose,
189  // Convert the raw pose into a pose vector.
190  pose->set_translation(Eigen::Translation<T, 3>(raw_pose.position[0],
191  raw_pose.position[1], 0));
192  const Vector3<T> z_axis{0.0, 0.0, 1.0};
193  const Eigen::AngleAxis<T> rotation(raw_pose.heading, z_axis);
194  pose->set_rotation(Eigen::Quaternion<T>(rotation));
195  }
196 
197  void ImplCalcVelocity(const PositionHeading& raw_pose,
198  const TrajectoryCarState<T>& state,
200  using std::cos;
201  using std::sin;
202 
203  // Convert the state derivatives into a spatial velocity.
205  output.translational().x() = state.speed() * cos(raw_pose.heading);
206  output.translational().y() = state.speed() * sin(raw_pose.heading);
207  output.translational().z() = T(0);
208  output.rotational().x() = T(0);
209  output.rotational().y() = T(0);
210  // N.B. The instantaneous rotation rate is always zero, as the Curve2 is
211  // constructed from line segments.
212  output.rotational().z() = T(0);
213  velocity->set_velocity(output);
214  }
215 
216  void ImplCalcTimeDerivatives(const TrajectoryCarParams<T>& params,
217  const TrajectoryCarState<T>& state,
218  const systems::BasicVector<T>& input,
219  TrajectoryCarState<T>* rates) const {
220  using std::max;
221 
222  // Create an acceleration profile that caps the maximum speed of the vehicle
223  // as it approaches or exceeds the `params.max_speed()` limit, passing the
224  // input acceleration through when away from the limit. Note that
225  // accelerations of zero are passed through unaffected.
226  const T desired_acceleration = input.GetAtIndex(0);
227  const T smooth_acceleration =
228  calc_smooth_acceleration(desired_acceleration, params.max_speed(),
229  params.speed_limit_kp(), state.speed());
230 
231  // Don't allow small negative velocities to affect position.
232  const T nonneg_velocity = max(T(0), state.speed());
233 
234  rates->set_position(nonneg_velocity);
235  rates->set_speed(smooth_acceleration);
236  }
237 
238  // Extract the appropriately-typed state from the context.
239  const TrajectoryCarState<T>& GetState(
240  const systems::Context<T>& context) const {
241  auto state = dynamic_cast<const TrajectoryCarState<T>*>(
242  &context.get_continuous_state_vector());
243  DRAKE_DEMAND(state != nullptr);
244  return *state;
245  }
246 
247  // Computes the PositionHeading of the trajectory car based on the car's
248  // current position along the curve.
249  const PositionHeading CalcRawPose(const TrajectoryCarState<T>& state) const {
250  using std::atan2;
251 
253 
254  // Compute the curve at the current longitudinal (along-curve) position.
255  const typename Curve2<double>::PositionResult pose =
256  curve_.GetPosition(ExtractDoubleOrThrow(state.position()));
257  // TODO(jadecastro): Now that the curve is a function of position rather
258  // than time, we are not acting on a `trajectory` anymore. Rename this
259  // System to PathFollowingCar or something similar.
260  DRAKE_ASSERT(pose.position_dot.norm() > 0.0);
261 
262  result.position = pose.position;
263  result.heading = atan2(pose.position_dot[1], pose.position_dot[0]);
264  return result;
265  }
266 
267  const Curve2<double> curve_;
268 };
269 
270 } // namespace automotive
271 
272 namespace systems {
273 namespace scalar_conversion {
274 // Disable symbolic support, because we use ExtractDoubleOrThrow.
275 template <>
276 struct Traits<automotive::TrajectoryCar> : public NonSymbolicTraits {};
277 } // namespace scalar_conversion
278 } // namespace systems
279 
280 } // 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:547
int size() const override
Returns the number of elements in the vector.
Definition: basic_vector.h:62
Definition: automotive_demo.cc:88
Curve2 represents a path through two-dimensional Cartesian space.
Definition: curve2.h:30
const systems::OutputPort< T > & raw_pose_output() const
See class description for details about the following ports.
Definition: trajectory_car.h:97
Specializes BasicVector with specific getters and setters.
Definition: trajectory_car_params.h:37
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:92
const T & speed() const
The along-curve speed of the vehicle.
Definition: trajectory_car_state.h:59
const Vector3< T > & rotational() const
Const access to the rotational component of this spatial vector.
Definition: spatial_vector.h:91
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
void set_y(const T &y)
Definition: simple_car_state.h:58
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
const InputPortDescriptor< T > & get_input_port(int port_index) const
Returns the descriptor of the input port at index port_index.
Definition: system.h:815
void DeclareContinuousState(int num_state_variables)
Declares that this System should reserve continuous state with num_state_variables state variables...
Definition: leaf_system.h:679
const systems::OutputPort< T > & velocity_output() const
Definition: trajectory_car.h:103
void CalcPoseOutput(const systems::Context< T > &context, systems::rendering::PoseVector< T > *pose) const
Definition: trajectory_car.h:122
InputPortDescriptor is a notation for specifying the kind of input a System accepts, on a given port.
Definition: input_port_descriptor.h:21
void set_heading(const T &heading)
Definition: simple_car_state.h:61
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
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:845
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:1302
#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:39
Definition: system_common.h:11
Eigen::Matrix< Scalar, 3, 1 > Vector3
A column vector of size 3, templated on scalar type.
Definition: eigen_types.h:34
Curve2< double >::Point2 Point2d
Definition: trajectory_car.h:65
void set_velocity(const multibody::SpatialVelocity< T > &velocity)
Assigns the entire spatial velocity Xdot_WA.
Definition: frame_velocity.cc:42
const T & speed_limit_kp() const
The smoothing constant for min/max speed limits.
Definition: trajectory_car_params.h:66
void CalcStateOutput(const systems::Context< T > &context, SimpleCarState< T > *output_vector) const
Definition: trajectory_car.h:115
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:136
void set_x(const T &x)
Definition: simple_car_state.h:55
double ExtractDoubleOrThrow(const Eigen::AutoDiffScalar< DerType > &scalar)
Returns the autodiff scalar&#39;s value() as a double.
Definition: autodiff_overloads.h:134
#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:47
friend class TrajectoryCar
Definition: trajectory_car.h:175
Data structure returned by CalcRawPose containing raw pose information.
Definition: trajectory_car.h:110
Point2d position
Definition: trajectory_car.h:111
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:32
VectorBase< T > * get_mutable_vector()
Returns a mutable pointer to the entire continuous state vector, which is never nullptr.
Definition: continuous_state.h:109
const T & position() const
The along-curve position of the vehicle.
Definition: trajectory_car_state.h:54
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
void set_position(const T &position)
Definition: trajectory_car_state.h:55
void set_speed(const T &speed)
Definition: trajectory_car_state.h:60
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:286
T path_length() const
Definition: curve2.h:50
void CalcVelocityOutput(const systems::Context< T > &context, systems::rendering::FrameVelocity< T > *velocity) const
Definition: trajectory_car.h:128
TrajectoryCar(const TrajectoryCar< U > &other)
Scalar-converting copy constructor. See System Scalar Conversion.
Definition: trajectory_car.h:88
void set_translation(const Eigen::Translation< T, 3 > &q)
Assigns the translation p_WA.
Definition: pose_vector.cc:46
A concrete traits class providing sugar to disable support for symbolic evaluation (i...
Definition: scalar_conversion_traits.h:54
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:105
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
Specializes BasicVector with specific getters and setters.
Definition: simple_car_state.h:39
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:826
double heading
Definition: trajectory_car.h:112
#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:35
void set_rotation(const Eigen::Quaternion< T > &q)
Assigns the rotation R_WA.
Definition: pose_vector.cc:53
Vector6< double > velocity
Definition: pose_smoother.cc:29
Specializes BasicVector with specific getters and setters.
Definition: trajectory_car_state.h:37
const T & max_speed() const
The limit on the car&#39;s forward speed.
Definition: trajectory_car_params.h:59
TrajectoryCar models a car that follows a pre-established trajectory.
Definition: trajectory_car.h:63
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:17
const systems::OutputPort< T > & pose_output() const
Definition: trajectory_car.h:100
void set_velocity(const T &velocity)
Definition: simple_car_state.h:64
Provides careful macros to selectively enable or disable the special member functions for copy-constr...