Comprobación con MATLAB

Programa Principal
%Solicitar datos por teclado
q1=degtorad(input('Ingresa valor para q1: '));
q2=degtorad(input('Ingresa Valor de q2: '));
q3=degtorad(input('Ingresa Valor de q3: '));
q4=degtorad(input('Ingresa Valor de q4: '));
q5=degtorad(input('Ingresa Valor de q5: '));
q6=degtorad(input('Ingresa Valor de q6: '));
l1=input('Ingresa Valor de l1: ');
l2=input('Ingresa Valor de l2: ');
l3=input('Ingresa Valor de l3: ');
l4=input('Ingresa Valor de l4: ');
l5=input('Ingresa Valor de l5: ');
l6=input('Ingresa Valor de l6: ');
disp('vector conocido s6')
B=['a';'b';'c'];
for i=1:3
rs6(i,1)=input(strcat(B(i,1),' del vector rs6: '));
end
rs4(4,1)=1;
disp('Vector rs6 ingresado:')
disp(rs6)

disp('Matriz A01')
A01=calculaA(q1,l1,l4,degtorad(-90))
disp('Matriz A12')
A12=calculaA(q2-degtorad(90),0,l2,degtorad(180))
disp('Matriz A23')
A23=calculaA(q3,0,l3,degtorad(-90))
disp('Matriz A34')
A34=calculaA(q4,-l5,0,degtorad(90))
disp('Matriz A45')
A45=calculaA(q5,0,0,degtorad(-90))
disp('Matriz A56')
A56=calculaA(q6,-l6,0,degtorad(0))
disp('Matriz de transformación encontrada')
Ttotal=A01*A12*A23*A34*A45*A56
disp('Vector s0 encontrado:')
vrso=Ttotal*rs4

Función calculaA
function A=calculaA(theta,pz1,px2,alpha)
%Matrices de transformación
T1=[cos(theta),-
sin(theta),0,0;sin(theta),cos(theta),0,0;0,0,1,0;0,0,0,1];
T2=[1,0,0,0;0,1,0,0;0,0,1,pz1;0,0,0,1];
T3=[1,0,0,px2;0,1,0,0;0,0,1,0;0,0,0,1];
T4=[1,0,0,0;0,cos(alpha),-
sin(alpha),0;0,sin(alpha),cos(alpha),0;0,0,0,1];
%Resultado
A=T1*T2*T3*T4;
Datos de prueba

Resultados del vector vrs0 para un vector vrs6=[0,0,0]

Observación: Como se puede apreciar, en la matriz Total de transformación o matriz T, se

comprueba que las direcciones corresponden a las obtenidas teóricamente. Además, se puede
comprobar de forma visual que el vector obtenido es únicamente la suma de las medidas del robot
en su posición de home, comprobando también de forma general que los cálculos teóricos
corresponden o son correctos.