function [P]=Newton(th)

th1=th(1:3);
th2=th(4:6);
th3=th(7:9);
th4=th(10:12);
th5=th(13:15);
%definicion de
sf6=[0 0 0];
sn6=[0 0 0];
%alfa
dena=[ pi/2,
0,
-pi/2,
pi/2,
-pi/2,

fuerzas en el extremo
a th
0, th1,
680, th2,
0, th3,
0, th4,
200, th5,

d
675;
0;
0;
675;
0];

i=1;
R01=[cos(dena(i,3)), -cos(dena(i,1))*sin(dena(i,3)), sin(dena(i,1))*sin(dena(i
,3));
sin(dena(i,3)),cos(dena(i,1))*cos(dena(i,3)),-sin(dena(i,1))*cos(dena(i,3))
;
0,sin(dena(i,1)),cos(dena(i,1))];
i=2;
R12=[cos(dena(i,3)), -cos(dena(i,1))*sin(dena(i,3)), sin(dena(i,1))*sin(dena(i
,3));
sin(dena(i,3)),cos(dena(i,1))*cos(dena(i,3)),-sin(dena(i,1))*cos(dena(i,3))
;
0,sin(dena(i,1)),cos(dena(i,1))];
i=3;
R23=[cos(dena(i,3)), -cos(dena(i,1))*sin(dena(i,3)), sin(dena(i,1))*sin(dena(i
,3));
sin(dena(i,3)),cos(dena(i,1))*cos(dena(i,3)),-sin(dena(i,1))*cos(dena(i,3))
;
0,sin(dena(i,1)),cos(dena(i,1))];
i=4;
R34=[cos(dena(i,3)), -cos(dena(i,1))*sin(dena(i,3)), sin(dena(i,1))*sin(dena(i
,3));
sin(dena(i,3)),cos(dena(i,1))*cos(dena(i,3)),-sin(dena(i,1))*cos(dena(i,3))
;
0,sin(dena(i,1)),cos(dena(i,1))];
i=5;
R45=[cos(dena(i,3)), -cos(dena(i,1))*sin(dena(i,3)), sin(dena(i,1))*sin(dena(i
,3));
sin(dena(i,3)),cos(dena(i,1))*cos(dena(i,3)),-sin(dena(i,1))*cos(dena(i,3))
;
0,sin(dena(i,1)),cos(dena(i,1))];
%parametros iniciales
w0=[0,0,0]';
dw0=[0,0,0]';
v0=[0 0 0]';
dv0=-[0 -9.8 0]';
z0=[0 0 1]';
l1=675;
l2=680;
l3=675;
l4=200;
m1=1;

m2=1;
m3=1;
m4=1;
m5=1;
%obtencion de la posicion, velocidad, aceleracion
q1=th1(1);qd1=th1(2);qdd1=th1(3);
q2=th2(1);qd2=th2(2);qdd2=th2(3);
q3=th3(1);qd3=th3(2);qdd3=th3(3);
q4=th4(1);qd4=th4(2);qdd4=th4(3);
q5=th5(1);qd5=th5(2);qdd5=th5(3);
%evalucion nuemrica de la fuerzas y pares en el TCP
n6=[0 0 0]';
f6=[0 0 0]';
%
%

n_6(tk,:)=n6';
f_6(tk,:)=f6';
%PASO evalucion de los vectores p, s de la matriz de inercias
p1=[0 0 l1]; %coordenadas de {S0} a {S1} en el sistema {S1}
p2=[l2 0 0]; %coordenadas de {S1} a {S2} en el sistema {S2}
p3=[0 0 0];
p4=[0 0 l3];
p5=[l4 0 0];
s1=[0 0 -l1/2];
s2=[-l2/2 0 0];
s3=[0 0 0];
s4=[0 0 -l3/2];
s5=[-l4/2 0 0];
I1=zeros(3);
I2=zeros(3);
I3=zeros(3);
I4=zeros(3);
I5=zeros(3);
%PASO 3 Obtencion del as matrices de rotacion
%R01 R12 R23 R34 R45
%PASO 4 Obtencion de las velocidades angulares de {Si}
w1=R01'*(w0+z0*qd1);
w2=R12'*(w1+z0*qd2);
w3=R23'*(w2+z0*qd3);
w4=R34'*(w3+z0*qd4);
w5=R45'*(w4+z0*qd5);
%PASO 5 Obtenacion de las aceleraciones angulares
dw1=R01'*(dw0+z0*qdd1)+cross(w0,z0*qd1);
dw2=R12'*(dw1+z0*qdd2)+cross(w1,z0*qd2);
dw3=R23'*(dw2+z0*qdd3)+cross(w2,z0*qd3);
dw4=R34'*(dw3+z0*qdd4)+cross(w3,z0*qd4);
dw5=R45'*(dw4+z0*qdd5)+cross(w4,z0*qd5);
%PASO 6 Obtencion de las aceleraciones angulares {Si}
dv1=cross(dw1,p1')+cross(w1,cross(w1,p1)')+R01'*dv0;
dv2=cross(dw2,p2')+cross(w2,cross(w2,p2)')+R12'*dv1;
dv3=cross(dw3,p3')+cross(w3,cross(w3,p3)')+R23'*dv2;
dv4=cross(dw4,p4')+cross(w4,cross(w4,p4)')+R34'*dv3;

dv5=cross(dw5,p5')+cross(w5,cross(w5,p5)')+R45'*dv4;
%PASO 7 Evaluacion d ela aceleracion lineal de los cdg
a1=cross(dw1,s1')+cross(w1,cross(w1,s1'))+dv1;
a2=cross(dw2,s2')+cross(w2,cross(w2,s2'))+dv2;
a3=cross(dw3,s3')+cross(w3,cross(w3,s3'))+dv3;
a4=cross(dw4,s4')+cross(w4,cross(w4,s4'))+dv4;
a5=cross(dw5,s5')+cross(w5,cross(w5,s5'))+dv5;
%Matriz de rotacion entre{S5} y TCP({S6})
R56=eye(3);
%PASO 8 Evaluacion de las fuerzas sobre los eslabones
f5=R56*f6+m5*a5;
f4=R45*f5+m4*a4;
f3=R34*f4+m3*a3;
f2=R23*f3+m2*a2;
f1=R12*f2+m1*a1;
%PASO 9 Evaluacion de los pares sobre los eslabones
n5=R56*(n6+cross((R56'*p5'),f6))+cross((p5'+s5'),m5*a5)+I5*dw5+cross(w5,(I5*w5
));
n4=R45*(n5+cross((R45'*p4'),f5))+cross((p4'+s4'),m4*a4)+I4*dw4+cross(w4,(I4*w4
));
n3=R34*(n4+cross((R34'*p3'),f4))+cross((p3'+s3'),m3*a3)+I3*dw3+cross(w3,(I3*w3
));
n2=R23*(n3+cross((R23'*p2'),f3))+cross((p2'+s2'),m2*a2)+I2*dw2+cross(w2,(I2*w2
));
n1=R12*(n2+cross((R12'*p1'),f2))+cross((p1'+s1'),m1*a1)+I1*dw1+cross(w1,(I1*w1
));
%PASO Evaluacion de los pares
P(1)=n1'*R01'*z0;
P(2)=n2'*R12'*z0;
P(3)=n3'*R23'*z0;
P(4)=n4'*R34'*z0;
P(5)=n5'*R45'*z0;