Drake
road_curve.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <utility>
5 
6 #include <Eigen/Dense>
7 
12 #include "drake/common/unused.h"
16 
17 namespace drake {
18 namespace maliput {
19 namespace multilane {
20 
21 /// An R^3 rotation parameterized by roll, pitch, yaw.
22 ///
23 /// This effects a compound rotation around space-fixed x-y-z axes:
24 ///
25 /// Rot3(roll, pitch, yaw) * V = RotZ(yaw) * RotY(pitch) * RotX(roll) * V
26 // TODO(Mitiguy) Deprecate this class in favor of math::RollPitchYaw.
27 class Rot3 {
28  public:
30 
31  Rot3(double roll, double pitch, double yaw) : rpy_(roll, pitch, yaw) {}
32 
33  /// Applies the rotation to a 3-vector.
35  const math::RollPitchYaw<double> roll_pitch_yaw(rpy_);
36  return roll_pitch_yaw.ToRotationMatrix() * in;
37  }
38 
39  double yaw() const { return rpy_(2); }
40  double pitch() const { return rpy_(1); }
41  double roll() const { return rpy_(0); }
42 
43  private:
44  Eigen::Matrix<double, 3, 1, Eigen::DontAlign> rpy_;
45 };
46 
47 /// A policy to guide all computations in a RoadCurve, in terms of
48 /// speed and accuracy.
49 enum class ComputationPolicy {
50  kPreferAccuracy, ///< Always prefer accurate results,
51  /// even if at a slower pace (e.g. using
52  /// expensive numerical approximations).
53  kPreferSpeed ///< Always prefer fast computations,
54  /// even if not accurate (e.g. using
55  /// approximated analytical expressions).
56 };
57 
58 /// Defines an interface for a path in a Segment object surface. The path is
59 /// defined by an elevation and superelevation CubicPolynomial objects and a
60 /// reference curve. This reference curve is a C1 function in the z=0 plane.
61 /// Its domain is constrained in [0;1] interval and it should map a ℝ² curve.
62 /// As per notation, p is the parameter of the reference curve, not necessarily
63 /// arc length s, and function interpolations and function derivatives as well
64 /// as headings and heading derivatives are expressed in world coordinates,
65 /// which is the same frame as api::GeoPosition.
66 /// By implementing this interface the road curve is defined and complete.
67 ///
68 /// The geometry here revolves around an abstract "world function"
69 ///
70 /// W: (p,r,h) --> (x,y,z)
71 ///
72 /// which maps a `Lane`-frame position to its corresponding representation in
73 /// world coordinates (with the caveat that instead of the lane's native
74 /// longitudinal coordinate 's', the reference curve parameter 'p' is used).
75 ///
76 /// W is derived from the three functions which define the lane:
77 ///
78 /// G: p --> (x,y) = the reference curve, a.k.a. xy_of_p()
79 /// Z: p --> z / q_max = the elevation function, a.k.a. elevation_
80 /// Θ: p --> θ / q_max = the superelevation function, a.k.a. superelevation_
81 ///
82 /// as:
83 ///
84 /// (x,y,z) = W(p,r,h) = (G(p), Z(p)) + R_αβγ*(0,r,h)
85 ///
86 /// where R_αβγ is the roll/pitch/yaw rotation given by angles:
87 ///
88 /// α = Θ(p)
89 /// β = -atan(dZ/dp) at p
90 /// γ = atan2(dG_y/dp, dG_x/dp) at p
91 ///
92 /// (R_αβγ is essentially the orientation of the (s,r,h) `Lane`-frame
93 /// at a location (s,0,0) on the reference-line of the lane. However, it
94 /// is *not* necessarily the correct orientation at r != 0 or h != 0.)
95 ///
96 /// The W(p,r,h) "world function" is defined by the RoadCurve referenced by a
97 /// Lane's Segment. A Lane is also defined by a r0 lateral offset with respect
98 /// to the reference curve of the RoadCurve. Thus, a mapping from the local
99 /// (s,r,h) lane-frame of the Lane becomes:
100 ///
101 /// (x,y,z) = L(s,r,h) = W(P(s, r0), r + r0, h),
102 ///
103 /// where P:(s, r0) --> (p) is a (potentially non-linear) function dependent on
104 /// the RoadCurve's reference-curve, elevation, and superelevation functions.
105 ///
106 /// TODO(maddog-tri) Add support for Lanes with both non-zero r0 and
107 /// superelevation polynomial.
108 class RoadCurve {
109  public:
111 
112  virtual ~RoadCurve() = default;
113 
114  const CubicPolynomial& elevation() const { return elevation_; }
115 
116  const CubicPolynomial& superelevation() const { return superelevation_; }
117 
118  const double& linear_tolerance() const { return linear_tolerance_; }
119 
120  const double& scale_length() const { return scale_length_; }
121 
123  return computation_policy_;
124  }
125 
126  /// Computes the parametric position p along the reference curve corresponding
127  /// to longitudinal position (in path-length) `s` along a parallel curve
128  /// laterally offset by `r` from the reference curve.
129  /// @return The parametric position p along an offset of the reference curve.
130  /// @throw std::runtime_error When `r` makes the radius of curvature be a non
131  /// positive number.
132  double CalcPFromS(double s, double r) const;
133 
134  /// Computes the path length integral in the interval of the parameter [0; p]
135  /// and along a parallel curve laterally offset by `r` the planar reference
136  /// curve.
137  /// @return The path length integral of the curve composed with the elevation
138  /// polynomial.
139  /// @throw std::runtime_error When `r` makes the radius of curvature be a non
140  /// positive number.
141  double CalcSFromP(double p, double r) const;
142 
143  /// Computes the reference curve.
144  /// @param p The reference curve parameter.
145  /// @return The reference curve itself, F(p).
146  virtual Vector2<double> xy_of_p(double p) const = 0;
147 
148  /// Computes the first derivative of the reference curve.
149  /// @param p The reference curve parameter.
150  /// @return The derivative of the curve with respect to p, at @p p, i.e.,
151  /// F'(p0) = (dx/dp, dy/dp) at p0.
152  virtual Vector2<double> xy_dot_of_p(double p) const = 0;
153 
154  /// Computes the heading of the reference curve.
155  /// @param p The reference curve parameter.
156  /// @return The heading of the curve at @p p, i.e.,
157  /// the angle of the tangent vector (with respect to x-axis) in the
158  /// increasing-p direction.
159  virtual double heading_of_p(double p) const = 0;
160 
161  /// Computes the first derivative heading of the reference curve.
162  /// @param p The reference curve parameter.
163  /// @return The derivative of the heading with respect to p, i.e.,
164  /// d_heading/dp evaluated at @p p.
165  virtual double heading_dot_of_p(double p) const = 0;
166 
167  /// Computes the path length integral of the reference curve for the interval
168  /// [0;1] of p.
169  /// @return The path length integral of the reference curve.
170  // TODO(maddog-tri) This method should be renamed to match the Maliput's
171  // documentation as well as other variable names along the
172  // implementation.
173  virtual double p_scale() const = 0;
174 
175  /// Converts a @p geo_coordinate in the world frame to the composed curve
176  /// frame, i.e., the superposition of the reference curve, elevation and
177  /// superelevation polynomials. The resulting coordinates [p, r, h] are
178  /// saturated in the following domain ranges.
179  ///
180  /// - p: [0, 1]
181  /// - r: [@p r_min, @p r_max]
182  /// - h: [@p height_bounds]
183  /// @param geo_coordinate A 3D vector in the world frame to be converted to
184  /// the composed curve frame.
185  /// @param r_min Minimum lateral distance from the composed curve to saturate,
186  /// if it is necessary, the result in the given direction.
187  /// @param r_max Maximum lateral distance from the composed curve to evaluate,
188  /// if it is necessary, the result in the given direction
189  /// @param height_bounds An api::HBounds object that represents the elevation
190  /// bounds of the surface mapping.
191  /// @return A 3D vector [p, r, h], that represent the domain coordinates of
192  /// the world function, that gives as world function output @p geo_cooridnate.
193  virtual Vector3<double> ToCurveFrame(
194  const Vector3<double>& geo_coordinate,
195  double r_min, double r_max,
196  const api::HBounds& height_bounds) const = 0;
197 
198  /// Checks that there are no self-intersections (singularities) in the volume
199  /// created by applying the constant @p r_min, @p r_max and @p height_bounds
200  /// to the RoadCurve.
201  /// @param r_min Minimum lateral distance from the composed curve to evaluate
202  /// the validity of the geometry.
203  /// @param r_max Maximum lateral distance from the composed curve to evaluate
204  /// the validity of the geometry.
205  /// @param height_bounds An api::HBounds object that represents the elevation
206  /// bounds of the surface mapping.
207  /// @return True when there are no self-intersections.
208  virtual bool IsValid(double r_min, double r_max,
209  const api::HBounds& height_bounds) const = 0;
210 
211  /// Returns W, the world function evaluated at @p p, @p r, @p h.
212  Vector3<double> W_of_prh(double p, double r, double h) const;
213 
214  /// Returns W' = ∂W/∂p, the partial differential of W with respect to p,
215  /// evaluated at @p p, @p r, @p h.
216  ///
217  /// (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
218  /// avoid recomputing it.)
219  /// (@p g_prime must be the result of elevation().f_dot_p(p) --- passed in
220  /// here to avoid recomputing it.)
221  Vector3<double> W_prime_of_prh(double p, double r, double h, const Rot3& Rabg,
222  double g_prime) const;
223 
224  /// Returns the rotation R_αβγ, evaluated at @p p along the reference curve.
225  Rot3 Rabg_of_p(double p) const;
226 
227  /// Returns the rotation R_αβγ, evaluated at @p p, @p r and @p h.
228  Rot3 Orientation(double p, double r, double h) const;
229 
230  /// Returns the s-axis unit-vector, expressed in the world frame,
231  /// of the (s,r,h) `Lane`-frame (with respect to the world frame).
232  ///
233  /// (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
234  /// avoid recomputing it.)
235  /// (@p g_prime must be the result of elevation().f_dot_p(p) --- passed in
236  /// here to avoid recomputing it.)
237  Vector3<double> s_hat_of_prh(double p, double r, double h, const Rot3& Rabg,
238  double g_prime) const;
239 
240  /// Returns the r-axis unit-vector, expressed in the world frame,
241  /// of the (s,r,h) `Lane`-frame (with respect to the world frame).
242  ///
243  /// (@p Rabg must be the result of Rabg_of_p(p) --- passed in here to
244  /// avoid recomputing it.)
245  Vector3<double> r_hat_of_Rabg(const Rot3& Rabg) const;
246 
247  /// Computes the most appropriate value for the elevation derivative g' at
248  /// @p p, that accounts for the limitations of the arc length parameterization
249  /// being used.
250  /// @param p The reference curve parameter.
251  /// @return The elevation derivative g'(@p p) value.
252  /// @sa W_prime_of_prh()
253  double CalcGPrimeAsUsedForCalcSFromP(double p) const;
254 
255  protected:
256  /// Constructs a road curve given elevation and superelevation curves.
257  /// @param linear_tolerance The linear tolerance, in meters, for all
258  /// computations. It is understood in the the absolute error sense i.e.
259  /// linear error must lie in the 0 ± linear tolerance interval, for
260  /// @p scale_length long features at most.
261  /// @param scale_length The minimum spatial period of variation in the
262  /// curve, in meters. This imposes an upper limit to the spatial frequency
263  /// (i.e. the Nyquist limit), which indicates the maximum level of detail
264  /// expressed by the curve.
265  /// @param elevation CubicPolynomial object that represents the elevation
266  /// function (see below for more details).
267  /// @param superelevation CubicPolynomial object that represents the
268  /// superelevation function (see below for more details).
269  /// @param computation_policy Policy to guide computations in terms of speed
270  /// and accuracy. Actual behavior may vary across implementations.
271  /// @pre The given @p scale_length is a positive number.
272  /// @pre The given @p linear_tolerance is a positive number.
273  /// @throw std::runtime_error if any of the preconditions is not met.
274  ///
275  /// @p elevation and @p superelevation are cubic-polynomial functions which
276  /// define the elevation and superelevation as a function of position along
277  /// the planar reference curve. @p elevation specifies the z-component of
278  /// the surface at (r,h) = (0,0). @p superelevation specifies the angle
279  /// of the r-axis with respect to the horizon, i.e., how the road twists.
280  /// Thus, non-zero @p superelevation contributes to the z-component at
281  /// r != 0.
282  ///
283  /// These two functions (@p elevation and @p superelevation) must be
284  /// isotropically scaled to operate over the domain p in [0, 1], where
285  /// p is linear in the path-length of the planar reference curve,
286  /// p = 0 corresponds to the start and p = 1 to the end. p_scale() is
287  /// the scale factor. In other words...
288  ///
289  /// Given:
290  /// * a reference curve R(p) parameterized by p in domain [0, 1], which
291  /// has a path-length q(p) in range [0, q_max], linearly related to p,
292  /// where q_max is the total path-length of R (in real-world units);
293  /// * the true elevation function E_true(q), parameterized by the
294  /// path-length q of R;
295  /// * the true superelevation function S_true(q), parameterized by the
296  /// path-length q of R;
297  ///
298  /// then:
299  /// * p_scale is q_max (and p = q / p_scale);
300  /// * @p elevation is E_scaled = (1 / p_scale) * E_true(p_scale * p);
301  /// * @p superelevation is S_scaled = (1 / p_scale) * S_true(p_scale * p).
302  RoadCurve(double linear_tolerance, double scale_length,
303  const CubicPolynomial& elevation,
304  const CubicPolynomial& superelevation,
305  ComputationPolicy computation_policy);
306 
307  private:
308  // Computes the minimum radius of curvature along a parallel curve at a
309  // lateral distance @p r from the reference curve. Useful to identify
310  // potentially ill-conditioned evaluations e.g. `r` offsets passing through
311  // and past the instantaneous center of rotation.
312  // @param r Lateral offset of the reference curve over the z = 0 plane.
313  // @return The minimum radius of curvature.
314  virtual double CalcMinimumRadiusAtOffset(double r) const = 0;
315 
316  // TODO(hidmic): Fast, analytical methods and the conditions in which these
317  // are expected to hold were tailored for the currently available
318  // implementations. This limitation could be overcome by e.g. providing a
319  // pair of virtual methods to check for the validity of the methods actually
320  // implemented in the subclasses.
321 
322  // Resources fast, analytical methods to compute the parametric position
323  // p along the reference curve corresponding to longitudinal position (in
324  // path-length) `s` along a parallel curve laterally offset by `r` from the
325  // reference curve.
326  //
327  // @remarks Said methods are only expected to be valid for curves that
328  // show no superelevation and linear elevation at most.
329  virtual double FastCalcPFromS(double s, double r) const = 0;
330 
331  // Resources fast, analytical methods to compute the path length integral
332  // in the interval of the parameter [0; p] and along a parallel curve
333  // laterally offset by `r` the planar reference curve.
334  //
335  // @remarks Said methods are only expected to be valid for curves that
336  // show no superelevation and linear elevation at most.
337  virtual double FastCalcSFromP(double p, double r) const = 0;
338 
339  // Checks whether fast, analytical methods would be accurate for the curve
340  // as defined. It boils down to checking if the curve shows no superelevation
341  // and linear elevation at most.
342  //
343  // @param r Lateral offset of the reference curve over the z=0 plane.
344  // @return true if fast, analytical results would be accurate, and false
345  // otherwise.
346  bool AreFastComputationsAccurate(double r) const;
347 
348  // The minimum length of variations that the curve expresses.
349  double scale_length_;
350  // The tolerance for all computations, in the absolute error sense, for scale
351  // length-long features in the road curve at most.
352  double linear_tolerance_;
353  // A polynomial that represents the elevation change as a function of p.
354  CubicPolynomial elevation_;
355  // A polynomial that represents the superelevation angle change as a function
356  // of p.
357  CubicPolynomial superelevation_;
358  // A policy to guide computations in terms of speed and accuracy.
359  ComputationPolicy computation_policy_;
360  // The inverse arc length IVP, or the parameter p as a function of the
361  // arc length s.
362  std::unique_ptr<systems::ScalarInitialValueProblem<double>> p_from_s_ivp_;
363  // The arc length function, or the arc length s as a function of the
364  // parameter p.
365  std::unique_ptr<systems::AntiderivativeFunction<double>> s_from_p_func_;
366 };
367 
368 } // namespace multilane
369 } // namespace maliput
370 } // namespace drake
This class represents the orientation between two arbitrary frames A and D associated with a Space-fi...
Definition: roll_pitch_yaw.h:65
Always prefer fast computations, even if not accurate (e.g.
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
const double & scale_length() const
Definition: road_curve.h:120
Definition: automotive_demo.cc:90
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:114
Defines an interface for a path in a Segment object surface.
Definition: road_curve.h:108
const double & linear_tolerance() const
Definition: road_curve.h:118
const api::HBounds height_bounds
Definition: multilane_arc_road_curve_test.cc:32
Always prefer accurate results, even if at a slower pace (e.g.
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:34
const ComputationPolicy & computation_policy() const
Definition: road_curve.h:122
#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
RotationMatrix< T > ToRotationMatrix() const
Returns the RotationMatrix representation of this RollPitchYaw.
Definition: roll_pitch_yaw.cc:18
An R^3 rotation parameterized by roll, pitch, yaw.
Definition: road_curve.h:27
double yaw() const
Definition: road_curve.h:39
A cubic polynomial, f(p) = a + b*p + c*p^2 + d*p^3.
Definition: cubic_polynomial.h:14
Bounds in the elevation dimension (h component) of a Lane-frame, consisting of a pair of minimum and ...
Definition: lane_data.h:360
ComputationPolicy
A policy to guide all computations in a RoadCurve, in terms of speed and accuracy.
Definition: road_curve.h:49
const CubicPolynomial & superelevation() const
Definition: road_curve.h:116
double pitch() const
Definition: road_curve.h:40
double roll() const
Definition: road_curve.h:41
#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...