Package org.team1126.lib.math

Class PAPFController

java.lang.Object
org.team1126.lib.math.PAPFController
All Implemented Interfaces:
Tunables.Tunable
public final class PAPFController extends Object implements Tunables.Tunable
Implements a predictive artificial potential field (P-APF), capable of real-time motion planning with obstacle avoidance.

This implementation improves upon traditional APF algorithms by predicting the future movement of the robot to determine intermediate setpoints, with the goal of achieving smoother motion and preventing oscillation caused by suboptimal interactions with potentials generated by obstacles.

  • Nested Class Summary

    Nested Classes
    Modifier and Type
    Class
    Description
    static final class 
    PAPFController.CircleObstacle
    A circle with a specified radius.
    static interface 
    PAPFController.ForceHeuristic
    Represents a heuristic for transforming the force profile of an obstacle.
    static interface 
    PAPFController.GoalHeuristic
    Represents a heuristic for calculating the strength of the attractive force generated by the goal location.
    static final class 
    PAPFController.LateralObstacle
    An infinite line that pushes parallel to the Y axis, to constrain left/right field-relative movement.
    static final class 
    PAPFController.LineObstacle
    A line segment that pushes perpendicular from its length.
    static final class 
    PAPFController.LongitudinalObstacle
    An infinite line that pushes parallel to the X axis, to constrain forwards/backwards field-relative movement.
    static class 
    PAPFController.Obstacle
    A generic obstacle.
    static final class 
    PAPFController.PointObstacle
    A simple point obstacle.

  • Constructor Summary

    Constructors
    Constructor
    Description
    PAPFController(double horizon, double resolution, double tolerance, boolean savePrediction, PAPFController.GoalHeuristic goalHeuristic, PAPFController.Obstacle[] obstacles)
    Creates a new P-APF controller.
    PAPFController(double horizon, double resolution, double tolerance, boolean savePrediction, PAPFController.Obstacle[] obstacles)
    Creates a new P-APF controller.

  • Method Summary

    Modifier and Type
    Method
    Description
    edu.wpi.first.math.kinematics.ChassisSpeeds
    calculate(edu.wpi.first.math.geometry.Pose2d currentPose, edu.wpi.first.math.geometry.Translation2d goal, double maxVelocity, double maxDeceleration)
    Returns the next velocity output of the potential field.
    edu.wpi.first.math.kinematics.ChassisSpeeds
    calculate(edu.wpi.first.math.geometry.Pose2d currentPose, edu.wpi.first.math.geometry.Translation2d goal, double maxVelocity, double maxDeceleration, PAPFController.Obstacle... tempObstacles)
    Returns the next velocity output of the potential field.
    edu.wpi.first.math.geometry.Translation2d
    getGoal()
    Returns the current goal of the controller from the last invocation of PAPFController.calculate().
    List<edu.wpi.first.math.geometry.Translation2d>
    getPrediction()
    Returns the current predicted motion of the robot from the last invocation of PAPFController.calculate().
    edu.wpi.first.math.geometry.Translation2d
    getSetpoint()
    Returns the current setpoint of the controller from the last invocation of PAPFController.calculate().
    void
    initTunable(TunableTable table)
    Initializes the object to be tuned.
    edu.wpi.first.math.kinematics.ChassisSpeeds
    repulsorDrive(edu.wpi.first.math.kinematics.ChassisSpeeds input, edu.wpi.first.math.geometry.Pose2d currentPose)
    Utilizes the potential field to alter an arbitrary input chassis speed and minimize collisions with obstacles.
    List<edu.wpi.first.math.geometry.Pose2d>
    visualizeField(int rows, double padding, double fieldLength, double fieldWidth)
    Returns a list of samples from the potential field for visualization in AdvantageScope.

    Methods inherited from class java.lang.Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Constructor Details

    • PAPFController

      public PAPFController(double horizon, double resolution, double tolerance, boolean savePrediction, PAPFController.Obstacle[] obstacles)
      Creates a new P-APF controller.
      Parameters:
      horizon - The horizon at which to predict the robot's motion, in meters. Larger horizons increase computational cost, with the benefit of generally smoother motion.
      resolution - The resolution at which to sample the horizon in meters. Higher resolutions (i.e. smaller values) will increase accuracy of predicted motion, however are more computationally expensive.
      tolerance - The distance from the goal in meters at which to return a velocity output of 0.
      savePrediction - If the robot's predicted motion should be saved. Useful for debugging, however does increase GC pressure.
      obstacles - The field's obstacles.

    • PAPFController

      public PAPFController(double horizon, double resolution, double tolerance, boolean savePrediction, PAPFController.GoalHeuristic goalHeuristic, PAPFController.Obstacle[] obstacles)
      Creates a new P-APF controller.
      Parameters:
      horizon - The horizon at which to predict the robot's motion, in meters. Larger horizons increase computational cost, with the benefit of generally smoother motion.
      resolution - The resolution at which to sample the horizon in meters. Higher resolutions (i.e. smaller values) will increase accuracy of predicted motion, however are more computationally expensive.
      tolerance - The distance from the goal in meters at which to return a velocity output of 0.
      savePrediction - If the robot's predicted motion should be saved. Useful for debugging, however does increase GC pressure.
      goalHeuristic - A heuristic for calculating the strength of the attractive force generated by the goal location.
      obstacles - The field's obstacles.

  • Method Details

    • getSetpoint

      public edu.wpi.first.math.geometry.Translation2d getSetpoint()
      Returns the current setpoint of the controller from the last invocation of PAPFController.calculate().

    • getGoal

      public edu.wpi.first.math.geometry.Translation2d getGoal()
      Returns the current goal of the controller from the last invocation of PAPFController.calculate().

    • getPrediction

      public List<edu.wpi.first.math.geometry.Translation2d> getPrediction()
      Returns the current predicted motion of the robot from the last invocation of PAPFController.calculate(). Note that this list will be empty if savePrediction is false.

    • calculate

      public edu.wpi.first.math.kinematics.ChassisSpeeds calculate(edu.wpi.first.math.geometry.Pose2d currentPose, edu.wpi.first.math.geometry.Translation2d goal, double maxVelocity, double maxDeceleration)
      Returns the next velocity output of the potential field. Note that while deceleration is accounted for, acceleration from rest is not. If acceleration limiting is desired, it must be implemented downstream.
      Parameters:
      currentPose - The current blue origin relative pose of the robot.
      goal - The desired position of the robot.
      maxVelocity - The desired cruise velocity of the robot, in m/s.
      maxDeceleration - The desired deceleration rate of the robot, in m/s/s.
      Returns:
      Blue origin relative chassis speeds.

    • calculate

      public edu.wpi.first.math.kinematics.ChassisSpeeds calculate(edu.wpi.first.math.geometry.Pose2d currentPose, edu.wpi.first.math.geometry.Translation2d goal, double maxVelocity, double maxDeceleration, PAPFController.Obstacle... tempObstacles)
      Returns the next velocity output of the potential field. Note that while deceleration is accounted for, acceleration from rest is not. If acceleration limiting is desired, it must be implemented downstream.
      Parameters:
      currentPose - The current blue origin relative pose of the robot.
      goal - The desired position of the robot.
      maxVelocity - The desired cruise velocity of the robot, in m/s.
      maxDeceleration - The desired deceleration rate of the robot, in m/s/s.
      tempObstacles - Additional temporary obstacles. Can be null.
      Returns:
      Blue origin relative chassis speeds.

    • repulsorDrive

      public edu.wpi.first.math.kinematics.ChassisSpeeds repulsorDrive(edu.wpi.first.math.kinematics.ChassisSpeeds input, edu.wpi.first.math.geometry.Pose2d currentPose)
      Utilizes the potential field to alter an arbitrary input chassis speed and minimize collisions with obstacles. Typically, this method is used for manipulating driver input such that the robot doesn't crash into field walls or game elements.
      Parameters:
      input - Blue origin relative input speeds, typically from the driver.
      currentPose - The current blue origin relative pose of the robot.
      Returns:
      The new blue origin relative chassis speeds.

    • visualizeField

      public List<edu.wpi.first.math.geometry.Pose2d> visualizeField(int rows, double padding, double fieldLength, double fieldWidth)
      Returns a list of samples from the potential field for visualization in AdvantageScope. Represented as Pose2ds, the translation of the pose is the sample location, and the rotation is the direction of force at the sample. Useful for debugging purposes, but is not recommended for regular use due to performance implications.
      Parameters:
      rows - The number of rows (X+ direction) to sample. Columns are sampled relative to the ratio of fieldWidth / fieldLength.
      padding - The distance past the field perimeter to sample in meters.
      fieldLength - The length of the field in meters.
      fieldWidth - The width of the field in meters.

    • initTunable

      public void initTunable(TunableTable table)
      Description copied from interface: Tunables.Tunable
      Initializes the object to be tuned. This method is called when adding the object to a TunableTable.
      Specified by:
      initTunable in interface Tunables.Tunable
      Parameters:
      table - The table the object is being added to.