# Trajectory Planning

In my robotics class, I had some trouble with a homework on trajectory planning. I made a simple turret mechanism and wrote some code to control it to better understand it.

## The math

The scripts I’ve written implement cubic and quintic path interpolation for joint trajectory planning. Here, q is a generic joint state variable.

Given a series of waypoints in the following format where q_n denotes a joint position and t_n denotes the time at which the joints are at those positions:

We want to generate a continuous set of positions, velocities, and accelerations for each joint over time that will interpolate between the waypoints.

The two methods that we learned in class are cubic (3rd order) and quintic (5th order) interpolating. Here’s the matrix equation that describes a cubic polynomial between two waypoints:

Here’s the matrix for a quintic polynomial:

The premise here is that if we know the joint state (and derivatives) for a single joint at two points in time, we can solve for the polynomial coefficients [a_n] that describe a polynomial tracing a path between that pair of waypoints. Note that in the waypoint list, we do not specify velocity or acceleration, only joint positions at certain times. In order to solve the polynomials, we need to pick our intermediate velocities and accelerations. In class, we defined the intermediate velocities and accelerations as the average velocity and acceleration between two pairs of adjacent waypoints. This is probably a place to improve.

## The math in practice

I won’t post the code in this post due to length, but this is a link to the repo: https://github.com/PAJohnson/Turret

All of the “magic” happens in trajectoryPlanner.py. The basic flow is that a series of waypoints are interpreted, interpolating polynomials are generated between each adjacent pair of waypoints, and the polynomials are sampled over time at the desired interval. The output of the planner is a time series of position, velocity, and acceleration for each joint at each time step. For the demo video, I’m using the joint velocity values to drive the stepper motors in “speed mode.” This isn’t ideal, but it does work! (It’s also the only easy way to drive stepper motors with a Raspberry Pi). Note also that the planner will work with an arbitrary number of joints and waypoints.

What’s interesting here is not that it works (the math guarantees it), but how the different interpolating methods perform. Here’s a plot showing a path:

The red diamonds represent the given waypoints, the green line is the cubic-interpolated path, and the yellow line is the quintic-interpolated path. Here, the motion starts at the rightmost waypoint – (30,0,0). The paths are almost the same! They don’t look very different at all, do they?

The story gets more interesting when we look at the joint states over time. Here’s the position, velocity, and acceleration using cubic interpolation:

Notice how the accelerations are discontinuous! Cubic interpolation only ensures that the velocity is continuous – the acceleration “is what it is.” In practice, this means that your robot must run slower to not exceed torque requirements. Compare the previous plots to quintic interpolation:

The acceleration has a significant amount of ripple, but it is continuous. This results in smoother motion at the cost of longer computation time.

## What’s next?

There are a few things that I’d like to optimize in the trajectory planner. It would be fun to experiment with other interpolating methods like linear interpolation with parabolic blends. I’d also like to try out some optimization techniques to find better intermediate joint state values. Ideally, the planner would accept waypoints without time values and figure out an optimal path while respecting given joint velocity/acceleration limits. This would allow a much smoother “teach” functionality, where the user would move the robot to different positions and record them as a sequence to be repeated.

I’m working on another robot mechanism, a 3DOF arm, to use with this code. My current plan is to leave the trajectory planner as-is and develop some inverse kinematics code. Should be fun!