Taller RoboticaMGD y MGI

Mateo Cortes Mayorga

clear all
clc
x=sym('q1')
P=sym('y')
A=cos(x)
B=sin(x)
%L1 de base a prisma
T1=sym('L1')
%L2
T2=sym('L2')
%Diferencia desde el prisma1 hasta prisma2 en ejeZ
T3=sym('q2')
%Angúlo de rotacion
T5=sym('q1')
%Diferencia desde el prisma1 hasta prisma2 en eje XY
T4=sym('a')
Rx = [1 0 0 0;
0 A -B 0;
0 B A 0;
0 0 0 1]
Ry = [A 0 B 0;
0 1 0 0;
-B 0 A 0;
0 0 0 1]
Rz = [A -B 0 0;B A 0 0;0 0 1 0;0 0 0 1]
Px = [1 0 0 T2;
0 1 0 0;
0 0 1 0;
0 0 0 1]
Py = [1 0 0 T4;
0 1 0 0;
0 0 1 0;
0 0 0 1]
Pz = [1 0 0 0;
0 1 0 0;
0 0 1 T1+T3;
0 0 0 1]
%Rotacion en Z punto a punto
op1=Rz*Pz;
op2=op1*Pz;
Rotacionz=Rz*Pz*Px*Py*Py
Rotacionzpunto=Rz.*Pz.*Pz.*Py.*Py
Matrizdet= det(Rotacionz)
y=inv(Rotacionz)