I am trying to write code in MATLAB to generate a trajectory for a scara manipulator in the robot operational space with trapezoidal velocity profile for each segment passing through 5 waypoints using the formulas shown in the attached image.
They are for creating a trajectory in operational space for each segment passing through multiple points. I am not sure how to implement $p_j - p_{j-1}$ since Matlab starts from index 1 and gives error on accessing previous index. Also how should one define $s'_j(t)$ for $t_{j-1} < t < t_j$ , for $t_0$ $s'_j(t) = 0$ and for $t_f$, $s'_j(t)$ becomes $||p_j - p_{j-1}||$ when the sampling time is 0.001 seconds.
If anybody has worked on trajectory planning and coded in MATLAB please can you help me with this, this image has been taken from one of the ppts shared by my Masters Professor but it can be found in the book -
Robotics: Modelling, Planning and Control by B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Chapter 4 Trajectory Planning. The book can be found on springer if anybody wants to check out, thank you