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

Co-Authored-By: Tyler Veness <calcmogul@gmail.com>

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());
+}

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();
+}

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

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

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