Added Path object to represent a path for the arm to follow.
Change-Id: Ib16a968dacbdc23517a9057f94c4c4c11d6bbdc2
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
new file mode 100644
index 0000000..2862a6a
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -0,0 +1,22 @@
+cc_library(
+ name = "trajectory",
+ srcs = [
+ "trajectory.cc",
+ ],
+ hdrs = [
+ "trajectory.h",
+ ],
+ deps = ["//third_party/eigen"],
+)
+
+cc_test(
+ name = "trajectory_test",
+ srcs = [
+ "trajectory_test.cc",
+ ],
+ deps = [
+ ":trajectory",
+ "//aos/testing:googletest",
+ "//third_party/eigen",
+ ],
+)
diff --git a/y2018/control_loops/superstructure/arm/trajectory.cc b/y2018/control_loops/superstructure/arm/trajectory.cc
new file mode 100644
index 0000000..e5fdfe5
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/trajectory.cc
@@ -0,0 +1,35 @@
+#include "y2018/control_loops/superstructure/arm/trajectory.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+Path::Path(::std::initializer_list<::std::array<double, 6>> list) {
+ distances_.reserve(list.size());
+ ::Eigen::Matrix<double, 2, 1> last_theta;
+ bool first = true;
+
+ // Pull apart the initializer list into points, and compute the distance
+ // lookup for efficient path lookups.
+ for (const auto &e : list) {
+ thetas_.push_back(
+ (::Eigen::Matrix<double, 2, 1>() << e[0], e[1]).finished());
+ omegas_.push_back(
+ (::Eigen::Matrix<double, 2, 1>() << e[2], e[3]).finished());
+ alphas_.push_back(
+ (::Eigen::Matrix<double, 2, 1>() << e[4], e[5]).finished());
+
+ if (first) {
+ distances_.push_back(0.0);
+ } else {
+ distances_.push_back((thetas_.back() - last_theta).norm() +
+ distances_.back());
+ }
+ first = false;
+ last_theta = thetas_.back();
+ }
+}
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/trajectory.h b/y2018/control_loops/superstructure/arm/trajectory.h
new file mode 100644
index 0000000..a9136b4
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/trajectory.h
@@ -0,0 +1,76 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
+
+#include "Eigen/Dense"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+// This class represents a path in theta0, theta1 space. It also returns the
+// position, velocity and acceleration of the path as a function of path
+// distance.
+class Path {
+ public:
+ // Constructs a path from an initializer list of (theta0, theta1, omega0,
+ // omega1, alpha0, alpha1)
+ Path(::std::initializer_list<::std::array<double, 6>> list);
+
+ // Returns the length of the path in radians.
+ double length() const { return distances_.back(); }
+
+ // Returns theta, omega, and alpha as a function of path distance. Distances
+ // before the beginning of the path or after the end of the path will get
+ // truncated to the nearest end.
+ ::Eigen::Matrix<double, 2, 1> Theta(double distance) const {
+ return Interpolate(thetas_, distance);
+ }
+ ::Eigen::Matrix<double, 2, 1> Omega(double distance) const {
+ return Interpolate(omegas_, distance);
+ }
+ ::Eigen::Matrix<double, 2, 1> Alpha(double distance) const {
+ return Interpolate(alphas_, distance);
+ }
+
+ private:
+ // Interpolates the function represented by the list of points at the provided
+ // distance. Distances outside the range will get truncated. This is a
+ // log(n) algorithm so we can run it live on the bot.
+ ::Eigen::Matrix<double, 2, 1> Interpolate(
+ const ::std::vector<::Eigen::Matrix<double, 2, 1>> &points,
+ double distance) const {
+ if (distance <= 0.0) {
+ return points[0];
+ }
+ if (distance >= length()) {
+ return points.back();
+ }
+ // Keep a max and min, and pull them in towards eachother until they bound
+ // our point exactly.
+ size_t before = ::std::distance(
+ distances_.begin(),
+ ::std::lower_bound(distances_.begin(), distances_.end(), distance));
+ size_t after = before + 1;
+ return (distance - distances_[before]) * (points[after] - points[before]) /
+ (distances_[after] - distances_[before]) +
+ points[before];
+ }
+
+ // The list of theta points in the path.
+ ::std::vector<::Eigen::Matrix<double, 2, 1>> thetas_;
+ // The list of omega points in the path.
+ ::std::vector<::Eigen::Matrix<double, 2, 1>> omegas_;
+ // The list of alpha points in the path.
+ ::std::vector<::Eigen::Matrix<double, 2, 1>> alphas_;
+
+ // The distance along the path of each point.
+ ::std::vector<double> distances_;
+};
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
diff --git a/y2018/control_loops/superstructure/arm/trajectory_test.cc b/y2018/control_loops/superstructure/arm/trajectory_test.cc
new file mode 100644
index 0000000..5d5350b
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/trajectory_test.cc
@@ -0,0 +1,66 @@
+#include "y2018/control_loops/superstructure/arm/trajectory.h"
+
+#include "gtest/gtest.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+namespace testing {
+
+// Tests that we can pull out values along the path.
+TEST(TrajectoryTest, Theta) {
+ Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+ {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+ {{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+ {{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+ {{4.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
+
+ EXPECT_TRUE(p.Theta(0.5).isApprox(
+ (::Eigen::Matrix<double, 2, 1>() << 0.5, 0.0).finished()));
+ EXPECT_TRUE(p.Omega(0.5).isApprox(
+ (::Eigen::Matrix<double, 2, 1>() << 1.0, 0.0).finished()));
+ EXPECT_TRUE(p.Alpha(0.5).isApprox(
+ (::Eigen::Matrix<double, 2, 1>() << 0.0, 0.0).finished()));
+
+ EXPECT_TRUE(p.Theta(1.5).isApprox(
+ (::Eigen::Matrix<double, 2, 1>() << 1.5, 0.0).finished()));
+
+ for (double d = 0.0; d <= 4.0; d += 0.01) {
+ EXPECT_TRUE(p.Theta(d).isApprox(
+ (::Eigen::Matrix<double, 2, 1>() << d, 0.0).finished()));
+ }
+}
+
+// Tests that a path with an even number of points works as expected.
+TEST(TrajectoryTest, EvenPointNumbers) {
+ Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+ {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+ {{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+ {{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
+ EXPECT_TRUE(p.Theta(1.5).isApprox(
+ (::Eigen::Matrix<double, 2, 1>() << 1.5, 0.0).finished()));
+
+ for (double d = 0.0; d <= 3.0; d += 0.01) {
+ EXPECT_TRUE(p.Theta(d).isApprox(
+ (::Eigen::Matrix<double, 2, 1>() << d, 0.0).finished()));
+ }
+}
+
+// Tests that out of range points work as expected.
+TEST(TrajectoryTest, OutOfBounds) {
+ Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+ {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+ {{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+ {{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
+ EXPECT_TRUE(p.Theta(-1.0).isApprox(
+ (::Eigen::Matrix<double, 2, 1>() << 0.0, 0.0).finished()));
+ EXPECT_TRUE(p.Theta(10.0).isApprox(
+ (::Eigen::Matrix<double, 2, 1>() << 3.0, 0.0).finished()));
+}
+
+} // namespace testing
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018