Added plotting

Added plotting for joint angle, velocity, and acceleration.  Created own
linear interpolation function