function [Q] = MoveTo(P0, P1, T) % move from point P0 to P1 in T seconds t = [0:0.01:T]; a = (1 - cos(pi*t/T))/2; TIP = P0 * (1-a) + P1 * a; Q = []; for i=1:length(TIP) Qi = InversePuma(TIP(:,i)); Q = [Q, Qi]; Puma(Qi, TIP); end end