Robotics - IMT 342

Laboratory 1: Forward Kinematics

Deadline: 20/08/2022

Introduction
The learning objectives for this practice are:

• Vectors • Forward Kinematics

• Rotation Matrix • Kuka KR6 Agilus

• Homogenous Transformation • Coordinate Frames

El objetivo principal de este laboratorio es calcular la cinemática directa de un brazo robótico KUKA. Luego,
se practica obtener diferentes representación de la orientación del efector final. Los ejercicios son implementar
varias funciones para el calculo de la cinemática directa mediante el uso de computación numérica y simbólica
en MATLAB. Se otorga varios scripts, para visualización del robot, para verificar sus resultados, entre otros.
Todos los ejercicios se basan en el Robot KUKA KR6 AGILUS R700, el cual tiene 6 grados de libertad. El
robot y sus grados de libertad se muestran en la Figura 1. Durante los ejercicios deberá implementar diversas
funciones en MATLAB las cuales deberá comprobar las cuidadosamente ya que en base a ellas se realizaran
los siguientes laboratorios.

Figure 1: Robotic Arm, KR6

1
UCB - Ingenierı́a Mecatrónica IMT 342

Exercise 1: Robotic Arm, KR6 Agilus

Se da el siguiente diagrama cinematico de un brazo de un robot KUKA KR6 AGILUS. Usaremos la siguiente
notación I se usara para el sistema de coordenadas mundo inercial (En la Figura 2 es el sistema de coordenadas
P0 ) y E es el sistema de coordenadas del efector final (sistema de coordenadas P6 ).
Note que se otorga una variedad de scripts para la elaboración de los ejercicios:

1. init workspace.m - Este deberá ser ejecutado una sola vez al momento de abrir MATLAB. Inicializa su
espacio de trabajo y hace visible las funciones de ayuda a los otros scripts.

2. evaluate problems.m - Verifica su implementación. Puede ejecutar este script múltiples veces para
verificar su implementación de sus funciones.

3. loadviz.m y testviz.m - Muestra una visualización del robot y sus sistemas de coordenadas. Una vez
implementa algunas funciones podra visualizar y mover el robot.

Se recomienda documentar los pasos de resolución de ejercicios y las ecuaciones obtenidas antes de su imple-
mentación en código.

Figure 2: Coordinate Frames

2
UCB - Ingenierı́a Mecatrónica IMT 342

Ejercicio 1.1
Defina el vector q de las coordenadas generalizadas que describan la configuración del brazo robótico KUKA
KR6 AGILUS.
Ejercicio 1.2
Suponga a partir de aquı́ que las coordenadas generalizadas q son los ángulos de articulación del manipulador y
estan numeradas de acuerdo con los sistemas de coordenadas mostrados en la Figura 2. Los ángulos positivos
implican rotaciones en contra de las agujas del reloj. Los grados de libertad del brazo robótico se muestran en
la Figura 1. Considerando los sistemas de coordenadas ya definidos derive lo siguiente:

• Calcule las matrices de transformaciones homogeneas Tk−1,k (q) ∀k, k = 1, ...6

• Adicionalmente, calcule las transformaciones homogeneas constantes TI,0 y T6,E

Se recomienda obtener la transformada homogenea en la siguiente forma para luego implementarla en código
en el siguiente ejercicio:
 
Ck−1,k (qk ) k−1 rk−1,k (qk )
Tk−1,k (qk ) = (1)
01×3 1

Ejercicio 1.3
Para visualizar el robot primero se necesitara implementar las transformadas homogeneas en el script KR6V isualization.m,
todas las funciones al final del script. Cambie las matrices identidades por su soluciones previamente obtenida.

• function T01 = jointT f 01(q) • function T34 = jointT f 34(q)

• function T12 = jointT f 12(q) • function T45 = jointT f 45(q)

• function T23 = jointT f 23(q) • function T56 = jointT f 56(q)

Una vez haya implementado las funciones mencionadas corra el script corra el script loadviz.m para visualizar
su robot. En la Figura 3 se muestra el resultado antes de la implementación y luego de la implementación.

Figure 3: Robotic Arm, KR6

3
UCB - Ingenierı́a Mecatrónica IMT 342

Ejercicio 1.4
Implemente las siguientes funciones, debe reemplazar la función zeros() con su solución obtenida en el ejercicio
1.2 anterior:

• getT ransf ormI0.m • jointT oT ransf orm34.m

• jointT oT ransf orm01.m • jointT oT ransf orm45.m

• jointT oT ransf orm12.m • jointT oT ransf orm56.m

• jointT oT ransf orm23.m • getT ransf orm6E.m

Una vez haya implementado las funciones mencionadas corra el script evaluate problems.m para verificar si
están correctas.
Ejercicio 1.5
Encuentre el vector de posición del efector final, I rIE = I rIE (q). Es decir encuentre la cinemática directa.
Luego, implemente la función jointT oP osition. Reemplace la funcion zeros con su solución.
Una vez haya implementado la función mencionada corra el script evaluate problems.m para verificar si esta
correcta.
Ejercicio 1.6
 
π/6
π/6
 
π/6
Encuentre la posición del efector final cuando q = 
 
π/6

π/6
π/6
Puede mover el robot a esta posición en la visualización:

1. Cargue la visualización corriendo el script loadviz.m

2. En la ventada de comandos cree el vector de configuración del manipular q = ...

3. Entregue la configuración al robot KUKA con el comando kukaRobot.setJointP ositions(q). Después

de correr este comando en podrá visualizar el robot moviéndose a dicha configuración.

4. Experimente con esta función kukaRobot.setJointP ositions(q) mandando configuraciones aleatorias

al manipulador. q = rand(6, 1).

Ejercicio 1.7
Encuentre la matriz de rotación, orientación, del efector final, CIE = CIE (q). Implemente la siguiente función,
jointT oRotM at.m.
Hint: Debe calcular la transformada TIE para luego extraer la matriz de rotación.

4
UCB - Ingenierı́a Mecatrónica IMT 342

Ejercicio 1.8
Encuentre el quaternion que representa la orientación del efector final ξIE = ξIE (q) . Implemente las siguientes
funciones:

• Dos funciones para convertir de quaternion a matrices de rotación y viceversa. Puede verificar sus
funciones convirtiendo una matriz de rotación a quaternion y luego ese quaternion convertirlo a una
matriz de rotación. Estas funciones son:

1. quatT oRotM at.m

2. rotM atT oQuat.m
N
• Multiplicación de dos quaterniones q p, quatM ult.m

• La rotación pasiva de un vector con un quaternion. Esto se puede implementar de diferentes maneras. La
forma más fácil es transformar el quaternion a la matriz de rotación correspondiente (usando la función
de arriba) y luego multiplicar la matriz con el vector a rotar. rotV ecW ithQuat.m

• jointT oQuat.m - Obtiene el quaternion dado la configuración de los joints. Utilice las funciones imple-
mentadas previamente, jointT oRotM at.m y rotM atT oQuat.m para resolver este problema.