1

Robótica – Práctica III

2
 Robótica – Práctica III

3
 Robótica – Práctica III
**Desacoplo cinemático
El cálculo de la TCI de un robot de 6GDL puede simplificarse usando el método del desacoplo cinemático si la
geometría del robot cumple la condición de que los ejes de las tres últimas articulaciones se cortan en un
punto.

La función que se presenta a continuación calcula la TCI de un robot de 6GDL por el método de desacoplo
cinemático. Para esta función se pide:

1. Identifique en el código las siguientes secciones:

a. Obtención de las coordenadas del punto de muñeca.
b. Cálculo de los valores de las coordenadas articulares de los tres primeros grados de libertad.
c. Cálculo de los valores de las coordenadas articulares de los tres últimos grados de libertad.
d. Complete la función para que permita obtener los valores de las coordenadas articulares
tanto en configuración ‘codo arriba’ como ‘codo abajo’.

4
 Robótica – Práctica III

Para cada uno de estos puntos, identifique las líneas de código que componen dicha sección y
explique cómo se obtiene cada uno de los resultados.

function [Qp, Qn] = TCI_antro(Geom, Loc)

l1 = Geom(1); l2
= Geom(2); l3 =
Geom(3); l4 =
Geom(4);

xe = Loc(1); ye
= Loc(2); ze =
Loc(3);
pr = [xe ye ze].'

yaw = Loc(4);
pitch = Loc(5);
roll = Loc(6);

A06 = Rz(roll)*Ry(pitch)*Rx(yaw); R06 =

A06(1:3, 1:3);

a = R06(:,3);

pm = pr - l4*a

%% EN ESTA PARTE

PODEMOS OBSERVAR

CÓMO LAS

COORDENADAS DEL

PUNTO DE MUÑECA

SE INSERTAN EN EL

PUNTO DE MUÑECA

xm = pm(1);
ym = pm(2);
zm = pm(3);

%% AQUÍ TENEMOS EL CÁLCULO DE LA PRIMERA COORDENADA ARTICULAR.

q1 = atan2(ym, xm);

cq3 = ((zm-l1).^2 + ym.^2 + xm.^2 - l2.^2 - l3.^2) / (2*l2*l3); sq3 = sqrt(1-

cq3.^2);

q3 = atan2(+sq3, cq3); % AQÚI TENERMOS EL CÁLCULO DE LA SEGUNDA COORDENADA ARTICULAR

q2 = atan2((zm-l1), sqrt(xm.^2+ym.^2)) - atan2(l3*sin(q3), l2+l3*cq3);% AQUÍ TENEMOS EL CÁLCULO DE LA TERCERA

CORRDENADA ARTICULAR

A01= matrizDH ( q1, l1, 0, -pi/2); A12=

matrizDH ( q2, 0, l2, 0);
A23= matrizDH ( q3, 0, 0, pi/2);

R01 = A01(1:3, 1:3);

R12 = A12(1:3, 1:3);
R23 = A23(1:3, 1:3);

R03 = R01*R12*R23 R30

= R03.'

5
 Robótica – Práctica III
R36 = R30*R06

q4 = asin( -(R36(2,3) / R36(3,3)) ); %AQUÍ

TENEMOS EL CÁLCULO DE LA CUERTA CORRDENADA

q5 = acos( R36(3,3) );
%AQUÍ TENEMOS EL CÁLCULO DE LA QUINTA
COORDENADA
q6 = atan( -(R36(3,2) / R36(3,1)) );

%AQUÍ TENEMOS EL CÁLCULO DE LA CAL

Qp = [q1, q2, q3, q4, q5, q6];

Qn = [0, 0, 0, 0, 0, 0];

en

6
 Robótica – Práctica III