% XY Control
 P0 = [0.5; 0];
 P1 = [1.5; 0];
 P2 = [1.5; 1];
 P3 = [0.5; 1];
 P4 = P0;
 X1 = spline1(P0, P1, 2);
 X2 = spline1(P1, P2, 2);
 X3 = spline1(P2, P3, 2);
 X4 = spline1(P3, P0, 2);

 Xr = [X1, X2, X3, X4];
 dXr = derivative(Xr);
 ddXr = derivative(dXr);
 
 TIP = Xr;
 
 Q = InverseRR(Xr(:,1));
 dQ = [0; 0];
 t = 0;
 dt = 0.001;

 % Start the simulation (dt = 0.001 for stability concerns)
 Xq = [];
 Tq = [];
 TIPxy = [];

 for i=1:length(Xr)
    Qr = InverseRR(Xr(:,i));
    for j=1:10
       X = [ cos(Q(1)) + cos(Q(1)+Q(2));
             sin(Q(1)) + sin(Q(1)+Q(2)) ];
       J = [ -(sin(Q(1)) + sin(Q(1)+Q(2))), -sin(Q(1)+Q(2)) ;
              (cos(Q(1)) + cos(Q(1)+Q(2))), cos(Q(1)+Q(2)) ];
       dX = J * dQ;

% Control Law and Feedforward Terms
       Facc = ddXr(:,i);
       Fpid = 40*(Xr(:,i) - X) + 14*(dXr(:,i) - dX );

 % gravity
       Tg = -9.8 * [ 4*cos(Q(1)) + 2*cos(Q(1) + Q(2));  2*cos(Q(1) + Q(2)) ];

       T = (J') * ( Fpid + Facc ) - Tg;

       ddQ = RR_Dynamics(Q, dQ, T);
       dQ = dQ + ddQ * dt;
       Q = Q + dQ*dt;
       t = t + dt;
       end

    T = RR(Q, Qr, TIP);
    TIPxy = [TIPxy, T];

    Xq = [Xq, X];
    Tq = [Tq, T];
 end
 plot(TIPxy(1,:),TIPxy(2,:), 'c');
 
 pause(5);
 
 t = [1:length(Xr)] * 0.01;
 clf
 subplot(211)
 plot(t,Xq,t,Xr);
 xlabel('Time (seconds)');
 ylabel('Tip (meters)');
 subplot(212)
 plot(t,Tq);
 xlabel('Time (seconds)');
 ylabel('Torque (Nm)');