Algorithm to compute the Work Space and an specific path for a Robotic Arm (two links).

1. Cálculo de trayectoria para
un Brazo Robótico
Usando la aplicación MATLAB y aplicando la cinemática inversa
Por José Sanabria García, BOLIVIA 2017

2. Brazo robótico de dos eslabones
L1=10cm
L2
=7cm
m
1cm
Pk(x,y)
M1
M2
Calcular la probabilidad de
que la abscisa de la muñeca
(coordenada x) se posicione en
el rango 7.99 ≤ x ≤ 8.01 cm
dentro del espacio de trabajo
del brazo robótico.

3. Definición del Brazo Robótico
>> close all;
>> clearvars;
>> clc;
>> L1 = 10;
>> L2 = 7;
>> dt = 5;
>> t1 = -90:dt:90;
>> t2 = 0:dt:180;
L1=10cm
L2
=7cm
m
1cm
Pk(x,y)
M1
M2

4. Cuadriculado angular en 2D (XY)
>> Lt1 = length(t1);
>> Lt2 = length(t2);
>> T1 = zeros(Lt2,Lt1);
>> T2 = zeros(Lt2,Lt1);
>> for k1 = 1:Lt2
T1(k1,:) = t1;
end
>> for k2 = 1:Lt1
T2(:,k2) = t2;
end
L1=10cm
L2
=7cm
m
1cm
Pk(x,y)
M1
M2

5. Cálculo del espacio de trabajo y escalamiento
(Cinemática Directa)
>> X = L1*cosd(T1)+L2*cosd(T1+T2);
>> Y = L1*sind(T1)+L2*sind(T1+T2);
>> plot(X(:), Y(:), 'k.');
>> ce = max(abs([xlim, ylim]));
>> vs = [-ce,+ce,-ce,+ce];
>> axis(vs);
>> axis square;
>> grid on;
L1=10cm
L2
=7cm
m
1cm
Pk(x,y)
M1
M2

6. Representación del espacio de trabajo
>> title('Espacio de Trabajo de un BR de dos eslabones');
>> xlabel('x, cm'); ylabel('y, cm');
>> line([0 0],[-ce ce],'LineWidth',2,'Color','r');
>> line([-ce ce],[0 0],'LineWidth',2,'Color','r');
>> hold on;
>> plot(0,0,'ko','MarkerFaceColor','k','MarkerSize',10);L1=10cm
L2
=7cm
m
1cm
Pk(x,y)
M1
M2

7. Representación de la trayectoria
>> pA = [8, 5];
>> plot(pA(1),pA(2),'bo','MarkerFaceColor','b','MarkerSize',8);
>> pB = [-5, 13];
>> plot(pB(1),pB(2),'bo','MarkerFaceColor','b','MarkerSize',8);
>> line([pA(1) pB(1)],[pA(2) pB(2)],'LineWidth',2,'Color','b');
L1=10cm
L2
=7cm
m
1cm
Pk(x,y)
M1
M2
Punto Objetivo Inicial
Trayectoria Target
Trayectoria Target

8. Cálculo de la cinemática inversa
>> N = 20;
>> m = (pB(2) - pA(2))/(pB(1) - pA(1));
>> y0 = pA(2) - m*pA(1);
>> x = linspace(pA(1), pB(1), N);
>> y = m*x + y0;
>> r2 = x.^2 + y.^2;
>> LL = L1^2 + L2^2;
>> cs2 = (r2 - LL)/(2*L1*L2);
>> sn2 = sqrt(1 - cs2.^2);
>> a = L1 + L2*cs2;
>> b = L2*sn2;
>> t1i = 180*(atan2(y,x) - atan2(b,a))/pi;
>> t2i = 180*(atan2(sn2,cs2))/pi;
>> TS = transpose([x; y; t1i; t2i]);
Algoritmo para recta inclinada

9. Tabla solución de la Cinemática Inversa 2D
>> TS
TS =
8.0000 5.0000 -10.0927 115.3769
7.3158 5.4211 -6.1267 118.1695
6.6316 5.8421 -1.6995 120.4224
5.9474 6.2632 3.1246 122.1028
5.2632 6.6842 8.2612 123.1812
4.5789 7.1053 13.6125 123.6362
3.8947 7.5263 19.0769 123.4580
3.2105 7.9474 24.5606 122.6505
2.5263 8.3684 29.9863 121.2304
1.8421 8.7895 35.2994 119.2241
1.1579 9.2105 40.4679 116.6637
0.4737 9.6316 45.4808 113.5819
-0.2105 10.0526 50.3433 110.0076
-0.8947 10.4737 55.0731 105.9626
-1.5789 10.8947 59.6964 101.4583
-2.2632 11.3158 64.2463 96.4928
-2.9474 11.7368 68.7617 91.0476
-3.6316 12.1579 73.2885 85.0818
-4.3158 12.5789 77.8832 78.5232
-5.0000 13.0000 82.6195 71.2507
x y q1 q2

10. Cálculo de la trayectoria para
un Brazo Robótico
GRACIAS POR SU ATENCIÓN
Por José Sanabria García, BOLIVIA 2017