From a3a8472b8297250acbc92a5d2f6049c549184c8d Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Sun, 12 Apr 2020 13:39:43 -0400 Subject: [PATCH] [wpilib] Trajectory: Add MaxVelocity and Region constraints (#2466) Co-Authored-By: Tyler Veness --- .../constraint/EllipticalRegionConstraint.cpp | 54 +++++++++++++ .../RectangularRegionConstraint.cpp | 46 +++++++++++ .../constraint/EllipticalRegionConstraint.h | 57 +++++++++++++ .../constraint/MaxVelocityConstraint.h | 44 ++++++++++ .../constraint/RectangularRegionConstraint.h | 57 +++++++++++++ .../EllipticalRegionConstraintTest.cpp | 59 ++++++++++++++ .../RectangularRegionConstraintTest.cpp | 54 +++++++++++++ .../EllipticalRegionConstraint.java | 80 +++++++++++++++++++ .../constraint/MaxVelocityConstraint.java | 40 ++++++++++ .../RectangularRegionConstraint.java | 73 +++++++++++++++++ .../EllipticalRegionConstraintTest.java | 77 ++++++++++++++++++ .../RectangularRegionConstraintTest.java | 65 +++++++++++++++ 12 files changed, 706 insertions(+) create mode 100644 wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp create mode 100644 wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp create mode 100644 wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h create mode 100644 wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h create mode 100644 wpilibc/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h create mode 100644 wpilibc/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp create mode 100644 wpilibc/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp new file mode 100644 index 0000000000..c70496dd99 --- /dev/null +++ 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 + +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::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()); +} diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp new file mode 100644 index 0000000000..a5927aecf0 --- /dev/null +++ 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 + +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::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(); +} diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h new file mode 100644 index 0000000000..03a13f0798 --- /dev/null +++ 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 + +#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 diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h new file mode 100644 index 0000000000..b0f9fbefd5 --- /dev/null +++ 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 + +#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 diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h new file mode 100644 index 0000000000..250f769e50 --- /dev/null +++ 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 + +#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 diff --git a/wpilibc/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp b/wpilibc/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp new file mode 100644 index 0000000000..c0bfd997ac --- /dev/null +++ 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 + +#include + +#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))); +} diff --git a/wpilibc/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp b/wpilibc/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp new file mode 100644 index 0000000000..4b11ecf6f2 --- /dev/null +++ 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 + +#include + +#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()))); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java new file mode 100644 index 0000000000..5ff2a7d073 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java @@ -0,0 +1,80 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.trajectory.constraint; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; + +/** + * Enforces a particular constraint only within an elliptical region. + */ +public class EllipticalRegionConstraint implements TrajectoryConstraint { + private final Translation2d m_center; + private final Translation2d m_radii; + private final TrajectoryConstraint m_constraint; + + /** + * 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. + */ + @SuppressWarnings("ParameterName") + public EllipticalRegionConstraint(Translation2d center, double xWidth, double yWidth, + Rotation2d rotation, TrajectoryConstraint constraint) { + m_center = center; + m_radii = new Translation2d(xWidth / 2.0, yWidth / 2.0).rotateBy(rotation); + m_constraint = constraint; + } + + @Override + public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter, + double velocityMetersPerSecond) { + if (isPoseInRegion(poseMeters)) { + return m_constraint.getMaxVelocityMetersPerSecond(poseMeters, curvatureRadPerMeter, + velocityMetersPerSecond); + } else { + return Double.POSITIVE_INFINITY; + } + } + + @Override + public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, + double curvatureRadPerMeter, + double velocityMetersPerSecond) { + if (isPoseInRegion(poseMeters)) { + return m_constraint.getMinMaxAccelerationMetersPerSecondSq(poseMeters, + curvatureRadPerMeter, velocityMetersPerSecond); + } else { + return new MinMax(); + } + } + + /** + * Returns whether the specified robot pose is within the region that the constraint + * is enforced in. + * + * @param robotPose The robot pose. + * @return Whether the robot pose is within the constraint region. + */ + public boolean isPoseInRegion(Pose2d robotPose) { + // 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 Math.pow(robotPose.getTranslation().getX() - m_center.getX(), 2) + * Math.pow(m_radii.getY(), 2) + + Math.pow(robotPose.getTranslation().getY() - m_center.getY(), 2) + * Math.pow(m_radii.getX(), 2) <= Math.pow(m_radii.getX(), 2) * Math.pow(m_radii.getY(), 2); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java new file mode 100644 index 0000000000..4d60623e83 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.trajectory.constraint; + +import edu.wpi.first.wpilibj.geometry.Pose2d; + +/** + * Represents a constraint that enforces a max velocity. This can be composed with the + * {@link EllipticalRegionConstraint} or {@link RectangularRegionConstraint} to enforce + * a max velocity in a region. + */ +public class MaxVelocityConstraint implements TrajectoryConstraint { + private final double m_maxVelocity; + + /** + * Constructs a new MaxVelocityConstraint. + * + * @param maxVelocityMetersPerSecond The max velocity. + */ + public MaxVelocityConstraint(double maxVelocityMetersPerSecond) { + m_maxVelocity = maxVelocityMetersPerSecond; + } + + @Override + public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter, + double velocityMetersPerSecond) { + return m_maxVelocity; + } + + @Override + public TrajectoryConstraint.MinMax getMinMaxAccelerationMetersPerSecondSq( + Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { + return new MinMax(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java new file mode 100644 index 0000000000..4fa8e7d99b --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.trajectory.constraint; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; + +/** + * Enforces a particular constraint only within a rectangular region. + */ +public class RectangularRegionConstraint implements TrajectoryConstraint { + private final Translation2d m_bottomLeftPoint; + private final Translation2d m_topRightPoint; + private final TrajectoryConstraint m_constraint; + + /** + * 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. + */ + public RectangularRegionConstraint(Translation2d bottomLeftPoint, Translation2d topRightPoint, + TrajectoryConstraint constraint) { + m_bottomLeftPoint = bottomLeftPoint; + m_topRightPoint = topRightPoint; + m_constraint = constraint; + } + + @Override + public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter, + double velocityMetersPerSecond) { + if (isPoseInRegion(poseMeters)) { + return m_constraint.getMaxVelocityMetersPerSecond(poseMeters, curvatureRadPerMeter, + velocityMetersPerSecond); + } else { + return Double.POSITIVE_INFINITY; + } + } + + @Override + public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, + double curvatureRadPerMeter, + double velocityMetersPerSecond) { + if (isPoseInRegion(poseMeters)) { + return m_constraint.getMinMaxAccelerationMetersPerSecondSq(poseMeters, + curvatureRadPerMeter, velocityMetersPerSecond); + } else { + return new MinMax(); + } + } + + /** + * Returns whether the specified robot pose is within the region that the constraint + * is enforced in. + * + * @param robotPose The robot pose. + * @return Whether the robot pose is within the constraint region. + */ + public boolean isPoseInRegion(Pose2d robotPose) { + return robotPose.getTranslation().getX() >= m_bottomLeftPoint.getX() + && robotPose.getTranslation().getX() <= m_topRightPoint.getX() + && robotPose.getTranslation().getY() >= m_bottomLeftPoint.getY() + && robotPose.getTranslation().getY() <= m_topRightPoint.getY(); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java new file mode 100644 index 0000000000..513db06afe --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java @@ -0,0 +1,77 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.trajectory; + +import java.util.List; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.trajectory.constraint.EllipticalRegionConstraint; +import edu.wpi.first.wpilibj.trajectory.constraint.MaxVelocityConstraint; +import edu.wpi.first.wpilibj.util.Units; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +public class EllipticalRegionConstraintTest { + @Test + void testConstraint() { + // Create constraints + double maxVelocity = Units.feetToMeters(3.0); + var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity); + var regionConstraint = new EllipticalRegionConstraint( + new Translation2d(Units.feetToMeters(5.0), Units.feetToMeters(5.0)), + Units.feetToMeters(10.0), Units.feetToMeters(5.0), Rotation2d.fromDegrees(180.0), + maxVelocityConstraint + ); + + // Get trajectory + var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint)); + + // Iterate through trajectory and check constraints + boolean exceededConstraintOutsideRegion = false; + for (var point : trajectory.getStates()) { + var translation = point.poseMeters.getTranslation(); + + if (translation.getX() < Units.feetToMeters(10) + && translation.getY() < Units.feetToMeters(5)) { + assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05); + } else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) { + exceededConstraintOutsideRegion = true; + } + } + assertTrue(exceededConstraintOutsideRegion); + } + + @Test + void testIsPoseWithinRegion() { + double maxVelocity = Units.feetToMeters(3.0); + var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity); + + var regionConstraintNoRotation = new EllipticalRegionConstraint( + new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)), + Units.feetToMeters(2.0), Units.feetToMeters(4.0), new Rotation2d(), + maxVelocityConstraint); + + assertFalse(regionConstraintNoRotation.isPoseInRegion(new Pose2d( + Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d() + ))); + + var regionConstraintWithRotation = new EllipticalRegionConstraint( + new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)), + Units.feetToMeters(2.0), Units.feetToMeters(4.0), Rotation2d.fromDegrees(90.0), + maxVelocityConstraint); + + assertTrue(regionConstraintWithRotation.isPoseInRegion(new Pose2d( + Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d() + ))); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java new file mode 100644 index 0000000000..94eeb359db --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java @@ -0,0 +1,65 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.trajectory; + +import java.util.List; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.trajectory.constraint.MaxVelocityConstraint; +import edu.wpi.first.wpilibj.trajectory.constraint.RectangularRegionConstraint; +import edu.wpi.first.wpilibj.util.Units; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +public class RectangularRegionConstraintTest { + @Test + void testConstraint() { + // Create constraints + double maxVelocity = Units.feetToMeters(3.0); + var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity); + var regionConstraint = new RectangularRegionConstraint( + new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)), + new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)), + maxVelocityConstraint + ); + + // Get trajectory + var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint)); + + // Iterate through trajectory and check constraints + boolean exceededConstraintOutsideRegion = false; + for (var point : trajectory.getStates()) { + if (regionConstraint.isPoseInRegion(point.poseMeters)) { + assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05); + } else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) { + exceededConstraintOutsideRegion = true; + } + } + assertTrue(exceededConstraintOutsideRegion); + } + + @Test + void testIsPoseWithinRegion() { + double maxVelocity = Units.feetToMeters(3.0); + var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity); + var regionConstraint = new RectangularRegionConstraint( + new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)), + new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)), + maxVelocityConstraint + ); + + assertFalse(regionConstraint.isPoseInRegion(new Pose2d())); + assertTrue(regionConstraint.isPoseInRegion(new Pose2d(Units.feetToMeters(3.0), + Units.feetToMeters(14.5), new Rotation2d()))); + } +}