Saltar al contenido principal

# 15.2: Cinemática de avance 2D

$$\newcommand{\vecs}[1]{\overset { \scriptstyle \rightharpoonup} {\mathbf{#1}} }$$

$$\newcommand{\vecd}[1]{\overset{-\!-\!\rightharpoonup}{\vphantom{a}\smash {#1}}}$$

$$\newcommand{\id}{\mathrm{id}}$$ $$\newcommand{\Span}{\mathrm{span}}$$

( \newcommand{\kernel}{\mathrm{null}\,}\) $$\newcommand{\range}{\mathrm{range}\,}$$

$$\newcommand{\RealPart}{\mathrm{Re}}$$ $$\newcommand{\ImaginaryPart}{\mathrm{Im}}$$

$$\newcommand{\Argument}{\mathrm{Arg}}$$ $$\newcommand{\norm}[1]{\| #1 \|}$$

$$\newcommand{\inner}[2]{\langle #1, #2 \rangle}$$

$$\newcommand{\Span}{\mathrm{span}}$$

$$\newcommand{\id}{\mathrm{id}}$$

$$\newcommand{\Span}{\mathrm{span}}$$

$$\newcommand{\kernel}{\mathrm{null}\,}$$

$$\newcommand{\range}{\mathrm{range}\,}$$

$$\newcommand{\RealPart}{\mathrm{Re}}$$

$$\newcommand{\ImaginaryPart}{\mathrm{Im}}$$

$$\newcommand{\Argument}{\mathrm{Arg}}$$

$$\newcommand{\norm}[1]{\| #1 \|}$$

$$\newcommand{\inner}[2]{\langle #1, #2 \rangle}$$

$$\newcommand{\Span}{\mathrm{span}}$$ $$\newcommand{\AA}{\unicode[.8,0]{x212B}}$$

$$\newcommand{\vectorA}[1]{\vec{#1}} % arrow$$

$$\newcommand{\vectorAt}[1]{\vec{\text{#1}}} % arrow$$

$$\newcommand{\vectorB}[1]{\overset { \scriptstyle \rightharpoonup} {\mathbf{#1}} }$$

$$\newcommand{\vectorC}[1]{\textbf{#1}}$$

$$\newcommand{\vectorD}[1]{\overrightarrow{#1}}$$

$$\newcommand{\vectorDt}[1]{\overrightarrow{\text{#1}}}$$

$$\newcommand{\vectE}[1]{\overset{-\!-\!\rightharpoonup}{\vphantom{a}\smash{\mathbf {#1}}}}$$

$$\newcommand{\vecs}[1]{\overset { \scriptstyle \rightharpoonup} {\mathbf{#1}} }$$

$$\newcommand{\vecd}[1]{\overset{-\!-\!\rightharpoonup}{\vphantom{a}\smash {#1}}}$$

$$\newcommand{\avec}{\mathbf a}$$ $$\newcommand{\bvec}{\mathbf b}$$ $$\newcommand{\cvec}{\mathbf c}$$ $$\newcommand{\dvec}{\mathbf d}$$ $$\newcommand{\dtil}{\widetilde{\mathbf d}}$$ $$\newcommand{\evec}{\mathbf e}$$ $$\newcommand{\fvec}{\mathbf f}$$ $$\newcommand{\nvec}{\mathbf n}$$ $$\newcommand{\pvec}{\mathbf p}$$ $$\newcommand{\qvec}{\mathbf q}$$ $$\newcommand{\svec}{\mathbf s}$$ $$\newcommand{\tvec}{\mathbf t}$$ $$\newcommand{\uvec}{\mathbf u}$$ $$\newcommand{\vvec}{\mathbf v}$$ $$\newcommand{\wvec}{\mathbf w}$$ $$\newcommand{\xvec}{\mathbf x}$$ $$\newcommand{\yvec}{\mathbf y}$$ $$\newcommand{\zvec}{\mathbf z}$$ $$\newcommand{\rvec}{\mathbf r}$$ $$\newcommand{\mvec}{\mathbf m}$$ $$\newcommand{\zerovec}{\mathbf 0}$$ $$\newcommand{\onevec}{\mathbf 1}$$ $$\newcommand{\real}{\mathbb R}$$ $$\newcommand{\twovec}[2]{\left[\begin{array}{r}#1 \\ #2 \end{array}\right]}$$ $$\newcommand{\ctwovec}[2]{\left[\begin{array}{c}#1 \\ #2 \end{array}\right]}$$ $$\newcommand{\threevec}[3]{\left[\begin{array}{r}#1 \\ #2 \\ #3 \end{array}\right]}$$ $$\newcommand{\cthreevec}[3]{\left[\begin{array}{c}#1 \\ #2 \\ #3 \end{array}\right]}$$ $$\newcommand{\fourvec}[4]{\left[\begin{array}{r}#1 \\ #2 \\ #3 \\ #4 \end{array}\right]}$$ $$\newcommand{\cfourvec}[4]{\left[\begin{array}{c}#1 \\ #2 \\ #3 \\ #4 \end{array}\right]}$$ $$\newcommand{\fivevec}[5]{\left[\begin{array}{r}#1 \\ #2 \\ #3 \\ #4 \\ #5 \\ \end{array}\right]}$$ $$\newcommand{\cfivevec}[5]{\left[\begin{array}{c}#1 \\ #2 \\ #3 \\ #4 \\ #5 \\ \end{array}\right]}$$ $$\newcommand{\mattwo}[4]{\left[\begin{array}{rr}#1 \amp #2 \\ #3 \amp #4 \\ \end{array}\right]}$$ $$\newcommand{\laspan}[1]{\text{Span}\{#1\}}$$ $$\newcommand{\bcal}{\cal B}$$ $$\newcommand{\ccal}{\cal C}$$ $$\newcommand{\scal}{\cal S}$$ $$\newcommand{\wcal}{\cal W}$$ $$\newcommand{\ecal}{\cal E}$$ $$\newcommand{\coords}[2]{\left\{#1\right\}_{#2}}$$ $$\newcommand{\gray}[1]{\color{gray}{#1}}$$ $$\newcommand{\lgray}[1]{\color{lightgray}{#1}}$$ $$\newcommand{\rank}{\operatorname{rank}}$$ $$\newcommand{\row}{\text{Row}}$$ $$\newcommand{\col}{\text{Col}}$$ $$\renewcommand{\row}{\text{Row}}$$ $$\newcommand{\nul}{\text{Nul}}$$ $$\newcommand{\var}{\text{Var}}$$ $$\newcommand{\corr}{\text{corr}}$$ $$\newcommand{\len}[1]{\left|#1\right|}$$ $$\newcommand{\bbar}{\overline{\bvec}}$$ $$\newcommand{\bhat}{\widehat{\bvec}}$$ $$\newcommand{\bperp}{\bvec^\perp}$$ $$\newcommand{\xhat}{\widehat{\xvec}}$$ $$\newcommand{\vhat}{\widehat{\vvec}}$$ $$\newcommand{\uhat}{\widehat{\uvec}}$$ $$\newcommand{\what}{\widehat{\wvec}}$$ $$\newcommand{\Sighat}{\widehat{\Sigma}}$$ $$\newcommand{\lt}{<}$$ $$\newcommand{\gt}{>}$$ $$\newcommand{\amp}{&}$$ $$\definecolor{fillinmathshade}{gray}{0.9}$$
%matplotlib inline
import matplotlib.pylab as plt
import numpy as np
from ipywidgets import interact
import sympy as sym
sym.init_printing(True)

Este robot puede moverse en el$$x−y$$ avión. Podemos representar la configuración del robot en su “Espacio Conjunto” conociendo los dos ángulos de articulación o$$[a_1,a_2]$$. Sin embargo lo que nos gustaría es representar la ubicación del extremo del robot (a menudo llamado el “efector final” o “mano”) en coordenadas “mundiales” (es decir,$$x−y$$ sistema de coordenadas).

Hoy, utilizaremos Álgebra Lineal y matrices de transformación simples para intentar calcular cómo pasar de coordenadas “conjuntas” a coordenadas “mundiales”.

from IPython.display import YouTubeVideo
YouTubeVideo("aCohcLYrYcY",width=640,height=360, cc_load_policy=True)

### Robot de un solo eje

El siguiente código dibuja un robot simple de un solo eje (junta simple) con su articulación centrada en el origen y su ángulo inicial de cero con una longitud de brazo de robot de 4 “unidades” de largo.

plt.scatter(4,0, s=200, facecolors='none', edgecolors='r') #plot end effector
plt.scatter(0,0, s=200, facecolors='r', edgecolors='r') # plot origin
plt.plot([0,4],[0,0]) #plot blue line for arm
plt.axis('square')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])
# 'Run' this cell to see the output

Una matriz de rotación 2D alrededor del origen se define de la siguiente manera:

\ [\ begin {split}
\ left [\ begin {matrix}
x_ {end}\\
y_ {end}
\ end {matrix}
\ right]
=
\ left [\ begin {matrix}
\ cos (a) & -\ sin (a)\\
\ sin (a) &\ cos (qa
\ end {matrix}
\ right]
\ left [\ begin {matrix}
x_ {start}\\
y_ {start}
\ end {matrix}
\ right]
\ end {split}\ nonumber\]

La siguiente matriz de rotación girará el punto$$45^o$$ alrededor del origen:

p = [[4],[0]]

a1=np.pi/4

R = np.matrix([[np.cos(a1), -np.sin(a1)], [np.sin(a1), np.cos(a1)]])

p2 = R*p

x1 = p2[0,0]
y1 = p2[1,0]

plt.scatter(x1,y1, s=200, facecolors='none', edgecolors='r') #plot end effector
plt.scatter(0,0, s=200, facecolors='r', edgecolors='r') # plot origin
plt.plot([0,x1],[0,y1]) #plot blue line for arm
plt.axis('square')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])
sym.Matrix(R)

El siguiente código utiliza la función Jupyter interact y numpy para hacer una vista interactiva de lo anterior. Esto nos permite cambiar el valor del motor de rotación y ver cómo cambia el robot. La entrada a la función es el ángulo del eje y la salida son las$$x−y$$ coordenadas.

##### Nota

Puede tomar algún tiempo para que la interacción se ponga al día. Intenta mover el deslizador lentamente...

def Robot_Simulator(q1=0):
a1 = q1/180  * np.pi
p0 = np.matrix([4,0]).T
p = p0
J1 = np.matrix([[np.cos(a1), -np.sin(a1)], [np.sin(a1), np.cos(a1)]])
p = np.concatenate( ( J1*p, np.matrix([0,0]).T), axis=1 )

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=200, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=200, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.axis('square')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])
ax = plt.gca()
plt.show()
return([ p[0,0], p[1,0] ])

p = interact(Robot_Simulator, q1=(-180,180,2));
0
##### Hacer esto

Inspeccione el código anterior.

##### Pregunta

¿Qué línea del código define la matriz de transformación para la articulación del robot?

##### Pregunta

¿Qué línea de código define la longitud del brazo del robot?

### Robot multieje

Ahora, consideremos el robot del video:

Observe que tiene dos juntas$$a_1$$ y$$a_2$$ y longitudes de desplazamiento de$$d_1$$,$$d_2$$ y$$d_e$$. El espacio conjunto para este robot son solo sus ángulos$$[a_1,a_2]$$. Sin embargo, lo que queremos es conocer la ubicación del punto efector final$$p_e$$ en la garra en el marco de referencia “mundial”, que la parte inferior más axies “en el suelo”.

En cada articulación, podemos definir un marco de referencia que gira y luego transforma el origen a la articulación ealier. Las matrices de transformación hacia adelante capturan la relación entre los marcos de referencia de diferentes enlaces del robot.

Por ejemplo, podemos pasar del motor base, o$$p_1$$, marco de referencia al mundo, o$$p_w$$, marco de referencia usando las siguientes ecuaciones:

\ [\ begin {split}
p_w
=
\ left [\ begin {matrix}
\ cos (a_1) & -\ sin (a_1)\\
\ sin (a_1) &\ cos (a_1)
\ end {matrix}
\ derecha]
p_1
+
\ left [\ begin {matrix}
0\\
d_1
\ fin {matriz}
\ derecha].
\ end {split}\ nonumber\]

La ecuación que se muestra arriba es un poco difícil de trabajar porque la parte\ (\ left [\ begin {matrix}
0\\
d_1
\ end {matrix}
\ right]\) hace que la ecuación no sea lineal (¿si no me crees? Recuerda las reglas para hacer lineal una función y aplícalas y compruébalo por ti mismo). Sin embargo, hay un truco fácil en Álgebra Lineal para convertir lo anterior en una matriz lineal grande. Este truco requiere que mantengamos un 1 (uno) extra por cada punto pero hace que las matemáticas funcionen muy bien. Básicamente el truco funciona de la siguiente manera:

\ [\ start {split}
\ left [\ begin {matrix}
x_w\\
y_w\\
1
\ end {matrix}
\ right]
=
\ left [\ begin {matrix}
cos (a_1) & -sin (a_1) & 0\\
sin ( a_1) & cos (a_1) & d_1\\
0 & 0 & 1
\ end {matriz}
\ derecha]
\ izquierda [\ comenzar {matriz}
x_1\\
y_1\\
1
\ end {matriz}
\ derecha]
\ fin {división}\ nonumber\]

Llamemos a la matriz de transformación para Joint 1,$$J_1$$, y podemos reescribir las ecuaciones de la siguiente manera:

$p_1 = J_1p_2 \nonumber$

##### Hacer esto

Por tu cuenta, escribe la multiplicación matricial anterior y convénzate de que es la misma que la anterior.

Ahora, podemos pasar del marco de$$p_2$$ referencia al marco de$$p_1$$ referencia básicamente la misma ecuación:

\ [\ start {split}
\ left [\ begin {matrix}
x_1\\
y_1\\
1
\ end {matrix}
\ right]
=
\ left [\ begin {matrix}
cos (a_2) & -sin (a_2) & d_2\\
sin (a_2) & cos (a_2) & 0\\
0 & 0 & 1
\ end {matriz}
\ derecha]
\ izquierda [\ comenzar {matriz}
x_2\\
y_2\\
1
\ end {matriz}
\ derecha]
\ end {split}\ nonumber\]

Para el último paso podemos hacer una transposición lineal simple desde el marco de$$p_e$$ referencia del efector final al marco de$$p_2$$ referencia:

\ [\ start {split}
\ left [\ begin {matrix}
x_2\\
y_2\\
1
\ end {matrix}
\ right]
=
\ left [\ begin {matrix}
1 & 0 & d_2\\
0 & 1 & 0 \\
0 & 0 & 1
\ end {matrix}
\ right]
\ left [\ begin {matrix}
x_e\\
y_e\\
1
\ end {matrix}
\ right]
\ end {split}\ nonumber\]

Si llamamos a cada matriz de transformación$$J_1$$,$$J_2$$,$$J_e$$ entonces ojalá pueda ver que podemos encadenar estas matrices de transformación juntas de tal manera que obtengamos una sola transformación desde el efector final hasta las coordenadas mundiales de la siguiente manera:

$p_w = J_1J_2J_ep_e \nonumber$

Veamos cómo se ve esto en Python. Voy a usar numpy. La trama se vuelve un poco incómoda pero ojalá tenga sentido.

Primero, inicialicemos las variables a algunos números discretos:

%matplotlib inline
import matplotlib.pylab as plt
import numpy as np
from ipywidgets import interact

#Inicial state
a1 = 0
a2 = 0

#Lenths of the offsets
d1 = 0.5
d2 = 3
de = 3

A continuación, voy a definir un conjunto de puntos en el sistema de coordenadas del efector final. Estos puntos se escogen para formar una especie de forma de “C” diseñada para parecerse a un efector final. Los voy a trazar para que te ayuden a mostrarte a lo que me refiero:

#Points needed to define a square
pe = np.matrix([[1, 0.5, 1],[0,0.5,1],[0,-0.5, 1],[1,-0.5, 1],[0,-0.5, 1],[0,0, 1]]).T
p = pe

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.axis('scaled')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])
# 'Run' this cell to see the output

El siguiente paso es aplicar la matriz de$$J_e$$ transformación a los puntos de agarre que los colocará en el sistema de$$p_2$$ coordenadas. Una vez transpuestos los puntos el código concatena el origen (0,0) en la lista de puntos para que podamos formar parte del brazo robot en la gráfica:

Je = np.matrix([[1, 0, de],
[0, 1, 0],
[0,0,1]])

p = np.concatenate( ( Je*p, np.matrix([0,0,1]).T), axis=1 )

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.axis('scaled')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])

Esto lo hacemos de nuevo. Aplicar la matriz de$$J_2$$ transformación para poner los puntos en el sistema de$$p_1$$ coordenadas, concatenar el origen y trazar los resultados.

J2 = np.matrix([[np.cos(a2), -np.sin(a2), 0],
[np.sin(a2), np.cos(a2), d2],
[0,0,1]])

p = np.concatenate( ( J2*p, np.matrix([0,0,1]).T), axis=1 )

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.axis('scaled')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])

Lo hacemos una vez más. Aplicar la matriz de$$J_1$$ transformación que pondrá los puntos en el sistema de$$p_w$$ coordenadas, concatenar el origen y trazar los resultados. El resultado es un marco esquelético que representa a nuestro robot.

J1 = np.matrix([[np.cos(a1), -np.sin(a1), 0],
[np.sin(a1), np.cos(a1), d1],
[0,0,1]])

p = np.concatenate( ( J1*p, np.matrix([0,0,1]).T), axis=1 )

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.axis('scaled')
plt.xlim([-8,8])
plt.ylim([-8,8])
##### Hacer esto

Modifique las variables de rotación a1 y a2 en el código anterior y vea si la nueva configuración del robot se ve bien.

HINTA asegúrese de que sus ángulos estén en radianes.

El siguiente es el mismo código que el anterior pero puesto en una función interactiva para que el código sea más fácil de jugar con:

from ipywidgets import interact

def Robot_Simulator(q1=0,q2=-0):
a1 = q1/180  * np.pi
a2 = q2/180  * np.pi

d1 = 0.5
d2 = 3
de = 3

target = np.matrix([-3,2, 1])
print(target)

pe = np.matrix([[1, 0.5, 1],[0,0.5,1],[0,-0.5, 1],[1,-0.5, 1],[0,-0.5, 1],[0,0, 1]]).T

Je = np.matrix([[1, 0, de],
[0, 1, 0],
[0,0,1]])
p = np.concatenate( ( Je*pe, np.matrix([0,0,1]).T), axis=1 )

J2 = np.matrix([[np.cos(a2), -np.sin(a2), 0],
[np.sin(a2), np.cos(a2), d2],
[0,0,1]])
p = np.concatenate( ( J2*p, np.matrix([0,0,1]).T), axis=1 )

J1 = np.matrix([[np.cos(a1), -np.sin(a1), 0],
[np.sin(a1), np.cos(a1), d1],
[0,0,1]])
p = np.concatenate( ( J1*p, np.matrix([0,0,1]).T), axis=1 )

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.plot(target[0,0], target[0,1],'*')
plt.axis('scaled')
plt.xlim([-8,8])
plt.ylim([-8,8])

plt.show()

target = interact(Robot_Simulator, q1=(-180,180), q2=(-180,180));
0
0
##### Nota

Si la trama de interacción es realmente entrecortada en su máquina esto se debe a que la función se está actualizando constantemente a medida que mueve los controles deslizantes. Considera reemplazar la función interact por interact_manual para eliminar las actualizaciones continuas. Tendrás que cambiar el código de importación en la línea 1.

##### Pregunta

Mueve el robot anterior para que el efector final esté “agarrando” el objetivo (estrella amarilla/anaranjada). Observe que hay más de un punto en el “espacio conjunto” que da la misma respuesta. Este es el problema cinemático inverso (que es más difícil). Sabemos el punto que queremos pero necesitamos encontrar las articulaciones que pongan al robot en ese punto.

##### Hacer esto

El código en la siguiente celda se corta y pega desde arriba. Modifique el código para agregar un tercer Joint al robot.

from ipywidgets import interact

def Robot_Simulator(q1=0,q2=-0):
a1 = q1/180  * np.pi
a2 = q2/180  * np.pi

d1 = 0.5
d2 = 3
de = 3

target = np.matrix([-3,2, 1])
print(target)

pe = np.matrix([[1, 0.5, 1],[0,0.5,1],[0,-0.5, 1],[1,-0.5, 1],[0,-0.5, 1],[0,0, 1]]).T

Je = np.matrix([[1, 0, de],
[0, 1, 0],
[0,0,1]])
p = np.concatenate( ( Je*pe, np.matrix([0,0,1]).T), axis=1 )

J2 = np.matrix([[np.cos(a2), -np.sin(a2), 0],
[np.sin(a2), np.cos(a2), d2],
[0,0,1]])
p = np.concatenate( ( J2*p, np.matrix([0,0,1]).T), axis=1 )

J1 = np.matrix([[np.cos(a1), -np.sin(a1), 0],
[np.sin(a1), np.cos(a1), d1],
[0,0,1]])

p = np.concatenate( ( J1*p, np.matrix([0,0,1]).T), axis=1 )

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.plot(target[0,0], target[0,1],'*')
plt.axis('scaled')
plt.xlim([-8,8])
plt.ylim([-8,8])

plt.show()

target = interact(Robot_Simulator, q1=(-180,180), q2=(-180,180));
0
0
##### Pregunta

Vuelva a hacer la cinemática inversa, y encuentre tres ángulos que coloquen al robot sobre la estrella.

This page titled 15.2: Cinemática de avance 2D is shared under a CC BY-NC 4.0 license and was authored, remixed, and/or curated by Dirk Colbry via source content that was edited to the style and standards of the LibreTexts platform.