Implement Trajectories with ROS

Let's start from the specifics of the robots, i.e. looking at the max velocity for each joint:

Equations to compute

Here are taken from the lecture slides:

Now, you will implement a trapezoidal velocity interpolator

This will generate the position and velocity trajectories to move from point A to point B, so that the position is smooth during trajectory.

1. open the script src/week5/scripts/trajectory.py in the editor.
2. Implement the Trapezoidal trajectory as described in the lecture. The code takes in input the initial and final joint position $q_0$ and $q_f$, the total time $t_f$, the maximum velocity $qd_max$ and the number of interpolation points we want to use.
3. Once you have finished, you can run the code by executing: python3 src/week5/scripts/trajectory.py. This will plot the joint values and velocities of your trajectory.
4. If you are satisfied, you can test it on the real robot by a. resetting the arm_7_joint position equal to 0.0 from the robot web interface. b. running: python3 src/week5/scripts/trajectory.py exec.
5. Compare the trajectory you have defined with the one that is actually executed by the controller.