Tp3 Control de Robots Noblia

Martín Noblía

Tp3

Control de Robots 2013

Ingeniería en Automatización y Control

Universidad Nacional de Quilmes

Ejercicio 1

Determinar el espacio de trabajo alcanzable para el manipulador de 3 brazos de la figura siguiente con

(cm), (cm), (cm), , ,

In [192]: from IPython.core.display import Image

In [193]: Image(filename='Imagenes/copy_left.png')

In [194]: Image(filename='Imagenes/dibujo_robot2_tp2.png')

In [195]: #imports

from sympy import *

import numpy as np

In [196]: #Con esto las salidas van a ser en LaTeX

init_printing(use_latex=True)

Recordemos que el espacio de trabajo alcanzable es la región espacial a la que el efector final puede llegar, con al

menos una orientación. Vamos a desarrollar primero la cinemática directa(como en el tp2) para luego evaluar

variando los angulos de articulaciones en los rangos dados y asi obtener el espacio de trabajo alcanzable.

= 15.0L1 = 10.0L2 = 3.0L3 0º < < 360ºθ1 0º < < 180ºθ2 0º < < 180ºθ3

Out[193]:

Out[194]:

In [197]: #Funcion simbólica para una rotación(transformacion homogenea) sobre el

eje X

def Rot_X(angle):

rad = angle*pi/180

M = Matrix([[1,0,0,0],[ 0,cos(rad),-sin(rad),0],[0,sin(rad), cos

(rad),0],[0,0,0,1]])

return M

#Funcion simbólica para una rotación(transformacion homogenea) sobre el

eje Y

def Rot_Y(angle):

rad = angle*pi/180

M = Matrix([[cos(rad),0,sin(rad),0],[ 0,1,0,0],[-sin(rad), 0,cos

(rad),0],[0,0,0,1]])

return M

#Funcion simbólica para una rotación(transformacion homogenea) sobre el

eje Z

def Rot_Z(angle):

rad = angle*pi/180

M = Matrix([[cos(rad),- sin(rad),0,0],[ sin(rad), cos(rad), 0,0]

,[0,0,1,0],[0,0,0,1]])

return M

In [198]: #Funcion simbolica para una traslacion en el eje X

def Traslacion_X(num):

D = Matrix([[1,0,0,num],[0,1,0,0],[0,0,1,0],[0,0,0,1]])

return D

#Funcion simbolica para una traslacion en el eje Y

def Traslacion_Y(num):

D = Matrix([[1,0,0,0],[0,1,0,num],[0,0,1,0],[0,0,0,1]])

return D

#Funcion simbolica para una traslacion en el eje Z

def Traslacion_Z(num):

D = Matrix([[1,0,0,0],[0,1,0,0],[0,0,1,num],[0,0,0,1]])

return D

In [199]: #estos son simbolos especiales que los toma como letras griegas directa

mente(muuy groso)

alpha, beta , gamma, phi, theta, a, d =symbols('alpha beta gamma phi

theta a d')

In [200]: #Generamos la transformacion

T = Rot_X(alpha) * Traslacion_X(a) * Rot_Z(theta) * Traslacion_Z(d)

T

In [201]: #Creamos los nuevos simbolos

theta_1, theta_2, theta_3, L_1, L_2, L_3 =symbols('theta_1, theta_2,

theta_3, L_1, L_2 L_3')

Out[200]: ⎡

⎣⎢⎢⎢⎢⎢⎢

cos ( πθ)1

180

sin ( πθ) cos ( πα)1

180

1

180

sin ( πα) sin ( πθ)1

180

1

180

0

− sin ( πθ)1

180

cos ( πα) cos ( πθ)1

180

1

180

sin ( πα) cos ( πθ)1

180

1

180

0

0

− sin ( πα)1

180

cos ( πα)1

180

0

a

−d sin ( πα)1

180

d cos ( πα)1

180

1

⎤

⎦⎥⎥⎥⎥⎥⎥

In [202]: T_0_1 = T.subs([(alpha,0),(a,0),(d,0),(theta,theta_1)])

T_0_1

In [203]: T_1_2 = T.subs([(alpha,90),(a,L_1),(d,0),(theta,theta_2)])

T_1_2

In [204]: T_2_3 = T.subs([(alpha,0),(a,L_2),(d,0),(theta,theta_3)])

T_2_3

In [205]: #Agregamos la ultima trama

T_w = Matrix([[1,0,0,L_3],[0,1,0,0],[0,0,1,0],[0,0,0,1]])

T_w

In [206]: T_B_W = T_0_1 * T_1_2 * T_2_3 * T_w

T_B_W.simplify()

T_B_W

In [207]: T_real = T_B_W.subs([(L_1,15),(L_2,10),(L_3,3)])

T_real

In [208]: #generamos una funcion numerica() a partir de la expresion simbolica

func = lambdify((theta_1,theta_2,theta_3),T_real,'numpy')

Out[202]: ⎡⎣⎢⎢⎢

cos ( π )1

180θ1

sin ( π )1

180θ1

00

− sin ( π )1

180θ1

cos ( π )1

180θ1

00

0

0

10

0

0

01

⎤⎦⎥⎥⎥

Out[203]: ⎡⎣⎢⎢⎢

cos ( π )1

180θ2

0

sin ( π )1

180θ2

0

− sin ( π )1

180θ2

0

cos ( π )1

180θ2

0

0

−1

0

0

L1

0

0

1

⎤⎦⎥⎥⎥

Out[204]: ⎡⎣⎢⎢⎢

cos ( π )1

180θ3

sin ( π )1

180θ3

00

− sin ( π )1

180θ3

cos ( π )1

180θ3

00

0

0

10

L2

0

01

⎤⎦⎥⎥⎥

Out[205]: ⎡⎣1000

0100

0010

L3001

⎤⎦

Out[206]: ⎡

⎣⎢⎢⎢⎢⎢⎢

(− sin ( π ) sin ( π )+cos ( π ) cos ( π )) cos ( π )1

180θ2

1

180θ3

1

180θ2

1

180θ3

1

180θ1

(− sin ( π ) sin ( π )+cos ( π ) cos ( π )) sin ( π )1

180θ2

1

180θ3

1

180θ2

1

180θ3

1

180θ1

sin ( π ) cos ( π )+sin ( π ) cos ( π )1

180θ2

1

180θ3

1

180θ3

1

180θ2

0

(− sin ( π ) cos ( π )−sin ( π1

180θ2

1

180θ3

1

180

(− sin ( π ) cos ( π )−sin (1

180θ2

1

180θ3

1

180

− sin ( π ) sin ( π )+cos1

180θ2

1

180θ3

0

Out[207]: ⎡

⎣⎢⎢⎢⎢⎢⎢

(− sin ( π ) sin ( π )+cos ( π ) cos ( π )) cos ( π )1

180θ2

1

180θ3

1

180θ2

1

180θ3

1

180θ1

(− sin ( π ) sin ( π )+cos ( π ) cos ( π )) sin ( π )1

180θ2

1

180θ3

1

180θ2

1

180θ3

1

180θ1

sin ( π ) cos ( π )+sin ( π ) cos ( π )1

180θ2

1

180θ3

1

180θ3

1

180θ2

0

(− sin ( π ) cos ( π )−sin ( π1

180θ2

1

180θ3

1

180

(− sin ( π ) cos ( π )−sin (1

180θ2

1

180θ3

1

180

− sin ( π ) sin ( π )+cos1

180θ2

1

180θ3

0

In [209]: #verificamos si funciona

func(10,30,10)

In [210]: def get_position(q_1,q_2,q_3):

"""

Funcion para extraer la posicion cartesiana de la transformacion

homogenea que describe la cinematica directa del manipulador RRR

espacial(ver ejercicio 2 tp2)

Inputs:

q_1 (angulo del link 1)



Outputs:

"""

M = func(q_1,q_2,q_3)

arr = np.asarray(M)

x = arr[0,3]

y = arr[1,3]

z = arr[2,3]

return x,y,z

In [211]: #probamos si funciona

L=get_position(10,10,10)

L

In [212]: %pylab inline

plt.rcParams['figure.figsize'] = 12,10

import mpl_toolkits.mplot3d.axes3d as axes3d

Out[209]: [[ 0.75440651 -0.63302222 0.17364818 25.56402113] [ 0.13302222 -0.1116189 -0.98480775 4.50762666] [ 0.64278761 0.76604444 0. 6.92836283] [ 0. 0. 0. 1. ]]

Out[211]: ( )27.2468291343, 4.80435111513, 2.76254220665

Populating the interactive namespace from numpy and matplotlib

WARNING: pylab import has clobbered these variables: ['prod', 'plotting', 'Circle', 'power', 'diag', 'sinh', 'trunc', 'binomial', 'plot', 'eye', 'tan', 'product', 'roots', 'vectorize', 'sin', 'transpose', 'zeros', 'cosh', 'conjugate', 'take', 'beta', 'ones', 'cos', 'interactive', 'solve', 'diff', 'invert', 'tanh', 'Polygon', 'reshape', 'sqrt', 'floor', 'source', 'add', 'multinomial', 'poly', 'mod', 'sign', 'gamma', 'log', 'var', 'seterr', 'flatten', 'nan', 'pi', 'exp', 'test']`%pylab --no-import-all` prevents importing * from pylab and numpy

In [213]: #TODO vectorizar

fig, ax = plt.subplots(subplot_kw=dict(projection='3d'))

#generamos los rangos de los angulos y evaluamos su posicion cartesiana

for i in xrange(0,360,8):

for j in xrange(0,180,8):

for k in xrange(0,180,8):

x,y,z = get_position(i,j,k)

ax.scatter(x,y,z,alpha=.2)

ax.view_init(elev=10., azim=10.)

plt.title('Espacio de trabajo alcanzable',fontsize=17)

plt.xlabel(r'$x$',fontsize=17)

plt.ylabel(r'$y$',fontsize=17)

#ax.set_aspect('equal')

plt.show()

In [214]: #TODO vectorizar

fig, ax = plt.subplots(subplot_kw=dict(projection='3d'))

#generamos los rangos de los angulos y evaluamos su posicion cartesiana

for i in xrange(0,360,8):

for j in xrange(0,180,8):

for k in xrange(0,180,8):

x,y,z = get_position(i,j,k)

ax.scatter(x,y,z,alpha=.2)

#ax.view_init(elev=10., azim=10.)


plt.xlabel(r'$x$',fontsize=17)

plt.ylabel(r'$y$',fontsize=17)

#ax.set_aspect('equal')

plt.show()

Ejercicio 2

En el manipulador 2R de la figura siguiente, y los rangos límites para las juntas son:

, . Determinar el espacio de trabajo alcanzable.

= 2L1 L20º < < 180ºθ1 −90º < < 180ºθ2

In [215]: Image(filename='Imagenes/robot2_tp3.png')

Sabemos que los puntos de la trama {3} los podemos obtener facilmente en función de los ángulos y

. Vamos a implementar la parametrización en la siguiente función:

In [216]: def brazo_RR(theta_1, theta_2, L_1, L_2):

"""

Posicion cartesiana del efector final de un brazo RR

Inputs:

theta_1(angulo del link 1)

theta_2(angulo del link 2)

L_1(Longitud del link 1)

L_2(longitud del link 2)

Outputs:

x(posicion cartesiana x del efector final)

y(posicion cartesiana y del efector final)

"""

x = L_1 * np.cos(theta_1) + L_2 * np.cos(theta_1 + theta_2)

y = L_1 * np.sin(theta_1) + L_2 * np.sin(theta_1 + theta_2)

return x, y

In [217]: theta_1_vec = np.linspace(0,np.pi,100) #vector de 100 muestras en el i

ntervalo[0,pi]

theta_2_vec = np.linspace(-np.pi/2,np.pi,100) #vector de 100 muestras

en el intervalo[-pi/2,pi]

#evaluamos a la funcion con varias combinaciones de vectores

x,y = brazo_RR(theta_1_vec,theta_2_vec,2,1)

x1,y1 = brazo_RR(0,theta_2_vec,2,1)

x2,y2 = brazo_RR(theta_1_vec,0,2,1)

#evaluamos con puntos aleatorios del rango

x3,y3 = brazo_RR(np.random.choice(theta_1_vec,2000),np.random.choice

(theta_2_vec,2000),2,1)

Out[215]:

(x, y) θ1θ2

In [218]: plt.plot(x,y , 'ro')

plt.plot(x1,y1 , 'go')

plt.plot(x2,y2, 'ko')

plt.plot(x3,y3,'yo')


plt.axis([-4,4,-2,5])

plt.legend([r'$0 < \theta_{1} < \pi$ ; $ -\pi/2 < \theta_{2} < \pi$ ',r'$\theta

_{1}=0$ ; $ -\pi/2 < \theta_{2} < \pi$ ',r'$0 < \theta_{1} < \pi$ ; $\theta_{2}

=0$','random'],fontsize=17)

plt.grid()

plt.show()

Ejercicio 3

Utilizando la substitución geométrica ‘tangente del semiángulo’, convertir la ecuación trascendental:

, esto es hallar en función de , y

La sustitución por la tangente de semiangulo es la siguiente:

para nuetro caso sustituimos en la ecuación trascendental las expresiones de

y

entonces

, luego expresamos la ecuación como un polinomio en

el siguiente paso es resolver la cuadrática:

acos(θ) + bsin(θ) = c θ a b c

u = tg( )θ2

cos(θ) = 1−u2

1+u2

sin(θ) = 2u

1+u2

acos(θ) + bsin(θ) = c cos(θ)sin(θ)

a( ) + b( ) = c1−u2

1+u22u

1+u2

a(1 − ) + b(2u) = c(1 + )u2 u2 u

(a + c) − 2bu + c − a = 0u2

Por lo tanto:

Ejercicio 4

Derive la cinemática inversa del robot RRR del ejercicio 2 de la práctica 2.

Si la transformación esta dada entonces hacemos:

y como , podemos escribir:

Además como sabemos del ejercicio 2 de la practica 2:

luego igualamos las componentes de ambas matrices, entonces:

luego igualamos los elementos , entonces:

, como vemos podemos estimar el valor de como:

Continuamos igualando los elementos y :

Entonces vemos que si

o

Luego igualando los elementos

Por último igualando los elementos y

Por lo tanto:

Ejercicio 5

Este ejercicio se enfoca en la solución de la cinemática de planteamiento inverso para el robot planar 3-DOF(tres

u = b± + −b2 a2 c2√a+c

θ = 2t ( )g−1 b± + −b2 a2 c2√a+c

TSW T = T ) T ) )B

W (BS (S

T (WT T −1

T = T )BW (0

3

T =03

⎡

⎣

⎢⎢⎢⎢⎢⎢⎢

r11

r21

r31

0

r12

r22

r32

0

r13

r23

r33

0

x

y

z

1

⎤

⎦

⎥⎥⎥⎥⎥⎥⎥

T =03

⎡

⎣

⎢⎢⎢⎢⎢⎢⎢⎢

c1c23

s1c23

s23

0

−c1c23

−s1s23

c23

0

s1

−c1

0

0

( + )c1 c2L2 L1

( + )s1 c2L2 L1

s2L2

1

⎤

⎦

⎥⎥⎥⎥⎥⎥⎥⎥(1, 3)

=s1 r13

(2, 3)

− =c1 r23 θ1

= Atan2( , − )θ1 r13 r23

(1, 4) (2, 4)

x = ( + )c1 c2L2 L1

y = ( + )s1 c2L2 L1

≠ 0c1 ∴ = ( − )c21

L2

xc1

L1

= ( − )c21

L2

y

s1L1

(3, 4) z = s2L2 ∴ = Atan2( ; )θ2z

L2c2

(3, 1) (3, 2)

=s23 r31

=c23 r32

= Atan2( ; ) −θ3 r31 r32 θ2

grados de libertad)(ver ejercicio 1 ). Se proporcionan los siguientes parámetros de longitud fija: ,

,

Derive en forma analítica y a mano, la solución de planteamiento inverso para este robot. Dado ,

calcule todas las múltiples soluciones posibles para

a.

Desarrolle un programa para resolver por completo este problema de cinemática de planteamiento

inverso para el robot planar (es decir, proporcione todas las múltiples soluciones). Pruebe su

programa utilizando los siguientes casos de entrada:

b.

c.

ii.

iii.

iv.

Para todos los casos emplee una comprobación circular para validar sus resultados: introduzca cada conjunto de

ángulos de articulación(para cada una de las múltiples soluciones ) de vuelta en el programa de planteamiento

directo para demostrar que obtiene las matrices

a)

Como sabemos las ecuaciones cinemáticas de este brazo son :

Vamos a suponer una configuración genérica del brazo relativa a la trama base, la cual es . Como estamos

trabajando con un manipulador planar, puede lograrse especificando tres números , en donde es la

orientación del vínculo 3 en el plano(relativo al eje ). Por ello nuestra transformación genérica es:

= 4L1= 3L2 = 2L3

T )(0H

[ , , ]θ1 θ2 θ3

3R

T =0H

⎡

⎣

⎢⎢⎢⎢⎢⎢⎢⎢

1

0

0

0

0

1

0

0

0

0

1

0

9

0

0

1

⎤

⎦

⎥⎥⎥⎥⎥⎥⎥⎥

T =0H

⎡

⎣

⎢⎢⎢⎢⎢⎢⎢⎢

0.5

0.866

0

0

−0.866

0.6

0

0

0

0

1

0

7.5373

3.9266

0

1

⎤

⎦

⎥⎥⎥⎥⎥⎥⎥⎥

T =0H

⎡

⎣

⎢⎢⎢⎢⎢⎢⎢⎢

0

−1

0

0

1

0

0

0

0

0

1

0

−3

2

0

1

⎤

⎦

⎥⎥⎥⎥⎥⎥⎥⎥

T =0H

⎡

⎣

⎢⎢⎢⎢⎢⎢⎢⎢

0.866

−0.5

0

0

0.5

0.866

0

0

0

0

1

0

−3.1245

9.1674

0

1

⎤

⎦

⎥⎥⎥⎥⎥⎥⎥⎥

T0H

T ) = T ) =(BW (0

3

⎡

⎣

⎢⎢⎢⎢⎢⎢⎢⎢

c123

s123

0

0

−s123

c123

0

0

0

0

1

0

+L1c1 L2c12

+L1s1 L2s12

0

1

⎤

⎦

⎥⎥⎥⎥⎥⎥⎥⎥T )(B

W

[x, y, ϕ] ϕ

X̂

T ) =(BW

⎡

⎣

⎢⎢⎢⎢⎢⎢⎢⎢

cϕ

sϕ

0

0

−sϕ

cϕ

0

0

0

0

1

0

x

y

0

1

⎤

⎦

⎥⎥⎥⎥⎥⎥⎥⎥

Todos los destinos alcanzables deben encontrarse en el subespacio implicado por la estructura de la ecuación

anterior. Si igualamos las dos matrices llegamos a las siguientes ecuaciones:

Asi si elevamos al cuadrado las últimas dos ecuaciones y las sumamos:

despejando

Entonces, vemos que para que pueda existir una solución el lado derecho de la ecuación anterior debe estar en el

intervalo

Luego suponiendo que se cumple esa condición, podemos hallar el valor del como:

Por último calculamos con la rutina de arco tangente de dos argumentos:

Dependiendo que signo hallamos elegido en la ecuación del corresponderá a una de las dos suluciones

múltiples "codo hacia arriba" o "codo hacia abajo"

Luego podemos resolver para de la siguiente manera:

sean :

en donde:

si llamamos y a entonces podemos escribir:

por lo tanto:

Usando el arreglo de dos elementos:

y por lo tanto :

Finalmente podemos resolver para la suma de a

De este último resultado podemos despejar ya que conocemos el valor de los otros ángulos.

=cϕ c123

=sϕ s123

x = +L1c1 L2c12

y = +L1s1 L2s12

+ = + + 2x2 y2 L21 L2

2 L1L2c2

c2

=c2+ − −x2 y2 L2

1 L22

2L1L2

[−1, 1]

s2

= ±s2 1 − c22

− −−−−√θ2

= Atan2( , )θ2 s2 c2

s2

θ1

x = −k1c1 k2s1

y = +k1s1 k2c1

= +k1 L1 L2c2

=k2 L2s2

r = ± +k21 k2

2

− −−−−−√ γ = Atan2( , )k2 k1

= cos(γ)cos( ) − sin(γ)sin( )xr

θ1 θ1

= cos(γ)sin( ) + sin(γ)cos( )y

rθ1 θ1

cos(γ + ) =θ1xr

sin(γ + ) =θ1y

r

γ + = Atan2( , ) = Atan2( , )θ1y

rxr

k2 k1

= Atan2(y, x) − Atan2( , )θ1 k2 k1

θ1 θ3

+ + = Atan2( , ) = ϕθ1 θ2 θ3 sϕ cϕ

θ3

b)

A continuación desarrolamos una implementación que resuelve la cinemática inversa anterior

In [219]: def inverse_kin(T, L_1, L_2):

"""

Funcion para resolver la cinematica inversa de un manipulador plana

r RRR

Inputs:

T(Matriz de tranformacion homogenea)



Outputs: Una tupla de 6 elementos con los angulos de las dos config

uraciones

codo arriba o codo abajo

(theta_1,theta_2,theta_3,theta_1_up,theta_2_up,theta_3_up)

"""

x = T[0,3]

y = T[1,3]

#calculamos si el punto es alcanzable

es_alc = (x**2 + y**2 - L_1**2 - L_2**2)/(2*L_1*L_2)

if (-1 <= es_alc <= 1):

print 'es alcanzable'

c_2 = es_alc

#Hay dos soluciones para elegir

s_2_elbow_up = np.sqrt(1-c_2**2)

s_2_elbow_down = -np.sqrt(1-c_2**2)

theta_2_up = np.arctan2(s_2_elbow_up,c_2)

theta_2_down = np.arctan2(s_2_elbow_down,c_2)

#cambio de variables

k_1 = L_1 + L_2*c_2

k_2_up = L_2*s_2_elbow_up

k_2_down = L_2*s_2_elbow_down

gamma_up = np.arctan2(k_2_up,k_1)

gamma_down = np.arctan2(k_2_down,k_1)

r_up = np.sqrt(k_1**2+k_2_up**2)

r_down= np.sqrt(k_1**2+k_2_down**2)

k_1_1 = r_up*np.cos(gamma_up)

k_1_2 = r_down*np.cos(gamma_down)

k_2_1 = r_up*np.sin(gamma_up)

k_2_2 = r_down*np.sin(gamma_down)

theta_1_up = np.arctan2(y,x) - np.arctan2(k_2_1,k_1_1)

theta_1_down = np.arctan2(y,x) - np.arctan2(k_2_2,k_1_2)

c_phi = T[0,0]

s_phi = T[1,0]

phi = np.arctan2(s_phi,c_phi)

theta_3_up = phi - theta_1_up - theta_2_up

theta_3_down = phi - theta_1_down - theta_2_down

fac = 180/np.pi #para pasar a grados

return theta_1_up*fac,theta_2_up*fac,theta_3_up*fac,theta_1_

down*fac,theta_2_down*fac,theta_3_down*fac

else:

print 'No es alcanzable'

Vamos a crear las matrices del enunciado para poder evaluarlas, además tenemos que tener en cuenta que las

transformaciones que nos dan en el enunciado son las que van de la trama base a la Herramienta, por ello

debemos transformarla para que nos quede

Donde :

In [220]: #Matriz de transformacion del insiso i

T_0_H_i = np.array([[1,0,0,9],[0,1,0,0],[0,0,1,0],[0,0,0,1]])

T_0_H_i

In [221]: #Matriz de la transformacion de la trama 3 a la herramienta

T_H_3 = np.array([[1,0,0,2],[0,1,0,0],[0,0,1,0],[0,0,0,1]])

T_H_3

In [222]: #Inversa de la matriz que representa la transformacion de la trama 3 a

la herramienta

T_3_H=np.linalg.inv(T_H_3)

T_3_H

In [223]: #Obtenemos la transformacion que necesitamos

T_0_3_i=np.dot(T_0_H_i,T_3_H)

T_0_3_i

In [224]: #calculamos los angulos(deberia dar cero(ya que en x tenemos la suma de

los L_1, L_2))

angulos_i = inverse_kin(T_0_3_i,4,3)

angulos_i

T = T ) T03 (0

H (H3 )−1

T =H3

⎡

⎣

⎢⎢⎢⎢⎢⎢⎢⎢

1

0

0

0

0

1

0

0

0

0

1

0

2

0

0

1

⎤

⎦

⎥⎥⎥⎥⎥⎥⎥⎥

Out[220]: [[1 0 0 9] [0 1 0 0] [0 0 1 0] [0 0 0 1]]

Out[221]: [[1 0 0 2] [0 1 0 0] [0 0 1 0] [0 0 0 1]]

Out[222]: [[ 1. 0. 0. -2.] [ 0. 1. 0. 0.] [ 0. 0. 1. 0.] [ 0. 0. 0. 1.]]

Out[223]: [[ 1. 0. 0. 7.] [ 0. 1. 0. 0.] [ 0. 0. 1. 0.] [ 0. 0. 0. 1.]]

es alcanzable

Out[224]: ( )0.0, 0.0, 0.0, 0.0, −0, 0.0

In [225]: #punto ii)

#Cargamos la matriz y repetimos el procedimiento anterior

T_0_H_ii = np.array([[.5,-0.866,0,7.5373],[0.866,0.6,0,3.9266],[0,0,

1,0],[0,0,0,1]])

T_0_H_ii

In [226]: T_0_3_ii = np.dot(T_0_H_ii,T_3_H)

T_0_3_ii

In [227]: #Calculamos los angulos (son 6 tres para una configuracion y tres para la otra)

angulos_ii = inverse_kin(T_0_3_ii,4,3)

angulos_ii

In [228]: #punto iii)


T_0_H_iii = np.array([[0,1,0,-3],[-1,0,0,2],[0,0,1,0],[0,0,0,1]])

T_0_H_iii

In [229]: T_0_3_iii = np.dot(T_0_H_iii,T_3_H)

T_0_3_iii

In [230]: #Calculamos los angulos (son 6 tres para una configuracion y tres para

la otra)

angulos_iii = inverse_kin(T_0_3_iii,4,3)

angulos_iii

Out[225]: [[ 0.5 -0.866 0. 7.5373] [ 0.866 0.6 0. 3.9266] [ 0. 0. 1. 0. ] [ 0. 0. 0. 1. ]]

Out[226]: [[ 0.5 -0.866 0. 6.5373] [ 0.866 0.6 0. 2.1946] [ 0. 0. 1. 0. ] [ 0. 0. 0. 1. ]]

es alcanzable

Out[227]: ( 9.99989065325, 20.0004358928, 29.9989456731, 27.1143607137, −20.0004358928, 52.8853473983

Out[228]: [[ 0 1 0 -3] [-1 0 0 2] [ 0 0 1 0] [ 0 0 0 1]]

Out[229]: [[ 0. 1. 0. -3.] [-1. 0. 0. 4.] [ 0. 0. 1. 0.] [ 0. 0. 0. 1.]]

es alcanzable

Out[230]: ( )90.0, 90.0, −270.0, 163.739795292, −90.0, −163.739795292

In [231]: #punto iv)


T_0_H_iv = np.array([[0.866,.5,0,-3.1245],[-.5,0.866,0,9.1674],[0,0,

1,0],[0,0,0,1]])

T_0_H_iv

In [232]: T_0_3_iv = np.dot(T_0_H_iv,T_3_H)

T_0_3_iv

In [233]: angulos_iv = inverse_kin(T_0_3_iv,4,3)

angulos_iv

Ahora vamos a realizar una funcion para verificar circularmente los resultados. Primero generamos la cinemática

directa como en el tp2 simbólicamente y luego la convertimos a numérica gracias a las bondades del lenguaje.

In [234]: #Generamos la cinematica directa del brazo planar RRR

T_0_1 = T.subs([(alpha,0),(a,0),(d,0),(theta,theta_1)])

T_1_2 = T.subs([(alpha,0),(a,L_1),(d,0),(theta,theta_2)])

T_2_3 = T.subs([(alpha,0),(a,L_2),(d,0),(theta,theta_3)])

T_0_3 = T_0_1 * T_1_2 * T_2_3

#Reemplazamos los valores de longitudes de link

T_0_3_real = T_0_3.subs([(L_1,4),(L_2,3)])

In [235]: #generamos una funcion numerica a partir de la simbolica

func_kin = lambdify((theta_1,theta_2,theta_3),T_0_3_real,'numpy')

In [236]: #evaluamos para los primeros tres elementos de la tupla que contiene lo

s angulos de una configuracion

func_kin(angulos_i[0],angulos_i[1],angulos_i[2])

In [237]: #evaluamos para los ultimos tres elementos de la tupla que contiene los

angulos de una configuracion

func_kin(angulos_i[3],angulos_i[4],angulos_i[5])

Out[231]: [[ 0.866 0.5 0. -3.1245] [-0.5 0.866 0. 9.1674] [ 0. 0. 1. 0. ] [ 0. 0. 0. 1. ]]

Out[232]: [[ 0.866 0.5 0. -4.8565] [ -0.5 0.866 0. 10.1674] [ 0. 0. 1. 0. ] [ 0. 0. 0. 1. ]]

No es alcanzable

Out[236]: [[ 1. -0. 0. 7.] [ 0. 1. 0. 0.] [ 0. 0. 1. 0.] [ 0. 0. 0. 1.]]

Out[237]: [[ 1. 0. 0. 7.] [ 0. 1. 0. 0.] [ 0. 0. 1. 0.] [ 0. 0. 0. 1.]]

Back to top



func_kin(angulos_ii[0],angulos_ii[1],angulos_ii[2])



func_kin(angulos_ii[3],angulos_ii[4],angulos_ii[5])



func_kin(angulos_iii[0],angulos_iii[1],angulos_iii[2])



func_kin(angulos_iii[3],angulos_iii[4],angulos_iii[5])

Vemos que vuelven a dar las mismas matrices de transformación

In [241]:

More info on IPython website (http://ipython.org). The code for this site (https://github.com/ipython

/nbviewer) is licensed under BSD (https://github.com/ipython/nbviewer/blob/master/LICENSE.txt). Some icons from

Glyphicons Free (http://glyphicons.com), built thanks to Twitter Bootstrap (http://twitter.github.com/bootstrap/)

This web site does not host notebooks, it only renders notebooks available on other websites. Thanks to all our

contributors (https://github.com/ipython/nbviewer/contributors).

Out[238]: [[ 0.500011 -0.86601905 0. 6.5373 ] [ 0.86601905 0.500011 0. 2.1946 ] [ 0. 0. 1. 0. ] [ 0. 0. 0. 1. ]]

Out[239]: [[ 0.500011 -0.86601905 0. 6.5373 ] [ 0.86601905 0.500011 0. 2.1946 ] [ 0. 0. 1. 0. ] [ 0. 0. 0. 1. ]]

Out[240]: [[ 6.12323400e-17 1.00000000e+00 0.00000000e+00 -3.00000000e+00] [ -1.00000000e+00 6.12323400e-17 0.00000000e+00 4.00000000e+00] [ 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00] [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]

Out[241]: [[ 5.55111512e-17 1.00000000e+00 0.00000000e+00 -3.00000000e+00] [ -1.00000000e+00 5.55111512e-17 0.00000000e+00 4.00000000e+00] [ 0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00] [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]

Tp3 Control de Robots Noblia

Documents

Transcript of Tp3 Control de Robots Noblia