Finish test script for joint velocity control

[Woolfrey]

Writing an executable for a numerical simulation to test the track_joint_trajectory() function in the SerialKinematicControl class.

[Woolfrey]

Here are intermediate results:

Note the tracking error on joint 2 - probably because it hit a joint limit (which means the controller is working properly!

It's probably also worth generating graphs for:

• [x] Normalised joint positions, over time
• [x] Normalised joint velocities, over time

[Woolfrey]

Completed. Plots of performance results for the SerialKinematicControl::track_joint_trajectory() are below.

Take note of:

1. Joint 2 tracking error in Fig 1:
   • It hits a joint limit, as can be seen in Fig 2, and simultaneously:
   • The joint velocity decelerates to zero, as seen in Fig 3.
2. Joint 5 tracking error in Fig 1: it hits a speed limit, as can be seen in Fig 3.