function xp = robot3gdl(t,x)
q1=x(1); q2=x(2); q3=x(3); q = [q1; q2; q3]; %vector de posición articular
qp1=x(4); qp2=x(5); qp3=x(6); qp = [qp1; qp2; qp3]; %vector de velocidad articular
Iz1=1.26; Iz2=0.084; Iz3=0.056; Iy1=0.089; Iy2=0.003; Iy3=0.0012; Ix1=0.03; Ix2=0.05; Ix3=0.009; m1=26.902; l1=0.45; b1=2.288; 
fc1=7.17; fe1=8.8; m2=30; l2=0.45; lc2=0.038; b2=0.2; 
fc2=1.9; fe2=2.1; m3=3.880; l3=0.45; lc3=0.048; b3=0.175; fc3=1.734; fe3=1.87;   

m11=Iy2*sin(q2)*sin(q2)+Iy3*sin(q2+q3)*sin(q2+q3)+Iz1+Iz2*cos(q2)*cos(q2)+Iz3*cos(q2+q3)*cos(q2+q3)+m2*lc2*lc2*cos(q2)*cos(q2)+m3*(l2*cos(q2)+lc3*cos(q2+q3))*(l2*cos(q2)+lc3*cos(q2+q3));
m12=0; m13=0; m21=0; m22=Ix2+Ix3+m3*l2*l2+m2*lc2*lc2+m3*lc3*lc3+ 2*m3*l2*lc3*cos(q3); 
m23=Ix3+m3*lc3*lc3+m3*l2*lc3*cos(q3);
m31=0; m32=Ix3+m3*lc3*lc3+m3*l2*lc3*cos(q3); m33=Ix3+m3*lc3*lc3;

M=[m11, m12, m13; m21, m22, m23; m31, m32, m33];

gamma112=(Iy2-Ix2-m2*lc2*lc2)*cos(q2)*sin(q2)+(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3)-m3*(l2*cos(q2)+lc3*cos(q2+q3))*(l2*sin(q2)+lc3*sin(q2+q3));
gamma113=(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3)-m3*lc3*sin(q2+q3)*(l2*cos(q2)+lc3*cos(q2+q3));
gamma121=(Iy2-Iz2-m2*lc2*lc2)*cos(q2)*sin(q2)+(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3)-m3*(l2*cos(q2)+lc3*cos(q2+q3))*(l2*sin(q2)+lc3*sin(q2+q3));
gamma131=(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3)-m3*lc3*sin(q2+q3)*(l2*cos(q2)+lc3*cos(q2+q3)); 
gamma211=(Ix2-Iy2+m2*lc2*lc2)*cos(q2)*sin(q2)+ (Iz3-Iy3)*cos(q2+q3)*sin(q2+q3)+m3*(l2*cos(q2)+lc3*cos(q2+q3))* (l2*sin(q2)+lc3*sin(q2+q3)); 
gamma223=-l2*m3*lc3*sin(q3); 
gamma232=-l2*m3*lc3*sin(q3); 
gamma233=-l2*m3*lc3*sin(q3);
gamma311=(Iz3-Iy3)*cos(q2+q3)*sin(q2+q3)+m3*lc3*sin(q2+q3)*(l2*cos(q2)+lc3*cos(q2+q3));
gamma322=l2*m3*lc3*sin(q3);

c11=gamma112*qp2+gamma113*qp3; c12=gamma121*qp1; c13=gamma131*qp1;
c21=gamma211*qp1; c22=gamma223*qp3; c23=gamma232*qp2+gamma233*qp3;
c31=gamma311*qp1; c32=gamma322*qp2; 
c33=0; 
C=[c11, c12, c13; c21, c22, c23; c31, c32, c33];  
gq11=0; gq21=(lc2*m1++m2*l2)*sin(q1)+m2*lc3*sin(q1+q2);
 gq31=m2*lc3*sin(q1+q2);g=9.81;  gq=g*[gq11; gq21; gq31]; 
 fr=[b1*qp1; b2*qp2; b3*qp3];
 
  
qd=[45*pi/180; 45*pi/180; 90*pi/180]; 
qt=[qd(1)-q1;qd(2)-q2; qd(3)-q3];

Kp=[5, 0,0;0, 5,0; 0,0,5]; 
Kv=[3, 0,0;0, 3,0;0,0,3]; 
tau=Kp*qt-Kv*qp+gq; 

qpp = inv(M)*(tau-C*qp-gq-fr); %aceleración articular
xp = [qp1; qp2; qp3; qpp(1); qpp(2); qpp(3)]; %vector de salida
end