# Motion Profiling

This last year we experimented with a number of different motion profiling techniques to allow our robot to follow complex paths reliably. In the final analysis the technique we settled on was called **Pure Pursuit** and is described in this **white paper** by R. Craig Coulter. We found this technique gave us the most accurate and reproducible path following.

The basic idea behind the **Pure Pursuit** path following is that at any given point in time we find the point on the desired path that is closest to the current robot’s position. We then look ahead on the path a certain distance and attempt to steer the robot to that position.

The Achilles heel of this technique is that we need to be able to keep track of the robots absolute position on the field. As it turns out, this is actually quite hard to do.

The method that we are currently using is to use the encoders on the drive wheels and the gyro to integrate the robot’s position over time. Every 20 ms or so, we get the current value of the drive wheel encoders and the gyro, and using this information and a little trigonometry compute the change in **x** and **y** during that interval. The problem with this is that we get an accumulated error which increases over time which can end up with fairly large error. However, we were able to get the robot to follow fairly complex paths reasonably reliably. Improving the accuracy of this is one of our primary goals.

Now in order for this all to work, we must have a reasonable path for the robot to follow. We initially used the **Pathfinder** code which can be found **here**. This creates paths using **quintic hermite splines**. While this worked fairly well it had some drawbacks. The biggest issue was that it only computed the path for the center of the robot. If you wanted the path for the wheels of a tank robot, you needed to call a second routine which computed those paths from the generated center path and the width of the robot. The problem with this is that you could not control the speed of the left and right wheels. In particular, if the robot attempted to make tight turns, the speed of the outside wheels often greatly exceeded the capabilities of the robot.

We decided, therefore to create a new pathfinder code which computed all three paths, left, center and right, simultaneously so as to ensure that neither the left or right speed exceeded whatever maximum velocity is set. In doing so, we also decided to switch from using 5th order splines to 5th order **Bezier** curves. This switch gave us the ability to more easily control the shape of the curve which was important to control the maximum acceleration of the wheels that was required for the robot to follow the desired path.

We also created a Java application which allowed you to create paths and display them graphically. The source (as a Eclipse project) for this application can be found in the **Pathplanner** folder of the **FRCTools2020 **which can be downloaded **here**. The remainder of this article will describe how to use this **Path Planner** tool.

The paths are constructed by a series of 5th order Bezier curves. The parametric curve is defined by 6 control points, P0, P1, P2, P3, P4 and P5. Only the first and last points lie on the curve, the other points control the shape of the curve.

When two curves of the path are joined, we want the slope (i.e. first derivative) of the curves to match. To achieve this we find that the last two points of the first curve (P4 and P5) must be co-linear with the first two points of the next curve (P0 and P1). In addition, we need the velocity of the left and right wheels to be continuous as well. In order to achieve this, the second derivative of the curves must also match at the join point. This requires that the last 3 points of the first curve (P3, P4, and P5) be co-linear with the first three points of the second curve (P0, P1, and P2). This constrains our choices for the values of the control points but still gives us the ability control the shape of the curves.

A pre-built version of the **PathPlanner** program can also be found in the **FRCTools2020** that you can download from the link above. Double clicking on the **PathPlanner.cmd** file opens the following screen:

The fields in the lower left have the following meanings:

**# Points**– Specifies the number of points that should be generated along each bezier curve. 1000 is a reasonable number here.**DT**– Specifies the time between points generated for the final output**Max Velocity**– Specifies the default maximum velocity for the robot in whatever units are convenient (e.g. feet/sec).**Max Accel**– Specifies the maximum acceleration for the robot in whatever units are convenient (e.g. feet/sec*sec).**Max Jerk**– Specifies the maximum jerk for the robot in whatever units are convenient (e.g. feet/sec*sec*sec).**Wheel Base**– Specifies the distance between the drive wheels of the tank drive robot in whatever units are convenient (e.g. feet)

In the **Waypoints** edit field we will enter the waypoints of our path. There are a number of formats for these values. The simplest consists of the starting point, angle in the following format:

**x, y, angle**

Using this format, the other bezier control points will be automatically generated. For example if we want to create a path that goes from (0,0) pointing up (90 degrees) to (10,10) pointing right (0 degrees) we would enter the following:

To see the generated path, click the **Plot** button:

Here I have marked the position of the 6 Bezier control points, P0, P1, P2, P3, P4, and P5. Note that when you use this format, the points P1 and P2 will always be coincident, as will the points P3 and P4. Also the distance the points P1, P2, P3 and P4 are from their respective end points will be automatically calculated based on the length of the path.

Once you have plotted the curve you can see the required velocities for the center and left and right wheels by clicking the **Velocity** radio button:

You can also see the acceleration that will be required by clicking the **Acceleration** radio button:

Now using the basic format to specify the curve endpoints is quick and easy, but we also want to control the position the other four Bezier control points (P1, P2, P3 and P4). In order to maintain the first and second derivatives, these points are constrained to a line that is tangent to the curve at the endpoints. However, the distance of these control points from the endpoints can be controlled and will affect the shape of the curve and the resultant velocities and accelerations required.

We can specify the distance from the endpoints by using 5 values for each line instead of 3. For example if we enter the following into the **Waypoints** field and display the curve we will see the following:

Now you can see that the control points have moved out and this produces a much tighter curve. If we look at the velocity curves we see:

You can see that this tighter curve requires a much greater velocity change for the middle of the path. Looking at the acceleration curves we see:

As you might expect, much larger acceleration is required in the center of the path. If you make the curves too tight, then the required acceleration can easily exceed the capabilities of the robot.

Now to add additional components to the path, we only need to add additional lines. For example:

Here the path starts at position **(-10,0)** moves to position **(0,10)** and then ends at position **(10,20)**.

Now sometimes it is desirable to use different speeds for different pieces of the path. For example, if you need the robot to execute a particularly tight curve you may need to lower the speed in order to keep the required accelerations within reason. To accommodate this, you can add an optional parameter which specifies the speed for that portion of the path. For example, consider the following:

This path contains the same control points but we have added the **3** parameter to the end of the second line. Notice that the curve itself looks the same (as it should), but if we view the velocities we see:

Here we see that the velocity of the robot has been reduced to **3** for the second portion of the path.

It is also possible to specify different positions for the **P1** and **P2** control points as well as different values for the **P3** and **P4** control points. To do that we add 2 more number to the path line. For example:

As you can see for this new curve, **P1** and **P2** are no long coincident, as are **P3** and **P4**. Although you can use this to get even greater control over the shape of the curve, we have found that this is probably not necessary.

To summarize, the following are the possible formats that can be used to specify the waypoints:

- x,y,angle
- x,y,angle,speed
- x,y,angle,p12,p34
- x,y,angle,p12,p34,speed
- x,y,angle,p1,p4,p2,p3
- x,y,angle,p1,p4,p2,p3,speed

To use the **Pathfinder** code in your program, you should add the **pathfinder** folder located in the **RobotTools\RaspberryPi\workspacePi2018\Source** to your robot’s project. You can then use the **Pathfinder.computePath** function to generate the needed paths. For example to generate the two piece path with the speed change shown above, you would make the following calls:

Waypoint[] waypoints = new Waypoint[3]; waypoints[0] = new Waypoint(-10,0,90*Math.PI/180,5,5,0); waypoints[1] = new Waypoint(0,10,0,5,5,3); waypoints[2] = new Waypoint(10,20,90*Math.PI/180); Path path = Pathfinder.computePath(waypoints, 1000, 0.02, 5, 5, 50, 2.6);

This will return an instance of **Path** which will contain three arrays which describe the motion of the robot over time. Complete documentation can be found here.