Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
Co-Authored-By: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Prateek Machiraju
2020-04-12 13:39:43 -04:00
committed by GitHub
parent 212182d991
commit a3a8472b82
12 changed files with 706 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp Normal file
View File

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

46
wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp Normal file
View File

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

57
wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h Normal file
View File

@@ -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

44
wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h Normal file
View File

@@ -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

57
wpilibc/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h Normal file
View File

@@ -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

59
wpilibc/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp Normal file
View File

@@ -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)));
}

54
wpilibc/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp Normal file
View File

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

80
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java Normal file
View File

@@ -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);
}
}

40
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java Normal file
View File

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

73
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java Normal file
View File

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

77
wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java Normal file
View File

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

65
wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java Normal file
View File

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