function q_t = TrjPoly3(q0,q1,T,dt)
h = q1-q0;
t = 0:dt:T;
q_t = q0+h*(3*(t/T).^2-2*(t/T).^3);