16.2: Recoger y Colocar

# Here are some libraries you may need to use
%matplotlib inline
import matplotlib.pylab as plt
import numpy as np
import sympy as sym
import math
sym.init_printing()

Considera el robot representado en la siguiente imagen.

Este estilo de robot a menudo se llama robot de “pick-and-place”. Tiene dos motores que giran alrededor del eje z para mover el efector final en el\(x−y\) plano; un “actuador lineal” que se mueve hacia arriba y hacia abajo en la\(z\) dirección -y luego finalmente una tercera articulación giratoria de “muñeca” que gira la “mano” del robot. Vamos a modelar nuestro robot usando el siguiente diagrama del sistema:

Nota

El origen de este robot se encuentra en la base de la primera “torre” y está en línea con la primera articulación. La\(x\) dirección -va del origen a la derecha y el\(z\) eje -va desde el origen hacia arriba.

Esto es un poco más complicado que el caso 2D donde todo giraba alrededor de los ejes que se proyectan fuera del\(x−y\) plano.

En 2D solo nos preocupamos realmente por un eje de rotación. Sin embargo en 3D podemos rotar alrededor del\(z\) eje\(x\)\(y\),, y. Las siguientes son las matrices de transformación 3D que combinan rotación y traducciones:

Rotación del eje X

\ [\ begin {split}
\ left [\ begin {matrix}
x'\\
y'\
z'\\
1
\ end {matrix}
\ right]
=
\ left [\ begin {matrix}
1 & 0 & 0 & dx\\
0 & cos (q) & -sin (q) & dy\\
0 & sin (q) & cos (q) & dz\\
0 & 0 & 0 & 1
\ end {matriz}
\ derecha]
\ izquierda [\ comenzar {matriz}
x\\
y\\
z\\
1
\ end {matriz}
\ derecha]
\ end {split}\ nonumber\]

Rotación del eje Y

\ [\ begin {split}
\ left [\ begin {matrix}
x'\
y'\
z'\\
1
\ end {matrix}
\ right]
=
\ left [\ begin {matrix}
cos (q) & 0 & sin (q) & dx \\
0 & 1 & 0 & dy\\
-sin (q) & 0 & cos (q) & dz\\
0 & 0 & 0 & 1
\ end {matriz}
\ derecha]
\ izquierda [\ begin {matriz}
x\\
y\\
z\\
1
\ end {matriz}
\ derecha]
\ end {split}\ nonumber\]

Rotación del eje Z

\ [\ begin {split}
\ left [\ begin {matrix}
x'\\
y'\
z'\\
1
\ end {matrix}
\ right]
=
\ left [\ begin {matrix}
cos (q) & -sin (q) & 0 & dx\\
sin (q) & cos (q) & 0 & dy\\
0 & 0 & 0 & 1 & dz\\
0 & 0 & 0 & 1
\ end {matriz}
\ derecha]
\ izquierda [\ comenzar {matriz}
x\\
y\\
z\\
1
\ end {matriz}
\ derecha]
\ end {split}\ nonumber\]

Pregunta a)

Construir una matriz de transformación conjunta llamada\(J_1\), que representa un sistema de coordenadas que se ubica en la parte superior de la primera “torre” (sholder del robot) y se mueve girando alrededor del\(z\) eje en\(\theta_1\) grados. Representa tu matriz usando sympy y los símbolos proporcionados:

#Use the following symbols

q1,q2,d4,q4,v1,v2,a1,a2 = sym.symbols('\Theta_1, \Theta_2, d_4, \Theta_4, V_1, V_2,A_1,A_2', negative=False)

#put your answer here
q1

Pregunta b)

Construir una matriz de transformación articular llamada\(J_2\), que representa un sistema de coordenadas que se ubica en la articulación “codo” entre los dos brazos giratorios y gira con el segundo brazo alrededor del\(z\) eje -eje en\(\theta_2\) grados. Representa tu matriz usando sympy y los símbolos proporcionados en la pregunta a:

#put your answer here

Pregunta c)

Construir una matriz de transformación articular llamada\(J_3\), que representa una traslación de coordenadas desde la articulación “codo” hasta el extremo horizontal del brazo del robot por encima de la articulación de la muñeca. Nota: no hay rotación en esta transformación. Representa tu matriz usando sympy y los símbolos proporcionados en la pregunta a:

#put your answer here

Pregunta d)

Construir una matriz de transformación conjunta llamada\(J_4\), que representa un sistema de coordenadas que se encuentra en la punta de la “mano” del robot y gira alrededor del\(z\) eje por\(\theta_4\). Este es un poco diferente, la configuración es tal que la mano toca la mesa cuando\(d_4=0\) así es el componente de traslación para la matriz en el eje z\(d_4−V_1−V_2\).

#put your answer here

Pregunta

Reescribir las matrices de transformación conjunta a partir de las preguntas a - d como matrices numpy con valores discretos (en lugar de simbólicos). Conecte sus transformaciones en el código a continuación y use esto para simular el robot:

from ipywidgets import interact
from mpl_toolkits.mplot3d import Axes3D

def Robot_Simulator(theta1=0,theta2=-0,d4=0,theta4=0):

    #Convert from degrees to radians
    q1 = theta1/180 * math.pi
    q2 = theta2/180 * math.pi
    q4 = theta4/180 * math.pi

    #Define robot geomitry
    V1 = 4 
    V2 = 0
    A1 = 2 
    A2 = 2 

    #Define your transfomraiton matrices here. 
    J1 = np.matrix([[1, 0, 0, 0 ], 
                    [0, 1, 0, 0 ], 
                    [0, 0, 1, 0],
                    [0, 0, 0, 1]])

    J2 = np.matrix([[1, 0, 0, 0 ], 
                    [0, 1, 0, 0 ], 
                    [0, 0, 1, 0],
                    [0, 0, 0, 1]])

    J3 = np.matrix([[1, 0, 0, 0 ], 
                    [0, 1, 0, 0 ], 
                    [0, 0, 1, 0],
                    [0, 0, 0, 1]])

    J4 = np.matrix([[1, 0, 0, 0 ], 
                    [0, 1, 0, 0 ], 
                    [0, 0, 1, 0],
                    [0, 0, 0, 1]])

    
    #Make the rigid end effector
    p = np.matrix([[-0.5,0,0, 1], [-0.5,0,0.5,1], [0.5,0,0.5, 1], [0.5,0,0,1],[0.5,0,0.5, 1], [0,0,0.5,1], [0,0,V1+V2,1]]).T
    
    #Propogate and add joint points though the simulation
    p = np.concatenate((J4*p, np.matrix([0,0,0,1]).T), axis=1 )
    p = np.concatenate((J3*p, np.matrix([0,0,0,1]).T), axis=1 )
    p = np.concatenate((J2*p, np.matrix([0,0,0,1]).T), axis=1 )
    p = np.concatenate((J1*p, np.matrix([0,0,0,1]).T), axis=1 )
        
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')

    ax.scatter(p[0,:].tolist()[0],(p[1,:]).tolist()[0], (p[2,:]).tolist()[0], s=20, facecolors='blue', edgecolors='r')
    ax.scatter(0,0,0, s=20, facecolors='r', edgecolors='r')
    ax.plot(p[0,:].tolist()[0],(p[1,:]).tolist()[0], (p[2,:]).tolist()[0])
    ax.set_xlim([-5,5])
    ax.set_ylim([-5,5])
    ax.set_zlim([0,6])
    ax.set_xlabel('x-axis')
    ax.set_ylabel('y-axis')    
    ax.set_zlabel('z-axis') 

    plt.show()
    
target = interact(Robot_Simulator, theta1=(-180,180), theta2=(-180,180), d4=(0,6), theta4=(-180,180)); ##TODO: Modify this line of code

theta1

0

theta2

0

d4

0

theta4

0

Pregunta

¿Podemos cambiar el orden de las matrices de transformación? ¿Por qué? Puedes probar y ver qué pasa.