Drake
arc_road_curve.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cmath>
4 
9 
10 namespace drake {
11 namespace maliput {
12 namespace multilane {
13 
14 /// RoadCurve specification for a reference curve that describes a piece
15 /// of an arc.
16 class ArcRoadCurve : public RoadCurve {
17  public:
19 
20  /// Constructor. The reference curve is created from the circle `center`, the
21  /// `radius`, initial angular position `theta0` and the angle span
22  /// `d_theta`. `elevation` and `superelevation` polynomials will be used to
23  /// feed RoadGeometry parent class.
24  /// @param center Center of the reference arc.
25  /// @param radius Radius of the reference arc (must be positive).
26  /// @param theta0 Angle of the start point of the reference arc with respect
27  /// to `center` (0 == parallel to x-axis).
28  /// @param d_theta Central angle of the arc, i.e., angular displacement
29  /// from start to end. d_theta > 0 is counter-clockwise.
30  /// @param elevation CubicPolynomial object that represents the elevation
31  /// polynomial. See RoadCurve class constructor for more details.
32  /// @param superelevation CubicPolynomial object that represents the
33  /// superelevation polynomial. See RoadCurve class constructor for more
34  /// details.
35  /// @throws std::runtime_error When `radius` is not positive.
36  explicit ArcRoadCurve(const Vector2<double>& center, double radius,
37  double theta0, double d_theta,
40  : RoadCurve(elevation, superelevation),
41  center_(center),
42  radius_(radius),
43  theta0_(theta0),
44  d_theta_(d_theta) {
45  DRAKE_THROW_UNLESS(radius > 0.0);
46  }
47 
48  ~ArcRoadCurve() override = default;
49 
50  /// @throws std::runtime_error When `r` makes the effective radius to be
51  /// negative or zero.
52  double p_from_s(double s, double r) const override;
53 
54  /// @throws std::runtime_error When `r` makes the effective radius to be
55  /// negative or zero.
56  double s_from_p(double p, double r) const override;
57 
58  Vector2<double> xy_of_p(double p) const override {
59  // The result will be computed with the following function:
60  // [x;y] = center + [radius * cos(θ); radius * sin(θ)]
61  // and:
62  // θ = θ₀ + (p * Δθ)
63  const double theta = theta_of_p(p);
64  return center_ + Vector2<double>(radius_ * std::cos(theta),
65  radius_ * std::sin(theta));
66  }
67 
68  Vector2<double> xy_dot_of_p(double p) const override {
69  // Given:
70  // [x;y] = center + [radius * cos(θ); radius * sin(θ)]
71  // and:
72  // θ = θ₀ + (p * Δθ)
73  // dθ/dp = Δθ
74  // then:
75  // [dx/dp; dy/dp] = [-radius * sin(θ) * dθ/dp; radius * cos(θ) * dθ/dp]
76  // = [-radius * sin(θ) * Δθ; radius * cos(θ) * Δθ]
77  const double theta = theta_of_p(p);
78  return Vector2<double>(-radius_ * std::sin(theta) * d_theta_,
79  radius_ * std::cos(theta) * d_theta_);
80  }
81 
82  double heading_of_p(double p) const override {
83  // Given:
84  // θ = θ_0 + (p * Δθ)
85  // then:
86  // heading = θ + sign(Δθ) * π / 2.0
87  // sign(Δθ) is used to express the increasing-p direction of the tangent.
88  const double theta = theta_of_p(p);
89  return theta + std::copysign(M_PI / 2.0, d_theta_);
90  }
91 
92  double heading_dot_of_p(double p) const override {
93  // Given:
94  // heading = θ + sign(Δθ) * π / 2.0
95  // and:
96  // θ = θ₀ + (p * Δθ)
97  // then:
98  // dheading/dp = dθ/dp = Δθ
99  unused(p);
100  return d_theta_;
101  }
102 
103  double p_scale() const override { return radius_ * std::abs(d_theta_); }
104 
106  const Vector3<double>& geo_coordinate,
107  double r_min, double r_max,
108  const api::HBounds& height_bounds) const override;
109 
110  bool IsValid(double r_min, double r_max,
111  const api::HBounds& height_bounds) const override;
112 
113  private:
114  // Computes the absolute position along reference arc as an angle in
115  // range [theta0_, (theta0 + d_theta_)],
116  // as a function of parameter @p p (in domain [0, 1]).
117  double theta_of_p(double p) const { return theta0_ + (p * d_theta_); }
118 
119  // Computes the radius of the reference arc offset at a distance @p r.
120  // Uses d_theta_'s sign (see ArcRoadCurve() for more details) to add or
121  // or subtract @p r distance.
122  // @param r Lateral offset of the reference curve over the z=0 plane.
123  // @return The reference arc offset radius.
124  double offset_radius(double r) const {
125  return radius_ - std::copysign(1., d_theta_) * r;
126  }
127 
128  // Center of rotation in z=0 plane, world coordinates, for the arc reference
129  // curve.
130  const Vector2<double> center_;
131  // The length of the radius of the arc.
132  const double radius_{};
133  // The angular position at which the piece of arc starts.
134  const double theta0_{};
135  // The aperture angle of the arc. Positive values are counter-clockwise.
136  const double d_theta_{};
137 };
138 
139 } // namespace multilane
140 } // namespace maliput
141 } // namespace drake
double p_scale() const override
Computes the path length integral of the reference curve for the interval [0;1] of p...
Definition: arc_road_curve.h:103
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Provides a convenient wrapper to throw an exception when a condition is unmet.
Definition: automotive_demo.cc:88
Expression sin(const Expression &e)
Definition: symbolic_expression.cc:599
Expression abs(const Expression &e)
Definition: symbolic_expression.cc:537
Vector3< double > ToCurveFrame(const Vector3< double > &geo_coordinate, double r_min, double r_max, const api::HBounds &height_bounds) const override
Converts a geo_coordinate in the world frame to the composed curve frame, i.e., the superposition of ...
Definition: arc_road_curve.cc:94
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
Vector2< double > xy_of_p(double p) const override
Computes the reference curve.
Definition: arc_road_curve.h:58
bool IsValid(double r_min, double r_max, const api::HBounds &height_bounds) const override
Checks that there are no self-intersections (singularities) in the volume created by applying the con...
Definition: arc_road_curve.cc:138
#define DRAKE_THROW_UNLESS(condition)
Evaluates condition and iff the value is false will throw an exception with a message showing at leas...
Definition: drake_throw.h:23
Definition: arc_road_curve.cc:11
double heading_of_p(double p) const override
Computes the heading of the reference curve.
Definition: arc_road_curve.h:82
double s_from_p(double p, double r) const override
Definition: arc_road_curve.cc:27
Eigen::AutoDiffScalar< DerType > copysign(const Eigen::AutoDiffScalar< DerType > &x, const T &y)
Overloads copysign from <cmath>.
Definition: autodiff_overloads.h:73
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
double heading_dot_of_p(double p) const override
Computes the first derivative heading of the reference curve.
Definition: arc_road_curve.h:92
const CubicPolynomial & superelevation() const
Definition: road_curve.h:100
Expression cos(const Expression &e)
Definition: symbolic_expression.cc:607
double p_from_s(double s, double r) const override
Definition: arc_road_curve.cc:13
RoadCurve specification for a reference curve that describes a piece of an arc.
Definition: arc_road_curve.h:16
#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
Vector2< double > xy_dot_of_p(double p) const override
Computes the first derivative of the reference curve.
Definition: arc_road_curve.h:68
void unused(const Args &...)
Documents the argument(s) as unused, placating GCC&#39;s -Wunused-parameter warning.
Definition: unused.h:51
Provides careful macros to selectively enable or disable the special member functions for copy-constr...