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 
11 
12 namespace drake {
13 namespace automotive {
14 
29 template <typename T>
30 class Curve2 {
31  public:
33 
34 
35  typedef Eigen::Matrix<T, 2, 1, Eigen::DontAlign> Point2;
36 
39  explicit Curve2(const std::vector<Point2>& waypoints)
40  : waypoints_(waypoints), path_length_(GetLength(waypoints_)) {
41  // TODO(jwnimmer-tri) We should reject duplicate adjacent
42  // waypoints (derivative problems); this will probably come for
43  // free as part of the spline refactoring.
44  }
45 
47  const std::vector<Point2>& waypoints() const { return waypoints_; }
48 
50  T path_length() const { return path_length_; }
51 
53  struct PositionResult {
54  Point2 position = Point2{Point2::Zero()};
55  Point2 position_dot = Point2{Point2::Zero()};
56  };
57 
73  PositionResult GetPosition(const T& path_distance) const {
74  // TODO(jwnimmer-tri) This implementation is slow (linear search)
75  // and incorrect (discontinuous; not a spline). But it will do
76  // for now, until we get a 2d spline code in C++.
77 
78  PositionResult result;
79 
80  // We need at least one segment. If not, we're just zero.
81  if (waypoints_.size() < 2) {
82  return result;
83  }
84 
85  // Iterate over the segments, up through the requested path_distance.
86  T remaining_distance = std::max(T{0.0}, path_distance);
87  for (auto point0 = waypoints_.begin(), point1 = point0 + 1;
88  point1 != waypoints_.end(); // BR
89  point0 = point1++) {
90  const Point2 relative_step{*point1 - *point0};
91  const T length = relative_step.norm();
92  if (remaining_distance <= length) {
93  auto fraction = remaining_distance / length;
94  result.position = Point2{*point0 + fraction * relative_step};
95  result.position_dot.head(2) = relative_step / length;
96  return result;
97  }
98  remaining_distance -= length;
99  }
100 
101  // Oops, we ran out of waypoints; return the final one.
102  // The position_dot is congruent with the final segment.
103  {
104  result.position = waypoints_.back();
105  Point2 ultimate = waypoints_.back();
106  Point2 penultimate = waypoints_.at(waypoints_.size() - 2);
107  const Point2 relative_step{ultimate - penultimate};
108  const T length = relative_step.norm();
109  result.position_dot.head(2) = relative_step / length;
110  }
111 
112  return result;
113  }
114 
115  private:
116  // TODO(jwnimmer-tri) Make sure this uses the spline length, not
117  // sum-segment-length, once this class uses a spline.
118  static T GetLength(const std::vector<Point2>& waypoints) {
119  T result{0.0};
120  if (waypoints.empty()) {
121  return result;
122  }
123  if (waypoints.size() == 1) {
124  throw std::invalid_argument{"single waypoint"};
125  }
126  for (auto point0 = waypoints.begin(), point1 = point0 + 1;
127  point1 != waypoints.end(); // BR
128  point0 = point1++) {
129  const Point2 relative_step{*point1 - *point0};
130  const T length = relative_step.norm();
131  result += length;
132  }
133  return result;
134  }
135 
136  std::vector<Point2> waypoints_;
137  T path_length_;
138 };
139 
140 } // namespace automotive
141 } // 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:73
Eigen::Matrix< double, 2, 1, Eigen::DontAlign > Point2
A two-dimensional Cartesian point that is alignment-safe.
Definition: curve2.h:35
const std::vector< Point2 > & waypoints() const
Definition: curve2.h:47
Definition: automotive_demo.cc:88
Curve2 represents a path through two-dimensional Cartesian space.
Definition: curve2.h:30
STL namespace.
Definition: autodiff_overloads.h:30
Point2 position
Definition: curve2.h:54
std::vector< double > vector
Definition: translator_test.cc:20
std::vector< Number > result
Definition: ipopt_solver.cc:151
A result type for the GetPosition method.
Definition: curve2.h:53
#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:59
Expression max(const Expression &e1, const Expression &e2)
Definition: symbolic_expression.cc:697
Point2 position_dot
Definition: curve2.h:55
T path_length() const
Definition: curve2.h:50
Provides careful macros to selectively enable or disable the special member functions for copy-constr...