MecanumPathPlanner

java.lang.Object
- org.usfirst.frc.team1736.lib.FalconPathPlanner.MecanumPathPlanner

```
public class MecanumPathPlanner
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 the four wheels of a mecanum 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 four wheels of a mecanum 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.

Nested Class Summary

Nested Classes
Modifier and Type Class and Description

static class MecanumPathPlanner.MotorType
The location of a motor on the robot for the purpose of determining velocity.

Nested Classes
Modifier and Type	Class and Description
`static class`	`MecanumPathPlanner.MotorType` The location of a motor on the robot for the purpose of determining velocity.

Field Summary

Fields
Modifier and Type	Field and Description
`double[][]`	`heading`
`double[][]`	`leftFrontPath`
`double[][]`	`leftRearPath`
`double[][]`	`nodeOnlyPath`
`double[][]`	`origCenterVelocity`
`double[][]`	`origLeftFrontVelocity`
`double[][]`	`origLeftRearVelocity`
`double[][]`	`origPath`
`double[][]`	`origRightFrontVelocity`
`double[][]`	`origRightRearVelocity`
`double[][]`	`rightFrontPath`
`double[][]`	`rightRearPath`
`double[][]`	`smoothCenterVelocity`
`double[][]`	`smoothLeftFrontVelocity`
`double[][]`	`smoothLeftRearVelocity`
`double[][]`	`smoothPath`
`double[][]`	`smoothRightFrontVelocity`
`double[][]`	`smoothRightRearVelocity`

Constructor Summary

Constructors
Constructor and Description
`MecanumPathPlanner(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 and a third heading coordinate (in degrees).

Method Summary

All Methods Static Methods Instance Methods Concrete Methods
Modifier and Type	Method and Description
`void`	`calculate(double totalTime, double timeStep, double robotTrackWidth, double robotTrackLength)` This code will calculate a smooth path based on the program parameters.
`void`	`calcWheelPaths(double[][] smoothPath, double robotTrackWidth, double robotTrackLength)` Calculates all wheel paths based on robot track width and length Big O: 2N
`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
`void`	`figure8Example()`
`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
`static double[]`	`getZVector(double[][] arr)` Returns the third 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.
`static void`	`main(java.lang.String[] args)`
`static double[][]`	`nodeOnlyWayPoints(double[][] path)` reduces the path into only nodes which change direction.
`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 Z
`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.
`double[][]`	`transformHeadingToGyroFrameOfRef(double[][] input)`
`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
```
  - leftFrontPath
```
public double[][] leftFrontPath
```
  - leftRearPath
```
public double[][] leftRearPath
```
  - rightFrontPath
```
public double[][] rightFrontPath
```
  - rightRearPath
```
public double[][] rightRearPath
```
  - origCenterVelocity
```
public double[][] origCenterVelocity
```
  - origLeftFrontVelocity
```
public double[][] origLeftFrontVelocity
```
  - origLeftRearVelocity
```
public double[][] origLeftRearVelocity
```
  - origRightFrontVelocity
```
public double[][] origRightFrontVelocity
```
  - origRightRearVelocity
```
public double[][] origRightRearVelocity
```
  - smoothCenterVelocity
```
public double[][] smoothCenterVelocity
```
  - smoothLeftFrontVelocity
```
public double[][] smoothLeftFrontVelocity
```
  - smoothLeftRearVelocity
```
public double[][] smoothLeftRearVelocity
```
  - smoothRightFrontVelocity
```
public double[][] smoothRightFrontVelocity
```
  - smoothRightRearVelocity
```
public double[][] smoothRightRearVelocity
```
  - heading
```
public double[][] heading
```
- Constructor Detail
  - MecanumPathPlanner
```
public MecanumPathPlanner(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 and a third heading coordinate (in degrees). The waypoints are traveled from one point to the next in sequence. This is an expansion on the original SmoothPathPlanner for use with a mecanum drive robot. For example: here is a properly formated waypoint array double[][][] waypointPath = new double[][][]{ {1, 1, 0}, {5, 1, 0}, {9, 12, 90}, {12, 9, 90}, {15, 6, 135}, {15, 4, 135} }; This path goes from {1,1} -> {5,1} -> {9,12} -> {12, 9} -> {15,6} -> {15,4} During the transition from {5,1} to {9,12} the robot rotates from a straight heading (relative to its start) to a 90 degree heading. Between position {12,9} and {15,6} the robot rotates to a heading of 135 degrees. 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 Z
    
    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. The direction change can come through either directional or rotational changes of the mecanum drive. 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 -
  - calcWheelPaths
```
public void calcWheelPaths(double[][] smoothPath,
                           double robotTrackWidth,
                           double robotTrackLength)
```
    Calculates all wheel paths based on robot track width and length Big O: 2N
    
    Parameters:
    
    smoothPath - - center smooth path of robot
    
    robotTrackWidth - - width between left and right wheels of robot of mecanum chassis.
    
    robotTrackLength - - length from front to back of robot mecanum 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
  - getZVector
```
public static double[] getZVector(double[][] arr)
```
    Returns the third 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,
                      double robotTrackLength)
```
    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, .leftFrontPath, .leftRearPath, .rightFrontPath, .rightRearPath, .smoothCenterVelocity, .smoothRightFrontVelocity, .smoothLeftFrontVelocity, .smoothRightRearVelocity, .smoothLeftRearVelocity as needed. After calling this method, the user only needs to pass .smoothRightFrontVelocity[1], .smoothRightRearVelocity[1], .smoothLeftFrontVelocity[1], and .smoothLeftRearVelocity[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 mecanum drive chassis. Known as the track width.
    
    robotTrackLength - - distance between front and rear wheels of a mecanum drive chassis. Known as track length.
  - transformHeadingToGyroFrameOfRef
```
public double[][] transformHeadingToGyroFrameOfRef(double[][] input)
```
  - main
```
public static void main(java.lang.String[] args)
```
  - figure8Example
```
public void figure8Example()
```

Class MecanumPathPlanner

Nested Class Summary

Field Summary

Constructor Summary

Method Summary

Methods inherited from class java.lang.Object

Field Detail

origPath

nodeOnlyPath

smoothPath

leftFrontPath

leftRearPath

rightFrontPath

rightRearPath

origCenterVelocity

origLeftFrontVelocity

origLeftRearVelocity

origRightFrontVelocity

origRightRearVelocity

smoothCenterVelocity

smoothLeftFrontVelocity

smoothLeftRearVelocity

smoothRightFrontVelocity

smoothRightRearVelocity

heading

Constructor Detail

MecanumPathPlanner

Method Detail

print

print

doubleArrayCopy

inject

smoother

nodeOnlyWayPoints

injectionCounter2Steps

calcWheelPaths

getXVector

getYVector

getZVector

transposeVector

setPathAlpha

setPathBeta

setPathTolerance

setVelocityAlpha

setVelocityBeta

calculate

transformHeadingToGyroFrameOfRef

main

figure8Example