Trajectory Planning with Trapezoidal Velocity Interpolator

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. ▶️ 🧑‍💻 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. ▶️ 🔎 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. ▶️ 🧑‍💻 Go to the function generate_trapezoidal_segment and complete the missing parts of the code (i.e. the parts with ## TODO).

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

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

   • ℹ️ 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. ▶️ 🧑‍💻 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} &lt; \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. ▶️ 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. ▶️ Modify the generate_trapezoidal_segment function to correctly handle this case updating $t_f$ when needed.

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

5. ▶️ 🧑‍💻 Modify the code to also compute and plot the acceleration for each segment of the trajectories.