Drake
curve2.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <algorithm>
4 #include <cmath>
5 #include <stdexcept>
6 #include <vector>
7 
8 #include <Eigen/Dense>
9 
12 
13 namespace drake {
14 namespace automotive {
15 
16 /// Curve2 represents a path through two-dimensional Cartesian space. Given a
17 /// list of waypoints, it traces a path between them.
18 ///
19 /// Instantiated templates for the following kinds of T's are provided:
20 /// - double
21 /// - drake::AutoDiffXd
22 ///
23 /// They are already available to link against in the containing library.
24 ///
25 /// TODO(jwnimmer-tri) We will soon trace the path using a spline, but
26 /// for now it's easiest to just interpolate straight segments, as a
27 /// starting point. Callers should not yet rely on <em>how</em> we
28 /// are traversing between the waypoints.
29 ///
30 template <typename T>
31 class Curve2 {
32  public:
34 
35  /// A two-dimensional Cartesian point that is alignment-safe.
36  typedef Eigen::Matrix<double, 2, 1, Eigen::DontAlign> Point2;
37  typedef Eigen::Matrix<T, 2, 1, Eigen::DontAlign> Point2T;
38 
39  /// Constructor that traces through the given @p waypoints in order.
40  /// Throws an error if @p waypoints.size() == 1.
41  explicit Curve2(const std::vector<Point2>& waypoints)
42  : waypoints_(waypoints), path_length_(GetLength(waypoints_)) {
43  // TODO(jwnimmer-tri) We should reject duplicate adjacent
44  // waypoints (derivative problems); this will probably come for
45  // free as part of the spline refactoring.
46  }
47 
48  /// @return the waypoints associated with this curve.
49  const std::vector<Point2>& waypoints() const { return waypoints_; }
50 
51  /// @return the length of this curve (the total distance traced).
52  double path_length() const { return path_length_; }
53 
54  /// A result type for the GetPosition method.
55  struct PositionResult {
56  Point2T position = Point2T{Point2T::Zero()};
57  Point2T position_dot = Point2T{Point2T::Zero()};
58  };
59 
60  /// Returns the Curve's @p PositionResult::position at @p path_distance,
61  /// as well as its first derivative @p PositionResult::position_dot with
62  /// respect to @p path_distance.
63  ///
64  /// The @p path_distance is clipped to the ends of the curve:
65  /// - A negative @p path_distance is interpreted as a @p path_distance
66  /// of zero.
67  /// - A @p path_distance that exceeds the @p path_length() of the curve
68  /// is interpreted as a @p path_distance equal to the @p path_length().
69  ///
70  /// The @p position_dot derivative, when evaluated exactly at a waypoint,
71  /// will be congruent with the direction of one of the (max two) segments
72  /// that neighbor the waypoint. (At the first and last waypoints, there
73  /// is only one neighboring segment.) TODO(jwnimmer-tri) This will no
74  /// longer be true once this class uses a spline.
75  PositionResult GetPosition(const T& path_distance) const {
76  using std::max;
77 
78  // TODO(jwnimmer-tri) This implementation is slow (linear search)
79  // and incorrect (discontinuous; not a spline). But it will do
80  // for now, until we get a 2d spline code in C++.
81 
83 
84  // We need at least one segment. If not, we're just zero.
85  if (waypoints_.size() < 2) {
86  return result;
87  }
88 
89  // Iterate over the segments, up through the requested path_distance.
90  T remaining_distance = max(T{0.0}, path_distance);
91  for (auto point0 = waypoints_.begin(), point1 = point0 + 1;
92  point1 != waypoints_.end(); // BR
93  point0 = point1++) {
94  Point2 relative_step{*point1 - *point0};
95  const double length = relative_step.norm();
96  if (remaining_distance <= length) {
97  auto fraction = remaining_distance / length;
98  result.position = *point0 + fraction * Point2T{relative_step};
99  Point2T position_dot = relative_step / length;
100  MakePointCoherent(path_distance, &position_dot);
101  result.position_dot.head(2) = position_dot;
102  return result;
103  }
104  remaining_distance -= length;
105  }
106 
107  // Oops, we ran out of waypoints; return the final one.
108  // The position_dot is congruent with the final segment.
109  {
110  Point2T final_waypoint = waypoints_.back();
111  MakePointCoherent(path_distance, &final_waypoint);
112  result.position = final_waypoint;
113  Point2 ultimate = waypoints_.back();
114  Point2 penultimate = waypoints_.at(waypoints_.size() - 2);
115  const Point2 relative_step{ultimate - penultimate};
116  const double length = relative_step.norm();
117  Point2T position_dot = relative_step / length;
118  MakePointCoherent(path_distance, &position_dot);
119  result.position_dot.head(2) = position_dot;
120  }
121 
122  return result;
123  }
124 
125  private:
126  // TODO(jwnimmer-tri) Make sure this uses the spline length, not
127  // sum-segment-length, once this class uses a spline.
128  static double GetLength(const std::vector<Point2>& waypoints) {
129  double result{0.0};
130  if (waypoints.empty()) {
131  return result;
132  }
133  if (waypoints.size() == 1) {
134  throw std::invalid_argument{"single waypoint"};
135  }
136  for (auto point0 = waypoints.begin(), point1 = point0 + 1;
137  point1 != waypoints.end(); // BR
138  point0 = point1++) {
139  const Point2 relative_step{*point1 - *point0};
140  const double length = relative_step.norm();
141  result += length;
142  }
143  return result;
144  }
145 
146  static void MakePointCoherent(const T& donor, Point2T* point2) {
147  autodiffxd_make_coherent(donor, &(*point2)(0));
148  autodiffxd_make_coherent(donor, &(*point2)(1));
149  }
150 
151  std::vector<Point2> waypoints_;
152  double path_length_;
153 };
154 
155 } // namespace automotive
156 } // namespace drake
PositionResult GetPosition(const T &path_distance) const
Returns the Curve&#39;s PositionResult::position at path_distance, as well as its first derivative Positi...
Definition: curve2.h:75
const std::vector< Point2 > & waypoints() const
Definition: curve2.h:49
Point2T position_dot
Definition: curve2.h:57
Definition: automotive_demo.cc:88
Curve2 represents a path through two-dimensional Cartesian space.
Definition: curve2.h:31
STL namespace.
Eigen::Matrix< double, 2, 1, Eigen::DontAlign > Point2
A two-dimensional Cartesian point that is alignment-safe.
Definition: curve2.h:36
Point2T position
Definition: curve2.h:56
Definition: autodiff_overloads.h:34
void autodiffxd_make_coherent(const AutoDiffXd &donor, AutoDiffXd *recipient)
Makes the derviatives of the recipient coherent with respect to those of the donor variable (see drak...
Definition: autodiffxd_make_coherent.h:13
std::vector< double > vector
Definition: translator_test.cc:20
std::vector< Number > result
Definition: ipopt_solver.cc:151
Eigen::Matrix< T, 2, 1, Eigen::DontAlign > Point2T
Definition: curve2.h:37
A result type for the GetPosition method.
Definition: curve2.h:55
#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
Expression max(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:697
double path_length() const
Definition: curve2.h:52
Provides careful macros to selectively enable or disable the special member functions for copy-constr...