diff --git a/pathplannerlib/build.gradle b/pathplannerlib/build.gradle index 513f7ca9..b68d551f 100644 --- a/pathplannerlib/build.gradle +++ b/pathplannerlib/build.gradle @@ -153,6 +153,7 @@ spotless { } javadoc { + exclude 'org/json/simple/**' options { links 'https://docs.oracle.com/en/java/javase/11/docs/api/', 'https://github.wpilib.org/allwpilib/docs/release/java/' } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java b/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java index 73d4f227..ea3ac839 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java @@ -26,6 +26,7 @@ import org.json.simple.JSONObject; import org.json.simple.parser.JSONParser; +/** Utility class used to build auto routines */ public class AutoBuilder { private static boolean configured = false; @@ -279,6 +280,12 @@ public static Command followPathWithEvents(PathPlannerPath path) { return new FollowPathWithEvents(pathFollowingCommandBuilder.apply(path), path, getPose); } + /** + * Get the starting pose from its JSON representation. This is only used internally. + * + * @param startingPoseJson JSON object representing a starting pose. + * @return The Pose2d starting pose + */ public static Pose2d getStartingPoseFromJson(JSONObject startingPoseJson) { JSONObject pos = (JSONObject) startingPoseJson.get("position"); double x = ((Number) pos.get("x")).doubleValue(); diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilderException.java b/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilderException.java index 1a226232..84611a5a 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilderException.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilderException.java @@ -1,6 +1,12 @@ package com.pathplanner.lib.auto; +/** An exception while building autos */ public class AutoBuilderException extends RuntimeException { + /** + * Create a new auto builder exception + * + * @param message Error message + */ public AutoBuilderException(String message) { super(message); } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/auto/CommandUtil.java b/pathplannerlib/src/main/java/com/pathplanner/lib/auto/CommandUtil.java index 1fb0f64f..8aef24f7 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/auto/CommandUtil.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/auto/CommandUtil.java @@ -5,6 +5,7 @@ import org.json.simple.JSONArray; import org.json.simple.JSONObject; +/** Utility class for building commands used in autos */ public class CommandUtil { /** * Wraps a command with a functional command that calls the command's initialize, execute, end, diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/auto/EventManager.java b/pathplannerlib/src/main/java/com/pathplanner/lib/auto/EventManager.java index c63365fd..50afb6ad 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/auto/EventManager.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/auto/EventManager.java @@ -6,6 +6,7 @@ import java.util.List; import java.util.Map; +/** Utility class for managing named commands */ public class EventManager { private static final HashMap eventMap = new HashMap<>(); diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java index 13065673..0fde03b3 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java @@ -14,6 +14,7 @@ import java.util.function.Consumer; import java.util.function.Supplier; +/** Base command for following a path */ public class FollowPathCommand extends Command { private final Timer timer = new Timer(); private final PathPlannerPath path; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathHolonomic.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathHolonomic.java index 9c263451..b5e06115 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathHolonomic.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathHolonomic.java @@ -11,6 +11,7 @@ import java.util.function.Consumer; import java.util.function.Supplier; +/** Follow a path using a PPHolonomicDriveController */ public class FollowPathHolonomic extends FollowPathCommand { /** * Construct a path following command that will use a holonomic drive controller for holonomic diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathLTV.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathLTV.java index 853140f5..80909b42 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathLTV.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathLTV.java @@ -12,6 +12,7 @@ import java.util.function.Consumer; import java.util.function.Supplier; +/** Follow a path using a PPLTVController */ public class FollowPathLTV extends FollowPathCommand { /** * Create a path following command that will use an LTV unicycle controller for differential drive diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathRamsete.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathRamsete.java index badae122..8ad853ab 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathRamsete.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathRamsete.java @@ -9,6 +9,7 @@ import java.util.function.Consumer; import java.util.function.Supplier; +/** Follow a path using a PPRamseteController */ public class FollowPathRamsete extends FollowPathCommand { /** * Construct a path following command that will use a Ramsete path following controller for diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathWithEvents.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathWithEvents.java index 9da144e2..ba480312 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathWithEvents.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathWithEvents.java @@ -8,6 +8,7 @@ import java.util.function.Supplier; import java.util.stream.Collectors; +/** Command that will run a path following command and trigger event markers along the way */ public class FollowPathWithEvents extends Command { private final Command pathFollowingCommand; private final PathPlannerPath path; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindHolonomic.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindHolonomic.java index 9cf4313e..889cf24d 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindHolonomic.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindHolonomic.java @@ -9,6 +9,7 @@ import java.util.function.Consumer; import java.util.function.Supplier; +/** Pathfind and follow the path with a PPHolonomicDriveController */ public class PathfindHolonomic extends PathfindingCommand { /** * Constructs a new PathfindHolonomic command that will generate a path towards the given path. diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindLTV.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindLTV.java index 319533b8..c2975dff 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindLTV.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindLTV.java @@ -14,6 +14,7 @@ import java.util.function.Consumer; import java.util.function.Supplier; +/** Pathfind and follow the path with a PPLTVController */ public class PathfindLTV extends PathfindingCommand { /** * Constructs a new PathfindLTV command that will generate a path towards the given path. diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindRamsete.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindRamsete.java index a844a391..f105f2e3 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindRamsete.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindRamsete.java @@ -11,6 +11,7 @@ import java.util.function.Consumer; import java.util.function.Supplier; +/** Pathfind and follow the path with a PPRamseteController */ public class PathfindRamsete extends PathfindingCommand { /** * Constructs a new PathfindRamsete command that will generate a path towards the given path. diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java index 4249e2d6..1860952b 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java @@ -17,6 +17,7 @@ import java.util.function.Consumer; import java.util.function.Supplier; +/** Base pathfinding command */ public class PathfindingCommand extends Command { private final Timer timer = new Timer(); private final PathPlannerPath targetPath; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java index 01140558..a1bc64ef 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; +/** Path following controller for holonomic drive trains */ public class PPHolonomicDriveController implements PathFollowingController { private final PIDController xController; private final PIDController yController; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPLTVController.java b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPLTVController.java index 5ad1740f..446083f7 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPLTVController.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPLTVController.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.numbers.N3; +/** LTV following controller */ public class PPLTVController extends LTVUnicycleController implements PathFollowingController { private double lastError = 0; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPRamseteController.java b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPRamseteController.java index fe3614ac..d9fd6458 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPRamseteController.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPRamseteController.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +/** Ramsete path following controller */ public class PPRamseteController extends RamseteController implements PathFollowingController { private double lastError = 0; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PathFollowingController.java b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PathFollowingController.java index 11674ef3..45d12d9a 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PathFollowingController.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PathFollowingController.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +/** Interface used by path following controllers */ public interface PathFollowingController { /** * Calculates the next output of the path following controller diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/ConstraintsZone.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/ConstraintsZone.java index 3ab3528c..45404406 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/ConstraintsZone.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/ConstraintsZone.java @@ -3,6 +3,7 @@ import java.util.Objects; import org.json.simple.JSONObject; +/** A zone on a path with different kinematic constraints */ public class ConstraintsZone { private final double minWaypointPos; private final double maxWaypointPos; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/EventMarker.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/EventMarker.java index 1db95b1b..12792c16 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/EventMarker.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/EventMarker.java @@ -7,6 +7,7 @@ import java.util.Objects; import org.json.simple.JSONObject; +/** Position along the path that will trigger a command when reached */ public class EventMarker { private final double waypointRelativePos; private final Command command; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/GoalEndState.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/GoalEndState.java index 55089f55..ba3ceae6 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/GoalEndState.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/GoalEndState.java @@ -4,6 +4,7 @@ import java.util.Objects; import org.json.simple.JSONObject; +/** Describes the goal end state of the robot when finishing a path */ public class GoalEndState { private final double velocity; private final Rotation2d rotation; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathConstraints.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathConstraints.java index 5114a944..91f4a7a5 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathConstraints.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathConstraints.java @@ -4,6 +4,7 @@ import java.util.Objects; import org.json.simple.JSONObject; +/** Kinematic path following constraints */ public class PathConstraints { private final double maxVelocityMps; private final double maxAccelerationMpsSq; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java index 97607d0b..18894a7d 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java @@ -19,6 +19,7 @@ import org.json.simple.JSONObject; import org.json.simple.parser.JSONParser; +/** A PathPlanner path. NOTE: This is not a trajectory and isn't directly followed. */ public class PathPlannerPath { private List bezierPoints; private List rotationTargets; @@ -71,6 +72,11 @@ private PathPlannerPath(PathConstraints globalConstraints, GoalEndState goalEndS this.allPoints = new ArrayList<>(); } + /** + * Hot reload the path. This is used internally. + * + * @param pathJson Updated JSONObject for the path + */ public void hotReload(JSONObject pathJson) { PathPlannerPath updatedPath = PathPlannerPath.fromJson(pathJson); @@ -192,22 +198,7 @@ public PathConstraints getConstraintsForPoint(int idx) { return globalConstraints; } - /** - * Create a path planner path from pre-generated path points. This is used internally, and you - * likely should not use this - */ - public static PathPlannerPath fromPathPoints( - List pathPoints, PathConstraints globalConstraints, GoalEndState goalEndState) { - PathPlannerPath path = new PathPlannerPath(globalConstraints, goalEndState); - path.allPoints.addAll(pathPoints); - - path.precalcValues(); - - return path; - } - - /** Generate path points for a path. This is used internally and should not be used directly. */ - public static List createPath( + private static List createPath( List bezierPoints, List holonomicRotations, List constraintZones) { diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java index 05870bca..eff30967 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerTrajectory.java @@ -9,6 +9,7 @@ import java.util.ArrayList; import java.util.List; +/** Trajectory created from a pathplanner path */ public class PathPlannerTrajectory { private final List states; @@ -222,25 +223,41 @@ public State getEndState() { return getState(getStates().size() - 1); } + /** A state along the trajectory */ public static class State { + /** The time at this state in seconds */ public double timeSeconds = 0; + /** The velocity at this state in m/s */ public double velocityMps = 0; + /** The acceleration at this state in m/s^2 */ public double accelerationMpsSq = 0; + /** The time at this state in seconds */ public double headingAngularVelocityRps = 0; + /** The position at this state in meters */ public Translation2d positionMeters = new Translation2d(); + /** The heading (direction of travel) at this state */ public Rotation2d heading = new Rotation2d(); + /** The target holonomic rotation at this state */ public Rotation2d targetHolonomicRotation = new Rotation2d(); + /** The curvature at this state in rad/m */ public double curvatureRadPerMeter = 0; - + /** The constraints to apply at this state */ public PathConstraints constraints; // Values only used during generation private double deltaPos = 0; + /** + * Interpolate between this state and the given state + * + * @param endVal State to interpolate with + * @param t Interpolation factor (0.0-1.0) + * @return Interpolated state + */ public State interpolate(State endVal, double t) { State lerpedState = new State(); diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPoint.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPoint.java index d1ce3820..eae2e641 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPoint.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPoint.java @@ -4,15 +4,29 @@ import edu.wpi.first.math.geometry.Translation2d; import java.util.Objects; +/** A point along a pathplanner path */ public class PathPoint { + /** The position of this point */ public final Translation2d position; + /** The distance of this point along the path, in meters */ public double distanceAlongPath = 0.0; + /** The curve radius at this point */ public double curveRadius = 0.0; + /** The max velocity at this point */ public double maxV = Double.POSITIVE_INFINITY; + /** The target rotation at this point */ public Rotation2d holonomicRotation = null; + /** The constraints applied to this point */ public PathConstraints constraints = null; + /** + * Create a path point + * + * @param position Position of the point + * @param holonomicRotation Rotation target at this point + * @param constraints The constraints at this point + */ public PathPoint( Translation2d position, Rotation2d holonomicRotation, PathConstraints constraints) { this.position = position; @@ -20,11 +34,22 @@ public PathPoint( this.constraints = constraints; } + /** + * Create a path point + * + * @param position Position of the point + * @param holonomicRotation Rotation target at this point + */ public PathPoint(Translation2d position, Rotation2d holonomicRotation) { this.position = position; this.holonomicRotation = holonomicRotation; } + /** + * Create a path point + * + * @param position Position of the point + */ public PathPoint(Translation2d position) { this.position = position; } diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathSegment.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathSegment.java index 52599a32..29bb977a 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathSegment.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathSegment.java @@ -7,7 +7,9 @@ import java.util.List; import java.util.Optional; +/** A bezier curve segment */ public class PathSegment { + /** The resolution used during path generation */ public static final double RESOLUTION = 0.05; private final List segmentPoints; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/RotationTarget.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/RotationTarget.java index 845ab1fd..2e5ee4fd 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/path/RotationTarget.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/path/RotationTarget.java @@ -4,6 +4,7 @@ import java.util.Objects; import org.json.simple.JSONObject; +/** A target holonomic rotation at a position along a path */ public class RotationTarget { private final double waypointRelativePosition; private final Rotation2d target; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/ADStar.java b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/ADStar.java index 495356d1..f4bc501b 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/ADStar.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/pathfinding/ADStar.java @@ -12,17 +12,18 @@ import org.json.simple.JSONObject; import org.json.simple.parser.JSONParser; +/** Implementation of the AD* pathfinding algorithm */ public class ADStar { private static final double SMOOTHING_ANCHOR_PCT = 0.8; private static final double SMOOTHING_CONTROL_PCT = 0.33; - public static double FIELD_LENGTH = 16.54; - public static double FIELD_WIDTH = 8.02; + private static double FIELD_LENGTH = 16.54; + private static double FIELD_WIDTH = 8.02; - public static double NODE_SIZE = 0.2; + private static double NODE_SIZE = 0.2; - public static int NODE_X = (int) Math.ceil(FIELD_LENGTH / NODE_SIZE); - public static int NODE_Y = (int) Math.ceil(FIELD_WIDTH / NODE_SIZE); + private static int NODE_X = (int) Math.ceil(FIELD_LENGTH / NODE_SIZE); + private static int NODE_Y = (int) Math.ceil(FIELD_WIDTH / NODE_SIZE); private static final double EPS = 2.5; @@ -53,6 +54,11 @@ public class ADStar { private static volatile List currentPath = new ArrayList<>(); + /** + * Ensure that the pathfinding thread is initialized and running. + * + *

This will do nothing if already initialized + */ public static void ensureInitialized() { if (!running) { running = true; @@ -171,10 +177,20 @@ private static void doWork() { } } + /** + * Get if a new path has been calculated since the last time a path was retrieved + * + * @return True if a new path is available + */ public static boolean isNewPathAvailable() { return newPathAvailable; } + /** + * Get the most recently calculated path + * + * @return The bezier points representing a path + */ public static List getCurrentPath() { if (!running) { DriverStation.reportWarning("ADStar path was retrieved before it was initialized", false); @@ -184,6 +200,12 @@ public static List getCurrentPath() { return currentPath; } + /** + * Set the start position to pathfind from + * + * @param start Start position on the field. If this is within an obstacle it will be moved to the + * nearest non-obstacle node. + */ public static void setStartPos(Translation2d start) { synchronized (lock) { GridPosition startPos = findClosestNonObstacle(getGridPos(start)); @@ -197,6 +219,12 @@ public static void setStartPos(Translation2d start) { } } + /** + * Set the goal position to pathfind to + * + * @param goal Goal position on the field. f this is within an obstacle it will be moved to the + * nearest non-obstacle node. + */ public static void setGoalPos(Translation2d goal) { synchronized (lock) { GridPosition gridPos = findClosestNonObstacle(getGridPos(goal)); @@ -237,6 +265,16 @@ private static GridPosition findClosestNonObstacle(GridPosition pos) { return null; } + /** + * Set the dynamic obstacles that should be avoided while pathfinding. This is an advanced usage + * feature. This can do some weird things so you should become very familiar with how this will + * work before being anywhere near comfortable using it in competition. + * + * @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> obs, Translation2d currentRobotPos) { Set newObs = new HashSet<>(); @@ -581,6 +619,12 @@ private static int comparePair(Pair a, Pair b) { } } + /** + * Converts a Translation2d into a grid position + * + * @param pos Position on the field + * @return The gird position containing the translation + */ public static GridPosition getGridPos(Translation2d pos) { int x = (int) Math.floor(pos.getX() / NODE_SIZE); int y = (int) Math.floor(pos.getY() / NODE_SIZE); @@ -593,10 +637,19 @@ private static Translation2d gridPosToTranslation2d(GridPosition pos) { (pos.x * NODE_SIZE) + (NODE_SIZE / 2.0), (pos.y * NODE_SIZE) + (NODE_SIZE / 2.0)); } + /** Represents a node in the pathfinding grid */ public static class GridPosition implements Comparable { + /** X index in the grid */ public final int x; + /** Y index in the grid */ public final int y; + /** + * Create a node within the pathfinding grid + * + * @param x X index in the grid + * @param y Y index in the grid + */ public GridPosition(int x, int y) { this.x = x; this.y = y; diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/GeometryUtil.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/GeometryUtil.java index 7f9e2ebf..1a174b94 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/GeometryUtil.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/GeometryUtil.java @@ -3,19 +3,53 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +/** Utility class for various geometry functions used during generation */ public class GeometryUtil { + /** + * Interpolate between two doubles + * + * @param startVal Start value + * @param endVal End value + * @param t Interpolation factor (0.0-1.0) + * @return Interpolated value + */ public static double doubleLerp(double startVal, double endVal, double t) { return startVal + (endVal - startVal) * t; } + /** + * Interpolate between two Rotation2ds + * + * @param startVal Start value + * @param endVal End value + * @param t Interpolation factor (0.0-1.0) + * @return Interpolated value + */ public static Rotation2d rotationLerp(Rotation2d startVal, Rotation2d endVal, double t) { return startVal.plus(endVal.minus(startVal).times(t)); } + /** + * Linear interpolation between Translation2ds + * + * @param a Position 1 + * @param b Position 2 + * @param t Interpolation factor (0.0-1.0) + * @return Interpolated value + */ public static Translation2d translationLerp(Translation2d a, Translation2d b, double t) { return a.plus((b.minus(a)).times(t)); } + /** + * Cubic interpolation between Translation2ds + * + * @param a Position 1 + * @param b Position 2 + * @param c Position 3 + * @param t Interpolation factor (0.0-1.0) + * @return Interpolated value + */ public static Translation2d quadraticLerp( Translation2d a, Translation2d b, Translation2d c, double t) { Translation2d p0 = translationLerp(a, b, t); @@ -23,6 +57,16 @@ public static Translation2d quadraticLerp( return translationLerp(p0, p1, t); } + /** + * Quadratic interpolation between Translation2ds + * + * @param a Position 1 + * @param b Position 2 + * @param c Position 3 + * @param d Position 4 + * @param t Interpolation factor (0.0-1.0) + * @return Interpolated value + */ public static Translation2d cubicLerp( Translation2d a, Translation2d b, Translation2d c, Translation2d d, double t) { Translation2d p0 = quadraticLerp(a, b, c, t); @@ -30,6 +74,14 @@ public static Translation2d cubicLerp( return translationLerp(p0, p1, t); } + /** + * Calculate the curve radius given 3 points on the curve + * + * @param a Point A + * @param b Point B + * @param c Point C + * @return Curve radius + */ public static double calculateRadius(Translation2d a, Translation2d b, Translation2d c) { Translation2d vba = a.minus(b); Translation2d vbc = c.minus(b); diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/HolonomicPathFollowerConfig.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/HolonomicPathFollowerConfig.java index 0d1307e9..49d52bea 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/HolonomicPathFollowerConfig.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/HolonomicPathFollowerConfig.java @@ -2,11 +2,17 @@ /** Configuration for the holonomic path following commands */ public class HolonomicPathFollowerConfig { + /** PIDConstants used for translation PID controllers */ public final PIDConstants translationConstants; + /** PIDConstants used for rotation PID controllers */ public final PIDConstants rotationConstants; + /** Max speed of a drive module in m/s */ public final double maxModuleSpeed; + /** Radius of the drive base in meters */ public final double driveBaseRadius; + /** Path replanning config */ public final ReplanningConfig replanningConfig; + /** Period of the robot control loop in seconds */ public final double period; /** @@ -17,9 +23,8 @@ public class HolonomicPathFollowerConfig { * @param rotationConstants {@link com.pathplanner.lib.util.PIDConstants} used for creating the * rotation PID controller * @param maxModuleSpeed Max speed of an individual drive module in meters/sec - * @param driveBaseRadius The radius of the drive base in meters. For swerve drive, this is the - * distance from the center of the robot to the furthest module. For mecanum, this is the - * drive base width / 2 + * @param driveBaseRadius The radius of the drive base in meters. This is the distance from the + * center of the robot to the furthest module. * @param replanningConfig Path replanning configuration * @param period Control loop period in seconds (Default = 0.02) */ diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/PIDConstants.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/PIDConstants.java index 97fc79bc..60d4765f 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/PIDConstants.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/PIDConstants.java @@ -2,9 +2,13 @@ /** PID constants used to create PID controllers */ public class PIDConstants { + /** P */ public final double kP; + /** I */ public final double kI; + /** D */ public final double kD; + /** Integral range */ public final double iZone; /** diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/PPLibTelemetry.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/PPLibTelemetry.java index 2e96fdc0..af6d3d93 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/PPLibTelemetry.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/PPLibTelemetry.java @@ -16,6 +16,7 @@ import org.json.simple.JSONObject; import org.json.simple.parser.JSONParser; +/** Utility class for sending data to the PathPlanner app via NT4 */ public class PPLibTelemetry { private static boolean compMode = false; @@ -35,10 +36,19 @@ public class PPLibTelemetry { private static NetworkTableListener hotReloadPathListener = null; private static NetworkTableListener hotReloadAutoListener = null; + /** Enable competition mode. This will disable all telemetry and hot reload. */ public static void enableCompetitionMode() { compMode = true; } + /** + * Set the path following actual/target velocities + * + * @param actualVel Actual robot velocity in m/s + * @param commandedVel Target robot velocity in m/s + * @param actualAngVel Actual angular velocity in rad/s + * @param commandedAngVel Target angular velocity in rad/s + */ public static void setVelocities( double actualVel, double commandedVel, double actualAngVel, double commandedAngVel) { if (!compMode) { @@ -46,18 +56,33 @@ public static void setVelocities( } } + /** + * Set the path following inaccuracy + * + * @param inaccuracy Inaccuracy in meters + */ public static void setPathInaccuracy(double inaccuracy) { if (!compMode) { inaccuracyPub.set(inaccuracy); } } + /** + * Set the current robot pose + * + * @param pose Current robot pose + */ public static void setCurrentPose(Pose2d pose) { if (!compMode) { posePub.set(new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); } } + /** + * Set the current path being followed + * + * @param path The current path + */ public static void setCurrentPath(PathPlannerPath path) { if (!compMode) { double[] arr = new double[path.numPoints() * 2]; @@ -74,6 +99,11 @@ public static void setCurrentPath(PathPlannerPath path) { } } + /** + * Set the target robot pose + * + * @param targetPose Target robot pose + */ public static void setTargetPose(Pose2d targetPose) { if (!compMode) { targetPosePub.set( @@ -83,6 +113,12 @@ public static void setTargetPose(Pose2d targetPose) { } } + /** + * Register a path for hot reload. This is used internally. + * + * @param pathName Name of the path + * @param path Reference to the path + */ public static void registerHotReloadPath(String pathName, PathPlannerPath path) { if (!compMode) { ensureHotReloadListenersInitialized(); @@ -94,6 +130,12 @@ public static void registerHotReloadPath(String pathName, PathPlannerPath path) } } + /** + * Register an auto for hot reload. This is used internally. + * + * @param autoName Name of the auto + * @param auto Reference to the auto + */ public static void registerHotReloadAuto(String autoName, PathPlannerAuto auto) { if (!compMode) { ensureHotReloadListenersInitialized(); diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/PathPlannerLogging.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/PathPlannerLogging.java index 8c8e11c2..2005e3dd 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/PathPlannerLogging.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/PathPlannerLogging.java @@ -7,35 +7,69 @@ import java.util.function.Consumer; import java.util.stream.Collectors; +/** Utility class for handling custom logging callbacks */ public class PathPlannerLogging { private static Consumer logCurrentPose = null; private static Consumer logTargetPose = null; private static Consumer> logActivePath = null; + /** + * Set the logging callback for the current robot pose + * + * @param logCurrentPose Consumer that accepts the current robot pose. Can be null to disable + * logging this value. + */ public static void setLogCurrentPoseCallback(Consumer logCurrentPose) { PathPlannerLogging.logCurrentPose = logCurrentPose; } + /** + * Set the logging callback for the target robot pose + * + * @param logTargetPose Consumer that accepts the target robot pose. Can be null to disable + * logging this value. + */ public static void setLogTargetPoseCallback(Consumer logTargetPose) { PathPlannerLogging.logTargetPose = logTargetPose; } + /** + * Set the logging callback for the active path + * + * @param logActivePath Consumer that accepts the active path as a list of poses. Can be null to + * disable logging this value. + */ public static void setLogActivePathCallback(Consumer> logActivePath) { PathPlannerLogging.logActivePath = logActivePath; } + /** + * Log the current robot pose. This is used internally. + * + * @param pose The current robot pose + */ public static void logCurrentPose(Pose2d pose) { if (logCurrentPose != null) { logCurrentPose.accept(pose); } } + /** + * Log the target robot pose. This is used internally. + * + * @param targetPose The target robot pose + */ public static void logTargetPose(Pose2d targetPose) { if (logTargetPose != null) { logTargetPose.accept(targetPose); } } + /** + * Log the active path. This is used internally. + * + * @param path The active path + */ public static void logActivePath(PathPlannerPath path) { if (logActivePath != null) { List poses = diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/ReplanningConfig.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/ReplanningConfig.java index 56bdc8ab..bc15209d 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/ReplanningConfig.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/ReplanningConfig.java @@ -1,9 +1,20 @@ package com.pathplanner.lib.util; +/** Configuration for path replanning */ public class ReplanningConfig { + /** + * Should the path be replanned at the start of path following if the robot is not already at the + * starting point? + */ public final boolean enableInitialReplanning; + /** + * Should the path be replanned if the error grows too large or if a large error spike happens + * while following the path? + */ public final boolean enableDynamicReplanning; + /** The total error threshold, in meters, that will cause the path to be replanned */ public final double dynamicReplanningTotalErrorThreshold; + /** The error spike threshold, in meters, that will cause the path to be replanned */ public final double dynamicReplanningErrorSpikeThreshold; /** diff --git a/pathplannerlib/src/main/java/org/json/simple/parser/JSONParser.java b/pathplannerlib/src/main/java/org/json/simple/parser/JSONParser.java index 6c700497..39d7a93f 100755 --- a/pathplannerlib/src/main/java/org/json/simple/parser/JSONParser.java +++ b/pathplannerlib/src/main/java/org/json/simple/parser/JSONParser.java @@ -83,17 +83,17 @@ public Object parse(Reader in, ContainerFactory containerFactory) switch (token.type) { case Yytoken.TYPE_VALUE: status = S_IN_FINISHED_VALUE; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); valueStack.addFirst(token.value); break; case Yytoken.TYPE_LEFT_BRACE: status = S_IN_OBJECT; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); valueStack.addFirst(createObjectContainer(containerFactory)); break; case Yytoken.TYPE_LEFT_SQUARE: status = S_IN_ARRAY; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); valueStack.addFirst(createArrayContainer(containerFactory)); break; default: @@ -115,7 +115,7 @@ public Object parse(Reader in, ContainerFactory containerFactory) String key = (String) token.value; valueStack.addFirst(key); status = S_PASSED_PAIR_KEY; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); } else { status = S_IN_ERROR; } @@ -153,7 +153,7 @@ public Object parse(Reader in, ContainerFactory containerFactory) List newArray = createArrayContainer(containerFactory); parent.put(key, newArray); status = S_IN_ARRAY; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); valueStack.addFirst(newArray); break; case Yytoken.TYPE_LEFT_BRACE: @@ -163,7 +163,7 @@ public Object parse(Reader in, ContainerFactory containerFactory) Map newObject = createObjectContainer(containerFactory); parent.put(key, newObject); status = S_IN_OBJECT; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); valueStack.addFirst(newObject); break; default: @@ -193,7 +193,7 @@ public Object parse(Reader in, ContainerFactory containerFactory) Map newObject = createObjectContainer(containerFactory); val.add(newObject); status = S_IN_OBJECT; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); valueStack.addFirst(newObject); break; case Yytoken.TYPE_LEFT_SQUARE: @@ -201,7 +201,7 @@ public Object parse(Reader in, ContainerFactory containerFactory) List newArray = createArrayContainer(containerFactory); val.add(newArray); status = S_IN_ARRAY; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); valueStack.addFirst(newArray); break; default: @@ -288,17 +288,17 @@ public void parse(Reader in, ContentHandler contentHandler, boolean isResume) switch (token.type) { case Yytoken.TYPE_VALUE: status = S_IN_FINISHED_VALUE; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); if (!contentHandler.primitive(token.value)) return; break; case Yytoken.TYPE_LEFT_BRACE: status = S_IN_OBJECT; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); if (!contentHandler.startObject()) return; break; case Yytoken.TYPE_LEFT_SQUARE: status = S_IN_ARRAY; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); if (!contentHandler.startArray()) return; break; default: @@ -326,7 +326,7 @@ public void parse(Reader in, ContentHandler contentHandler, boolean isResume) if (token.value instanceof String) { String key = (String) token.value; status = S_PASSED_PAIR_KEY; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); if (!contentHandler.startObjectEntry(key)) return; } else { status = S_IN_ERROR; @@ -360,16 +360,16 @@ public void parse(Reader in, ContentHandler contentHandler, boolean isResume) break; case Yytoken.TYPE_LEFT_SQUARE: statusStack.removeFirst(); - statusStack.addFirst(new Integer(S_IN_PAIR_VALUE)); + statusStack.addFirst(S_IN_PAIR_VALUE); status = S_IN_ARRAY; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); if (!contentHandler.startArray()) return; break; case Yytoken.TYPE_LEFT_BRACE: statusStack.removeFirst(); - statusStack.addFirst(new Integer(S_IN_PAIR_VALUE)); + statusStack.addFirst(S_IN_PAIR_VALUE); status = S_IN_OBJECT; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); if (!contentHandler.startObject()) return; break; default: @@ -406,12 +406,12 @@ public void parse(Reader in, ContentHandler contentHandler, boolean isResume) break; case Yytoken.TYPE_LEFT_BRACE: status = S_IN_OBJECT; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); if (!contentHandler.startObject()) return; break; case Yytoken.TYPE_LEFT_SQUARE: status = S_IN_ARRAY; - statusStack.addFirst(new Integer(status)); + statusStack.addFirst(status); if (!contentHandler.startArray()) return; break; default: