[wpilib] Trajectory: Add MaxVelocity and Region constraints (#2466)

Co-Authored-By: Tyler Veness <calcmogul@gmail.com>
2026-07-06 03:31:43 +00:00 · 2020-04-12 13:39:43 -04:00
parent 212182d991
commit a3a8472b82
12 changed files with 706 additions and 0 deletions
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 "frc/trajectory/constraint/EllipticalRegionConstraint.h"
+
+#include <limits>
+
+using namespace frc;
+
+EllipticalRegionConstraint::EllipticalRegionConstraint(
+    const Translation2d& center, units::meter_t xWidth, units::meter_t yWidth,
+    const Rotation2d& rotation, TrajectoryConstraint& constraint)
+    : m_center(center),
+      m_radii(xWidth / 2.0, yWidth / 2.0),
+      m_constraint(constraint) {
+  m_radii = m_radii.RotateBy(rotation);
+}
+
+units::meters_per_second_t EllipticalRegionConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) {
+  if (IsPoseInRegion(pose)) {
+    return m_constraint.MaxVelocity(pose, curvature, velocity);
+  } else {
+    return units::meters_per_second_t(std::numeric_limits<double>::infinity());
+  }
+}
+
+TrajectoryConstraint::MinMax EllipticalRegionConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) {
+  if (IsPoseInRegion(pose)) {
+    return m_constraint.MinMaxAcceleration(pose, curvature, speed);
+  } else {
+    return {};
+  }
+}
+
+bool EllipticalRegionConstraint::IsPoseInRegion(const Pose2d& pose) {
+  // The region (disk) bounded by the ellipse is given by the equation:
+  // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
+  // If the inequality is satisfied, then it is inside the ellipse; otherwise
+  // it is outside the ellipse.
+  // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
+  return units::math::pow<2>(pose.Translation().X() - m_center.X()) *
+                 units::math::pow<2>(m_radii.Y()) +
+             units::math::pow<2>(pose.Translation().Y() - m_center.Y()) *
+                 units::math::pow<2>(m_radii.X()) <=
+         units::math::pow<2>(m_radii.X()) * units::math::pow<2>(m_radii.Y());
+}
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 "frc/trajectory/constraint/RectangularRegionConstraint.h"
+
+#include <limits>
+
+using namespace frc;
+
+RectangularRegionConstraint::RectangularRegionConstraint(
+    const Translation2d& bottomLeftPoint, const Translation2d& topRightPoint,
+    TrajectoryConstraint& constraint)
+    : m_bottomLeftPoint(bottomLeftPoint),
+      m_topRightPoint(topRightPoint),
+      m_constraint(constraint) {}
+
+units::meters_per_second_t RectangularRegionConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) {
+  if (IsPoseInRegion(pose)) {
+    return m_constraint.MaxVelocity(pose, curvature, velocity);
+  } else {
+    return units::meters_per_second_t(std::numeric_limits<double>::infinity());
+  }
+}
+
+TrajectoryConstraint::MinMax RectangularRegionConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) {
+  if (IsPoseInRegion(pose)) {
+    return m_constraint.MinMaxAcceleration(pose, curvature, speed);
+  } else {
+    return {};
+  }
+}
+
+bool RectangularRegionConstraint::IsPoseInRegion(const Pose2d& pose) {
+  return pose.Translation().X() >= m_bottomLeftPoint.X() &&
+         pose.Translation().X() <= m_topRightPoint.X() &&
+         pose.Translation().Y() >= m_bottomLeftPoint.Y() &&
+         pose.Translation().Y() <= m_topRightPoint.Y();
+}
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * Enforces a particular constraint only within an elliptical region.
+ */
+class EllipticalRegionConstraint : public TrajectoryConstraint {
+ public:
+  /**
+   * Constructs a new EllipticalRegionConstraint.
+   *
+   * @param center The center of the ellipse in which to enforce the constraint.
+   * @param xWidth The width of the ellipse in which to enforce the constraint.
+   * @param yWidth The height of the ellipse in which to enforce the constraint.
+   * @param rotation The rotation to apply to all radii around the origin.
+   * @param constraint The constraint to enforce when the robot is within the
+   * region.
+   */
+  EllipticalRegionConstraint(const Translation2d& center, units::meter_t xWidth,
+                             units::meter_t yWidth, const Rotation2d& rotation,
+                             TrajectoryConstraint& constraint);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, units::curvature_t curvature,
+      units::meters_per_second_t velocity) override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
+                            units::meters_per_second_t speed) override;
+
+  /**
+   * Returns whether the specified robot pose is within the region that the
+   * constraint is enforced in.
+   *
+   * @param pose The robot pose.
+   * @return Whether the robot pose is within the constraint region.
+   */
+  bool IsPoseInRegion(const Pose2d& pose);
+
+ private:
+  Translation2d m_center;
+  Translation2d m_radii;
+  TrajectoryConstraint& m_constraint;
+};
+}  // namespace frc
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * Represents a constraint that enforces a max velocity. This can be composed
+ * with the EllipticalRegionConstraint or RectangularRegionConstraint to enforce
+ * a max velocity within a region.
+ */
+class MaxVelocityConstraint : public TrajectoryConstraint {
+ public:
+  /**
+   * Constructs a new MaxVelocityConstraint.
+   *
+   * @param maxVelocity The max velocity.
+   */
+  explicit MaxVelocityConstraint(units::meters_per_second_t maxVelocity)
+      : m_maxVelocity(units::math::abs(maxVelocity)) {}
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, units::curvature_t curvature,
+      units::meters_per_second_t velocity) override {
+    return m_maxVelocity;
+  }
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
+                            units::meters_per_second_t speed) override {
+    return {};
+  }
+
+ private:
+  units::meters_per_second_t m_maxVelocity;
+};
+}  // namespace frc
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * Enforces a particular constraint only within a rectangular region.
+ */
+class RectangularRegionConstraint : public TrajectoryConstraint {
+ public:
+  /**
+   * Constructs a new RectangularRegionConstraint.
+   *
+   * @param bottomLeftPoint The bottom left point of the rectangular region in
+   * which to enforce the constraint.
+   * @param topRightPoint The top right point of the rectangular region in which
+   * to enforce the constraint.
+   * @param constraint The constraint to enforce when the robot is within the
+   * region.
+   */
+  RectangularRegionConstraint(const Translation2d& bottomLeftPoint,
+                              const Translation2d& topRightPoint,
+                              TrajectoryConstraint& constraint);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, units::curvature_t curvature,
+      units::meters_per_second_t velocity) override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
+                            units::meters_per_second_t speed) override;
+
+  /**
+   * Returns whether the specified robot pose is within the region that the
+   * constraint is enforced in.
+   *
+   * @param pose The robot pose.
+   * @return Whether the robot pose is within the constraint region.
+   */
+  bool IsPoseInRegion(const Pose2d& pose);
+
+ private:
+  Translation2d m_bottomLeftPoint;
+  Translation2d m_topRightPoint;
+  TrajectoryConstraint& m_constraint;
+};
+}  // namespace frc
--- a/wpilibc/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
+++ b/wpilibc/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <units/units.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/EllipticalRegionConstraint.h"
+#include "frc/trajectory/constraint/MaxVelocityConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(EllipticalRegionConstraintTest, Constraint) {
+  constexpr auto maxVelocity = 2_fps;
+
+  auto config = TrajectoryConfig(13_fps, 13_fps_sq);
+  MaxVelocityConstraint maxVelConstraint(maxVelocity);
+  EllipticalRegionConstraint regionConstraint(frc::Translation2d{5_ft, 2.5_ft},
+                                              10_ft, 5_ft, Rotation2d(180_deg),
+                                              maxVelConstraint);
+  config.AddConstraint(regionConstraint);
+
+  auto trajectory = TestTrajectory::GetTrajectory(config);
+
+  bool exceededConstraintOutsideRegion = false;
+  for (auto& point : trajectory.States()) {
+    auto translation = point.pose.Translation();
+    if (translation.X() < 10_ft && translation.Y() < 5_ft) {
+      EXPECT_TRUE(units::math::abs(point.velocity) < maxVelocity + 0.05_mps);
+    } else if (units::math::abs(point.velocity) >= maxVelocity + 0.05_mps) {
+      exceededConstraintOutsideRegion = true;
+    }
+  }
+
+  EXPECT_TRUE(exceededConstraintOutsideRegion);
+}
+
+TEST(EllipticalRegionConstraintTest, IsPoseInRegion) {
+  constexpr auto maxVelocity = 2_fps;
+  MaxVelocityConstraint maxVelConstraint(maxVelocity);
+  EllipticalRegionConstraint regionConstraintNoRotation(
+      frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, Rotation2d(0_deg),
+      maxVelConstraint);
+  EXPECT_FALSE(regionConstraintNoRotation.IsPoseInRegion(
+      frc::Pose2d(2.1_ft, 1_ft, 0_rad)));
+
+  EllipticalRegionConstraint regionConstraintWithRotation(
+      frc::Translation2d{1_ft, 1_ft}, 2_ft, 4_ft, Rotation2d(90_deg),
+      maxVelConstraint);
+  EXPECT_TRUE(regionConstraintWithRotation.IsPoseInRegion(
+      frc::Pose2d(2.1_ft, 1_ft, 0_rad)));
+}
--- a/wpilibc/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
+++ b/wpilibc/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <units/units.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/MaxVelocityConstraint.h"
+#include "frc/trajectory/constraint/RectangularRegionConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(RectangularRegionConstraintTest, Constraint) {
+  constexpr auto maxVelocity = 2_fps;
+
+  auto config = TrajectoryConfig(13_fps, 13_fps_sq);
+  MaxVelocityConstraint maxVelConstraint(maxVelocity);
+  RectangularRegionConstraint regionConstraint(frc::Translation2d{1_ft, 1_ft},
+                                               frc::Translation2d{5_ft, 27_ft},
+                                               maxVelConstraint);
+  config.AddConstraint(regionConstraint);
+
+  auto trajectory = TestTrajectory::GetTrajectory(config);
+
+  bool exceededConstraintOutsideRegion = false;
+  for (auto& point : trajectory.States()) {
+    if (regionConstraint.IsPoseInRegion(point.pose)) {
+      EXPECT_TRUE(units::math::abs(point.velocity) < maxVelocity + 0.05_mps);
+    } else if (units::math::abs(point.velocity) >= maxVelocity + 0.05_mps) {
+      exceededConstraintOutsideRegion = true;
+    }
+  }
+
+  EXPECT_TRUE(exceededConstraintOutsideRegion);
+}
+
+TEST(RectangularRegionConstraintTest, IsPoseInRegion) {
+  constexpr auto maxVelocity = 2_fps;
+  MaxVelocityConstraint maxVelConstraint(maxVelocity);
+  RectangularRegionConstraint regionConstraint(frc::Translation2d{1_ft, 1_ft},
+                                               frc::Translation2d{5_ft, 27_ft},
+                                               maxVelConstraint);
+
+  EXPECT_FALSE(regionConstraint.IsPoseInRegion(Pose2d()));
+  EXPECT_TRUE(
+      regionConstraint.IsPoseInRegion(Pose2d(3_ft, 14.5_ft, Rotation2d())));
+}