Working set up for trapezoidal shpe speed. Saving of the trajectiry added.