diff --git a/src/main/java/xbot/common/injection/electrical_contract/XSwerveDriveElectricalContract.java b/src/main/java/xbot/common/injection/electrical_contract/XSwerveDriveElectricalContract.java index 94aa545b5..88edee833 100644 --- a/src/main/java/xbot/common/injection/electrical_contract/XSwerveDriveElectricalContract.java +++ b/src/main/java/xbot/common/injection/electrical_contract/XSwerveDriveElectricalContract.java @@ -70,4 +70,10 @@ default Distance getDriveWheelDiameter() { * @return The gear ratio for the steering motors. */ double getSteeringGearRatio(); + + /** + * Returns the largest radius of the robot. + * @return The gear ratio for the steering motors. + */ + Distance getRadiusOfRobot(); } diff --git a/src/main/java/xbot/common/subsystems/drive/BaseSwerveDriveSubsystem.java b/src/main/java/xbot/common/subsystems/drive/BaseSwerveDriveSubsystem.java index 66343654f..7abc65234 100644 --- a/src/main/java/xbot/common/subsystems/drive/BaseSwerveDriveSubsystem.java +++ b/src/main/java/xbot/common/subsystems/drive/BaseSwerveDriveSubsystem.java @@ -509,6 +509,33 @@ public SwerveModuleSubsystem getRearRightSwerveModuleSubsystem() { return this.rearRightSwerveModuleSubsystem; } + /** + * Gets the current robot-relative chassis speeds by converting the current swerve module states + * through inverse kinematics. This is needed by PathPlanner's AutoBuilder. + * @return The current robot-relative ChassisSpeeds. + */ + public ChassisSpeeds getRobotRelativeSpeeds() { + var states = getCurrentSwerveStates(); + return swerveDriveKinematics.toChassisSpeeds(states.toArray()); + } + + /** + * Drives the robot using the given robot-relative ChassisSpeeds. Converts the ChassisSpeeds + * to individual swerve module states and applies them. This is needed by PathPlanner's AutoBuilder. + * @param chassisSpeeds The desired robot-relative chassis speeds. + */ + public void driveWithChassisSpeeds(ChassisSpeeds chassisSpeeds) { + SwerveModuleState[] moduleStates = swerveDriveKinematics.toSwerveModuleStates(chassisSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, maxTargetSpeedMps.get()); + + aKitLog.setLogLevel(AKitLogger.LogLevel.INFO); + aKitLog.record("DesiredSwerveState", moduleStates); + this.getFrontLeftSwerveModuleSubsystem().setTargetState(moduleStates[0]); + this.getFrontRightSwerveModuleSubsystem().setTargetState(moduleStates[1]); + this.getRearLeftSwerveModuleSubsystem().setTargetState(moduleStates[2]); + this.getRearRightSwerveModuleSubsystem().setTargetState(moduleStates[3]); + } + /*** * Gets the current states of all four swerve modules. * @return The current states of all four swerve modules. diff --git a/src/main/java/xbot/common/subsystems/oracle/SwervePointPathPlanning.java b/src/main/java/xbot/common/subsystems/oracle/SwervePointPathPlanning.java new file mode 100644 index 000000000..1ad8f9e85 --- /dev/null +++ b/src/main/java/xbot/common/subsystems/oracle/SwervePointPathPlanning.java @@ -0,0 +1,175 @@ +package xbot.common.subsystems.oracle; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.units.measure.Distance; +import static edu.wpi.first.units.Units.Meters; +import edu.wpi.first.wpilibj.MockPowerDistributionPanel; +import org.apache.logging.log4j.LogManager; +import org.apache.logging.log4j.Logger; +import xbot.common.injection.electrical_contract.XSwerveDriveElectricalContract; +import xbot.common.trajectory.XbotSwervePoint; +import xbot.common.subsystems.pose.GameField; +import xbot.common.subsystems.pose.IFieldObstacle; +import xbot.common.subsystems.pose.ObstacleMap; + +import java.awt.geom.Point2D; +import javax.inject.Inject; +import java.util.ArrayList; +import java.util.List; + +public class SwervePointPathPlanning { + private final Distance radius; + private final GameField gameField; + private final ObstacleMap obstacleMap; + + private static Logger log = LogManager.getLogger(SwervePointPathPlanning.class); + + @Inject + public SwervePointPathPlanning(ObstacleMap obstacleMap, GameField gameField, XSwerveDriveElectricalContract electrical_contract) { + this.obstacleMap = obstacleMap; + this.gameField = gameField; + this.radius = electrical_contract.getRadiusOfRobot(); + } + + /** + * Generates a list of swerve points from Point A -> B while avoiding the big reef + * @param startingPose you are at + * @param endingPose you want to go to + * @return a list of swerve points to destination + */ + public List generateSwervePoints(Pose2d startingPose, Pose2d endingPose, boolean allowToughTerrain) { + List swervePoints = new ArrayList<>(); + Translation2d start = startingPose.getTranslation(); + Translation2d end = endingPose.getTranslation(); + + // If there's no intersection between the starting point and the collision circle, just proceed + // directly to the ending point. + if (!this.obstacleMap.doesRobotPathIntersect(start, end, allowToughTerrain)) { + swervePoints.add(new XbotSwervePoint(endingPose, 0.001)); // Set to small number so SSTC does not complain. + return swervePoints; + } + var closestObstacle = this.obstacleMap.closestObstacle(startingPose, false).orElseThrow(); + + Translation2d tangentPoint = null; + // If we are currently in a rough terrain then it doesn't matter. + if (this.obstacleMap.doesRobotPathIntersect(start, start, false)) { + // If we're inside the routing circle and there is a collision, we need to first back out to the routing circle. + Translation2d direction = start.minus(closestObstacle.center()); + tangentPoint = closestObstacle.center().plus(direction.times(closestObstacle.avoidanceRadius().in(Meters) / direction.getNorm())); + swervePoints.add(new XbotSwervePoint(new Pose2d(tangentPoint, startingPose.getRotation()), 10)); + } else { + // otherwise, we're outside the routing circle, and we need to first move to the tangent point. + tangentPoint = findClosestTangentPoint(start, end, closestObstacle); + swervePoints.add(new XbotSwervePoint(new Pose2d(tangentPoint, endingPose.getRotation()), 0.001)); + } + + int escape = 0; + while (this.obstacleMap.doesRobotPathIntersect(tangentPoint, end, allowToughTerrain)) { + escape++; + tangentPoint = moveAlongCircumference(tangentPoint, end, 0.25, closestObstacle); + swervePoints.add(new XbotSwervePoint(new Pose2d(tangentPoint, endingPose.getRotation()), 10)); + if (escape > 100) { + log.warn("Infinite loop detected in generateSwervePoints, breaking out!"); + break; + } + } + + swervePoints.add(new XbotSwervePoint(endingPose, 10)); + return swervePoints; + } + + // Note - the math in this class is AI generated and has not been fully verified. + // Some tests have been run, but it needs to be more hardened for robotics (e.g. protection against division by zero) + // TODO: additional protections + private Translation2d findClosestTangentPoint(Translation2d point, Translation2d endPoint, IFieldObstacle closestObstacle) { + double cx = closestObstacle.center().getX(); + double cy = closestObstacle.center().getY(); + double r = radius.in(Meters); + double px = point.getX(); + double py = point.getY(); + + // Distance from circle center to external point + double dx = px - cx; + double dy = py - cy; + double dSq = dx * dx + dy * dy; + double d = Math.sqrt(dSq); + + // Check for cases like already on the circle or inside the circle + if ((d < 1e-12) || (d Math.PI) { + angleDifference -= 2 * Math.PI; + } else if (angleDifference < -Math.PI) { + angleDifference += 2 * Math.PI; + } + + double newAngle = angleCurrent + (angleDifference > 0 ? angleStep : -angleStep); + return new Translation2d(closestObstacle.center().getX() + radius.in(Meters) * Math.cos(newAngle), + closestObstacle.center().getY() + radius.in(Meters) * Math.sin(newAngle)); + } + + private Trajectory visualizeCircleAsTrajectory(Translation2d center, double radius, int numberOfSteps) { + var wpiStates = new ArrayList(); + double angleStep = 2 * Math.PI / numberOfSteps; + + for (int i = 0; i < numberOfSteps; i++) { + double angle = i * angleStep; + double x = center.getX() + radius * Math.cos(angle); + double y = center.getY() + radius * Math.sin(angle); + Pose2d pose = new Pose2d(x, y, new Rotation2d(angle)); + Trajectory.State state = new Trajectory.State(); + state.poseMeters = pose; + wpiStates.add(state); + } + + return new Trajectory(wpiStates); + } +} diff --git a/src/main/java/xbot/common/subsystems/pose/CircleFieldObstacle.java b/src/main/java/xbot/common/subsystems/pose/CircleFieldObstacle.java new file mode 100644 index 000000000..bc6362dc9 --- /dev/null +++ b/src/main/java/xbot/common/subsystems/pose/CircleFieldObstacle.java @@ -0,0 +1,80 @@ +package xbot.common.subsystems.pose; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.Units; + + +public class CircleFieldObstacle implements IFieldObstacle { + private final Translation2d center; + private final Distance radius; + private final boolean isToughTerrain; + + public CircleFieldObstacle(Translation2d center, Distance radius, boolean isToughTerrain) { + this.center = center; + this.radius = radius; + this.isToughTerrain = isToughTerrain; + } + + @Override + public boolean isToughTerrain() { + return this.isToughTerrain; + } + + @Override + public Translation2d center() { + return this.center(); + } + + @Override + public Distance avoidanceRadius() { + return this.radius; + } + + // Note - the math in this class is AI generated and has not been fully verified. + @Override + public boolean doesRobotPathIntersect(Translation2d start, Translation2d end, Distance robotRadius) { + final double r = robotRadius.in(Units.Meters) + this.radius.in(Units.Meters); + final double r2 = r * r; + + final double ax = start.getX(); + final double ay = start.getY(); + final double bx = end.getX(); + final double by = end.getY(); + final double cx = center.getX(); + final double cy = center.getY(); + + // AB + final double abx = bx - ax; + final double aby = by - ay; + + final double abLen2 = abx * abx + aby * aby; + + // Degenerate segment: treat as point + if (abLen2 == 0.0) { + final double dx = cx - ax; + final double dy = cy - ay; + return (dx * dx + dy * dy) <= r2; + } + + // Project AC onto AB, clamp to [0,1] + final double acx = cx - ax; + final double acy = cy - ay; + + double t = (acx * abx + acy * aby) / abLen2; + if (t < 0.0) { + t = 0.0; + } else if (t > 1.0) { + t = 1.0; + } + + // Closest point P on segment + final double px = ax + t * abx; + final double py = ay + t * aby; + + final double dx = cx - px; + final double dy = cy - py; + + return (dx * dx + dy * dy) <= r2; + } +} diff --git a/src/main/java/xbot/common/subsystems/pose/IFieldObstacle.java b/src/main/java/xbot/common/subsystems/pose/IFieldObstacle.java new file mode 100644 index 000000000..e5e208ce8 --- /dev/null +++ b/src/main/java/xbot/common/subsystems/pose/IFieldObstacle.java @@ -0,0 +1,38 @@ +package xbot.common.subsystems.pose; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Distance; + +public interface IFieldObstacle { + /** + * Returns this obstacle can still be moved through, but is slower terrain or more dangerous terrain to move through. + * + * @return Whether the obstacle matches the description above for tough terrain. + */ + public boolean isToughTerrain(); + + /** + * Returns the center of the obstacle to help determine closest obstacle. + * + * @return A translation 2d for where the center of this obstacle + */ + public Translation2d center(); + + /** + * What's the radius that we'll attempt to avoid when we need to move away from the obstacle. + * + * @return Whether the obstacle matches the description above for tough terrain. + */ + public Distance avoidanceRadius(); + + /** + * Whether the described robot path will collide with this field obstacle or not. + * + * @param start The start of the path. + * @param end The end of the path. + * @param robotRadius The radius we should use to determine whether the robot can make the path through this obstacle. + * + * @return Whether the path will work for this obstacle. + */ + public boolean doesRobotPathIntersect(Translation2d start, Translation2d end, Distance robotRadius); +} diff --git a/src/main/java/xbot/common/subsystems/pose/ObstacleMap.java b/src/main/java/xbot/common/subsystems/pose/ObstacleMap.java new file mode 100644 index 000000000..bf4da72fd --- /dev/null +++ b/src/main/java/xbot/common/subsystems/pose/ObstacleMap.java @@ -0,0 +1,44 @@ +package xbot.common.subsystems.pose; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Distance; +import xbot.common.injection.electrical_contract.XSwerveDriveElectricalContract; + +import java.util.List; +import java.util.Optional; + +public abstract class ObstacleMap { + private final List fieldObstacles; + private final Distance robotRadius; + + public ObstacleMap(List fieldObstacles, XSwerveDriveElectricalContract electricalContract) { + this.fieldObstacles = fieldObstacles; + this.robotRadius = electricalContract.getRadiusOfRobot(); + } + + public Optional closestObstacle(Pose2d pose, boolean allowToughTerrain) { + if (this.fieldObstacles.size() == 0) { + return Optional.empty(); + } + + var translation = pose.getTranslation(); + var obstacleLocations = this.fieldObstacles.stream() + .filter(obstacle -> !obstacle.isToughTerrain() || !allowToughTerrain) + .map(obstacle -> obstacle.center()) + .toList(); + + var closestObstacleLocation = translation.nearest(obstacleLocations); + return this.fieldObstacles.stream() + .filter(obstacle -> !obstacle.isToughTerrain() || !allowToughTerrain) + .filter(obstacle -> obstacle.center() == closestObstacleLocation) + .findFirst(); + } + + public boolean doesRobotPathIntersect(Translation2d start, Translation2d end, boolean allowToughTerrain) { + return this.fieldObstacles.stream() + .filter(obstacle -> !obstacle.isToughTerrain() || !allowToughTerrain) + .anyMatch(obstacle -> obstacle.doesRobotPathIntersect(start, end, this.robotRadius)); + + } +} diff --git a/src/main/java/xbot/common/subsystems/pose/RectangleFieldObstacle.java b/src/main/java/xbot/common/subsystems/pose/RectangleFieldObstacle.java new file mode 100644 index 000000000..56f4e6f8e --- /dev/null +++ b/src/main/java/xbot/common/subsystems/pose/RectangleFieldObstacle.java @@ -0,0 +1,125 @@ +package xbot.common.subsystems.pose; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.Units; + +/** + * Rectangle obstacle defined by center + half extents. + * + * Collision is tested as: (segment swept by robotRadius) intersects rectangle + * <=> segment intersects the rectangle inflated by robotRadius in X and Y. + */ +public class RectangleFieldObstacle implements IFieldObstacle { + private final Translation2d center; + private final Distance halfWidth; + private final Distance halfHeight; + private final boolean isToughTerrain; + + public RectangleFieldObstacle( + Translation2d center, + Distance halfWidth, + Distance halfHeight, + boolean isToughTerrain + ) { + this.center = center; + this.halfWidth = halfWidth; + this.halfHeight = halfHeight; + this.isToughTerrain = isToughTerrain; + } + + @Override + public boolean isToughTerrain() { + return this.isToughTerrain; + } + + @Override + public Translation2d center() { + return this.center(); + } + + @Override + public Distance avoidanceRadius() { + return Units.Meters.of(Math.max(this.halfWidth.in(Units.Meters), this.halfHeight.in(Units.Meters))); + } + + @Override + public boolean doesRobotPathIntersect(Translation2d start, Translation2d end, Distance robotRadius) { + final double expand = robotRadius.in(Units.Meters); + + final double hx = halfWidth.in(Units.Meters) + expand; + final double hy = halfHeight.in(Units.Meters) + expand; + + final double minX = center.getX() - hx; + final double maxX = center.getX() + hx; + final double minY = center.getY() - hy; + final double maxY = center.getY() + hy; + + return segmentIntersectsRectangle(start, end, minX, minY, maxX, maxY); + } + + // Note - the math in this class is AI generated and has not been fully verified. + // Segment vs axis-aligned bounding box intersection using the parametric "slab" method. + private static boolean segmentIntersectsRectangle( + Translation2d a, + Translation2d b, + double minX, double minY, + double maxX, double maxY + ) { + final double ax = a.getX(); + final double ay = a.getY(); + final double bx = b.getX(); + final double by = b.getY(); + + final double dx = bx - ax; + final double dy = by - ay; + + double tMin = 0.0; + double tMax = 1.0; + + // X slab + if (dx == 0.0) { + if (ax < minX || ax > maxX) { + return false; + } + } else { + final double invDx = 1.0 / dx; + double t1 = (minX - ax) * invDx; + double t2 = (maxX - ax) * invDx; + if (t1 > t2) { + double tmp = t1; + t1 = t2; + t2 = tmp; + } + tMin = Math.max(tMin, t1); + tMax = Math.min(tMax, t2); + if (tMin > tMax) { + return false; + } + } + + // Y slab + if (dy == 0.0) { + if (ay < minY || ay > maxY) { + return false; + } + } else { + final double invDy = 1.0 / dy; + double invDyVal = 1.0 / dy; + double t1 = (minY - ay) * invDyVal; + double t2 = (maxY - ay) * invDyVal; + if (t1 > t2) { + double tmp = t1; + t1 = t2; + t2 = tmp; + } + tMin = Math.max(tMin, t1); + tMax = Math.min(tMax, t2); + if (tMin > tMax) { + return false; + } + } + + return true; + } +} diff --git a/src/main/java/xbot/common/subsystems/pose/SquareFieldObstacle.java b/src/main/java/xbot/common/subsystems/pose/SquareFieldObstacle.java new file mode 100644 index 000000000..219597c65 --- /dev/null +++ b/src/main/java/xbot/common/subsystems/pose/SquareFieldObstacle.java @@ -0,0 +1,17 @@ +package xbot.common.subsystems.pose; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Distance; + +/** + * Square obstacle defined by center + half extent. + */ +public final class SquareFieldObstacle extends RectangleFieldObstacle { + public SquareFieldObstacle( + Translation2d center, + Distance halfWidth, + boolean isToughTerrain + ) { + super(center, halfWidth, halfWidth, isToughTerrain); + } +} diff --git a/src/test/java/xbot/common/injection/electrical_contract/MockSwerveDriveElectricalContract.java b/src/test/java/xbot/common/injection/electrical_contract/MockSwerveDriveElectricalContract.java index 63e6f3c7f..8fe96b191 100644 --- a/src/test/java/xbot/common/injection/electrical_contract/MockSwerveDriveElectricalContract.java +++ b/src/test/java/xbot/common/injection/electrical_contract/MockSwerveDriveElectricalContract.java @@ -1,6 +1,7 @@ package xbot.common.injection.electrical_contract; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Distance; import xbot.common.injection.swerve.SwerveInstance; import javax.inject.Inject; @@ -160,4 +161,9 @@ public double getDriveGearRatio() { public double getSteeringGearRatio() { return 1; } + + @Override + public Distance getRadiusOfRobot() { + return Inches.of(18); + } }