X0 = Spline3(20, 20, 0, 0, 2);
X1 = Spline3(20, 20, 0, 0, 2);
X2 = Spline3(20, 20, 0, 0, 2);
X = [X0, X1, X2];

Y0 = Spline3(-40, 40,  0, 0, 2);
Y1 = Spline3( 40,  0,  0, 0, 2);
Y2 = Spline3(  0, -40, 0, 0, 2);
Y = [Y0, Y1, Y2];

Z0 = Spline3(20, 20, 0, 0, 2);
Z1 = Spline3(20, 90, 0, 0, 2);
Z2 = Spline3(90, 20, 0, 0, 2);
Z = [Z0, Z1, Z2];

TIP = [X ; Y ; Z];
Q = [];


for i=1:length(TIP)
   q = InversePuma(TIP(:,i));
   T = Puma(q, TIP);
   Q = [Q, q];
   pause(0.01);
   end