Taller 4: Cinemática diferencial de primer orden - Jacobiano de

robots
Eduardo Cuadros

import ETS3.*

Punto 1
Se realiza la cinemática directa.

l1 = 0.5; l2 = 0.4; l3 = 0.2; % metros

L1(1) = Link([0 l1 0 pi/2 0]);
L1(2) = Link([0 0 l2 0 0]);
L1(3) = Link([0 0 l3 0 0]);
Rob1 = SerialLink(L1)

Rob1 =

noname:: 3 axis, RRR, stdDH, slowRNE

+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+
| 1| q1| 0.5| 0| 1.5708| 0|
| 2| q2| 0| 0.4| 0| 0|
| 3| q3| 0| 0.2| 0| 0|
+---+-----------+-----------+-----------+-----------+-----------+

Rob1.name = "Punto 1";

Rob1.plot([0 0 0])
xlim([-0.2 1.10])
ylim([-0.5 0.5])
zlim([-0.5 1.10])
view([40 20])

1
Se encuentran las matrices de transformación homogénea.

syms q1 q2 q3
H01 = trotz(q1) * transl(0,0,L1(1).d) * transl(L1(1).a,0,0) * trotx(L1(1).alpha);
H12 = trotz(q2) * transl(0,0,L1(2).d) * transl(L1(2).a,0,0) * trotx(L1(2).alpha);
H23 = trotz(q3) * transl(0,0,L1(3).d) * transl(L1(3).a,0,0) * trotx(L1(3).alpha);
H02 = H01*H12;
H03 = simplify(H01*H12*H23)

H03 =

coord_xyz = H03(1:3,4)

2
 coord_xyz =

rpy = [atan(H03(2,3)/H03(1,3));acos(H03(3,3));atan(-H03(3,2)/H03(3,1))]

rpy =

Se determina el jacobiano por método geométrico y por toolbox

z0 = [0;0;1];
z1 = H01(1:3,1:3)*z0;
z2 = H02(1:3,1:3)*z0;
O1 = H01(1:3,4);
O2 = H02(1:3,4);
O3 = H03(1:3,4);

Jacobiano1 = [cross(z0, (O3)), cross(z1, (O3-O1)), cross(z2, (O3-O2)); z0, z1, z2]

Jacobiano1 =

3
 Jacobiano1 = subs(Jacobiano1,q1, pi/4);
Jacobiano1 = subs(Jacobiano1,q2, pi/2);
Jacobiano1 = subs(Jacobiano1,q3, pi/4);
double(round(Jacobiano1,4))

ans = 6×3
0.1000 -0.3828 -0.1000
-0.1000 -0.3828 -0.1000
0 -0.1414 -0.1414
0 0.7071 0.7071
0 -0.7071 -0.7071
1.0000 0 0

Jacobiano1_toolb = round(jacob0(Rob1,[pi/4,pi/2,pi/4]),4)

Jacobiano1_toolb = 6×3
0.1000 -0.3828 -0.1000
-0.1000 -0.3828 -0.1000
0 -0.1414 -0.1414
0 0.7071 0.7071
0 -0.7071 -0.7071
1.0000 0 0

Al comparar el jacobiano obtenido por método geométrico y por el toolbox, se puede ver que los valores lógicos
de la comparación son 1 en cada punto de la matriz, lo que significa que son iguales.

ans == Jacobiano1_toolb

4
 ans = 6×3 logical array
1 1 1
1 1 1
1 1 1
1 1 1
1 1 1
1 1 1

Punto 2
Se calcula el torque necesario para sostener el mecanismo cuando se aplica una fuerza en el efector final.

F = [0; 10; 15; 0; 0; 0];

Jacobiano1 = subs(Jacobiano1,q1,pi/2);
Jacobiano1 = subs(Jacobiano1,q2,pi/4);
Jacobiano1 = subs(Jacobiano1,q1,0);
double(Jacobiano1)

ans = 6×3
0.1000 -0.3828 -0.1000
-0.1000 -0.3828 -0.1000
0 -0.1414 -0.1414
0 0.7071 0.7071
0 -0.7071 -0.7071
1.0000 0 0

T = double(Jacobiano1'*F)

T = 3×1
-1.0000
-5.9497
-3.1213

Punto 3
l1 = 1;
L2(1) = Link([0 l1 0 0 0]);
L2(2) = Link([pi/2 0 0 -pi/2 1]);
L2(3) = Link([0 0 0 0 0]);
L2(2).qlim = [0 1];
Rob3 = SerialLink(L2,'base', troty(pi/2))

Rob3 =

noname:: 3 axis, RPR, stdDH, slowRNE

+---+-----------+-----------+-----------+-----------+-----------+
| j | theta | d | a | alpha | offset |
+---+-----------+-----------+-----------+-----------+-----------+
| 1| q1| 1| 0| 0| 0|
| 2| 1.5708| q2| 0| -1.5708| 0|
| 3| q3| 0| 0| 0| 0|
+---+-----------+-----------+-----------+-----------+-----------+
base: t = (0, 0, 0), RPY/xyz = (0, 90, 0) deg

5
 Rob3.name = "Punto 3";
Rob3.plot([0 0 0])
xlim([-0.2 3])
ylim([-1 1])
zlim([-1 1.10])
view([40 20])

Matrices de transformación homogénea

syms q1 q2 q3
H01 = trotz(q1) * transl(0,0,L2(1).d) * transl(L2(1).a,0,0) * trotx(L2(1).alpha);
H12 = trotz(L2(2).theta) * transl(0,0,q2) * transl(L2(2).a,0,0) *
trotx(L2(2).alpha);
H23 = trotz(q3) * transl(0,0,L2(3).d) * transl(L2(3).a,0,0) * trotx(L2(3).alpha);
H02 = H01*H12;
H03 = H01*H12*H23

H03 =

Jacobiano: Método geométrico

z0 = [0;0;1];

6
 z1 = H01(1:3,1:3)*z0;
z2 = H02(1:3,1:3)*z0;
O1 = H01(1:3,4);
O2 = H02(1:3,4);
O3 = H03(1:3,4);

Jacobiano3 = [cross(z0, (O3)), z1, cross(z2, (O3-O2)); z0, [0;0;0], z2]

Jacobiano3 =

Jacobiano3 = subs(Jacobiano3,q1,0);
Jacobiano3 = subs(Jacobiano3,q2,0);
Jacobiano3 = subs(Jacobiano3,q3,0);
Jacobiano3 = double(Jacobiano3)

Jacobiano3 = 6×3
0 0 0
0 0 0
0 1 0
0 0 -1
0 0 0
1 0 0

Jacobiano: Método analítico

xyz = H03(1:3,4) % Coordenadas x, y, z

xyz =

rpy = [atan(H03(2,3)/H03(1,3)); acos(H03(3,3)); atan(-H03(3,2)/H03(3,1))]

rpy =

Jacobiano3_an = [diff(xyz(1,1),q1) diff(xyz(1,1),q2) diff(xyz(1,1),q3);

diff(xyz(2,1),q1) diff(xyz(2,1),q2) diff(xyz(2,1),q3);
diff(xyz(3,1),q1) diff(xyz(3,1),q2) diff(xyz(3,1),q3);

7
 diff(rpy(1,1),q1) diff(rpy(1,1),q2) diff(rpy(1,1),q3);
diff(rpy(2,1),q1) diff(rpy(2,1),q2) diff(rpy(2,1),q3);
diff(rpy(3,1),q1) diff(rpy(3,1),q2) diff(rpy(3,1),q3)]

Jacobiano3_an =

Jacobiano3_an = subs(Jacobiano3_an,q1,0);
Jacobiano3_an = subs(Jacobiano3_an,q2,0);
Jacobiano3_an = subs(Jacobiano3_an,q3,0);
Jacobiano3_an = double(Jacobiano3_an)

Jacobiano3_an = 6×3
0 0 0
0 0 0
0 1 0
1 0 0
0 0 0
0 0 1

Comparación

Jacobiano3 == Jacobiano3_an

ans = 6×3 logical array

1 1 1
1 1 1
1 1 1
0 1 0
1 1 1
0 1 0

Como se puede ver en la matriz lógica, las velociades lineales de los jacobianos geométrico y analítico son
iguales pero las las velocidades angulares difieren un poco. Esto puede ser debido a la orientación del TCP
para cada uno.

syms fx fy fz
Jacobiano3' * [fx; fy; fz; 0; 0; 0]

ans =

Punto 4
Una posición singular de un robot es cuando el jacobiano de velocidades o de fuerzas pierde rango. Esto
significa que uno de sus vectores fila o columna es 0 en todos sus componentes y, por lo tanto, el determinante

8
de la matriz es 0. Físicamente lo que se tiene es el robot con una posición fija en un grado de libertad, puede
aguantar una fuerza externa pero es incapaz de moverse en esa dirección.

Punto 5
La función realiza un jacobiano geométrico del robot en una pose determinada. En este jacobiano se
muestran las velocidades lineales y angulares del efector final con las velocidades articulares.