% 5 jun 2013 - Niceto R. Luque % Direct Kinematics for a three joints robot % 8-shaped trajectory clear all; clc; syms q1 q2 q3 l1 l2 l3 x y z A01=[cos(q1) 0 sin(q1) 0;sin(q1) 0 -cos(q1) 0;0 1 0 l1; 0 0 0 1]; A12=[cos(q2) -sin(q2) 0 l2*cos(q2); sin(q2) cos(q2) 0 l2*sin(q2);0 0 1 0; 0 0 0 1]; A23=[cos(q3) -sin(q3) 0 l3*cos(q3); sin(q3) cos(q3) 0 l3*sin(q3);0 0 1 0; 0 0 0 1]; A03=A01*A12*A23; A03b=simple(A03); x=A03b(1,4); y=A03b(2,4); z=A03b(3,4); step = 0.002; t = 0:step:1.00; A=0.1; % joint positions x1=[]; y1=[]; z1=[]; qt1 =A*sin(2*pi*t); qt2 =A*sin(2*pi*t+pi/4); qt3 =A*sin(2*pi*t+pi/2); % joint velocities qvt1 = 2*pi*A*cos(2*pi*t); qvt2 = 2*pi*A*cos(2*pi*t+pi/4); qvt3 = 2*pi*A*cos(2*pi*t+pi/2); % joint acceleration qat1 = -4*pi^2*A*sin(2*pi*t); qat2 = -4*pi^2*A*sin(2*pi*t+pi/4); qat3 = -4*pi^2*A*sin(2*pi*t+pi/2); lr1 = 0.310; lr2 = 0.4; lr3 = 0.390; x1=subs(x,{q1,q2,q3,l1,l2,l3},{qt1,qt2,qt3,lr1,lr2,lr3}); y1=subs(y,{q1,q2,q3,l1,l2,l3},{qt1,qt2,qt3,lr1,lr2,lr3}); z1=subs(z,{q1,q2,q3,l1,l2,l3},{qt1,qt2,qt3,lr1,lr2,lr3}); figure () plot3(x1,y1,z1); grid on; figure() hold on plot3(qt1,qt2,qt3); grid on