diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathFollowingCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java similarity index 86% rename from pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathFollowingCommand.java rename to pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java index b2174176..13065673 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathFollowingCommand.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathCommand.java @@ -14,7 +14,7 @@ import java.util.function.Consumer; import java.util.function.Supplier; -public class PathFollowingCommand extends Command { +public class FollowPathCommand extends Command { private final Timer timer = new Timer(); private final PathPlannerPath path; private final Supplier poseSupplier; @@ -25,7 +25,19 @@ public class PathFollowingCommand extends Command { private PathPlannerTrajectory generatedTrajectory; - public PathFollowingCommand( + /** + * Construct a base path following command + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param outputRobotRelative Function that will apply the robot-relative output speeds of this + * command + * @param controller Path following controller that will be used to follow the path + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ + public FollowPathCommand( PathPlannerPath path, Supplier poseSupplier, Supplier speedsSupplier, 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 b2d560cc..9c263451 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathHolonomic.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathHolonomic.java @@ -11,7 +11,26 @@ import java.util.function.Consumer; import java.util.function.Supplier; -public class FollowPathHolonomic extends PathFollowingCommand { +public class FollowPathHolonomic extends FollowPathCommand { + /** + * Construct a path following command that will use a holonomic drive controller for holonomic + * drive trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param outputRobotRelative Function that will apply the robot-relative output speeds of this + * command + * @param translationConstants PID constants for the translation PID controllers + * @param rotationConstants PID constants for the rotation controller + * @param maxModuleSpeed The max speed of a 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 period Period of the control loop in seconds, default is 0.02s + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ public FollowPathHolonomic( PathPlannerPath path, Supplier poseSupplier, @@ -35,6 +54,24 @@ public FollowPathHolonomic( requirements); } + /** + * Construct a path following command that will use a holonomic drive controller for holonomic + * drive trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param outputRobotRelative Function that will apply the robot-relative output speeds of this + * command + * @param translationConstants PID constants for the translation PID controllers + * @param rotationConstants PID constants for the rotation controller + * @param maxModuleSpeed The max speed of a 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 replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ public FollowPathHolonomic( PathPlannerPath path, Supplier poseSupplier, @@ -60,6 +97,18 @@ public FollowPathHolonomic( requirements); } + /** + * Construct a path following command that will use a holonomic drive controller for holonomic + * drive trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param outputRobotRelative Function that will apply the robot-relative output speeds of this + * command + * @param config Holonomic path follower configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ public FollowPathHolonomic( PathPlannerPath path, Supplier poseSupplier, 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 48ab9561..853140f5 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathLTV.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathLTV.java @@ -12,7 +12,19 @@ import java.util.function.Consumer; import java.util.function.Supplier; -public class FollowPathLTV extends PathFollowingCommand { +public class FollowPathLTV extends FollowPathCommand { + /** + * Create a path following command that will use an LTV unicycle controller for differential drive + * trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this command + * @param dt The amount of time between each robot control loop, default is 0.02s + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ public FollowPathLTV( PathPlannerPath path, Supplier poseSupplier, @@ -31,6 +43,20 @@ public FollowPathLTV( requirements); } + /** + * Create a path following command that will use an LTV unicycle controller for differential drive + * trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this command + * @param qelems The maximum desired error tolerance for each state. + * @param relems The maximum desired control effort for each input. + * @param dt The amount of time between each robot control loop, default is 0.02s + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ public FollowPathLTV( PathPlannerPath path, Supplier poseSupplier, 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 fc71a05b..badae122 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathRamsete.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathRamsete.java @@ -9,7 +9,22 @@ import java.util.function.Consumer; import java.util.function.Supplier; -public class FollowPathRamsete extends PathFollowingCommand { +public class FollowPathRamsete extends FollowPathCommand { + /** + * Construct a path following command that will use a Ramsete path following controller for + * differential drive trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this command + * @param b Tuning parameter (b > 0 rad^2/m^2) for which larger values make convergence more + * aggressive like a proportional term. + * @param zeta Tuning parameter (0 rad^-1 < zeta < 1 rad^-1) for which larger values provide + * more damping in response. + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ public FollowPathRamsete( PathPlannerPath path, Supplier poseSupplier, @@ -29,6 +44,17 @@ public FollowPathRamsete( requirements); } + /** + * Construct a path following command that will use a Ramsete path following controller for + * differential drive trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this command + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ public FollowPathRamsete( PathPlannerPath path, Supplier poseSupplier, 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 9f328b9f..f4ef6e29 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindingCommand.java @@ -32,6 +32,19 @@ public class PathfindingCommand extends Command { private PathPlannerTrajectory currentTrajectory; private Pose2d startingPose; + /** + * Constructs a new base pathfinding command that will generate a path towards the given path. + * + * @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 outputRobotRelative a consumer for the output speeds (robot relative) + * @param controller Path following controller that will be used to follow the path + * @param rotationDelayDistance How far the robot should travel before attempting to rotate to the + * final rotation + * @param requirements the subsystems required by this command + */ public PathfindingCommand( PathPlannerPath targetPath, PathConstraints constraints, @@ -66,6 +79,20 @@ public PathfindingCommand( this.rotationDelayDistance = rotationDelayDistance; } + /** + * 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 outputRobotRelative a consumer for the output speeds (robot relative) + * @param controller Path following controller that will be used to follow the path + * @param rotationDelayDistance How far the robot should travel before attempting to rotate to the + * final rotation + * @param requirements the subsystems required by this command + */ public PathfindingCommand( Pose2d targetPose, PathConstraints constraints, 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 7c7d876b..01140558 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPHolonomicDriveController.java @@ -89,48 +89,66 @@ public void setEnabled(boolean enabled) { this.isEnabled = enabled; } + /** + * Resets the controller based on the current state of the robot + * + * @param currentPose Current robot pose + * @param currentSpeeds Current robot relative chassis speeds + */ @Override public void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds) { rotationController.reset( currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond); } + /** + * Calculates the next output of the path following controller + * + * @param currentPose The current robot pose + * @param targetState The desired trajectory state + * @return The next robot relative output of the path following controller + */ @Override public ChassisSpeeds calculateRobotRelativeSpeeds( - Pose2d currentPose, PathPlannerTrajectory.State referenceState) { - double xFF = referenceState.velocityMps * referenceState.heading.getCos(); - double yFF = referenceState.velocityMps * referenceState.heading.getSin(); + Pose2d currentPose, PathPlannerTrajectory.State targetState) { + double xFF = targetState.velocityMps * targetState.heading.getCos(); + double yFF = targetState.velocityMps * targetState.heading.getSin(); - this.translationError = currentPose.getTranslation().minus(referenceState.positionMeters); + this.translationError = currentPose.getTranslation().minus(targetState.positionMeters); if (!this.isEnabled) { return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, 0, currentPose.getRotation()); } double xFeedback = - this.xController.calculate(currentPose.getX(), referenceState.positionMeters.getX()); + this.xController.calculate(currentPose.getX(), targetState.positionMeters.getX()); double yFeedback = - this.yController.calculate(currentPose.getY(), referenceState.positionMeters.getY()); + this.yController.calculate(currentPose.getY(), targetState.positionMeters.getY()); - double angVelConstraint = referenceState.constraints.getMaxAngularVelocityRps(); + double angVelConstraint = targetState.constraints.getMaxAngularVelocityRps(); // Approximation of available module speed to do rotation with - double maxAngVelModule = Math.max(0, maxModuleSpeed - referenceState.velocityMps) * mpsToRps; + double maxAngVelModule = Math.max(0, maxModuleSpeed - targetState.velocityMps) * mpsToRps; double maxAngVel = Math.min(angVelConstraint, maxAngVelModule); var rotationConstraints = new TrapezoidProfile.Constraints( - maxAngVel, referenceState.constraints.getMaxAngularAccelerationRpsSq()); + maxAngVel, targetState.constraints.getMaxAngularAccelerationRpsSq()); double targetRotationVel = rotationController.calculate( currentPose.getRotation().getRadians(), - new TrapezoidProfile.State(referenceState.targetHolonomicRotation.getRadians(), 0), + new TrapezoidProfile.State(targetState.targetHolonomicRotation.getRadians(), 0), rotationConstraints); return ChassisSpeeds.fromFieldRelativeSpeeds( xFF + xFeedback, yFF + yFeedback, targetRotationVel, currentPose.getRotation()); } + /** + * Get the current positional error between the robot's actual and target positions + * + * @return Positional error, in meters + */ @Override public double getPositionalError() { return translationError.getNorm(); 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 bff2c914..5ad1740f 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPLTVController.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPLTVController.java @@ -69,6 +69,13 @@ public PPLTVController(Vector qelems, Vector relems, double dt, double m super(qelems, relems, dt, maxVelocity); } + /** + * Calculates the next output of the path following controller + * + * @param currentPose The current robot pose + * @param targetState The desired trajectory state + * @return The next robot relative output of the path following controller + */ @Override public ChassisSpeeds calculateRobotRelativeSpeeds( Pose2d currentPose, PathPlannerTrajectory.State targetState) { @@ -81,11 +88,22 @@ public ChassisSpeeds calculateRobotRelativeSpeeds( targetState.headingAngularVelocityRps); } + /** + * Resets the controller based on the current state of the robot + * + * @param currentPose Current robot pose + * @param currentSpeeds Current robot relative chassis speeds + */ @Override public void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds) { lastError = 0; } + /** + * Get the current positional error between the robot's actual and target positions + * + * @return Positional error, in meters + */ @Override public double getPositionalError() { return lastError; 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 a1a648e7..fe3614ac 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPRamseteController.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPRamseteController.java @@ -28,6 +28,13 @@ public PPRamseteController() { super(); } + /** + * Calculates the next output of the path following controller + * + * @param currentPose The current robot pose + * @param targetState The desired trajectory state + * @return The next robot relative output of the path following controller + */ @Override public ChassisSpeeds calculateRobotRelativeSpeeds( Pose2d currentPose, PathPlannerTrajectory.State targetState) { @@ -40,11 +47,22 @@ public ChassisSpeeds calculateRobotRelativeSpeeds( targetState.headingAngularVelocityRps); } + /** + * Resets the controller based on the current state of the robot + * + * @param currentPose Current robot pose + * @param currentSpeeds Current robot relative chassis speeds + */ @Override public void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds) { lastError = 0; } + /** + * Get the current positional error between the robot's actual and target positions + * + * @return Positional error, in meters + */ @Override public double getPositionalError() { return lastError; 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 c93200b5..11674ef3 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PathFollowingController.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PathFollowingController.java @@ -23,5 +23,10 @@ ChassisSpeeds calculateRobotRelativeSpeeds( */ void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds); + /** + * Get the current positional error between the robot's actual and target positions + * + * @return Positional error, in meters + */ double getPositionalError(); } 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 0c614606..56bdc8ab 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/util/ReplanningConfig.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/util/ReplanningConfig.java @@ -6,6 +6,18 @@ public class ReplanningConfig { public final double dynamicReplanningTotalErrorThreshold; public final double dynamicReplanningErrorSpikeThreshold; + /** + * Create a path replanning configuration + * + * @param enableInitialReplanning Should the path be replanned at the start of path following if + * the robot is not already at the starting point? + * @param enableDynamicReplanning Should the path be replanned if the error grows too large or if + * a large error spike happens while following the path? + * @param dynamicReplanningTotalErrorThreshold The total error threshold, in meters, that will + * cause the path to be replanned + * @param dynamicReplanningErrorSpikeThreshold The error spike threshold, in meters, that will + * cause the path to be replanned + */ public ReplanningConfig( boolean enableInitialReplanning, boolean enableDynamicReplanning, @@ -17,10 +29,22 @@ public ReplanningConfig( this.dynamicReplanningErrorSpikeThreshold = dynamicReplanningErrorSpikeThreshold; } + /** + * Create a path replanning configuration with default dynamic replanning error thresholds + * + * @param enableInitialReplanning Should the path be replanned at the start of path following if + * the robot is not already at the starting point? + * @param enableDynamicReplanning Should the path be replanned if the error grows too large or if + * a large error spike happens while following the path? + */ public ReplanningConfig(boolean enableInitialReplanning, boolean enableDynamicReplanning) { this(enableInitialReplanning, enableDynamicReplanning, 1.0, 0.25); } + /** + * Create a path replanning configuration with the default config. This will only have initial + * replanning enabled. + */ public ReplanningConfig() { this(true, false); } diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathFollowingCommand.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp similarity index 89% rename from pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathFollowingCommand.cpp rename to pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp index 393db77f..8d32a6dc 100644 --- a/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/PathFollowingCommand.cpp +++ b/pathplannerlib/src/main/native/cpp/pathplanner/lib/commands/FollowPathCommand.cpp @@ -1,9 +1,8 @@ -#include "pathplanner/lib/commands/PathFollowingCommand.h" +#include "pathplanner/lib/commands/FollowPathCommand.h" using namespace pathplanner; -PathFollowingCommand::PathFollowingCommand( - std::shared_ptr path, +FollowPathCommand::FollowPathCommand(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, std::function output, @@ -15,8 +14,7 @@ PathFollowingCommand::PathFollowingCommand( AddRequirements(requirements); } -PathFollowingCommand::PathFollowingCommand( - std::shared_ptr path, +FollowPathCommand::FollowPathCommand(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, std::function output, @@ -28,7 +26,7 @@ PathFollowingCommand::PathFollowingCommand( AddRequirements(requirements); } -void PathFollowingCommand::Initialize() { +void FollowPathCommand::Initialize() { frc::Pose2d currentPose = m_poseSupplier(); frc::ChassisSpeeds currentSpeeds = m_speedsSupplier(); @@ -50,7 +48,7 @@ void PathFollowingCommand::Initialize() { m_timer.Start(); } -void PathFollowingCommand::Execute() { +void FollowPathCommand::Execute() { units::second_t currentTime = m_timer.Get(); PathPlannerTrajectory::State targetState = m_generatedTrajectory.sample( currentTime); @@ -92,11 +90,11 @@ void PathFollowingCommand::Execute() { m_output(targetSpeeds); } -bool PathFollowingCommand::IsFinished() { +bool FollowPathCommand::IsFinished() { return m_timer.HasElapsed(m_generatedTrajectory.getTotalTime()); } -void PathFollowingCommand::End(bool interrupted) { +void FollowPathCommand::End(bool interrupted) { m_timer.Stop(); // Only output 0 speeds when ending a path that is supposed to stop, this allows interrupting diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathFollowingCommand.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h similarity index 59% rename from pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathFollowingCommand.h rename to pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h index a234c312..e22aed2a 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/PathFollowingCommand.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathCommand.h @@ -20,10 +20,22 @@ #include "pathplanner/lib/util/PPLibTelemetry.h" namespace pathplanner { -class PathFollowingCommand: public frc2::CommandHelper { +class FollowPathCommand: public frc2::CommandHelper { public: - PathFollowingCommand(std::shared_ptr path, + /** + * Construct a base path following command + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this + * command + * @param controller Path following controller that will be used to follow the path + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ + FollowPathCommand(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, std::function output, @@ -31,7 +43,19 @@ class PathFollowingCommand: public frc2::CommandHelper requirements); - PathFollowingCommand(std::shared_ptr path, + /** + * Construct a base path following command + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this + * command + * @param controller Path following controller that will be used to follow the path + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ + FollowPathCommand(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, std::function output, diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathHolonomic.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathHolonomic.h index 1b848537..ab03243f 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathHolonomic.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathHolonomic.h @@ -1,13 +1,32 @@ #pragma once -#include "pathplanner/lib/commands/PathFollowingCommand.h" +#include "pathplanner/lib/commands/FollowPathCommand.h" #include "pathplanner/lib/controllers/PPHolonomicDriveController.h" #include "pathplanner/lib/util/PIDConstants.h" #include "pathplanner/lib/util/HolonomicPathFollowerConfig.h" namespace pathplanner { -class FollowPathHolonomic: public PathFollowingCommand { +class FollowPathHolonomic: public FollowPathCommand { public: + /** + * Construct a path following command that will use a holonomic drive controller for holonomic + * drive trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this + * command + * @param translationConstants PID constants for the translation PID controllers + * @param rotationConstants PID constants for the rotation controller + * @param maxModuleSpeed The max speed of a 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 replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + * @param period Period of the control loop in seconds, default is 0.02s + */ FollowPathHolonomic(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, @@ -16,13 +35,32 @@ class FollowPathHolonomic: public PathFollowingCommand { units::meters_per_second_t maxModuleSpeed, units::meter_t driveBaseRadius, ReplanningConfig replanningConfig, std::initializer_list requirements, - units::second_t period = 0.02_s) : PathFollowingCommand(path, + units::second_t period = 0.02_s) : FollowPathCommand(path, poseSupplier, speedsSupplier, output, std::make_unique < PPHolonomicDriveController > (translationConstants, rotationConstants, maxModuleSpeed, driveBaseRadius, period), replanningConfig, requirements) { } + /** + * Construct a path following command that will use a holonomic drive controller for holonomic + * drive trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this + * command + * @param translationConstants PID constants for the translation PID controllers + * @param rotationConstants PID constants for the rotation controller + * @param maxModuleSpeed The max speed of a 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 replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + * @param period Period of the control loop in seconds, default is 0.02s + */ FollowPathHolonomic(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, @@ -31,13 +69,25 @@ class FollowPathHolonomic: public PathFollowingCommand { units::meters_per_second_t maxModuleSpeed, units::meter_t driveBaseRadius, ReplanningConfig replanningConfig, std::span requirements, units::second_t period = - 0.02_s) : PathFollowingCommand(path, poseSupplier, + 0.02_s) : FollowPathCommand(path, poseSupplier, speedsSupplier, output, std::make_unique < PPHolonomicDriveController > (translationConstants, rotationConstants, maxModuleSpeed, driveBaseRadius, period), replanningConfig, requirements) { } + /** + * Construct a path following command that will use a holonomic drive controller for holonomic + * drive trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this + * command + * @param config Holonomic path follower configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ FollowPathHolonomic(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, @@ -50,6 +100,18 @@ class FollowPathHolonomic: public PathFollowingCommand { config.replanningConfig, requirements, config.period) { } + /** + * Construct a path following command that will use a holonomic drive controller for holonomic + * drive trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this + * command + * @param config Holonomic path follower configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ FollowPathHolonomic(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathLTV.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathLTV.h index 3d9fe1e4..4f5d28f8 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathLTV.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathLTV.h @@ -1,11 +1,25 @@ #pragma once -#include "pathplanner/lib/commands/PathFollowingCommand.h" +#include "pathplanner/lib/commands/FollowPathCommand.h" #include "pathplanner/lib/controllers/PPLTVController.h" namespace pathplanner { -class FollowPathLTV: public PathFollowingCommand { +class FollowPathLTV: public FollowPathCommand { public: + /** + * Create a path following command that will use an LTV unicycle controller for differential drive + * trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this command + * @param Qelems The maximum desired error tolerance for each state. + * @param Relems The maximum desired control effort for each input. + * @param dt The amount of time between each robot control loop, default is 0.02s + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ FollowPathLTV(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, @@ -13,12 +27,26 @@ class FollowPathLTV: public PathFollowingCommand { const wpi::array &Qelms, const wpi::array &Relms, units::second_t dt, ReplanningConfig replanningConfig, - std::initializer_list requirements) : PathFollowingCommand( + std::initializer_list requirements) : FollowPathCommand( path, poseSupplier, speedsSupplier, output, std::make_unique < PPLTVController > (Qelms, Relms, dt), replanningConfig, requirements) { } + /** + * Create a path following command that will use an LTV unicycle controller for differential drive + * trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this command + * @param Qelems The maximum desired error tolerance for each state. + * @param Relems The maximum desired control effort for each input. + * @param dt The amount of time between each robot control loop, default is 0.02s + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ FollowPathLTV(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, @@ -26,30 +54,54 @@ class FollowPathLTV: public PathFollowingCommand { const wpi::array &Qelms, const wpi::array &Relms, units::second_t dt, ReplanningConfig replanningConfig, - std::span requirements) : PathFollowingCommand( - path, poseSupplier, speedsSupplier, output, + std::span requirements) : FollowPathCommand(path, + poseSupplier, speedsSupplier, output, std::make_unique < PPLTVController > (Qelms, Relms, dt), replanningConfig, requirements) { } + /** + * Create a path following command that will use an LTV unicycle controller for differential drive + * trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this command + * @param dt The amount of time between each robot control loop, default is 0.02s + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ FollowPathLTV(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, std::function output, units::second_t dt, ReplanningConfig replanningConfig, - std::initializer_list requirements) : PathFollowingCommand( + std::initializer_list requirements) : FollowPathCommand( path, poseSupplier, speedsSupplier, output, std::make_unique < PPLTVController > (dt), replanningConfig, requirements) { } + /** + * Create a path following command that will use an LTV unicycle controller for differential drive + * trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this command + * @param dt The amount of time between each robot control loop, default is 0.02s + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ FollowPathLTV(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, std::function output, units::second_t dt, ReplanningConfig replanningConfig, - std::span requirements) : PathFollowingCommand( - path, poseSupplier, speedsSupplier, output, + std::span requirements) : FollowPathCommand(path, + poseSupplier, speedsSupplier, output, std::make_unique < PPLTVController > (dt), replanningConfig, requirements) { } diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathRamsete.h b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathRamsete.h index 794ddb77..8dafef0b 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathRamsete.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/commands/FollowPathRamsete.h @@ -1,11 +1,26 @@ #pragma once -#include "pathplanner/lib/commands/PathFollowingCommand.h" +#include "pathplanner/lib/commands/FollowPathCommand.h" #include "pathplanner/lib/controllers/PPRamseteController.h" namespace pathplanner { -class FollowPathRamsete: public PathFollowingCommand { +class FollowPathRamsete: public FollowPathCommand { public: + /** + * Construct a path following command that will use a Ramsete path following controller for + * differential drive trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this command + * @param b Tuning parameter (b > 0 rad^2/m^2) for which larger values make convergence more + * aggressive like a proportional term. + * @param zeta Tuning parameter (0 rad^-1 < zeta < 1 rad^-1) for which larger values provide + * more damping in response. + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ FollowPathRamsete(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, @@ -13,12 +28,27 @@ class FollowPathRamsete: public PathFollowingCommand { units::unit_t b, units::unit_t zeta, ReplanningConfig replanningConfig, - std::initializer_list requirements) : PathFollowingCommand( + std::initializer_list requirements) : FollowPathCommand( path, poseSupplier, speedsSupplier, output, std::make_unique < PPRamseteController > (b, zeta), replanningConfig, requirements) { } + /** + * Construct a path following command that will use a Ramsete path following controller for + * differential drive trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this command + * @param b Tuning parameter (b > 0 rad^2/m^2) for which larger values make convergence more + * aggressive like a proportional term. + * @param zeta Tuning parameter (0 rad^-1 < zeta < 1 rad^-1) for which larger values provide + * more damping in response. + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ FollowPathRamsete(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, @@ -26,30 +56,52 @@ class FollowPathRamsete: public PathFollowingCommand { units::unit_t b, units::unit_t zeta, ReplanningConfig replanningConfig, - std::span requirements) : PathFollowingCommand( - path, poseSupplier, speedsSupplier, output, + std::span requirements) : FollowPathCommand(path, + poseSupplier, speedsSupplier, output, std::make_unique < PPRamseteController > (b, zeta), replanningConfig, requirements) { } + /** + * Construct a path following command that will use a Ramsete path following controller for + * differential drive trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this command + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ FollowPathRamsete(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, std::function output, ReplanningConfig replanningConfig, - std::initializer_list requirements) : PathFollowingCommand( + std::initializer_list requirements) : FollowPathCommand( path, poseSupplier, speedsSupplier, output, std::make_unique(), replanningConfig, requirements) { } + /** + * Construct a path following command that will use a Ramsete path following controller for + * differential drive trains + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param output Function that will apply the robot-relative output speeds of this command + * @param replanningConfig Path replanning configuration + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ FollowPathRamsete(std::shared_ptr path, std::function poseSupplier, std::function speedsSupplier, std::function output, ReplanningConfig replanningConfig, - std::span requirements) : PathFollowingCommand( - path, poseSupplier, speedsSupplier, output, + std::span requirements) : FollowPathCommand(path, + poseSupplier, speedsSupplier, output, std::make_unique(), replanningConfig, requirements) { } diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PathFollowingController.h b/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PathFollowingController.h index 7e57e176..387ea5b4 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PathFollowingController.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/controllers/PathFollowingController.h @@ -31,6 +31,11 @@ class PathFollowingController { virtual void reset(const frc::Pose2d ¤tPose, const frc::ChassisSpeeds ¤tSpeeds) = 0; + /** + * Get the current positional error between the robot's actual and target positions + * + * @return Positional error, in meters + */ virtual units::meter_t getPositionalError() = 0; }; } diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/util/ReplanningConfig.h b/pathplannerlib/src/main/native/include/pathplanner/lib/util/ReplanningConfig.h index bf77aabc..0f428343 100644 --- a/pathplannerlib/src/main/native/include/pathplanner/lib/util/ReplanningConfig.h +++ b/pathplannerlib/src/main/native/include/pathplanner/lib/util/ReplanningConfig.h @@ -10,6 +10,18 @@ class ReplanningConfig { const units::meter_t dynamicReplanningTotalErrorThreshold; const units::meter_t dynamicReplanningErrorSpikeThreshold; + /** + * Create a path replanning configuration + * + * @param enableInitialReplanning Should the path be replanned at the start of path following if + * the robot is not already at the starting point? + * @param enableDynamicReplanning Should the path be replanned if the error grows too large or if + * a large error spike happens while following the path? + * @param dynamicReplanningTotalErrorThreshold The total error threshold, in meters, that will + * cause the path to be replanned. Default = 1.0m + * @param dynamicReplanningErrorSpikeThreshold The error spike threshold, in meters, that will + * cause the path to be replanned. Default = 0.25m + */ constexpr ReplanningConfig(const bool enableInitialReplanning = true, const bool enableDynamicReplanning = false, const units::meter_t dynamicReplanningTotalErrorThreshold = 1_m,