Package trclib.pathdrive

Class TrcHolonomicPurePursuitDriveV2

java.lang.Object
trclib.pathdrive.TrcHolonomicPurePursuitDriveV2
public class TrcHolonomicPurePursuitDriveV2 extends Object
This class implements a platform independent Pure Pursuit drive for holonomic robots. Essentially, a pure pursuit drive navigates the robot to chase a point along the path. The point to chase is chosen by intersecting a proximity circle centered on the robot with a specific radius with the path, and chasing the "furthest" intersection. The smaller the radius is, the more "tightly" the robot will follow a path, but it will be more prone to oscillation and sharp turns. A larger radius will tend to smooth out turns and corners. Note that the error tolerance must be less than the proximity radius, so choose them accordingly.

A path consists of an array of waypoints, specifying position, velocity, and optionally heading. All other properties of the TrcWaypoint object may be ignored.The path may be low resolution, as this automatically interpolates between waypoints. If you want the robot to maintain heading, call setMaintainHeading(true) and it will ignore all the heading values. Otherwise, call setMaintainHeading(false), ensure that the heading tolerance and pid coefficients are set, and it will follow the heading values specified by the path.

A somewhat similar idea is here: ... or ...

Note that this paper is for non-holonomic robots. This means that all the turning radius stuff isn't very relevant. Technically, we could impose limits on the turning radius as a function of robot velocity and max rot vel, but that's unnecessarily complicated, in my view. Additionally, it does point injection instead of interpolation, and path smoothing, which we don't do, since a nonzero proximity radius will naturally smooth it anyway.

  • Field Details

    • tracer

      public final TrcDbgTrace tracer

    • instanceName

      private final String instanceName

    • driveBase

      private final TrcDriveBase driveBase

    • driveTaskObj

      private final TrcTaskMgr.TaskObject driveTaskObj

    • turnPidCtrl

      private final TrcPidController turnPidCtrl

    • velPidCtrl

      private final TrcPidController velPidCtrl

    • logRobotPoseEvents

      private boolean logRobotPoseEvents

    • tracePidInfo

      private boolean tracePidInfo

    • verbosePidInfo

      private boolean verbosePidInfo

    • battery

      private TrcRobotBattery battery

    • posTolerance

      private volatile double posTolerance

    • proximityRadius

      private volatile double proximityRadius

    • turnTolerance

      private double turnTolerance

    • velTolerance

      private volatile double velTolerance

    • path

      private TrcPath path

    • pathIndex

      private int pathIndex

    • onFinishedEvent

      private TrcEvent onFinishedEvent

    • timedOutTime

      private double timedOutTime

    • warpSpace

      private final TrcWarpSpace warpSpace

    • startHeading

      private double startHeading

    • referencePose

      private TrcPose2D referencePose

    • moveOutputLimit

      private double moveOutputLimit

    • rotOutputLimit

      private double rotOutputLimit

    • accelFF

      private final double accelFF

  • Constructor Details

    • TrcHolonomicPurePursuitDriveV2

      public TrcHolonomicPurePursuitDriveV2(String instanceName, TrcDriveBase driveBase, double proximityRadius, double posTolerance, double turnTolerance, double velTolerance, TrcPidController.PidCoefficients turnPidCoeff, TrcPidController.PidCoefficients velPidCoeff, double accelFF)
      Constructor: Create an instance of the object.
      Parameters:
      instanceName - specifies the instance name.
      driveBase - specifies the reference to the drive base.
      proximityRadius - specifies the distance between the robot and next following point.
      posTolerance - specifies the position tolerance.
      turnTolerance - specifies the turn tolerance.
      velTolerance - specifies the velocity tolerance.
      turnPidCoeff - specifies the turn PID coefficients.
      velPidCoeff - specifies the velocity PID coefficients.
      accelFF - specifies the acceleration Feed Forward value.

    • TrcHolonomicPurePursuitDriveV2

      public TrcHolonomicPurePursuitDriveV2(String instanceName, TrcDriveBase driveBase, double proximityRadius, double posTolerance, double velTolerance, TrcPidController.PidCoefficients velPidCoeff, double accelFF)
      Constructor: Create an instance of the object.
      Parameters:
      instanceName - specifies the instance name.
      driveBase - specifies the reference to the drive base.
      proximityRadius - specifies the distance between the robot and next following point.
      posTolerance - specifies the position tolerance.
      velTolerance - specifies the velocity tolerance.
      velPidCoeff - specifies the velocity PID coefficients.
      accelFF - specifies the acceleration Feed Forward value.

  • Method Details

    • toString

      public String toString()
      This method returns the instance name.
      Overrides:
      toString in class Object
      Returns:
      instance name.

    • setTraceLevel

      public void setTraceLevel(TrcDbgTrace.MsgLevel msgLevel, boolean logRobotPoseEvents, boolean tracePidInfo, boolean verbosePidInfo, TrcRobotBattery battery)
      This method sets the trace level for logging trace messages.
      Parameters:
      msgLevel - specifies the message level.
      logRobotPoseEvents - specifies true to log robot pose events, false otherwise.
      tracePidInfo - specifies true to enable tracing of PID info, false otherwise.
      verbosePidInfo - specifies true to trace verbose PID info, false otherwise.
      battery - specifies the battery object to get battery info for the message.

    • setTraceLevel

      public void setTraceLevel(TrcDbgTrace.MsgLevel msgLevel, boolean logRobotPoseEvents, boolean tracePidInfo, boolean verbosePidInfo)
      This method sets the trace level for logging trace messages.
      Parameters:
      msgLevel - specifies the message level.
      logRobotPoseEvents - specifies true to log robot pose events, false otherwise.
      tracePidInfo - specifies true to enable tracing of PID info, false otherwise.
      verbosePidInfo - specifies true to trace verbose PID info, false otherwise.

    • setTurnTolerance

      public void setTurnTolerance(double turnTolerance)
      Set the turn tolerance to end the path. Units need to be consistent.
      Parameters:
      turnTolerance - The angle error at which the controller will stop itself.

    • setVelTolerance

      public void setVelTolerance(double velTolerance)
      Set the velocity tolerance. The velocity must be less than this value for the controller to finish.
      Parameters:
      velTolerance - Positive velocity.

    • setPositionToleranceAndProximityRadius

      public void setPositionToleranceAndProximityRadius(double posTolerance, double proximityRadius)
      Set both the position tolerance and proximity radius.
      Parameters:
      posTolerance - specifies the distance at which the controller will stop itself.
      proximityRadius - specifies the distance between the robot and next following point.

    • setPositionTolerance

      public void setPositionTolerance(double posTolerance)
      Set the position tolerance to end the path. Units need to be consistent.
      Parameters:
      posTolerance - The distance at which the controller will stop itself.

    • setProximityRadius

      public void setProximityRadius(double proximityRadius)
      Set the proximity radius for the pure pursuit controller.
      Parameters:
      proximityRadius - specifies the distance between the robot and next following point.

    • setTurnPidCoefficients

      public void setTurnPidCoefficients(TrcPidController.PidCoefficients pidCoefficients)
      Sets the pid coefficients for the turn controller. This will work in the middle of an operation as well.
      Parameters:
      pidCoefficients - The new PID coefficients for the heading controller.

    • setVelocityPidCoefficients

      public void setVelocityPidCoefficients(TrcPidController.PidCoefficients pidCoefficients)
      Sets the pid coefficients for the position controller. This will work in the middle of an operation as well. Note that velocity controllers should have an F term as well.
      Parameters:
      pidCoefficients - The new PIDF coefficients for the velocity controller.

    • setMoveOutputLimit

      public void setMoveOutputLimit(double limit)
      Sets the movement output power limit.
      Parameters:
      limit - specifies the output power limit for movement (X and Y).

    • setRotOutputLimit

      public void setRotOutputLimit(double limit)
      Sets the rotation output power limit.
      Parameters:
      limit - specifies the output power limit for rotation.

    • start

      public void start(TrcPath path, TrcEvent onFinishedEvent, double timeout)
      Start following the supplied path using a pure pursuit controller. The velocity must always be positive, and the path must start at (0,0). Heading is absolute and position is relative in the starting robot reference frame.
      Parameters:
      path - The path to follow. Must start at (0,0).
      onFinishedEvent - When finished, signal this event.
      timeout - Number of seconds after which to cancel this operation. 0.0 for no timeout.

    • start

      public void start(TrcPath path, TrcEvent onFinishedEvent)
      Start following the supplied path using a pure pursuit controller. The velocity must always be positive, and the path must start at (0,0). Heading is absolute and position is relative in the starting robot reference frame.
      Parameters:
      path - The path to follow. Must start at (0,0).
      onFinishedEvent - When finished, signal this event.

    • start

      public void start(TrcPath path)
      Start following the supplied path using a pure pursuit controller.
      Parameters:
      path - The path to follow. Must start at (0,0). Velocity is per second.

    • start

      public void start(TrcEvent onFinishedEvent, double timeout, TrcPose2D startingPose, boolean incrementalPath, TrcPose2D... poses)
      This method starts the Pure Pursuit drive with the specified poses in the drive path.
      Parameters:
      onFinishedEvent - When finished, signal this event.
      timeout - specifies the maximum time allowed for this operation, 0.0 for no timeout.
      startingPose - specifies the starting pose at the beginning of the path.
      incrementalPath - specifies true if appending point is relative to the previous point in the path, false if appending point is in the same reference frame as startingPose.
      poses - specifies an array of waypoint poses in the drive path.

    • start

      public void start(TrcEvent onFinishedEvent, TrcPose2D startingPose, boolean incrementalPath, TrcPose2D... poses)
      This method starts the Pure Pursuit drive with the specified poses in the drive path.
      Parameters:
      onFinishedEvent - When finished, signal this event.
      startingPose - specifies the starting pose at the beginning of the path.
      incrementalPath - specifies true if appending point is relative to the previous point in the path, false if appending point is in the same reference frame as startingPose.
      poses - specifies an array of waypoint poses in the drive path.

    • start

      public void start(TrcPose2D startingPose, boolean incrementalPath, TrcPose2D... poses)
      This method starts the Pure Pursuit drive with the specified poses in the drive path.
      Parameters:
      startingPose - specifies the starting pose at the beginning of the path.
      incrementalPath - specifies true if appending point is relative to the previous point in the path, false if appending point is in the same reference frame as startingPose.
      poses - specifies an array of waypoint poses in the drive path.

    • isActive

      public boolean isActive()
      Checks if the robot is currently following a path.
      Returns:
      True if the pure pursuit controller is active, false otherwise.

    • cancel

      public void cancel()
      If the controller is currently active, cancel the path following operation. Otherwise, do nothing. If there is an event to signal, mark it as cancelled.

    • getVelocityInput

      private double getVelocityInput()
      This method is called by the Velocity PID controller to get the polar magnitude of the robot's velocity.
      Returns:
      robot's velocity magnitude.

    • stop

      private void stop()
      Stops PurePursuit drive.

    • driveTask

      private void driveTask(TrcTaskMgr.TaskType taskType, TrcRobot.RunMode runMode, boolean slowPeriodicLoop)
      This task is called periodically to calculate the next target point on the path. The next target point on the path has a distance of followDistance from the current robot position intersecting with the path segment towards the end of the endpoint of the path segment.
      Parameters:
      taskType - specifies the type of task being run.
      runMode - specifies the competition mode that is about to end (e.g. Autonomous, TeleOp, Test).
      slowPeriodicLoop - specifies true if it is running the slow periodic loop on the main robot thread, false otherwise.

    • interpolate

      private TrcWaypoint interpolate(TrcWaypoint point1, TrcWaypoint point2, double weight)
      Interpolates a waypoint that's weighted between two given waypoints.
      Parameters:
      point1 - specifies the start point of the path segment.
      point2 - specifies the end point of the path segment.
      weight - specifies the weight between the two provided points.
      Returns:
      weighted interpolated waypoint.

    • interpolate

      private double interpolate(double start, double end, double weight)
      Returns a weighted value between given values.
      Parameters:
      start - specifies the start value.
      end - specifies the end value.
      weight - specifies the weight between the values.
      Returns:
      weighted value between the given values.

    • getFollowingPointOnSegment

      private TrcWaypoint getFollowingPointOnSegment(TrcWaypoint prev, TrcWaypoint point, TrcPose2D robotPose)
      This method calculates the waypoint on the path segment that intersects the robot's proximity circle that is closest to the end point of the path segment. The algorithm is based on this article: ...
      Parameters:
      prev - specifies the start point of the path segment.
      point - specifies the end point of the path segment.
      robotPose - specifies the robot's position.
      Returns:
      calculated waypoint.

    • getTargetPointDistParameterized

      private TrcWaypoint getTargetPointDistParameterized(TrcPose2D robotPose)

    • getFollowingPoint

      private TrcWaypoint getFollowingPoint(TrcPose2D robotPose)
      Determines the next target point for Pure Pursuit Drive to follow.
      Parameters:
      robotPose - specifies the robot's location.
      Returns:
      next target point for the robot to follow.