Add trajectory generation using hermite splines (#1843)

wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp

@@ -52,3 +52,15 @@ TEST(Pose2dTest, Inequality) {
   const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
   EXPECT_TRUE(a != b);
 }
+
+TEST(Pose2dTest, Minus) {
+  const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
+  const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
+
+  const auto transform = final - initial;
+
+  EXPECT_NEAR(transform.Translation().X().to<double>(), 5.0 * std::sqrt(2.0),
+              kEpsilon);
+  EXPECT_NEAR(transform.Translation().Y().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(transform.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+}

wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp

@@ -56,3 +56,14 @@ TEST(Twist2dTest, Inequality) {
   const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
   EXPECT_TRUE(one != two);
 }
+
+TEST(Twist2dTest, Pose2dLog) {
+  const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
+  const Pose2d start{};
+
+  const auto twist = start.Log(end);
+
+  EXPECT_NEAR(twist.dx.to<double>(), 5 / 2.0 * wpi::math::pi, kEpsilon);
+  EXPECT_NEAR(twist.dy.to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.to<double>(), wpi::math::pi / 2.0, kEpsilon);
+}

wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp

@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <chrono>
+#include <iostream>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/spline/QuinticHermiteSpline.h"
+#include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+namespace frc {
+class CubicHermiteSplineTest : public ::testing::Test {
+ protected:
+  static void Run(const Pose2d& a, const std::vector<Translation2d>& waypoints,
+                  const Pose2d& b) {
+    // Start the timer.
+    const auto start = std::chrono::high_resolution_clock::now();
+
+    // Generate and parameterize the spline.
+    const auto splines =
+        SplineHelper::CubicSplinesFromWaypoints(a, waypoints, b);
+    std::vector<Spline<3>::PoseWithCurvature> poses;
+
+    poses.push_back(splines[0].GetPoint(0.0));
+
+    for (auto&& spline : splines) {
+      auto x = SplineParameterizer::Parameterize(spline);
+      poses.insert(std::end(poses), std::begin(x) + 1, std::end(x));
+    }
+
+    // End timer.
+    const auto finish = std::chrono::high_resolution_clock::now();
+
+    // Calculate the duration (used when benchmarking)
+    const auto duration =
+        std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
+
+    for (unsigned int i = 0; i < poses.size() - 1; i++) {
+      auto& p0 = poses[i];
+      auto& p1 = poses[i + 1];
+
+      // Make sure the twist is under the tolerance defined by the Spline class.
+      auto twist = p0.first.Log(p1.first);
+      EXPECT_LT(std::abs(twist.dx.to<double>()),
+                SplineParameterizer::kMaxDx.to<double>());
+      EXPECT_LT(std::abs(twist.dy.to<double>()),
+                SplineParameterizer::kMaxDy.to<double>());
+      EXPECT_LT(std::abs(twist.dtheta.to<double>()),
+                SplineParameterizer::kMaxDtheta.to<double>());
+    }
+
+    // Check first point.
+    EXPECT_NEAR(poses.front().first.Translation().X().to<double>(),
+                a.Translation().X().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.front().first.Translation().Y().to<double>(),
+                a.Translation().Y().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
+                a.Rotation().Radians().to<double>(), 1E-9);
+
+    // Check last point.
+    EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
+                b.Translation().X().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.back().first.Translation().Y().to<double>(),
+                b.Translation().Y().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
+                b.Rotation().Radians().to<double>(), 1E-9);
+
+    static_cast<void>(duration);
+  }
+};
+}  // namespace frc
+
+TEST_F(CubicHermiteSplineTest, StraightLine) {
+  Run(Pose2d(), std::vector<Translation2d>(), Pose2d(3_m, 0_m, Rotation2d()));
+}
+
+TEST_F(CubicHermiteSplineTest, SCurve) {
+  Pose2d start{0_m, 0_m, Rotation2d(90_deg)};
+  std::vector<Translation2d> waypoints{Translation2d(1_m, 1_m),
+                                       Translation2d(2_m, -1_m)};
+  Pose2d end{3_m, 0_m, Rotation2d{90_deg}};
+  Run(start, waypoints, end);
+}

wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp

@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <chrono>
+#include <iostream>
+
+#include <units/units.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/spline/QuinticHermiteSpline.h"
+#include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+namespace frc {
+class QuinticHermiteSplineTest : public ::testing::Test {
+ protected:
+  static void Run(const Pose2d& a, const Pose2d& b) {
+    // Start the timer.
+    const auto start = std::chrono::high_resolution_clock::now();
+
+    // Generate and parameterize the spline.
+    const auto spline = SplineHelper::QuinticSplinesFromWaypoints({a, b})[0];
+    const auto poses = SplineParameterizer::Parameterize(spline);
+
+    // End timer.
+    const auto finish = std::chrono::high_resolution_clock::now();
+
+    // Calculate the duration (used when benchmarking)
+    const auto duration =
+        std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
+
+    for (unsigned int i = 0; i < poses.size() - 1; i++) {
+      auto& p0 = poses[i];
+      auto& p1 = poses[i + 1];
+
+      // Make sure the twist is under the tolerance defined by the Spline class.
+      auto twist = p0.first.Log(p1.first);
+      EXPECT_LT(std::abs(twist.dx.to<double>()),
+                SplineParameterizer::kMaxDx.to<double>());
+      EXPECT_LT(std::abs(twist.dy.to<double>()),
+                SplineParameterizer::kMaxDy.to<double>());
+      EXPECT_LT(std::abs(twist.dtheta.to<double>()),
+                SplineParameterizer::kMaxDtheta.to<double>());
+    }
+
+    // Check first point.
+    EXPECT_NEAR(poses.front().first.Translation().X().to<double>(),
+                a.Translation().X().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.front().first.Translation().Y().to<double>(),
+                a.Translation().Y().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
+                a.Rotation().Radians().to<double>(), 1E-9);
+
+    // Check last point.
+    EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
+                b.Translation().X().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.back().first.Translation().Y().to<double>(),
+                b.Translation().Y().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
+                b.Rotation().Radians().to<double>(), 1E-9);
+
+    static_cast<void>(duration);
+  }
+};
+}  // namespace frc
+
+TEST_F(QuinticHermiteSplineTest, StraightLine) {
+  Run(Pose2d(), Pose2d(3_m, 0_m, Rotation2d()));
+}
+
+TEST_F(QuinticHermiteSplineTest, SimpleSCurve) {
+  Run(Pose2d(), Pose2d(1_m, 1_m, Rotation2d()));
+}
+
+TEST_F(QuinticHermiteSplineTest, SquigglyCurve) {
+  Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)),
+      Pose2d(-1_m, 0_m, Rotation2d(90_deg)));
+}

wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <memory>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(CentripetalAccelerationConstraintTest, Constraint) {
+  const auto maxCentripetalAcceleration = 7_fps_sq;
+
+  std::vector<std::unique_ptr<TrajectoryConstraint>> constraints;
+  constraints.emplace_back(std::make_unique<CentripetalAccelerationConstraint>(
+      maxCentripetalAcceleration));
+
+  auto trajectory = TestTrajectory::GetTrajectory(std::move(constraints));
+
+  units::second_t time = 0_s;
+  units::second_t dt = 20_ms;
+  units::second_t duration = trajectory.TotalTime();
+
+  while (time < duration) {
+    const Trajectory::State point = trajectory.Sample(time);
+    time += dt;
+
+    auto centripetalAcceleration =
+        units::math::pow<2>(point.velocity) * point.curvature / 1_rad;
+
+    EXPECT_TRUE(centripetalAcceleration <
+                maxCentripetalAcceleration + 0.05_mps_sq);
+  }
+}

wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <memory>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(DifferentialDriveKinematicsConstraintTest, Constraint) {
+  const auto maxVelocity = 12_fps;
+  const DifferentialDriveKinematics kinematics{27_in};
+
+  std::vector<std::unique_ptr<TrajectoryConstraint>> constraints;
+  constraints.emplace_back(
+      std::make_unique<DifferentialDriveKinematicsConstraint>(kinematics,
+                                                              maxVelocity));
+
+  auto trajectory = TestTrajectory::GetTrajectory(std::move(constraints));
+
+  units::second_t time = 0_s;
+  units::second_t dt = 20_ms;
+  units::second_t duration = trajectory.TotalTime();
+
+  while (time < duration) {
+    const Trajectory::State point = trajectory.Sample(time);
+    time += dt;
+
+    const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
+                                      point.velocity * point.curvature};
+
+    auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+    EXPECT_TRUE(left < maxVelocity + 0.05_mps);
+    EXPECT_TRUE(right < maxVelocity + 0.05_mps);
+  }
+}

wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(TrajectoryGenerationTest, ObeysConstraints) {
+  auto trajectory = TestTrajectory::GetTrajectory(
+      std::vector<std::unique_ptr<TrajectoryConstraint>>());
+
+  units::second_t time = 0_s;
+  units::second_t dt = 20_ms;
+  units::second_t duration = trajectory.TotalTime();
+
+  while (time < duration) {
+    const Trajectory::State point = trajectory.Sample(time);
+    time += dt;
+
+    EXPECT_TRUE(units::math::abs(point.velocity) <= 12_fps + 0.01_fps);
+    EXPECT_TRUE(units::math::abs(point.acceleration) <=
+                12_fps_sq + 0.01_fps_sq);
+  }
+}