NOTE: These feedforwards are assuming unoptimized module states. When you optimize your - * module states, you will need to reverse the feedforwards for modules that have been flipped - * @param controller Path following controller that will be used to follow the path - * @param robotConfig The robot configuration - * @param shouldFlipPath Should the target path be flipped to the other side of the field? This - * will maintain a global blue alliance origin. - * @param requirements the subsystems required by this command + * @param targetPath the path to pathfind to + * @param constraints the path constraints to use while pathfinding + * @param poseSupplier a supplier for the robot's current pose + * @param speedsSupplier a supplier for the robot's current robot relative + * speeds + * @param output Output function that accepts robot-relative + * ChassisSpeeds and feedforwards for + * each drive motor. If using swerve, these feedforwards + * will be in FL, FR, BL, BR order. If + * using a differential drive, they will be in L, R order. + *
+ * NOTE: These feedforwards are assuming unoptimized + * module states. When you optimize your + * module states, you will need to reverse the + * feedforwards for modules that have been flipped + * @param controller Path following controller that will be used to follow + * the path + * @param robotConfig The robot configuration + * @param shouldFlipPath Should the target path be flipped to the other side of + * the field? This + * will maintain a global blue alliance origin. + * @param requirements the subsystems required by this command */ public PathfindingCommand( PathPlannerPath targetPath, @@ -86,7 +96,8 @@ public PathfindingCommand( Rotation2d targetRotation = Rotation2d.kZero; double goalEndVel = targetPath.getGlobalConstraints().maxVelocityMPS(); if (targetPath.isChoreoPath()) { - // Can get() here without issue since all choreo trajectories have ideal trajectories + // Can get() here without issue since all choreo trajectories have ideal + // trajectories PathPlannerTrajectory choreoTraj = targetPath.getIdealTrajectory(robotConfig).orElseThrow(); targetRotation = choreoTraj.getInitialState().pose.getRotation(); goalEndVel = choreoTraj.getInitialState().linearVelocity; @@ -101,8 +112,7 @@ public PathfindingCommand( this.targetPath = targetPath; this.targetPose = new Pose2d(this.targetPath.getPoint(0).position, targetRotation); - this.originalTargetPose = - new Pose2d(this.targetPose.getTranslation(), this.targetPose.getRotation()); + this.originalTargetPose = new Pose2d(this.targetPose.getTranslation(), this.targetPose.getRotation()); this.goalEndState = new GoalEndState(goalEndVel, targetRotation); this.constraints = constraints; this.controller = controller; @@ -117,22 +127,31 @@ public PathfindingCommand( } /** - * Constructs a new base pathfinding command that will generate a path towards the given pose. + * Constructs a new base pathfinding command that will generate a path towards + * the given pose. * - * @param targetPose the pose to pathfind to, the rotation component is only relevant for - * holonomic drive trains - * @param constraints the path constraints to use while pathfinding - * @param goalEndVel The goal end velocity when reaching the target pose - * @param poseSupplier a supplier for the robot's current pose - * @param speedsSupplier a supplier for the robot's current robot relative speeds - * @param output Output function that accepts robot-relative ChassisSpeeds and feedforwards for - * each drive motor. If using swerve, these feedforwards will be in FL, FR, BL, BR order. If - * using a differential drive, they will be in L, R order. - *
NOTE: These feedforwards are assuming unoptimized module states. When you optimize your - * module states, you will need to reverse the feedforwards for modules that have been flipped - * @param controller Path following controller that will be used to follow the path - * @param robotConfig The robot configuration - * @param requirements the subsystems required by this command + * @param targetPose the pose to pathfind to, the rotation component is only + * relevant for + * holonomic drive trains + * @param constraints the path constraints to use while pathfinding + * @param goalEndVel The goal end velocity when reaching the target pose + * @param poseSupplier a supplier for the robot's current pose + * @param speedsSupplier a supplier for the robot's current robot relative + * speeds + * @param output Output function that accepts robot-relative + * ChassisSpeeds and feedforwards for + * each drive motor. If using swerve, these feedforwards + * will be in FL, FR, BL, BR order. If + * using a differential drive, they will be in L, R order. + *
+ * NOTE: These feedforwards are assuming unoptimized + * module states. When you optimize your + * module states, you will need to reverse the + * feedforwards for modules that have been flipped + * @param controller Path following controller that will be used to follow + * the path + * @param robotConfig The robot configuration + * @param requirements the subsystems required by this command */ public PathfindingCommand( Pose2d targetPose, @@ -150,8 +169,7 @@ public PathfindingCommand( this.targetPath = null; this.targetPose = targetPose; - this.originalTargetPose = - new Pose2d(this.targetPose.getTranslation(), this.targetPose.getRotation()); + this.originalTargetPose = new Pose2d(this.targetPose.getTranslation(), this.targetPose.getRotation()); this.goalEndState = new GoalEndState(goalEndVel, targetPose.getRotation()); this.constraints = constraints; this.controller = controller; @@ -166,22 +184,31 @@ public PathfindingCommand( } /** - * Constructs a new base pathfinding command that will generate a path towards the given pose. + * Constructs a new base pathfinding command that will generate a path towards + * the given pose. * - * @param targetPose the pose to pathfind to, the rotation component is only relevant for - * holonomic drive trains - * @param constraints the path constraints to use while pathfinding - * @param goalEndVel The goal end velocity when reaching the target pose - * @param poseSupplier a supplier for the robot's current pose - * @param speedsSupplier a supplier for the robot's current robot relative speeds - * @param output Output function that accepts robot-relative ChassisSpeeds and feedforwards for - * each drive motor. If using swerve, these feedforwards will be in FL, FR, BL, BR order. If - * using a differential drive, they will be in L, R order. - *
NOTE: These feedforwards are assuming unoptimized module states. When you optimize your - * module states, you will need to reverse the feedforwards for modules that have been flipped - * @param controller Path following controller that will be used to follow the path - * @param robotConfig The robot configuration - * @param requirements the subsystems required by this command + * @param targetPose the pose to pathfind to, the rotation component is only + * relevant for + * holonomic drive trains + * @param constraints the path constraints to use while pathfinding + * @param goalEndVel The goal end velocity when reaching the target pose + * @param poseSupplier a supplier for the robot's current pose + * @param speedsSupplier a supplier for the robot's current robot relative + * speeds + * @param output Output function that accepts robot-relative + * ChassisSpeeds and feedforwards for + * each drive motor. If using swerve, these feedforwards + * will be in FL, FR, BL, BR order. If + * using a differential drive, they will be in L, R order. + *
+ * NOTE: These feedforwards are assuming unoptimized + * module states. When you optimize your + * module states, you will need to reverse the + * feedforwards for modules that have been flipped + * @param controller Path following controller that will be used to follow + * the path + * @param robotConfig The robot configuration + * @param requirements the subsystems required by this command */ public PathfindingCommand( Pose2d targetPose, @@ -206,21 +233,30 @@ public PathfindingCommand( } /** - * Constructs a new base pathfinding command that will generate a path towards the given pose. + * Constructs a new base pathfinding command that will generate a path towards + * the given pose. * - * @param targetPose the pose to pathfind to, the rotation component is only relevant for - * holonomic drive trains - * @param constraints the path constraints to use while pathfinding - * @param poseSupplier a supplier for the robot's current pose - * @param speedsSupplier a supplier for the robot's current robot relative speeds - * @param output Output function that accepts robot-relative ChassisSpeeds and feedforwards for - * each drive motor. If using swerve, these feedforwards will be in FL, FR, BL, BR order. If - * using a differential drive, they will be in L, R order. - *
NOTE: These feedforwards are assuming unoptimized module states. When you optimize your - * module states, you will need to reverse the feedforwards for modules that have been flipped - * @param controller Path following controller that will be used to follow the path - * @param robotConfig The robot configuration - * @param requirements the subsystems required by this command + * @param targetPose the pose to pathfind to, the rotation component is only + * relevant for + * holonomic drive trains + * @param constraints the path constraints to use while pathfinding + * @param poseSupplier a supplier for the robot's current pose + * @param speedsSupplier a supplier for the robot's current robot relative + * speeds + * @param output Output function that accepts robot-relative + * ChassisSpeeds and feedforwards for + * each drive motor. If using swerve, these feedforwards + * will be in FL, FR, BL, BR order. If + * using a differential drive, they will be in L, R order. + *
+ * NOTE: These feedforwards are assuming unoptimized
+ * module states. When you optimize your
+ * module states, you will need to reverse the
+ * feedforwards for modules that have been flipped
+ * @param controller Path following controller that will be used to follow
+ * the path
+ * @param robotConfig The robot configuration
+ * @param requirements the subsystems required by this command
*/
public PathfindingCommand(
Pose2d targetPose,
@@ -254,8 +290,7 @@ public void initialize() {
controller.reset(currentPose, speedsSupplier.get());
if (targetPath != null) {
- originalTargetPose =
- new Pose2d(this.targetPath.getPoint(0).position, originalTargetPose.getRotation());
+ originalTargetPose = new Pose2d(this.targetPath.getPoint(0).position, originalTargetPose.getRotation());
if (shouldFlipPath.getAsBoolean()) {
targetPose = FlippingUtil.flipFieldPose(this.originalTargetPose);
goalEndState = new GoalEndState(goalEndState.velocityMPS(), targetPose.getRotation());
@@ -284,20 +319,19 @@ public void execute() {
PPLibTelemetry.setCurrentPose(currentPose);
// Skip new paths if we are close to the end
- boolean skipUpdates =
- currentTrajectory != null
- && currentPose
- .getTranslation()
- .getDistance(currentTrajectory.getEndState().pose.getTranslation())
- < 2.0;
+ boolean skipUpdates = currentTrajectory != null
+ && currentPose
+ .getTranslation()
+ .getDistance(
+ currentTrajectory.getEndState().pose.getTranslation()) < Pathfinding.getMinDistanceToPathfind()
+ .in(Meters);
if (!skipUpdates && Pathfinding.isNewPathAvailable()) {
currentPath = Pathfinding.getCurrentPath(constraints, goalEndState);
if (currentPath != null) {
- currentTrajectory =
- new PathPlannerTrajectory(
- currentPath, currentSpeeds, currentPose.getRotation(), robotConfig);
+ currentTrajectory = new PathPlannerTrajectory(
+ currentPath, currentSpeeds, currentPose.getRotation(), robotConfig);
if (!Double.isFinite(currentTrajectory.getTotalTimeSeconds())) {
finish = true;
return;
@@ -307,18 +341,14 @@ public void execute() {
int closestState1Idx = 0;
int closestState2Idx = 1;
while (closestState2Idx < currentTrajectory.getStates().size() - 1) {
- double closest2Dist =
- currentTrajectory
- .getState(closestState2Idx)
- .pose
- .getTranslation()
- .getDistance(currentPose.getTranslation());
- double nextDist =
- currentTrajectory
- .getState(closestState2Idx + 1)
- .pose
- .getTranslation()
- .getDistance(currentPose.getTranslation());
+ double closest2Dist = currentTrajectory
+ .getState(closestState2Idx).pose
+ .getTranslation()
+ .getDistance(currentPose.getTranslation());
+ double nextDist = currentTrajectory
+ .getState(closestState2Idx + 1).pose
+ .getTranslation()
+ .getDistance(currentPose.getTranslation());
if (nextDist < closest2Dist) {
closestState1Idx++;
closestState2Idx++;
@@ -332,17 +362,17 @@ public void execute() {
var closestState1 = currentTrajectory.getState(closestState1Idx);
var closestState2 = currentTrajectory.getState(closestState2Idx);
- double d =
- closestState1.pose.getTranslation().getDistance(closestState2.pose.getTranslation());
- double t =
- (currentPose.getTranslation().getDistance(closestState1.pose.getTranslation())) / d;
+ double d = closestState1.pose.getTranslation().getDistance(closestState2.pose.getTranslation());
+ double t = (currentPose.getTranslation().getDistance(closestState1.pose.getTranslation())) / d;
t = MathUtil.clamp(t, 0.0, 1.0);
timeOffset = MathUtil.interpolate(closestState1.timeSeconds, closestState2.timeSeconds, t);
- // If the robot is stationary and at the start of the path, set the time offset to the next
+ // If the robot is stationary and at the start of the path, set the time offset
+ // to the next
// loop
- // This can prevent an issue where the robot will remain stationary if new paths come in
+ // This can prevent an issue where the robot will remain stationary if new paths
+ // come in
// every loop
if (timeOffset <= 0.02
&& Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond) < 0.1) {
@@ -360,11 +390,9 @@ public void execute() {
if (currentTrajectory != null) {
var targetState = currentTrajectory.sample(timer.get() + timeOffset);
- ChassisSpeeds targetSpeeds =
- controller.calculateRobotRelativeSpeeds(currentPose, targetState);
+ ChassisSpeeds targetSpeeds = controller.calculateRobotRelativeSpeeds(currentPose, targetState);
- double currentVel =
- Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond);
+ double currentVel = Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond);
PPLibTelemetry.setCurrentPose(currentPose);
PathPlannerLogging.logCurrentPose(currentPose);
@@ -392,12 +420,10 @@ public boolean isFinished() {
Pose2d currentPose = poseSupplier.get();
ChassisSpeeds currentSpeeds = speedsSupplier.get();
- double currentVel =
- Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond);
+ double currentVel = Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond);
double stoppingDistance = Math.pow(currentVel, 2) / (2 * constraints.maxAccelerationMPSSq());
- return currentPose.getTranslation().getDistance(targetPose.getTranslation())
- <= stoppingDistance;
+ return currentPose.getTranslation().getDistance(targetPose.getTranslation()) <= stoppingDistance;
}
if (currentTrajectory != null) {
@@ -411,7 +437,8 @@ public boolean isFinished() {
public void end(boolean interrupted) {
timer.stop();
- // Only output 0 speeds when ending a path that is supposed to stop, this allows interrupting
+ // Only output 0 speeds when ending a path that is supposed to stop, this allows
+ // interrupting
// the command to smoothly transition into some auto-alignment routine
if (!interrupted && goalEndState.velocityMPS() < 0.1) {
output.accept(new ChassisSpeeds(), DriveFeedforwards.zeros(robotConfig.numModules));
@@ -427,19 +454,20 @@ public void end(boolean interrupted) {
*/
public static Command warmupCommand() {
return new PathfindingCommand(
- new Pose2d(15.0, 4.0, Rotation2d.k180deg),
- new PathConstraints(4, 3, 4, 4),
- () -> new Pose2d(1.5, 4, Rotation2d.kZero),
- ChassisSpeeds::new,
- (speeds, feedforwards) -> {},
- new PPHolonomicDriveController(
- new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)),
- new RobotConfig(
- 75,
- 6.8,
- new ModuleConfig(
- 0.048, 5.0, 1.2, DCMotor.getKrakenX60(1).withReduction(6.14), 60.0, 1),
- 0.55))
+ new Pose2d(15.0, 4.0, Rotation2d.k180deg),
+ new PathConstraints(4, 3, 4, 4),
+ () -> new Pose2d(1.5, 4, Rotation2d.kZero),
+ ChassisSpeeds::new,
+ (speeds, feedforwards) -> {
+ },
+ new PPHolonomicDriveController(
+ new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)),
+ new RobotConfig(
+ 75,
+ 6.8,
+ new ModuleConfig(
+ 0.048, 5.0, 1.2, DCMotor.getKrakenX60(1).withReduction(6.14), 60.0, 1),
+ 0.55))
.andThen(Commands.print("[PathPlanner] PathfindingCommand finished warmup"))
.ignoringDisable(true);
}
diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java
index f5a2a74f2..57e71d7bb 100644
--- a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java
+++ b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/Pathfinding.java
@@ -5,14 +5,20 @@
import com.pathplanner.lib.path.PathPlannerPath;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.units.measure.Distance;
+
+import static edu.wpi.first.units.Units.Meters;
+
import java.util.List;
/**
- * Static class for interacting with the chosen pathfinding implementation from the pathfinding
+ * Static class for interacting with the chosen pathfinding implementation from
+ * the pathfinding
* commands
*/
public class Pathfinding {
private static Pathfinder pathfinder = null;
+ private static Distance minDistanceToPathfind = Meters.of(2.0);
/**
* Set the pathfinder that should be used by the path following commands
@@ -23,7 +29,10 @@ public static void setPathfinder(Pathfinder pathfinder) {
Pathfinding.pathfinder = pathfinder;
}
- /** Ensure that a pathfinding implementation has been chosen. If not, set it to the default. */
+ /**
+ * Ensure that a pathfinding implementation has been chosen. If not, set it to
+ * the default.
+ */
public static void ensureInitialized() {
if (pathfinder == null) {
// Hasn't been initialized yet, use the default implementation
@@ -33,7 +42,8 @@ public static void ensureInitialized() {
}
/**
- * Get if a new path has been calculated since the last time a path was retrieved
+ * Get if a new path has been calculated since the last time a path was
+ * retrieved
*
* @return True if a new path is available
*/
@@ -41,12 +51,39 @@ public static boolean isNewPathAvailable() {
return pathfinder.isNewPathAvailable();
}
+ /**
+ * Sets the minimum distance required between the robot's current position and
+ * the path
+ * before a new pathfinding update is triggered. If the given value is negative,
+ * it will be clamped to 0 and a warning will be issued.
+ *
+ * @param minDistance The minimum distance to trigger a pathfinding update.
+ */
+ public static void setMinDistanceToPathfind(Distance minDistance) {
+ if (minDistance.in(Meters) < 0) {
+ System.err.println(
+ "[Pathplanner] Warning: minDistanceToPathfind was set to a negative value, which will have no affect.");
+ }
+
+ minDistanceToPathfind = minDistance;
+ }
+
+ /**
+ * Gets the current minimum distance required to trigger a pathfinding update.
+ *
+ * @return The minimum distance as a {@link Distance} object.
+ */
+ public static Distance getMinDistanceToPathfind() {
+ return minDistanceToPathfind;
+ }
+
/**
* Get the most recently calculated path
*
- * @param constraints The path constraints to use when creating the path
+ * @param constraints The path constraints to use when creating the path
* @param goalEndState The goal end state to use when creating the path
- * @return The PathPlannerPath created from the points calculated by the pathfinder
+ * @return The PathPlannerPath created from the points calculated by the
+ * pathfinder
*/
public static PathPlannerPath getCurrentPath(
PathConstraints constraints, GoalEndState goalEndState) {
@@ -56,8 +93,9 @@ public static PathPlannerPath getCurrentPath(
/**
* Set the start position to pathfind from
*
- * @param startPosition Start position on the field. If this is within an obstacle it will be
- * moved to the nearest non-obstacle node.
+ * @param startPosition Start position on the field. If this is within an
+ * obstacle it will be
+ * moved to the nearest non-obstacle node.
*/
public static void setStartPosition(Translation2d startPosition) {
pathfinder.setStartPosition(startPosition);
@@ -66,8 +104,9 @@ public static void setStartPosition(Translation2d startPosition) {
/**
* Set the goal position to pathfind to
*
- * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved
- * to the nearest non-obstacle node.
+ * @param goalPosition Goal position on the field. f this is within an obstacle
+ * it will be moved
+ * to the nearest non-obstacle node.
*/
public static void setGoalPosition(Translation2d goalPosition) {
pathfinder.setGoalPosition(goalPosition);
@@ -76,10 +115,13 @@ public static void setGoalPosition(Translation2d goalPosition) {
/**
* Set the dynamic obstacles that should be avoided while pathfinding.
*
- * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents
- * opposite corners of a bounding box.
- * @param currentRobotPos The current position of the robot. This is needed to change the start
- * position of the path if the robot is now within an obstacle.
+ * @param obs A List of Translation2d pairs representing obstacles.
+ * Each Translation2d represents
+ * opposite corners of a bounding box.
+ * @param currentRobotPos The current position of the robot. This is needed to
+ * change the start
+ * position of the path if the robot is now within an
+ * obstacle.
*/
public static void setDynamicObstacles(
List