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