Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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

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