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