Drake
road_curve.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <utility>
4 
5 #include <Eigen/Dense>
6 
11 #include "drake/common/unused.h"
13 
14 namespace drake {
15 namespace maliput {
16 namespace multilane {
17 
18 /// An R^3 rotation parameterized by roll, pitch, yaw.
19 ///
20 /// This effects a compound rotation around space-fixed x-y-z axes:
21 ///
22 /// Rot3(roll, pitch, yaw) * V = RotZ(yaw) * RotY(pitch) * RotX(roll) * V
23 class Rot3 {
24  public:
26 
27  Rot3(double roll, double pitch, double yaw) : rpy_(roll, pitch, yaw) {}
28 
29  /// Applies the rotation to a 3-vector.
31  return math::rpy2rotmat(rpy_) * in;
32  }
33 
34  double yaw() const { return rpy_(2); }
35  double pitch() const { return rpy_(1); }
36  double roll() const { return rpy_(0); }
37 
38  private:
39  Eigen::Matrix<double, 3, 1, Eigen::DontAlign> rpy_;
40 };
41 
42 /// Defines an interface for a path in a Segment object surface. The path is
43 /// defined by an elevation and superelevation CubicPolynomial objects and a
44 /// reference curve. This reference curve is a C1 function in the z=0 plane.
45 /// Its domain is constrained in [0;1] interval and it should map a ℝ² curve.
46 /// As per notation, p is the parameter of the reference curve, not necessarily
47 /// arc length s, and function interpolations and function derivatives as well
48 /// as headings and heading derivatives are expressed in world coordinates,
49 /// which is the same frame as api::GeoPosition.
50 /// By implementing this interface the road curve is defined and complete.
51 ///
52 /// The geometry here revolves around an abstract "world function"
53 ///
54 /// W: (p,r,h) --> (x,y,z)
55 ///
56 /// which maps a `Lane`-frame position to its corresponding representation in
57 /// world coordinates (with the caveat that instead of the lane's native
58 /// longitudinal coordinate 's', the reference curve parameter 'p' is used).
59 ///
60 /// W is derived from the three functions which define the lane:
61 ///
62 /// G: p --> (x,y) = the reference curve, a.k.a. xy_of_p()
63 /// Z: p --> z / q_max = the elevation function, a.k.a. elevation_
64 /// Θ: p --> θ / q_max = the superelevation function, a.k.a. superelevation_
65 ///
66 /// as:
67 ///
68 /// (x,y,z) = W(p,r,h) = (G(p), Z(p)) + R_αβγ*(0,r,h)
69 ///
70 /// where R_αβγ is the roll/pitch/yaw rotation given by angles:
71 ///
72 /// α = Θ(p)
73 /// β = -atan(dZ/dp) at p
74 /// γ = atan2(dG_y/dp, dG_x/dp) at p
75 ///
76 /// (R_αβγ is essentially the orientation of the (s,r,h) `Lane`-frame
77 /// at a location (s,0,0) on the reference-line of the lane. However, it
78 /// is *not* necessarily the correct orientation at r != 0 or h != 0.)
79 ///
80 /// The W(p,r,h) "world function" is defined by the RoadCurve referenced by a
81 /// Lane's Segment. A Lane is also defined by a r0 lateral offset with respect
82 /// to the reference curve of the RoadCurve. Thus, a mapping from the local
83 /// (s,r,h) lane-frame of the Lane becomes:
84 ///
85 /// (x,y,z) = L(s,r,h) = W(P(s, r0), r + r0, h),
86 ///
87 /// where P:(s, r0) --> (p) is a (potentially non-linear) function dependent on
88 /// the RoadCurve's reference-curve, elevation, and superelevation functions.
89 ///
90 /// TODO(maddog-tri) Add support for Lanes with both non-zero r0 and
91 /// superelevation polynomial.
92 class RoadCurve {
93  public:
95 
96  virtual ~RoadCurve() = default;
97 
98  const CubicPolynomial& elevation() const { return elevation_; }
99 
100  const CubicPolynomial& superelevation() const { return superelevation_; }
101 
102  /// Computes the parametric position p along the reference curve corresponding
103  /// to longitudinal position (in path-length) `s` along a parallel curve
104  /// laterally offset by `r` from the reference curve.
105  /// @return The parametric position p along an offset of the reference curve.
106  virtual double p_from_s(double s, double r) const = 0;
107 
108  /// Computes the path length integral in the interval of the parameter [0; p]
109  /// and along a parallel curve laterally offset by `r` the planar reference
110  /// curve.
111  /// @return The path length integral of the curve composed with the elevation
112  /// polynomial.
113  virtual double s_from_p(double p, double r) const = 0;
114 
115  /// Computes the reference curve.
116  /// @param p The reference curve parameter.
117  /// @return The reference curve itself, F(p).
118  virtual Vector2<double> xy_of_p(double p) const = 0;
119 
120  /// Computes the first derivative of the reference curve.
121  /// @param p The reference curve parameter.
122  /// @return The derivative of the curve with respect to p, at @p p, i.e.,
123  /// F'(p0) = (dx/dp, dy/dp) at p0.
124  virtual Vector2<double> xy_dot_of_p(double p) const = 0;
125 
126  /// Computes the heading of the reference curve.
127  /// @param p The reference curve parameter.
128  /// @return The heading of the curve at @p p, i.e.,
129  /// the angle of the tangent vector (with respect to x-axis) in the
130  /// increasing-p direction.
131  virtual double heading_of_p(double p) const = 0;
132 
133  /// Computes the first derivative heading of the reference curve.
134  /// @param p The reference curve parameter.
135  /// @return The derivative of the heading with respect to p, i.e.,
136  /// d_heading/dp evaluated at @p p.
137  virtual double heading_dot_of_p(double p) const = 0;
138 
139  /// Computes the path length integral of the reference curve for the interval
140  /// [0;1] of p.
141  /// @return The path length integral of the reference curve.
142  // TODO(maddog-tri) This method should be renamed to match the Maliput's
143  // documentation as well as other variable names along the
144  // implementation.
145  virtual double p_scale() const = 0;
146 
147  /// Converts a @p geo_coordinate in the world frame to the composed curve
148  /// frame, i.e., the superposition of the reference curve, elevation and
149  /// superelevation polynomials. The resulting coordinates [p, r, h] are
150  /// saturated in the following domain ranges.
151  ///
152  /// - p: [0, 1]
153  /// - r: [@p r_min, @p r_max]
154  /// - h: [@p height_bounds]
155  /// @param geo_coordinate A 3D vector in the world frame to be converted to
156  /// the composed curve frame.
157  /// @param r_min Minimum lateral distance from the composed curve to saturate,
158  /// if it is necessary, the result in the given direction.
159  /// @param r_max Maximum lateral distance from the composed curve to evaluate,
160  /// if it is necessary, the result in the given direction
161  /// @param height_bounds An api::HBounds object that represents the elevation
162  /// bounds of the surface mapping.
163  /// @return A 3D vector [p, r, h], that represent the domain coordinates of
164  /// the world function, that gives as world function output @p geo_cooridnate.
165  virtual Vector3<double> ToCurveFrame(
166  const Vector3<double>& geo_coordinate,
167  double r_min, double r_max,
168  const api::HBounds& height_bounds) const = 0;
169 
170  /// Checks that there are no self-intersections (singularities) in the volume
171  /// created by applying the constant @p r_min, @p r_max and @p height_bounds
172  /// to the RoadCurve.
173  /// @param r_min Minimum lateral distance from the composed curve to evaluate
174  /// the validity of the geometry.
175  /// @param r_max Maximum lateral distance from the composed curve to evaluate
176  /// the validity of the geometry.
177  /// @param height_bounds An api::HBounds object that represents the elevation
178  /// bounds of the surface mapping.
179  /// @return True when there are no self-intersections.
180  virtual bool IsValid(double r_min, double r_max,
181  const api::HBounds& height_bounds) const = 0;
182 
183  /// Returns W, the world function evaluated at @p p, @p r, @p h.
184  Vector3<double> W_of_prh(double p, double r, double h) const;
185 
186  /// Returns W' = ∂W/∂p, the partial differential of W with respect to p,
187  /// evaluated at @p p, @p r, @p h.
188  ///
189  /// (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
190  /// avoid recomputing it.)
191  /// (@p g_prime must be the result of elevation().f_dot_p(p) --- passed in
192  /// here to avoid recomputing it.)
193  Vector3<double> W_prime_of_prh(double p, double r, double h, const Rot3& Rabg,
194  double g_prime) const;
195 
196  /// Returns the rotation R_αβγ, evaluated at @p p along the reference curve.
197  Rot3 Rabg_of_p(double p) const;
198 
199  /// Returns the rotation R_αβγ, evaluated at @p p, @p r and @p h.
200  Rot3 Orientation(double p, double r, double h) const;
201 
202  /// Returns the s-axis unit-vector, expressed in the world frame,
203  /// of the (s,r,h) `Lane`-frame (with respect to the world frame).
204  ///
205  /// (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
206  /// avoid recomputing it.)
207  /// (@p g_prime must be the result of elevation().f_dot_p(p) --- passed in
208  /// here to avoid recomputing it.)
209  Vector3<double> s_hat_of_prh(double p, double r, double h, const Rot3& Rabg,
210  double g_prime) const;
211 
212  /// Returns the r-axis unit-vector, expressed in the world frame,
213  /// of the (s,r,h) `Lane`-frame (with respect to the world frame).
214  ///
215  /// (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
216  /// avoid recomputing it.)
217  Vector3<double> r_hat_of_Rabg(const Rot3& Rabg) const;
218 
219  protected:
220  /// Constructs a road curve given elevation and superelevation curves.
221  /// @param elevation CubicPolynomial object that represents the elevation
222  /// function (see below for more details).
223  /// @param superelevation CubicPolynomial object that represents the
224  /// superelevation function (see below for more details).
225  ///
226  /// @p elevation and @p superelevation are cubic-polynomial functions which
227  /// define the elevation and superelevation as a function of position along
228  /// the planar reference curve. @p elevation specifies the z-component of
229  /// the surface at (r,h) = (0,0). @p superelevation specifies the angle
230  /// of the r-axis with respect to the horizon, i.e., how the road twists.
231  /// Thus, non-zero @p superelevation contributes to the z-component at
232  /// r != 0.
233  ///
234  /// These two functions (@p elevation and @p superelevation) must be
235  /// isotropically scaled to operate over the domain p in [0, 1], where
236  /// p is linear in the path-length of the planar reference curve,
237  /// p = 0 corresponds to the start and p = 1 to the end. p_scale() is
238  /// the scale factor. In other words...
239  ///
240  /// Given:
241  /// * a reference curve R(p) parameterized by p in domain [0, 1], which
242  /// has a path-length q(p) in range [0, q_max], linearly related to p,
243  /// where q_max is the total path-length of R (in real-world units);
244  /// * the true elevation function E_true(q), parameterized by the
245  /// path-length q of R;
246  /// * the true superelevation function S_true(q), parameterized by the
247  /// path-length q of R;
248  ///
249  /// then:
250  /// * p_scale is q_max (and p = q / p_scale);
251  /// * @p elevation is E_scaled = (1 / p_scale) * E_true(p_scale * p);
252  /// * @p superelevation is S_scaled = (1 / p_scale) * S_true(p_scale * p).
253  RoadCurve(const CubicPolynomial& elevation,
254  const CubicPolynomial& superelevation)
255  : elevation_(elevation), superelevation_(superelevation) {}
256 
257  private:
258  // A polynomial that represents the elevation change as a function of p.
259  CubicPolynomial elevation_;
260  // A polynomial that represents the superelevation angle change as a function
261  // of p.
262  CubicPolynomial superelevation_;
263 };
264 
265 } // namespace multilane
266 } // namespace maliput
267 } // namespace drake
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Definition: automotive_demo.cc:88
Eigen::Matrix< Scalar, 2, 1 > Vector2
A column vector of size 2, templated on scalar type.
Definition: eigen_types.h:30
const CubicPolynomial & elevation() const
Definition: road_curve.h:98
Defines an interface for a path in a Segment object surface.
Definition: road_curve.h:92
Definition: arc_road_curve.cc:11
Vector3< double > apply(const Vector3< double > &in) const
Applies the rotation to a 3-vector.
Definition: road_curve.h:30
#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
An R^3 rotation parameterized by roll, pitch, yaw.
Definition: road_curve.h:23
double yaw() const
Definition: road_curve.h:34
A cubic polynomial, f(p) = a + b*p + c*p^2 + d*p^3.
Definition: cubic_polynomial.h:13
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:343
const CubicPolynomial & superelevation() const
Definition: road_curve.h:100
double pitch() const
Definition: road_curve.h:35
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
double roll() const
Definition: road_curve.h:36
RoadCurve(const CubicPolynomial &elevation, const CubicPolynomial &superelevation)
Constructs a road curve given elevation and superelevation curves.
Definition: road_curve.h:253
#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
Provides careful macros to selectively enable or disable the special member functions for copy-constr...