org.usfirst.frc.team1736.lib.PathPlanner

Class FalconPathPlanner

java.lang.Object

org.usfirst.frc.team1736.lib.PathPlanner.FalconPathPlanner

public class FalconPathPlanner
extends java.lang.Object

This Class provides many useful algorithms for Robot Path Planning. It uses optimization techniques and knowledge of Robot Motion in order to calculate smooth path trajectories, if given only discrete waypoints. The Benefit of these optimization algorithms are very efficient path planning that can be used to Navigate in Real-time. This Class uses a method of Gradient Decent, and other optimization techniques to produce smooth Velocity profiles for both left and right wheels of a differential drive robot. This Class does not attempt to calculate quintic or cubic splines for best fitting a curve. It is for this reason, the algorithm can be ran on embedded devices with very quick computation times. The output of this function are independent velocity profiles for the left and right wheels of a differential drive chassis. The velocity profiles start and end with 0 velocity and maintain smooth transitions throughout the path. This algorithm is a port from a similar algorithm running on a Robot used for my PhD thesis. I have not fully optimized these functions, so there is room for some improvement. Initial tests on the 2015 FRC NI RoboRio, the complete algorithm finishes in under 15ms using the Java System Timer for paths with less than 50 nodes.

Field Summary

Fields

Modifier and Type	Field and Description
double[][]	heading
double[][]	leftPath
double[][]	nodeOnlyPath
double[][]	origCenterVelocity
double[][]	origLeftVelocity
double[][]	origPath
double[][]	origRightVelocity
double[][]	rightPath
double[][]	smoothCenterVelocity
double[][]	smoothLeftVelocity
double[][]	smoothPath
double[][]	smoothRightVelocity

Constructor Summary

Constructors

Constructor and Description
FalconPathPlanner(double[][] path) Constructor, takes a Path of Way Points defined as a double array of column vectors representing the global cartesian points of the path in {x,y} coordinates.

Method Summary

Modifier and Type	Method and Description
void	calculate(double totalTime, double timeStep, double robotTrackWidth) This code will calculate a smooth path based on the program parameters.
static double[][]	doubleArrayCopy(double[][] arr) Performs a deep copy of a 2 Dimensional Array looping thorough each element in the 2D array BigO: Order N x M
static double[]	getXVector(double[][] arr) Returns the first column of a 2D array of doubles
static double[]	getYVector(double[][] arr) Returns the second column of a 2D array of doubles
double[][]	inject(double[][] orig, int numToInject) Method upsamples the Path by linear injection.
int[]	injectionCounter2Steps(double numNodeOnlyPoints, double maxTimeToComplete, double timeStep) This method calculates the optimal parameters for determining what amount of nodes to inject into the path to meet the time restraint.
void	leftRight(double[][] smoothPath, double robotTrackWidth) Calculates the left and right wheel paths based on robot track width Big O: 2N
static void	main(java.lang.String[] args)
static double[][]	nodeOnlyWayPoints(double[][] path) reduces the path into only nodes which change direction.
void	poofExample()
static void	print(double[] path)
static void	print(double[][] path) Prints Cartesian Coordinates to the System Output as Column Vectors in the Form X Y
void	setPathAlpha(double alpha)
void	setPathBeta(double beta)
void	setPathTolerance(double tolerance)
void	setVelocityAlpha(double alpha)
void	setVelocityBeta(double beta)
double[][]	smoother(double[][] path, double weight_data, double weight_smooth, double tolerance) Optimization algorithm, which optimizes the data points in path to create a smooth trajectory.
static double[][]	transposeVector(double[][] arr)

Methods inherited from class java.lang.Object

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

Field Detail

origPath

public double[][] origPath

nodeOnlyPath

public double[][] nodeOnlyPath

smoothPath

public double[][] smoothPath

leftPath

public double[][] leftPath

rightPath

public double[][] rightPath

origCenterVelocity

public double[][] origCenterVelocity

origRightVelocity

public double[][] origRightVelocity

origLeftVelocity

public double[][] origLeftVelocity

smoothCenterVelocity

public double[][] smoothCenterVelocity

smoothRightVelocity

public double[][] smoothRightVelocity

smoothLeftVelocity

public double[][] smoothLeftVelocity

heading

public double[][] heading

Constructor Detail

FalconPathPlanner

public FalconPathPlanner(double[][] path)

Constructor, takes a Path of Way Points defined as a double array of column vectors representing the global cartesian points of the path in {x,y} coordinates. The waypoint are traveled from one point to the next in sequence. For example: here is a properly formated waypoint array double[][] waypointPath = new double[][]{ {1, 1}, {5, 1}, {9, 12}, {12, 9}, {15,6}, {15, 4} }; This path goes from {1,1} -> {5,1} -> {9,12} -> {12, 9} -> {15,6} -> {15,4} The units of these coordinates are position units assumed by the user (i.e inch, foot, meters)

Parameters:

path -

Method Detail

print

public static void print(double[] path)

print

public static void print(double[][] path)

Prints Cartesian Coordinates to the System Output as Column Vectors in the Form X Y

Parameters:

path -

doubleArrayCopy

public static double[][] doubleArrayCopy(double[][] arr)

Performs a deep copy of a 2 Dimensional Array looping thorough each element in the 2D array BigO: Order N x M

Parameters:

arr -

Returns:

inject

public double[][] inject(double[][] orig,
                         int numToInject)

Method upsamples the Path by linear injection. The result providing more waypoints along the path. BigO: Order N * injection#

Parameters:

orig -

numToInject -

Returns:

smoother

public double[][] smoother(double[][] path,
                           double weight_data,
                           double weight_smooth,
                           double tolerance)

Optimization algorithm, which optimizes the data points in path to create a smooth trajectory. This optimization uses gradient descent. While unlikely, it is possible for this algorithm to never converge. If this happens, try increasing the tolerance level. BigO: N^x, where X is the number of of times the while loop iterates before tolerance is met.

Parameters:

path -

weight_data -

weight_smooth -

tolerance -

Returns:

nodeOnlyWayPoints

public static double[][] nodeOnlyWayPoints(double[][] path)

reduces the path into only nodes which change direction. This allows the algorithm to know at what points the original WayPoint vector changes. BigO: Order N + Order M, Where N is length of original Path, and M is length of Nodes found in Path

Parameters:

path -

Returns:

injectionCounter2Steps

public int[] injectionCounter2Steps(double numNodeOnlyPoints,
                                    double maxTimeToComplete,
                                    double timeStep)

This method calculates the optimal parameters for determining what amount of nodes to inject into the path to meet the time restraint. This approach uses an iterative process to inject and smooth, yielding more desirable results for the final smooth path. Big O: Constant Time

Parameters:

numNodeOnlyPoints -

maxTimeToComplete -

timeStep -

leftRight

public void leftRight(double[][] smoothPath,
                      double robotTrackWidth)

Calculates the left and right wheel paths based on robot track width Big O: 2N

Parameters:

smoothPath - - center smooth path of robot

robotTrackWidth - - width between left and right wheels of robot of skid steer chassis.

getXVector

public static double[] getXVector(double[][] arr)

Returns the first column of a 2D array of doubles

Parameters:

arr - 2D array of doubles

Returns:

array of doubles representing the 1st column of the initial parameter

getYVector

public static double[] getYVector(double[][] arr)

Returns the second column of a 2D array of doubles

Parameters:

arr - 2D array of doubles

Returns:

array of doubles representing the 1st column of the initial parameter

transposeVector

public static double[][] transposeVector(double[][] arr)

setPathAlpha

public void setPathAlpha(double alpha)

setPathBeta

public void setPathBeta(double beta)

setPathTolerance

public void setPathTolerance(double tolerance)

setVelocityAlpha

public void setVelocityAlpha(double alpha)

setVelocityBeta

public void setVelocityBeta(double beta)

calculate

public void calculate(double totalTime,
                      double timeStep,
                      double robotTrackWidth)

This code will calculate a smooth path based on the program parameters. If the user doesn't set any parameters, the will use the defaults optimized for most cases. The results will be saved into the corresponding class members. The user can then access .smoothPath, .leftPath, .rightPath, .smoothCenterVelocity, .smoothRightVelocity, .smoothLeftVelocity as needed. After calling this method, the user only needs to pass .smoothRightVelocity[1], .smoothLeftVelocity[1] to the corresponding speed controllers on the Robot, and step through each setPoint.

Parameters:

totalTime - - time the user wishes to complete the path in seconds. (this is the maximum amount of time the robot is allowed to take to traverse the path.)

timeStep - - the frequency at which the robot controller is running on the robot.

robotTrackWidth - - distance between left and right side wheels of a skid steer chassis. Known as the track width.

main

public static void main(java.lang.String[] args)

poofExample

public void poofExample()