% 2-Link robot in free-fall (T=0) % rev 6/1/18 % ECE 463 Lecture #9 Q = [0; 0]; dQ = [0; 0]; T = [0; 0]; t = 0; dt = 0.01; while(t<10) c1 = cos(Q(1)); s1 = sin(Q(1)); c2 = cos(Q(2)); s2 = sin(Q(2)); % Freefall T = [-0*dQ(1);-0*dQ(2)]; ddQ = TwoLinkDynamics(Q, dQ, T); dQ = dQ + ddQ * dt; Q = Q + dQ*dt; t = t + dt; % Plot the Robot x0 = 0; y0 = 0; x1 = cos(Q(1)); y1 = sin(Q(1)); x2 = x1 + cos(Q(1) + Q(2)); y2 = y1 + sin(Q(1) + Q(2)); clf; plot([-2,2],[-2,2],'x'); hold on; plot([-2,2],[0,0], 'r'); plot([0,0],[-2,2], 'r'); plot([y0, y1, y2], [x0, x1, x2], 'b.-'); pause(0.01); end