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

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

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