Task 1 - Implement a Trapezoidal Velocity Interpolator trajectory

In this task, you will implement a trajectory planning algorithm between a series of cartesian points such that each trajectories between points follow a three-segment path of acceleration, constant velocity, and deceleration. This leads to a trapezoidal velocity profile, and a “linear segment with parabolic blend” (LSPB) position profile, as shown in the lecture:

1. :arrow_forward: :technologist: Open the script week9/trapezoidal_trajectory.py and go to the function plan_cartesian_trajectory(self) which contains a template for solving the task.

2. :arrow_forward: :mag_right: explore how the function is already implemented. It defines the cartesian points waypoint, a maximum velocity pdmax and the time for completing each segment of the trajectory time. It then calls the function generate_trapezoidal_segment for each dimension (X, Y and Z) and each pair of points in order to generate the wanted trajectories. Finally, it plots all the generated position and velocity profiles.

3. :arrow_forward: :technologist: Go to the function generate_trapezoidal_segment and complete the missing parts of the code (i.e. the parts with ## TODO).

   • :information_source: To make debugging easier, you can start with only two points (i.e. temporarily removing the third waypoint).

   • :information_source: Remember that if $\dot{p}_{max} \geq 2\cdot \frac{p_f-p_0}{t_f}$ you need to use a triangular profile.

   • :information_source: Remember the trajectory is split into 3 segments as follows:

   p(t)=\begin{cases}
          p_0 + \frac{1}{2} \ddot{p}_{max} t^2, & \text{if}\;\; t \leq t_b\\
          p_0 + \frac{1}{2} \ddot{p}_{max} t_b^2 + \dot{p}_{max} (t - t_b), & \text{if}\;\; t_b < t \leq t_f - t_b\\
          p_f - \frac{1}{2} \ddot{p}_{max} (t_f -t)^2, & \text{if}\;\; t_f - t_b < t \leq t_f 
   \end{cases}

4. :arrow_forward: :technologist: Now let's add a fourth waypoint at the end of our waypoints list equal to [0.1, 0.1, 0.0], i.e. equal to the first point, so that we generate a circular motion and our robot's end-effector returns back to the original position. NOTE: that in this case $\dot{p}_{max} < \frac{p_f - p_0}{t_f}$ which means that we cannot reach the goal in the given time within the velocity limits, therefore we need to adjusting the time for computing the last segment.

   a. :arrow_forward: Can you come up with an equation that computes the time needed to reach the final position with trapezoidal velocity and the given $\dot{p}_{max}$?

   b. :arrow_forward: Modify the generate_trapezoidal_segment function to correctly handle this case updating $t_f$ when needed.

   :arrow_forward: If you have done everything correctly, at the end you will be generating a plot like the following:

5. :arrow_forward: :technologist: Modify the code to also compute and plot the acceleration for each segment of the trajectories.