Jacobiano del Robot_RRR

%JACOBIANO MANIPULADOR ANTROPOMORFICODE 4GDL

clc;clear all;close all
%-----------------------%
%PARAMETROS MECANICOS
L1=0.2;
L2=1.4;
L3=1.6;
L4=0.627
%1. Definiocion del sistema inercial
%--------------------------%1.1 Matriz Homogenea
T0=eye(4);
%1.2 ploteo del sistema
plot_frame(T0,'color','k','view','auto')
axis([-2 4 -2 4 0 3])
hold on
grid on
xlabel('x(m)')
ylabel('y(m)')
%------------------------------%
%2.Sistema coordenado 1
%------------------------------%
%2.1 Matriz A1
theta1=0;
d1=L1;
a1=0;
alpha1=pi/2;
A1=matriz_homogenea_DH(theta1,d1,a1,alpha1);
T01=A1;
%2.2 Ploteamos el sistema coordenado
h1=plot_frame(T01,'frame','1','color','b');

%3.Sistema coordenado 3
%------------------------------%
%2.1 Matriz A1
theta2=0;
d2=0;
a2=L2;
alpha2=0;
A2=matriz_homogenea_DH(theta2,d2,a2,alpha2);
T02=T01*A2;
%2.2 Ploteamos el sistema coordenado
h2=plot_frame(T02,'frame','2','color','G');
%4.Sistema coordenado 3
%------------------------------%
%2.1 Matriz A3
theta3=0*pi/6; %variable
d3=0;
a3=L3;
alpha3=0;
A3=matriz_homogenea_DH(theta3,d3,a3,alpha3);
T03=T02*A3;

%2.2 Ploteamos el sistema coordenado

h3=plot_frame(T03,'frame','3','color','m');
%4.Sistema coordenado 4
%------------------------------%
%2.1 Matriz A3
theta4=0*pi/6; %variable
d4=0;
a4=L4;
alpha4=0;
A4=matriz_homogenea_DH(theta4,d4,a4,alpha4);
T04=T03*A4;
%2.2 Ploteamos el sistema coordenado
h4=plot_frame(T04,'frame','4','color','m');
%5.Configuracion de velocidades
%-----------%5.1Velocidades angulares(rad/s)
dq1=0*pi/8;
dq2=1*pi/8;
dq3=0*pi/8;
dq4=0*pi/8;
dq=[dq1;dq2;dq3;dq4];
%5.2 Calculo de las velocidades iniciales
%->Calculo del Jacobiano
PARAMS.a2=a2;
PARAMS.a3=a3;
PARAMS.a4=a4;
theta=[theta1;theta2;theta3;theta4];
[Jv,Jw]=jacobiano_robot_antropomorfico_4GDL(theta,PARAMS);
%->Calculo de las velocidaddes
v=Jv*dq;
w=Jw*dq;
DT=0.05;
%6.2 Lazo principal
STEPS=80;
disp('presione cualquier tecla para continuar')
pause
for i=1:STEPS
%A.MESSAGES
fprintf('STEP:%d\n',i)
%B,ESTABLECEMOS NUEVOS VALORES DE LAS JUNTAS
theta1=theta1 + dq1*DT;
theta2=theta2 + dq2*DT;
theta3=theta3 + dq3*DT;
theta4=theta4 + dq4*DT;
theta=[theta1; theta2;theta3;theta4];
%C.MATRICES DE TRANSFORMACION HOMOGENEAS
A1=matriz_homogenea_DH(theta1,d1,a1,alpha1);
A2=matriz_homogenea_DH(theta2,d2,a2,alpha2);
A3=matriz_homogenea_DH(theta3,d3,a3,alpha3);
A4=matriz_homogenea_DH(theta4,d4,a4,alpha4);
T01=A1;
T02=A1*A2;

T03=T02*A3;
T04=T03*A4;
%D.PLOTEAMOS LOS SISTEMAS COORDENADAS
plot_frame(h1,T01);
plot_frame(h2,T02);
plot_frame(h3,T03);
plot_frame(h4,T04)
%E.Analisis de velocidades
%.>Calculo del jacobiano
[Jv,Jw]=jacobiano_robot_antropomorfico_4GDL(theta,PARAMS);
%.>velocidad lineal del sistema final
v=Jv*dq;
fprintf('Velocidad lineal: [%2.4f,%2.4f,%2.4f]\n',v)
fprintf('
Modulo:%2.4f\n',norm(v))
w=Jw*dq;
fprintf('Velocidad ANGULAR: [%2.4f,%2.4f,%2.4f]\n',w)
fprintf('
Modulo:%2.4f\n',norm(w))
%F.DELTA DE TIEMPO
pause(0.3)
end

Calculo del Jacobiano usando variables simblicas

%Tema:Jacobiano de un manipulador 4GL
clc;clear all;close all
%............................
%1.Definicion de variables simbolicas
%..................................
syms th1 d1 a1 alpha1
syms th2 d2 a2 alpha2
syms th3 d3 a3 alpha3
syms th4 d4 a4 alpha4
syms q1 q2 q3 q4
syms dq1 dq2 dq3 dq4
syms L1 L2 L3 L4
%..................%
%2. Definicion del sistema coordenado 1
% ->Parametros D-H
th1= q1;
d1=L1;
a1=0;
alpha1=pi/2;
%Matriz A
A1=matriz_homogenea_DH(th1,d1,a1,alpha1);
%2.2.Sistema Coordenada 2
%-> Parametros D-H
th2=q2;
d2=0;
a2=L2;
alpha2=0;
%Matriz A
A2=matriz_homogenea_DH(th2,d2,a2,alpha2);

%2.3.Sistema Coordenada 3
%-> Parametros D-H
th3=q3;
d3=0;
a3=L3;
alpha3=0;
%Matriz A
A3=matriz_homogenea_DH(th3,d3,a3,alpha3);
%2.4.Sistema Coordenada 4
%-> Parametros D-H
th4=q4;
d4=0;
a4=L4;
alpha4=0;
%Matriz A
A4=matriz_homogenea_DH(th4,d4,a4,alpha4);

%------------------%3.Calculo de las ecuaciones cinemticas

%-----------------T01=A1;
T12=A2;
T23=A3;
T34=A4;
T02=T01*T12;
T03=T02*T23;
T04=T03*T34;
T02=simplify(T02);
T03=simplify(T03);
T04=simplify(T04);
disp('T02')
pretty(T02)
disp('T03')
pretty(T03)
disp('T04')
pretty(T04)
%-----------------%
%4.Calculo Del Jacobiano
%---------------%->Origenes de coordenadas
p0=[0;0;0];
p1=T01(1:3,4);
p2=T02(1:3,4);
p3=T03(1:3,4);
p4=T04(1:3,4);
%->VECTORES UNITARIOS
z0=[0;0;1];
z1=T01(1:3,3);
z2=T02(1:3,3);
z3=T03(1:3,3);
z4=T04(1:3,3);

%4.2 Calculo del termino 'Jv'

Jv=[cross(z0,(p4-p0)) cross(z1,(p4-p1)) cross(z2,(p4-p2)) cross(z3,(p4p3))];
Jv=simplify(Jv);
disp('Jv')
disp(Jv)
%4.3 Calculo del termino 'Jw'
Jw=[z0 z1 z2 z3];
Jw=simplify(Jw);
disp('Jw')
disp(Jw)
%5.CALCULO DE VELOCIDADES
v=Jv*[dq1;dq2;dq3;dq4];
w=Jw*[dq1;dq2;dq3;dq4];
v=simplify(v);
w=simplify(w);
disp('velocidades de traslacion vx,vy,vz')
disp(v)
disp('velocidades de rotacion wx,wy,wz')
disp(w)
Resultados de la compilacin