Drake
lane.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 #include <memory>
5 
6 #include <Eigen/Dense>
7 
14 #include "drake/common/unused.h"
16 
17 namespace drake {
18 namespace maliput {
19 namespace monolane {
20 
22 
23 typedef Vector2<double> V2;
25 
26 /// An R^3 rotation parameterized by roll, pitch, yaw.
27 ///
28 /// This effects a compound rotation around space-fixed x-y-z axes:
29 ///
30 /// Rot3(yaw,pitch,roll) * V = RotZ(yaw) * RotY(pitch) * RotX(roll) * V
31 // TODO(Mitiguy) Deprecate this class in favor of math::RollPitchYaw.
32 class Rot3 {
33  public:
35 
36  Rot3(double roll, double pitch, double yaw) : rpy_(roll, pitch, yaw) {}
37 
38  /// Applies the rotation to a 3-vector.
39  V3 apply(const V3& in) const {
40  const math::RollPitchYaw<double> roll_pitch_yaw(rpy_);
41  return roll_pitch_yaw.ToRotationMatrix() * in;
42  }
43 
44  double yaw() const { return rpy_(2); }
45  double pitch() const { return rpy_(1); }
46  double roll() const { return rpy_(0); }
47 
48  private:
49  Eigen::Matrix<double, 3, 1, Eigen::DontAlign> rpy_;
50 };
51 
52 
53 /// A cubic polynomial, f(p) = a + b*p + c*p^2 + d*p^3.
55  public:
57 
58  /// Default constructor, all zero coefficients.
59  CubicPolynomial() : CubicPolynomial(0., 0., 0., 0.) {}
60 
61  /// Constructs a cubic polynomial given all four coefficients.
62  CubicPolynomial(double a, double b, double c, double d)
63  : a_(a), b_(b), c_(c), d_(d) {
64  const double df = f_p(1.) - f_p(0.);
65  s_1_ = std::sqrt(1. + (df * df));
66  }
67 
68  // Returns the a coefficient.
69  double a() const { return a_; }
70 
71  // Returns the b coefficient.
72  double b() const { return b_; }
73 
74  // Returns the c coefficient.
75  double c() const { return c_; }
76 
77  // Returns the d coefficient.
78  double d() const { return d_; }
79 
80  /// Evaluates the polynomial f at @p p.
81  double f_p(double p) const {
82  return a_ + (b_ * p) + (c_ * p * p) + (d_ * p * p * p);
83  }
84 
85  /// Evaluates the derivative df/dp at @p p.
86  double f_dot_p(double p) const {
87  return b_ + (2. * c_ * p) + (3. * d_ * p * p);
88  }
89 
90  /// Evaluates the double-derivative d^2f/dp^2 at @p p.
91  double f_ddot_p(double p) const {
92  return (2. * c_) + (6. * d_ * p);
93  }
94 
95  // TODO(maddog@tri.global) s_p() and p_s() need to be replaced with a
96  // properly integrated path-length parameterization.
97  // For the moment, we are calculating the length by
98  // approximating the curve with a single linear
99  // segment from (0, f(0)) to (1, f(1)), which is
100  // not entirely awful for relatively flat curves.
101  /// Returns the path-length s along the curve (p, f(p)) from p = 0 to @p p.
102  double s_p(double p) const {
103  return s_1_ * p;
104  }
105 
106  /// Returns the inverse of the path-length parameterization s_p(p).
107  double p_s(double s) const {
108  return s / s_1_;
109  }
110 
111  // TODO(maddog@tri.global) Until s(p) is a properly integrated path-length
112  // parameterization, we have a need to calculate the
113  // derivative of the actual linear function
114  // involved in our bogus path-length approximation.
115  double fake_gprime(double p) const {
116  unused(p);
117  // return df; which is...
118  return f_p(1.) - f_p(0.);
119  }
120 
121  private:
122  double a_{};
123  double b_{};
124  double c_{};
125  double d_{};
126  double s_1_{};
127 };
128 
129 
130 /// Base class for the monolane implementation of api::Lane.
131 class Lane : public api::Lane {
132  public:
134 
135  /// Constructs a Lane.
136  ///
137  /// @param id the ID
138  /// @param segment the Segment to which this Lane will belong, which must
139  /// remain valid for the lifetime of this class
140  /// @param lane_bounds nominal bounds of the lane, uniform along the entire
141  /// reference path, which must be a subset of @p driveable_bounds
142  /// @param driveable_bounds driveable bounds of the lane, uniform along the
143  /// entire reference path
144  /// @param elevation_bounds elevation bounds of the lane, uniform along the
145  /// entire driveable surface
146  /// @param l_max isotropic scale factor for elevation and superelevation
147  /// @param elevation elevation function (see below)
148  /// @param superelevation superelevation function (see below)
149  ///
150  /// This is the base class for subclasses, each of which describe a
151  /// primitive reference curve in the xy ground-plane of the world frame.
152  /// The specific curve is expressed by a subclass's implementations of
153  /// private virtual functions; see the private method xy_of_p().
154  ///
155  /// @p elevation and @p superelevation are cubic-polynomial functions which
156  /// define the elevation and superelevation as a function of position along
157  /// the planar reference curve. @p elevation specifies the z-component of
158  /// the surface at (r,h) = (0,0). @p superelevation specifies the angle
159  /// of the r-axis with respect to the horizon, i.e., how the road twists.
160  /// Thus, non-zero @p superelevation contributes to the z-component at
161  /// r != 0.
162  ///
163  /// These two functions (@p elevation and @p superelevation) must be
164  /// isotropically scaled to operate over the domain p in [0, 1], where
165  /// p is linear in the path-length of the planar reference curve,
166  /// p = 0 corresponds to the start and p = 1 to the end. @p l_max is
167  /// the scale factor. In other words...
168  ///
169  /// Given:
170  /// * a reference curve R(p) parameterized by p in domain [0, 1], which
171  /// has a path-length ℓ(p) in range [0, l_max], linearly related to p,
172  /// where l_max is the total path-length of R (in real-world units);
173  /// * the true elevation function E_true(ℓ), parameterized by the
174  /// path-length ℓ of R;
175  /// * the true superelevation function S_true(ℓ), parameterized by the
176  /// path-length ℓ of R;
177  ///
178  /// then:
179  /// * p = ℓ / l_max;
180  /// * @p elevation is E_scaled = (1 / l_max) * E_true(l_max * p);
181  /// * @p superelevation is S_scaled = (1 / l_max) * S_true(l_max * p).
182  ///
183  /// N.B. The override Lane::ToLanePosition() is currently restricted to lanes
184  /// in which superelevation and elevation change are both zero.
185  Lane(const api::LaneId& id, const api::Segment* segment,
186  const api::RBounds& lane_bounds,
187  const api::RBounds& driveable_bounds,
188  const api::HBounds& elevation_bounds,
189  double l_max,
190  const CubicPolynomial& elevation,
191  const CubicPolynomial& superelevation)
192  : id_(id), segment_(segment),
193  lane_bounds_(lane_bounds),
194  driveable_bounds_(driveable_bounds),
195  elevation_bounds_(elevation_bounds),
196  l_max_(l_max),
197  elevation_(elevation),
198  superelevation_(superelevation) {
199  DRAKE_DEMAND(lane_bounds_.min() >= driveable_bounds_.min());
200  DRAKE_DEMAND(lane_bounds_.max() <= driveable_bounds_.max());
201  }
202 
203  // TODO(maddog@tri.global) Allow superelevation to have a center-of-rotation
204  // which is different from r = 0.
205 
206  const CubicPolynomial& elevation() const { return elevation_; }
207 
208  const CubicPolynomial& superelevation() const { return superelevation_; }
209 
210  void SetStartBp(BranchPoint* bp) { start_bp_ = bp; }
211  void SetEndBp(BranchPoint* bp) { end_bp_ = bp; }
212 
213  BranchPoint* start_bp() { return start_bp_; }
214 
215  BranchPoint* end_bp() { return end_bp_; }
216 
217  ~Lane() override = default;
218 
219  private:
220  const api::LaneId do_id() const override { return id_; }
221 
222  const api::Segment* do_segment() const override;
223 
224  int do_index() const override { return 0; } // Only one lane per segment!
225 
226  const api::Lane* do_to_left() const override { return nullptr; }
227 
228  const api::Lane* do_to_right() const override { return nullptr; }
229 
230  const api::BranchPoint* DoGetBranchPoint(
231  const api::LaneEnd::Which which_end) const override;
232 
233  const api::LaneEndSet* DoGetConfluentBranches(
234  const api::LaneEnd::Which which_end) const override;
235 
236  const api::LaneEndSet* DoGetOngoingBranches(
237  const api::LaneEnd::Which which_end) const override;
238 
239  optional<api::LaneEnd> DoGetDefaultBranch(
240  const api::LaneEnd::Which which_end) const override;
241 
242  double do_length() const override;
243 
244  api::RBounds do_lane_bounds(double) const override { return lane_bounds_; }
245 
246  api::RBounds do_driveable_bounds(double) const override {
247  return driveable_bounds_;
248  }
249 
250  api::HBounds do_elevation_bounds(double, double) const override {
251  return elevation_bounds_;
252  }
253 
254  api::GeoPosition DoToGeoPosition(
255  const api::LanePosition& lane_pos) const override;
256 
257  api::Rotation DoGetOrientation(
258  const api::LanePosition& lane_pos) const override;
259 
260  api::LanePosition DoEvalMotionDerivatives(
262  const api::IsoLaneVelocity& velocity) const override;
263 
264  // The following virtual methods define a reference curve in the xy-plane
265  // of the world frame (i.e., the Earth ground plane). The curve is a
266  // parametric curve:
267  //
268  // F: p --> (x, y) for p in [0, 1]
269  //
270  // defined such that parameter p is linear in the path-length of the curve.
271 
272  // Returns the reference curve itself, F(p).
273  virtual V2 xy_of_p(const double p) const = 0;
274 
275  // Returns the derivative of the curve with respect to p, at p,
276  // i.e., F'(p0) = (dx/dp, dy/dp) at p0
277  virtual V2 xy_dot_of_p(const double p) const = 0;
278 
279  // Returns the heading of the curve at p, i.e., the angle of the tangent
280  // vector (with respect to x-axis) in the increasing-p direction.
281  virtual double heading_of_p(const double p) const = 0;
282 
283  // Returns the derivative of the heading with respect to p,
284  // i.e., d_heading/dp evaluated at p.
285  virtual double heading_dot_of_p(const double p) const = 0;
286 
287 
288  // The geometry here revolves around an abstract "world function"
289  //
290  // W: (p,r,h) --> (x,y,z)
291  //
292  // which maps a `Lane`-frame position to its corresponding representation in
293  // world coordinates (with the caveat that instead of the lane's native
294  // longitudinal coordinate 's', the reference curve parameter 'p' is used).
295  //
296  // W is derived from the three functions which define the lane:
297  //
298  // G: p --> (x,y) = the reference curve, a.k.a. xy_of_p()
299  // Z: p --> z / q_max = the elevation function, a.k.a. elevation_
300  // Θ: p --> θ / q_max = the superelevation function, a.k.a. superelevation_
301  //
302  // as:
303  //
304  // (x,y,z) = W(p,r,h) = (G(p), Z(p)) + R_αβγ*(0,r,h)
305  //
306  // where R_αβγ is the roll/pitch/yaw rotation given by angles:
307  //
308  // α = Θ(p)
309  // β = -atan(dZ/dp) at p
310  // γ = atan2(dG_y/dp, dG_x/dp) at p
311  //
312  // (R_αβγ is essentially the orientation of the (s,r,h) `Lane`-frame
313  // at a location (s,0,0) on the reference-line of the lane. However, it
314  // is *not* necessarily the correct orientation at r != 0 or h != 0.)
315  //
316  // The following methods compute various terms derived from the above which
317  // see repeated use.
318 
319  // Returns the parametric position p along the reference curve corresponding
320  // to longitudinal position @p s along the lane.
321  double p_from_s(const double s) const;
322 
323  // Returns the rotation R_αβγ, evaluated at @p p along the reference curve.
324  Rot3 Rabg_of_p(const double p) const;
325 
326  // Returns W' = ∂W/∂p, the partial differential of W with respect to p,
327  // evaluated at @p p, @p r, @p h.
328  //
329  // (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
330  // avoid recomputing it.)
331  V3 W_prime_of_prh(const double p, const double r, const double h,
332  const Rot3& Rabg) const;
333 
334  // Returns the s-axis unit-vector, expressed in the world frame,
335  // of the (s,r,h) `Lane`-frame (with respect to the world frame).
336  //
337  // (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
338  // avoid recomputing it.)
339  V3 s_hat_of_prh(const double p, const double r, const double h,
340  const Rot3& Rabg) const;
341 
342  // Returns the r-axis unit-vector, expressed in the world frame,
343  // of the (s,r,h) `Lane`-frame (with respect to the world frame).
344  //
345  // (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
346  // avoid recomputing it.)
347  V3 r_hat_of_Rabg(const Rot3& Rabg) const;
348 
349  const api::LaneId id_;
350  const api::Segment* segment_{};
351  BranchPoint* start_bp_{};
352  BranchPoint* end_bp_{};
353 
354  const api::RBounds lane_bounds_;
355  const api::RBounds driveable_bounds_;
356  const api::HBounds elevation_bounds_;
357  const double l_max_{};
358  const CubicPolynomial elevation_;
359  const CubicPolynomial superelevation_;
360 };
361 
362 
363 } // namespace monolane
364 } // namespace maliput
365 } // namespace drake
This class represents the orientation between two arbitrary frames A and D associated with a Space-fi...
Definition: roll_pitch_yaw.h:64
double b() const
Definition: lane.h:72
BranchPoint * start_bp()
Definition: lane.h:213
const api::Segment * segment_
Definition: dragway_test.cc:338
double f_dot_p(double p) const
Evaluates the derivative df/dp at p.
Definition: lane.h:86
CubicPolynomial(double a, double b, double c, double d)
Constructs a cubic polynomial given all four coefficients.
Definition: lane.h:62
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
A cubic polynomial, f(p) = a + b*p + c*p^2 + d*p^3.
Definition: lane.h:54
A BranchPoint is a node in the network of a RoadGeometry at which Lanes connect to one another...
Definition: branch_point.h:67
const CubicPolynomial & superelevation() const
Definition: lane.h:208
Definition: bullet_model.cc:22
double d() const
Definition: lane.h:78
const double position
Definition: robot_plan_interpolator_test.cc:65
const Variable a_
Definition: symbolic_decompose_test.cc:35
Eigen::Matrix< Scalar, 2, 1 > Vector2
A column vector of size 2, templated on scalar type.
Definition: eigen_types.h:30
double pitch() const
Definition: lane.h:45
Vector2< double > V2
Definition: lane.h:21
A set of LaneEnds.
Definition: branch_point.h:23
A Lane represents a lane of travel in a road network.
Definition: lane.h:35
stx::optional< T > optional
Definition: drake_optional.h:22
double p_s(double s) const
Returns the inverse of the path-length parameterization s_p(p).
Definition: lane.h:107
Isometric velocity vector in a Lane-frame.
Definition: lane_data.h:283
double yaw() const
Definition: lane.h:44
const Variable b_
Definition: symbolic_decompose_test.cc:36
#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:57
double s_p(double p) const
Returns the path-length s along the curve (p, f(p)) from p = 0 to p.
Definition: lane.h:102
const CubicPolynomial & elevation() const
Definition: lane.h:206
TypeSpecificIdentifier< class Lane > LaneId
Persistent identifier for a Lane element.
Definition: lane.h:23
void SetEndBp(BranchPoint *bp)
Definition: lane.h:211
RotationMatrix< T > ToRotationMatrix() const
Returns the RotationMatrix representation of this RollPitchYaw.
Definition: roll_pitch_yaw.cc:18
#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
const api::HBounds elevation_bounds
Definition: multilane_line_road_curve_test.cc:34
GeoPositionT< double > GeoPosition
Definition: lane_data.h:186
Provides drake::optional as an alias for the appropriate implementation of std::optional or std::expe...
Bounds in the lateral dimension (r component) of a Lane-frame, consisting of a pair of minimum and ma...
Definition: lane_data.h:315
A 3-dimensional rotation.
Definition: lane_data.h:56
Base class for the monolane implementation of api::Lane.
Definition: lane.h:131
Vector3< double > V3
Definition: lane.h:24
double c() const
Definition: lane.h:75
An R^3 rotation parameterized by roll, pitch, yaw.
Definition: lane.h:32
An api::Segment implementation.
Definition: segment.h:20
void SetStartBp(BranchPoint *bp)
Definition: lane.h:210
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:360
A Segment represents a bundle of adjacent Lanes which share a continuously traversable road surface...
Definition: segment.h:27
Which
Labels for the endpoints of a Lane.
Definition: lane_data.h:27
An implementation of api::BranchPoint.
Definition: branch_point.h:42
BranchPoint * end_bp()
Definition: lane.h:215
double a() const
Definition: lane.h:69
double f_ddot_p(double p) const
Evaluates the double-derivative d^2f/dp^2 at p.
Definition: lane.h:91
Expression sqrt(const Expression &e)
Definition: symbolic_expression.cc:566
double f_p(double p) const
Evaluates the polynomial f at p.
Definition: lane.h:81
V3 apply(const V3 &in) const
Applies the rotation to a 3-vector.
Definition: lane.h:39
double fake_gprime(double p) const
Definition: lane.h:115
#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
double roll() const
Definition: lane.h:46
void unused(const Args &...)
Documents the argument(s) as unused, placating GCC&#39;s -Wunused-parameter warning.
Definition: unused.h:51
const Variable c_
Definition: symbolic_decompose_test.cc:37
Provides careful macros to selectively enable or disable the special member functions for copy-constr...