Off[General::"spell1",General::"spell"];
Clear[γ];
(*Matrices de rotacion*)
Qx[θx_]:={{1,0,0,0},{0,Cos[θx],-Sin[θx],0},{0,Sin[θx],Cos[θx],0},{0,0,0,1}}
Qy[θy_]:={{Cos[θy],0,Sin[θy],0},{0,1,0,0},{-Sin[θy],0,Cos[θy],0},{0,0,0,1}}
Qz[θz_]:={{Cos[θz],-Sin[θz],0,0},{Sin[θz],Cos[θz],0,0},{0,0,1,0},{0,0,0,1}}
(*MATRICES DE TRASLACION*)
Tx[x_]:={{1,0,0,x},{0,1,0,0},{0,0,1,0},{0,0,0,1}}
Ty[y_]:={{1,0,0,0},{0,1,0,y},{0,0,1,0},{0,0,0,1}}
Tz[z_]:={{1,0,0,0},{0,1,0,0},{0,0,1,z},{0,0,0,1}}
Txyz[x_,y_,z_]:={{1,0,0,x},{0,1,0,y},{0,0,1,z},{0,0,0,1}}

(*TRANSFORMACION A 3D, funciones auxiliares de transformacion a puntos de 3 coordenadas*)
T3D[p_]:={p[[1]],p[[2]],p[[3]]};
T3D1[p_]:={p[[1]],p[[2]],p[[3]]+3};
(*Matrix"n"*)
n={0,0,0,1};

Ri=Txyz[k,l,m];(*Traslacion al centro del manipulador*)
k=-10; (*traslacion en x *)
l=-1; (*traslacion en y*)
m=-10; (*traslacion en z*)
d=60; (*medida de centro de la base a primer junta*)
e=110; (*medida eslabon primario*)
f=200; (*medida eslabon secundario*)
g=30; (*medida inccesaria de planteamiento sin base rotatoria*)
l1=21.4; (*Medida de eslabon de base movil*)
l2=21.4; (*medida de eslabon de base movil*)

(*Primera cadena*)
Rd1=Tx[d];                                (*Asumiendo un sistema de ejes coordenados que coinciden con el sistema Si, solo se tiene que trasladar*)
Re1=Qy[-θa1].Tx[e];           (*Se hace una rotacion sobre Y y una traslacion en el primer eslabon*)
Rf1=Qy[-θa1].Qy[-θa2].Qz[θa3].Tx[f];  (*Se hacen dos giros, uno en Y y otro en Z, previamente se giro en Y para acomodar los nuevos giros y una traslacion sobre el segundo eslabon*)

Rff1=Qz[135*Degree].Tx[l2/2];         (*Dado que la base es rotatoria un angulo fijo ya no era posible, por eso mediante un analisis en el cuaderno se llego a dos vectores, uno fijo*)
Rgg1=Qz[(225*Degree)-γ].Tx[l1/2];  (*Y otro movil, con el angulo de roatcion que tendra*)

R1g=(Ri+Rd1+Re1+Rf1+Rff1).n;

Rg1=Rff1+Rgg1;
(*Se generan los puntos con los vectores*)

Riv=Txyz[k,l,m].n;

Rid1=(Ri+Rd1).n;
Rie1=(Ri+Rd1+Re1).n;
Rif1=(Ri+Rd1+Re1+Rf1).n;
Rig1=(Ri+Rd1+Re1+Rf1+Rg1).n;

Rigv1=(Ri+Rd1+Re1+Rf1+Rg1).n;

R1=(Ri+Rd1+Re1+Rf1+Rg1).n//Simplify//MatrixForm

(*Segunda cadena*) (*ANALOGAMENTE SE HARA LO MISMO CON LAS CADENAS RESTANTES, GIRANDO 90 GRADOS CADA NUEVA CADENA*)

Rd2=Qz[90*Degree].Tx[d];
Re2=Qz[90*Degree].Qy[-θb1].Tx[e];
Rf2=Qz[90*Degree].Qy[-θb1].Qy[-θb2].Qz[θb3].Tx[f];

Rff2=Qz[315*Degree].Tx[l2/2];
Rgg2=Qz[(225*Degree)-γ].Tx[l1/2];

Rg2=Rff2+Rgg2;

Rid2=(Ri+Rd2).n;
Rie2=(Ri+Rd2+Re2).n;
Rif2=(Ri+Rd2+Re2+Rf2).n;
Rig2=(Ri+Rd2+Re2+Rf2+Rg2).n;

Rigv2=(Ri+Rd2+Re2+Rf2+Rg2).n;

R2=(Ri+Rd2+Re2+Rf2+Rg2).n//Simplify//MatrixForm

(*Tercera cadena*)

Rd3=Qz[180*Degree].Tx[d];
Re3=Qz[180*Degree].Qy[-θc1].Tx[e];
Rf3=Qz[180*Degree].Qy[-θc1].Qy[-θc2].Qz[θc3].Tx[f];

Rff3=Qz[315*Degree].Tx[l2/2];
Rgg3=Qz[(45*Degree)-γ].Tx[l1/2];

R3g=(Ri+Rd3+Re3+Rf3+Rff3).n;

Rg3=Rff3+Rgg3;

Rid3=(Ri+Rd3).n;
Rie3=(Ri+Rd3+Re3).n;
Rif3=(Ri+Rd3+Re3+Rf3).n;
Rig3=(Ri+Rd3+Re3+Rf3+Rg3).n;

Rigv3=(Ri+Rd3+Re3+Rf3+Rg3).n;

R3=(Ri+Rd3+Re3+Rf3+Rg3).n//FullSimplify//MatrixForm

(*Cuarta cadena*)

Rd4=Qz[270*Degree].Tx[d];
Re4=Qz[270*Degree].Qy[-θd1].Tx[e];
Rf4=Qz[270*Degree].Qy[-θd1].Qy[-θd2].Qz[θd3].Tx[f];

Rff4=Qz[135*Degree].Tx[l2/2];
Rgg4=Qz[(45*Degree)-γ].Tx[l1/2];

Rg4=Rff4+Rgg4;

Rid4=(Ri+Rd4).n;
Rie4=(Ri+Rd4+Re4).n;
Rif4=(Ri+Rd4+Re4+Rf4).n;
Rig4=(Ri+Rd4+Re4+Rf4+Rg4).n;

Rigv4=(Ri+Rd4+Re4+Rf4+Rg4).n;


R4=(Ri+Rd4+Re4+Rf4+Rg4).n//FullSimplify//MatrixForm

(*Definicion de cada cadena para el putno final*)



θa1i=91*Degree;
θa2i=91*Degree;
θa3i=0*Degree;

θb1i=91*Degree;
θb2i=91*Degree;
θb3i=0*Degree;

θc1i=91*Degree;
θc2i=91*Degree;
θc3i=0*Degree;

θd1i=91*Degree;
θd2i=91*Degree;
θd3i=0*Degree;

γi=-90;
γ1=-60*Degree;
γ2=60*Degree;
γ11=0*Degree;
γ22=60*Degree;

pi={xi,yi,zi};

xi=5;
yi=-15;
zi=150;
xf=-50;
yf=-50;
zf=200;

Clear[γ];
tf=60;
Angulo=0°;
Increm=10/100;
(*Se dan valores de rotacion a la base movil*)


(*For[j=50,j≤100,j+=1;γγ[j]=γ11+(γ22-γ11)*(j/50);]*)  (*Se realiza un metodo numerico para encontrar los valores de las articulaciones*)

For[j = 0,j <= 60, j += 1,
x[j] =pi[[1]]+0.05*(292.125 Cos[Angulo]-8.6 Cos[2. Angulo]+64.4 Cos[3. Angulo]-23.4 Cos[4. Angulo]-5.2 Cos[5. Angulo]-7.66667 Cos[6. Angulo]+35.75 Cos[7. Angulo]-2.75 Cos[8. Angulo]-10.3333 Cos[9. Angulo]-3.25 Cos[10. Angulo]-4.5 Cos[11. Angulo]+2.05 Cos[12. Angulo]+8. Cos[13. Angulo]+0.666667 Cos[14. Angulo]+6. Cos[15. Angulo]+4.25 Cos[16. Angulo]-1.5 Cos[17. Angulo]-2.9 Cos[18. Angulo]+1.83333 Cos[19. Angulo]+2.4 Cos[20. Angulo]+1.5 Cos[21. Angulo]+0.916667 Cos[22. Angulo]-0.8 Cos[23. Angulo]+Cos[24. Angulo]+2.125 Cos[25. Angulo]-3.5 Cos[26. Angulo]-0.833333 Cos[27. Angulo]-1.1 Cos[28. Angulo]+0.5 Cos[29. Angulo]-0.2 Cos[30. Angulo]-180.25 Sin[Angulo]+65.3333 Sin[2. Angulo]-28.6667 Sin[3. Angulo]-65.5 Sin[4. Angulo]+34.0714 Sin[5. Angulo]+27. Sin[6. Angulo]-14.5 Sin[7. Angulo]+13.6 Sin[8. Angulo]+0.1 Sin[9. Angulo]+5.75 Sin[10. Angulo]-9.5 Sin[12. Angulo]-4.04762 Sin[13. Angulo]+0.666667 Sin[14. Angulo]+5.4 Sin[15. Angulo]+1.75 Sin[16. Angulo]+1.88889 Sin[17. Angulo]-4. Sin[18. Angulo]-0.5 Sin[19. Angulo]+0.166667 Sin[20. Angulo]+0.857143 Sin[21. Angulo]-0.125 Sin[22. Angulo]+0.333333 Sin[23. Angulo]+1.5 Sin[24. Angulo]+2.6 Sin[25. Angulo]+Sin[26. Angulo]-2. Sin[27. Angulo]+0.6 Sin[28. Angulo]-0.2 Sin[29. Angulo]+0.2 Sin[30. Angulo]);

 y[j] = pi[[2]]+0.05(-62.5 Cos[Angulo]-57.8889 Cos[2. Angulo]-119.667 Cos[3. Angulo]+15.6667 Cos[4. Angulo]-16.5 Cos[5. Angulo]-1.25 Cos[6. Angulo]+3.875 Cos[7. Angulo]+0.9 Cos[8. Angulo]-29.75 Cos[9. Angulo]-8.5 Cos[10. Angulo]+7.33333 Cos[11. Angulo]+3.75 Cos[12. Angulo]-2.5 Cos[13. Angulo]+3.16667 Cos[14. Angulo]+1.75 Cos[15. Angulo]+7.75 Cos[16. Angulo]-1. Cos[17. Angulo]+1.1 Cos[18. Angulo]-0.666667 Cos[19. Angulo]+4.33333 Cos[20. Angulo]-1.25 Cos[21. Angulo]+0.666667 Cos[22. Angulo]+0.25 Cos[23. Angulo]+0.833333 Cos[24. Angulo]+0.75 Cos[26. Angulo]-0.5 Cos[27. Angulo]-0.1 Cos[28. Angulo]-0.333333 Cos[29. Angulo]-0.0526316 Cos[30. Angulo]-318.5 Sin[Angulo]-37.6 Sin[2. Angulo]-1.57143 Sin[3. Angulo]-2.4 Sin[4. Angulo]+3.66667 Sin[5. Angulo]-9.25 Sin[6. Angulo]+2.66667 Sin[7. Angulo]+10.8333 Sin[8. Angulo]-6.4 Sin[9. Angulo]-10.25 Sin[10. Angulo]-12.6667 Sin[11. Angulo]-5.875 Sin[12. Angulo]+1.25 Sin[13. Angulo]-5.85714 Sin[14. Angulo]-2.33333 Sin[15. Angulo]-1.85714 Sin[16. Angulo]+4.25 Sin[17. Angulo]-2.25 Sin[18. Angulo]+0.888889 Sin[19. Angulo]+0.6 Sin[20. Angulo]-0.4 Sin[21. Angulo]+1.33333 Sin[22. Angulo]+0.333333 Sin[23. Angulo]+0.6 Sin[24. Angulo]-0.6 Sin[25. Angulo]+1.2 Sin[26. Angulo]-0.2 Sin[27. Angulo]+1.11111 Sin[28. Angulo]+0.333333 Sin[29. Angulo]-0.75 Sin[30. Angulo]

);

 z[j] =pi[[3]];
Angulo=Angulo +Increm;
γγ[j]=γ1*(j/100);
Resuelve[j]=FindRoot[{Rigv1[[1]]==x[j],Rigv1[[2]]==y[j],Rigv1[[3]]==z[j],Rigv2[[1]]==x[j],Rigv2[[2]]==y[j],Rigv2[[3]]==z[j],Rigv3[[1]]==x[j],Rigv3[[2]]==y[j],Rigv3[[3]]==z[j],Rigv4[[1]]==x[j],Rigv4[[2]]==y[j],Rigv4[[3]]==z[j],γ==γγ[j]},
{θa1,θa1i},{θa2,θa2i},{θa3,θa3i},{θb1,θb1i},{θb2,θb2i},{θb3,θb3i},{θc1,θc1i},{θc2,θc2i},{θc3,θc3i},{θd1,θd1i},{θd2,θd2i},{θd3,θd3i},{γ,γi},MaxIterations->300];

θa1i=θa1/.Resuelve[j];θa2i=θa2/.Resuelve[j];θa3i=θa3/.Resuelve[j];θb1i=θb1/.Resuelve[j];θb2i=θb2/.Resuelve[j];θb3i=θb3/.Resuelve[j];θc1i=θc1/.Resuelve[j];θc2i=θc2/.Resuelve[j];θc3i=θc3/.Resuelve[j];θd1i=θd1/.Resuelve[j];θd2i=θd2/.Resuelve[j];θd3i=θd3/.Resuelve[j];γi=γ/.Resuelve[j];
tab=Table[{x[j],y[j],z[j]},{j,0,60,1}];
]
TablXY=Table[{x[t],y[t],z[t]},{t,0,60,1}]
LG=Line[TablXY];
robot
(*Dibujo prueba*)
(*Definiendo los puntos*)


P0={0,0,0};


Pg1=T3D[R1g];
Pg3=T3D[R3g];

Pa1=T3D[Rid1];
Pa2=T3D[Rie1];
Pa3=T3D[Rif1];
Pa4=T3D[Rig1];
Paa1=T3D1[Rid1];
Paa4=T3D1[Rif1];


Pb1=T3D[Rid2];
Pb2=T3D[Rie2];
Pb3=T3D[Rif2];
Pb4=T3D[Rig2];
Pbb1=T3D1[Rid2];
Pbb4=T3D1[Rif2];


Pc1=T3D[Rid3];
Pc2=T3D[Rie3];
Pc3=T3D[Rif3];
Pc4=T3D[Rig3];
Pcc1=T3D1[Rid3];
Pcc4=T3D1[Rif3];


Pd1=T3D[Rid4];
Pd2=T3D[Rie4];
Pd3=T3D[Rif4];
Pd4=T3D[Rig4];
Pdd1=T3D1[Rid4];
Pdd4=T3D1[Rif4];

chain1=Line[{Pa1,Pa2,Pa3}];
chain2=Line[{Pb1,Pb2,Pb3}];
chain3=Line[{Pc1,Pc2,Pc3}];
chain4=Line[{Pd1,Pd2,Pd3}];
interchain1=Line[{Pa3,Pc3}];
interchain2=Line[{Pb3,Pd3}];
bm=Line[{Pa3,Pb3,Pc3,Pd3,Pa3}];
bf=Line[{Pa1,Pb1,Pc1,Pd1,Pa1}];
iinter1=Line[{Pb3,Pb4}];
iinter2=Line[{Pa3,Pa4}];


Clear[γ];

(*Definiendo opciones de Graphics3D*)
aspecto=AspectRatio->1/1;
ejes=Axes-> True;
borde=AxesEdge->Automatic;
indicadores=AxesLabel->{x,y,z};
estiloejes=AxesStyle-> {Red,Green,Blue};
rango=PlotRange->{{-200,200},{-200,200},{-200,200}};
Animate[imagen=Graphics3D[{
chain1/.Resuelve[j],
chain2/.Resuelve[j],
chain3/.Resuelve[j],
chain4/.Resuelve[j],
bm/.Resuelve[j],
bf,
interchain1/.Resuelve[j],
interchain2/.Resuelve[j],
iinter1/.Resuelve[j],
iinter2/.Resuelve[j]},
aspecto,ejes,borde,indicadores,estiloejes,rango],{j,1,100,1}];

(*Para animar con volumen*)

chain11=Tube[chain1,1];
chain22=Tube[chain2,1];
chain33=Tube[chain3,1];
chain44=Tube[chain4,1];

joint1=Sphere[Pa1,2];
joint2=Sphere[Pa2,2];
joint3=Sphere[Pa3,2];
joint4=Sphere[Pb1,2];
joint5=Sphere[Pb2,2];
joint6=Sphere[Pb3,2];
joint7=Sphere[Pc1,2];
joint8=Sphere[Pc2,2];
joint9=Sphere[Pc3,2];
joint10=Sphere[Pd1,2];
joint11=Sphere[Pd2,2];
joint12=Sphere[Pd3,2];

Basefija=Polygon[{Pa1,Pb1,Pc1,Pd1,Paa1,Pbb1,Pcc1,Pdd1}];
Basemovil=Polygon[{Pa3,Pb3,Pc3,Pd3,Paa4,Pbb4,Pcc4,Pdd4}];

basem1=Tube[Line[{Pa3,Pb3}],2];
basem2=Tube[Line[{Pc3,Pd3}],2];
basem3=Tube[Line[{Pg1,Pg3}],2];


Animate[imagen=Graphics3D[
{
Red,chain11/.Resuelve[j],chain22/.Resuelve[j],chain33/.Resuelve[j],chain44/.Resuelve[j],Orange,joint1/.Resuelve[j],joint2/.Resuelve[j],joint3/.Resuelve[j],joint4/.Resuelve[j],joint5/.Resuelve[j],joint6/.Resuelve[j],joint7/.Resuelve[j],joint9/.Resuelve[j],joint8/.Resuelve[j],joint10/.Resuelve[j],joint11/.Resuelve[j],joint12/.Resuelve[j],Gray,Basefija,Black,Basemovil/.Resuelve[j],Blue,basem1/.Resuelve[j],basem2/.Resuelve[j],Yellow,basem3/.Resuelve[j],Blue,LG
},aspecto,ejes,borde,indicadores,estiloejes,rango],
{j,1,100,1}]