Skip to content
This repository has been archived by the owner on Jan 10, 2025. It is now read-only.

Commit

Permalink
Incorporate a curvature based dynamic lookahead
Browse files Browse the repository at this point in the history
  • Loading branch information
@simonstoryparker
simonstoryparker committed Jan 2, 2025
1 parent 1956920 commit 2f57a15
Show file tree
Hide file tree
Showing 2 changed files with 196 additions and 107 deletions.
29 changes: 20 additions & 9 deletions src/main/java/frc/robot/autos/amp/TestAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,22 +30,33 @@ protected Command getRedAutoCommand() {
new Pose2d(15.18, 6.41, Rotation2d.fromDegrees(0.0)))),
trailblazer.followSegment(
new AutoSegment(
new AutoPoint(new Pose2d(12.00, 6.28, Rotation2d.fromDegrees(-100))),
new AutoPoint(new Pose2d(10.00, 5.28, Rotation2d.fromDegrees(40))),
new AutoPoint(new Pose2d(8.00, 6.28, Rotation2d.fromDegrees(-50))),
new AutoPoint(new Pose2d(6.00, 5.28, Rotation2d.fromDegrees(100))),
new AutoPoint(new Pose2d(4.00, 6.28, Rotation2d.fromDegrees(0)))

)),
new AutoPoint(new Pose2d(12.00, 5, Rotation2d.fromDegrees(-10))),
new AutoPoint(new Pose2d(10.00, 5, Rotation2d.fromDegrees(10))),
new AutoPoint(new Pose2d(8.00, 5, Rotation2d.fromDegrees(10))),
new AutoPoint(new Pose2d(7.00, 3.8, Rotation2d.fromDegrees(-10))),
new AutoPoint(new Pose2d(5.00, 4.8, Rotation2d.fromDegrees(10))),
new AutoPoint(new Pose2d(4.00, 3.28, Rotation2d.fromDegrees(-10))))),
trailblazer.followSegment(
new AutoSegment(new AutoPoint(new Pose2d(11.82, 6.59, Rotation2d.fromDegrees(24.0))))),
new AutoSegment(
new AutoPoint(new Pose2d(7.0, 7.0, Rotation2d.fromDegrees(135.0))),
new AutoPoint(new Pose2d(12.0, 7.0, Rotation2d.fromDegrees(45.0))),
new AutoPoint(new Pose2d(12.0, 2.0, Rotation2d.fromDegrees(-45.0))),
new AutoPoint(new Pose2d(7.0, 2.0, Rotation2d.fromDegrees(-135.0))),
new AutoPoint(new Pose2d(8.0, 6.0, Rotation2d.fromDegrees(135.0))),
new AutoPoint(new Pose2d(11.0, 6.0, Rotation2d.fromDegrees(45.0))),
new AutoPoint(new Pose2d(11.0, 3.0, Rotation2d.fromDegrees(-45.0))),
new AutoPoint(new Pose2d(8.0, 3.0, Rotation2d.fromDegrees(-135.0))),
new AutoPoint(new Pose2d(9.0, 5.0, Rotation2d.fromDegrees(135.0))),
new AutoPoint(new Pose2d(10.0, 5.0, Rotation2d.fromDegrees(45.0))),
new AutoPoint(new Pose2d(10.0, 4.0, Rotation2d.fromDegrees(-45.0))),
new AutoPoint(new Pose2d(9.0, 4.0, Rotation2d.fromDegrees(-135.0))))),
trailblazer.followSegment(
new AutoSegment(
new AutoPoint(new Pose2d(9.98, 6.55, Rotation2d.fromDegrees(-5.0))),
new AutoPoint(new Pose2d(8.60, 5.85, Rotation2d.fromDegrees(100.0))))),
trailblazer.followSegment(
new AutoSegment(
new AutoPoint(new Pose2d(10.78, 7.34, Rotation2d.fromDegrees(-80.0))),
new AutoPoint(new Pose2d(13.3, 6.59, Rotation2d.fromDegrees(40.0))))));
new AutoPoint(new Pose2d(13.3, 6.59, Rotation2d.fromDegrees(-100))))));
}
}
274 changes: 176 additions & 98 deletions src/main/java/frc/robot/autos/trailblazer/trackers/pure_pursuit/PurePursuitPathTracker.java
View file
Original file line number Diff line number Diff line change
@@ -1,24 +1,33 @@
package frc.robot.autos.trailblazer.trackers.pure_pursuit;

import java.util.List;

import dev.doglog.DogLog;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.Timer;
import frc.robot.autos.trailblazer.AutoPoint;
import frc.robot.autos.trailblazer.trackers.PathTracker;
import java.util.List;

// TODO: Implement https://github.com/team581/2024-offseason-bot/issues/95
public class PurePursuitPathTracker implements PathTracker {
private static final double LOOKAHEAD_DISTANCE = 1.0;
private static final double FINISHED_THRESHOLD = 0.05;
private static final boolean USE_DYNAMIC_LOOKAHEAD = true;
private static final double NON_DYNAMIC_LOOKAHEAD_DISTANCE = 1.5;
private static final double AT_END_OF_SEGMENT_DISTANCE_THRESHOLD = 0.1;
private static final double DYNAMIC_LOOKAHEAD_TRANSITION_TIME = 0.5;
private static final double DYNAMIC_LOOKAHEAD_SCALE = 0.5;
private static final double DYNAMIC_LOOKAHEAD_MAX = 3.0;
private double lookaheadDistance = 0.0;
private double lastRequestedLookaheadDistance = Double.MAX_VALUE;
private double transitionStartTime = 0.0;
private double lastStartTime = 0.0;

private List<AutoPoint> points = List.of();
private Pose2d currentRobotPose = new Pose2d();
private Pose2d startingRobotPose = new Pose2d();
private boolean startingRobotPoseUpdated = false;
private Pose2d lastTargetWaypoint = new Pose2d();
private Pose2d currentTargetWaypoint = new Pose2d();
private Pose2d nextTargetWaypoint = new Pose2d();
private int currentPointIndex = 0;

@Override
Expand All @@ -32,99 +41,89 @@ public void resetAndSetPoints(List<AutoPoint> points) {
@Override
public void updateRobotState(Pose2d currentPose, ChassisSpeeds currentFieldRelativeRobotSpeeds) {
this.currentRobotPose = currentPose;

if (!startingRobotPoseUpdated) {
startingRobotPose = currentPose;
startingRobotPoseUpdated = true;
}
DogLog.log(
"Autos/Trailblazer/PurePursuitPathTracker/CurrentPointIndex", getCurrentPointIndex());
DogLog.log(
"Autos/Trailblazer/PurePursuitPathTracker/StartingRobotPose/Point", startingRobotPose);
DogLog.log(
"Autos/Trailblazer/PurePursuitPathTracker/StartingRobotPose/Updated",
startingRobotPoseUpdated);

if (!startingRobotPoseUpdated) {
startingRobotPose = currentPose;
startingRobotPoseUpdated = true;
}
DogLog.log("Autos/Trailblazer/PurePursuitPathTracker/LookaheadDistance", lookaheadDistance);
DogLog.log("Autos/Trailblazer/PurePursuitPathTracker/Waypoints/Start", lastTargetWaypoint);
DogLog.log("Autos/Trailblazer/PurePursuitPathTracker/Waypoints/End", currentTargetWaypoint);
}

@Override
public Pose2d getTargetPose() {
DogLog.log("Autos/Trailblazer/PurePursuitPathTracker/Size", points.size());

if (points.isEmpty()) {
return new Pose2d();
}

if (points.size() == 1) {
currentTargetWaypoint = startingRobotPose;
if (getCurrentPointIndex() == 0) {
lastTargetWaypoint = startingRobotPose;
} else {
currentTargetWaypoint = points.get(getCurrentPointIndex()).poseSupplier.get();
lastTargetWaypoint = points.get(getCurrentPointIndex() - 1).poseSupplier.get();
}

if (getCurrentPointIndex() < points.size() - 1) {
nextTargetWaypoint = points.get(getCurrentPointIndex() + 1).poseSupplier.get();
} else {
nextTargetWaypoint = points.get(getCurrentPointIndex()).poseSupplier.get();
}
var startX = currentTargetWaypoint.getX();
var startY = currentTargetWaypoint.getY();
var endX = nextTargetWaypoint.getX();
var endY = nextTargetWaypoint.getY();
currentTargetWaypoint = points.get(getCurrentPointIndex()).poseSupplier.get();
var perpendicularPoint =
getPerpendicularPoint(currentTargetWaypoint, nextTargetWaypoint, currentRobotPose);
getPerpendicularPoint(lastTargetWaypoint, currentTargetWaypoint, currentRobotPose);

updateLookahead();

var lookaheadPoint =
getLookaheadPoint(
currentTargetWaypoint, nextTargetWaypoint, perpendicularPoint, LOOKAHEAD_DISTANCE);

var lookaheadX = lookaheadPoint.getX();
var lookaheadY = lookaheadPoint.getY();
// Check if lookahead point is outside of the line segment
if (!((lookaheadX - startX) * (lookaheadX - endX) <= 0
&& (lookaheadY - startY) * (lookaheadY - endY) <= 0)) {

var distanceToStart =
Math.sqrt(Math.pow(lookaheadX - startX, 2) + Math.pow(lookaheadY - startY, 2));
var distanceToEnd =
Math.sqrt(Math.pow(lookaheadX - endX, 2) + Math.pow(lookaheadY - endY, 2));
// Check if lookaheadpoint is outside on the ending side of the line
if (distanceToStart > distanceToEnd) {
var perpDistanceToEnd =
Math.sqrt(
Math.pow(perpendicularPoint.getX() - endX, 2)
+ Math.pow(perpendicularPoint.getY() - endY, 2));
// check if we're at corner
if (getCurrentPointIndex() < points.size() - 2) {
var futurePoint = points.get(getCurrentPointIndex() + 2).poseSupplier.get();
currentPointIndex++;
var targetPose =
getLookaheadPoint(
nextTargetWaypoint,
futurePoint,
nextTargetWaypoint,
LOOKAHEAD_DISTANCE - perpDistanceToEnd);
DogLog.log("Autos/Trailblazer/PurePursuitPathTracker/TargetPose", targetPose);
return targetPose;

}
// otherwise just return the end point
else {
var targetPose = new Pose2d(endX, endY, getPointToPointInterpolatedRotation(currentTargetWaypoint, nextTargetWaypoint, currentRobotPose));
DogLog.log("Autos/Trailblazer/PurePursuitPathTracker/TargetPose", targetPose);
return targetPose;
}
} else {
var targetPose =
lastTargetWaypoint, currentTargetWaypoint, perpendicularPoint, lookaheadDistance);
var lookaheadOutside =
!((lookaheadPoint.getX() - lastTargetWaypoint.getX())
* (lookaheadPoint.getX() - currentTargetWaypoint.getX())
<= 0
&& (lookaheadPoint.getY() - lastTargetWaypoint.getY())
* (lookaheadPoint.getY() - currentTargetWaypoint.getY())
<= 0);
var lookaheadToStartDistance =
lookaheadPoint.getTranslation().getDistance(lastTargetWaypoint.getTranslation());
var lookaheadToEndDistance =
lookaheadPoint.getTranslation().getDistance(currentTargetWaypoint.getTranslation());
if (lookaheadOutside) {
if (lookaheadToEndDistance > lookaheadToStartDistance) {
return new Pose2d(
lastTargetWaypoint.getTranslation(),
getPointToPointInterpolatedRotation(
lastTargetWaypoint,
currentTargetWaypoint,
getPerpendicularPoint(
lastTargetWaypoint, currentTargetWaypoint, currentRobotPose)));
}
if (getCurrentPointIndex() < points.size() - 1) {
var futurePoint = points.get(getCurrentPointIndex() + 1).poseSupplier.get();
var perpendicularToCurrentEndDistance =
perpendicularPoint.getTranslation().getDistance(currentTargetWaypoint.getTranslation());
var newLookaheadPoint =
getLookaheadPoint(
startingRobotPose,
currentTargetWaypoint,
getPerpendicularPoint(startingRobotPose, currentTargetWaypoint, currentRobotPose),
LOOKAHEAD_DISTANCE);
DogLog.log("Autos/Trailblazer/PurePursuitPathTracker/TargetPose", targetPose);
return targetPose;
futurePoint,
currentTargetWaypoint,
lookaheadDistance - perpendicularToCurrentEndDistance);

currentPointIndex++;

return newLookaheadPoint;
} else {
return new Pose2d(
currentTargetWaypoint.getTranslation(),
getPointToPointInterpolatedRotation(
lastTargetWaypoint,
currentTargetWaypoint,
getPerpendicularPoint(
lastTargetWaypoint, currentTargetWaypoint, currentRobotPose)));
}
}
var targetPose = lookaheadPoint;
DogLog.log("Autos/Trailblazer/PurePursuitPathTracker/TargetPose", targetPose);
return targetPose;
return lookaheadPoint;
}

@Override
Expand All @@ -140,12 +139,87 @@ public boolean isFinished() {
if (currentRobotPose
.getTranslation()
.getDistance(points.get(points.size() - 1).poseSupplier.get().getTranslation())
< FINISHED_THRESHOLD) {
< AT_END_OF_SEGMENT_DISTANCE_THRESHOLD) {
return true;
}
return false;
}

private void updateLookahead() {
if (USE_DYNAMIC_LOOKAHEAD && points.size() > 1) {

if (points.size() == 2) {
requestNewLookaheadDistance(
getDynamicLookaheadDistance(
startingRobotPose,
points.get(0).poseSupplier.get(),
points.get(1).poseSupplier.get()),
false);
} else if (getCurrentPointIndex() == points.size() - 1) {
requestNewLookaheadDistance(
getDynamicLookaheadDistance(
points.get(getCurrentPointIndex() - 2).poseSupplier.get(),
lastTargetWaypoint,
currentTargetWaypoint),
false);
} else {
var thirdPoint = points.get(getCurrentPointIndex() + 1).poseSupplier.get();
requestNewLookaheadDistance(
getDynamicLookaheadDistance(lastTargetWaypoint, currentTargetWaypoint, thirdPoint),
false);
}
} else {
requestNewLookaheadDistance(NON_DYNAMIC_LOOKAHEAD_DISTANCE, true);
}
}

private double getDynamicLookaheadDistance(
Pose2d firstPoint, Pose2d secondPoint, Pose2d thirdPoint) {
var x1 = firstPoint.getX();
var y1 = firstPoint.getY();
var x2 = secondPoint.getX();
var y2 = secondPoint.getY();
var x3 = thirdPoint.getX();
var y3 = thirdPoint.getY();

var AB = Math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
var BC = Math.sqrt((x3 - x2) * (x3 - x2) + (y3 - y2) * (y3 - y2));
var AC = Math.sqrt((x3 - x1) * (x3 - x1) + (y3 - y1) * (y3 - y1));
var s = (AB + BC + AC) / 2;
var area = Math.sqrt(s * (s - AB) * (s - BC) * (s - AC));

var curvature = (AB * BC * AC) / (4 * area);
double value = curvature * DYNAMIC_LOOKAHEAD_SCALE;
Pose2d[] curvaturepoints = {firstPoint, secondPoint, thirdPoint};
DogLog.log("Autos/Trailblazer/PurePursuitPathTracker/CurvaturePoints", curvaturepoints);
return Math.min(value, DYNAMIC_LOOKAHEAD_MAX);
}

public void requestNewLookaheadDistance(double targetLookahead, boolean immediateChnage) {
double currentTime = Timer.getFPGATimestamp();
if (lastRequestedLookaheadDistance != targetLookahead) {
transitionStartTime = currentTime;
} else {
transitionStartTime = lastStartTime;
}
if ((currentTime < transitionStartTime + DYNAMIC_LOOKAHEAD_TRANSITION_TIME)
&& !immediateChnage) {
// Calculate the progress of the transition
double progress = (currentTime - transitionStartTime) / DYNAMIC_LOOKAHEAD_TRANSITION_TIME;

// Linear interpolation (you can use other easing functions here)
double smoothLookahead = lookaheadDistance + (targetLookahead - lookaheadDistance) * progress;
lastRequestedLookaheadDistance = targetLookahead;
lastStartTime = transitionStartTime;
lookaheadDistance = smoothLookahead;
} else {
// Transition completed, return target lookahead
lastRequestedLookaheadDistance = targetLookahead;
lastStartTime = transitionStartTime;
lookaheadDistance = targetLookahead;
}
}

private Pose2d getPerpendicularPoint(Pose2d startPoint, Pose2d endPoint, Pose2d robotPose) {
var x1 = startPoint.getX();
var y1 = startPoint.getY();
Expand Down Expand Up @@ -196,32 +270,36 @@ private Pose2d getLookaheadPoint(
y
+ lookaheadDistance
* ((y2 - y1) / (Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2))));
return new Pose2d(xLookahead, yLookahead, getPointToPointInterpolatedRotation(startPoint, endPoint, pointOnPath));
return new Pose2d(
xLookahead,
yLookahead,
getPointToPointInterpolatedRotation(startPoint, endPoint, pointOnPath));
}


private Rotation2d getPointToPointInterpolatedRotation(Pose2d startPoint, Pose2d endPoint, Pose2d pointOnPath) {
//TODO: do unit tests for interpolated rotation
var totalDistance = startPoint.getTranslation().getDistance(endPoint.getTranslation());
var pointToStart = pointOnPath.getTranslation().getDistance(startPoint.getTranslation());
var pointToEnd = pointOnPath.getTranslation().getDistance(endPoint.getTranslation());

if (!((pointOnPath.getX() - startPoint.getX()) * (pointOnPath.getX() - endPoint.getX()) <= 0
&& (pointOnPath.getY() - startPoint.getY()) * (pointOnPath.getY() - endPoint.getY()) <= 0)) {
if (pointToEnd > pointToStart) {
return startPoint.getRotation();
} else {
return endPoint.getRotation();
private Rotation2d getPointToPointInterpolatedRotation(
Pose2d startPoint, Pose2d endPoint, Pose2d pointOnPath) {
// TODO: do unit tests for interpolated rotation
var totalDistance = startPoint.getTranslation().getDistance(endPoint.getTranslation());
var pointToStart = pointOnPath.getTranslation().getDistance(startPoint.getTranslation());
var pointToEnd = pointOnPath.getTranslation().getDistance(endPoint.getTranslation());

if (!((pointOnPath.getX() - startPoint.getX()) * (pointOnPath.getX() - endPoint.getX()) <= 0
&& (pointOnPath.getY() - startPoint.getY()) * (pointOnPath.getY() - endPoint.getY())
<= 0)) {
if (pointToEnd > pointToStart) {
return startPoint.getRotation();
} else {
return endPoint.getRotation();
}
}
var progressPercent = Math.abs((pointToStart / totalDistance));
if (progressPercent > 0.9) {
progressPercent = 1.0;
}
}
var progressPercent = Math.abs((pointToStart / totalDistance));
if (progressPercent > 0.8) {
progressPercent = 1.0;
}

var interpolatedRotation = startPoint.getRotation().interpolate(endPoint.getRotation(), progressPercent);

return interpolatedRotation;
var interpolatedRotation =
startPoint.getRotation().interpolate(endPoint.getRotation(), progressPercent);

}
return interpolatedRotation;
}
}

0 comments on commit 2f57a15

Please sign in to comment.