Diseño y construcción de una impresora delta (1)

Comienzo una serie donde se estudiará, se diseñará y se construirá una impresora 3D de tipo delta lineal.

Enlaces de interés (En construcción):

Modelo 3D de la impresora OnShape

Notebooks de Jupyter donde se realizan los cálculos GitHub

Notebook específico de este post NbViewer

Descripción del Problema

Se tiene una impresora 3D de tipo delta con 3 columnas equidistantes como se muestra en la siguiente figura:

prototipo

Figura 1: Impresora 3D delta lineal a analizar

Se desea obtener:

  1. Las expresiones que resuelven el problema cinemático en función de las variables geométricas del prototipo.
  2. El cálculo del espacio de trabajo.
  3. La variación de la resolución en el espacio de trabajo.
  4. La variación de la velocidad de la herramienta.

Descripción Geométrica

Consideremos la representación simplificada:

simplificado.png

Figura 2: Impresora 3D simplificada

Ubicamos los puntos importantes de la siguiente manera

puntos.png

Figura 3: Ubicación de puntos en la impresora 3D

En la figura podemos observar los puntos:

  • O el punto central de la cama caliente.
  • A el punto justo debajo de O, está separado una distancia \epsilon que se deja a propósito para la calibración o nivelación de la plataforma.
  • B el punto entre los centros de los tubos de una columna.
  • C el punto que denota la altura del carro respecto a la coluna.
  • D el punto ubicado en el punto medio de las dos juntas esféricas superiores.
  • F el punto ubicado en el punto medio de las dos juntas esféricas inferiores.
  • G el punto ubicado encima del centro de la plataforma móvil.
  • P el punto ubicado en la punta del extrusor.

Vemos que el robot estudiado mueve la punta de la herramienta, donde se encuentra el punto P, a través del movimiento lineal de los carros en el eje Z, es decir, a través del movimiento vertical del punto C accionado por el motor paso a paso.

Entradas y salidas

El robot analizado posee como entrada las distancias verticales del carro respecto a la base (l) y como salida el movimiento del punto P en dirección x, y y z.

Estudio Cinemático

 El problema cinemático

Existen dos problemas fundamentales para resolver la cinemática del robot; el primero de ellos se conoce como el problema cinemático directo, y consiste en determinar cual es la posición y orientación del extremo final del robot, con respecto a un sistema de coordenadas que se toma como referencia, conocidos los valores de las articulaciones y los parámetros geométricos de los elementos del robot, el segundo denominado problema cinemático inverso resuelve la configuración que debe adoptar el robot para una posición y orientación del extremo conocidas (Barrientos 2007).

Grados de Libertad

Podemos calcular el número de grados de libertad usando la fórmula de Grübler:

NGDL = \lambda (n - j - 1) + \sum_{i=1}^{j} f_i

Donde:
\lambda: Grados de libertad del espacio de trabajo (3 en el plano, 6 en el espacio).
n: Número de eslabones (incluyendo la base).
j: Número de articulaciones.
f_i: Grados de libertad de la articulación i.

Aplicando la ecuación:
NGDL = 6 (17-21-1) + 21 = -9

Lo cual es incorrecto,para hacer que la fórmula funcione debemos tratar las barras paralelas como un solo eslabón por brazo.

NGDL = 6 (14-15-1) + 15 = 3

Los que corresponde a las traslaciones en los ejes x, y y z. Cabe destacar que esta configuración no posee rotación de ningún tipo, así la plataforma móvil es siempre paralela a la plataforma fija.

Planteamiento Geométrico

Si observamos de nuevo la figura 3 podemos establecer la siguiente igualdad vectorial:

^B\vec{OA}_i + ^B\vec{AB}_i + ^B\vec{BC}_i + ^B\vec{CD}_i + ^B\vec{DE}_i = ^B\vec{P_P} + ^B_P\vec{R} ^P\vec{P_i}

Donde el subíndice i indica la columna y el superíndice indica respecto a cual sistema de coordenadas, B respecto a la base o plataforma fija y P respecto a la plataforma móvil.

Como no existe rotación entre la plataforma móvil y la base ^B_P\vec{R} = 1 obteniendo entonces:

^B\vec{OA}_i + ^B\vec{AB}_i + ^B\vec{BC}_i + ^B\vec{CD}_i + ^B\vec{DE}_i = ^B\vec{P_P} + ^P\vec{PF}_i + ^P\vec{FE}_i

Entonces el problema cinemático se resume a determinar cada una de estos vectores.

Determinación de los Vectores

El vector ^B\vec{OA}_i para todas las columnas es el mismo y es ^B\vec{OA}_i = [0, 0, \epsilon ]

# Importamos la librería de álgebra simbólica
import sympy as sym
# Activamos la salida en latex
sym.init_printing()

#Creamos la variable simbólica epsilon
e = sym.Symbol("\epsilon", positive=True)

# Definimos los vectores
OA_1 = sym.Matrix([0,0,-e])
OA_2 = sym.Matrix([0,0,-e])
OA_3 = sym.Matrix([0,0,-e])

^B\vec{OA}_1 = \left[\begin{matrix}0\\0\\- \epsilon\end{matrix}\right]

^B\vec{OA}_2 = \left[\begin{matrix}0\\0\\- \epsilon\end{matrix}\right]

^B\vec{OA}_3 = \left[\begin{matrix}0\\0\\- \epsilon\end{matrix}\right]

Para determinar los vectores ^B\vec{AB}_i debemos mirar la base:

base

Figura 4:  Base o cama caliente de la impresora

Definamos la distancia entre columnas como s, sabeos que la base formará entonces un triángulo equilátero de lado s.

# Creamos la variable simbólica s
s = sym.Symbol("s", positive=True);

AB_1x = s / 2
AB_1y = -AB_1x * sym.tan(sym.pi / 6)
AB_1 = sym.Matrix([AB_1x, AB_1y, 0])

AB_2x = 0
AB_2y = (s / 2) / sym.cos(sym.pi / 6)
AB_2 = sym.Matrix([AB_2x, AB_2y, 0])

AB_3x = -s / 2
AB_3y = AB_1y
AB_3 = sym.Matrix([AB_3x, AB_3y, 0])

 

^B\vec{AB}_1 = \left[\begin{matrix}\frac{s}{2}\\- \frac{\sqrt{3} s}{6}\\0\end{matrix}\right]

^B\vec{AB}_2 = \left[\begin{matrix}0\\\frac{\sqrt{3} s}{3}\\0\end{matrix}\right]

^B\vec{AB}_3 = \left[\begin{matrix}-\frac{s}{2}\\- \frac{\sqrt{3} s}{6}\\0\end{matrix}\right]

Los vectores ^B\vec{BC}_i Estń en dirección z y tienen magnitud l_i la cual es la distancia vertical entre el carro y la base

l_1 = sym.Symbol("l_1", positive=True)
BC_1 = sym.Matrix([0, 0, l_1])

l_2 = sym.Symbol("l_2", positive=True)
BC_2 = sym.Matrix([0, 0, l_2])

l_3 = sym.Symbol("l_3", positive=True)
BC_3 = sym.Matrix([0, 0, l_3])

^B\vec{BC}_1 = \left[\begin{matrix}0\\0\\l_{1}\end{matrix}\right]

^B\vec{BC}_2 = \left[\begin{matrix}0\\0\\l_{2}\end{matrix}\right]

^B\vec{BC}_3 = \left[\begin{matrix}0\\0\\l_{3}\end{matrix}\right]

Los vectores ^B\vec{CD}_i están en la misma dirección que los ^B\vec{AB}_i pero en sentido opuesto. La magnitud de este vector la denominaremos a

a = sym.Symbol("a", positive=True)

CD_1 = -a * AB_1 / sym.sqrt((AB_1.dot(AB_1)))
CD_2 = -a * AB_2 / sym.sqrt((AB_2.dot(AB_2)))
CD_3 = -a * AB_3 / sym.sqrt((AB_3.dot(AB_3)))

^B\vec{CD}_1 = \left[\begin{matrix}- \frac{\sqrt{3} a}{2}\\\frac{a}{2}\\0\end{matrix}\right]

^B\vec{CD}_2 = \left[\begin{matrix}0\\- a\\0\end{matrix}\right]

^B\vec{CD}_3 = \left[\begin{matrix}\frac{\sqrt{3} a}{2}\\\frac{a}{2}\\0\end{matrix}\right]

Las coordenadas del punto del extrusor ^B\vec{P_P} son

x = sym.Symbol("x")
y = sym.Symbol("y")
z = sym.Symbol("z")

P_P = sym.Matrix([x, y, z])

^B\vec{P_P} = \left[\begin{matrix}x\\y\\z\end{matrix}\right]

Los vectores ^P\vec{PF}_i son todos iguales, van en dirección z y a su magnitud la llamaremos b

b = sym.Symbol("b", positive=True)

PF_1 = sym.Matrix([0, 0, b])
PF_2 = sym.Matrix([0, 0, b])
PF_3 = sym.Matrix([0, 0, b])

^P\vec{PF}_1 = \left[\begin{matrix}0\\0\\b\end{matrix}\right]

^P\vec{PF}_1 = \left[\begin{matrix}0\\0\\b\end{matrix}\right]

^P\vec{PF}_3 = \left[\begin{matrix}0\\0\\b\end{matrix}\right]

Para hallar los vectores ^P\vec{FE}_i debemos observar la plataforma móvil. Además llamamos c a la distancia entre los puntos E_i

plataforma

Figura 5: Plataforma Móvil

c = sym.Symbol("c", positive=True);

FE_1x = c / 2
FE_1y = -FE_1x * sym.tan(sym.pi / 6)
FE_1 = sym.Matrix([FE_1x, FE_1y, 0])

FE_2x = 0
FE_2y = (c / 2) / sym.cos(sym.pi / 6)
FE_2 = sym.Matrix([FE_2x, FE_2y, 0])

FE_3x = -c / 2
FE_3y = FE_1y
FE_3 = sym.Matrix([FE_3x, FE_3y, 0])

^P\vec{FE}_1 = \left[\begin{matrix}\frac{c}{2}\\- \frac{\sqrt{3} c}{6}\\0\end{matrix}\right]

^P\vec{FE}_2 =\left[\begin{matrix}0\\\frac{\sqrt{3} c}{3}\\0\end{matrix}\right]

^P\vec{FE}_3 = \left[\begin{matrix}- \frac{c}{2}\\- \frac{\sqrt{3} c}{6}\\0\end{matrix}\right]

Finalmente sustituiremos los vectores en la ecuación

^B\vec{OA}_i + ^B\vec{AB}_i + ^B\vec{BC}_i + ^B\vec{CD}_i + ^B\vec{DE}_i = ^B\vec{P_P} + ^P\vec{PF}_i + ^P\vec{FE}_i

^B\vec{DE}_i = ^B\vec{P_P} + ^P\vec{PF}_i + ^P\vec{FE}_i - ^B\vec{OA}_i - ^B\vec{AB}_i - ^B\vec{BC}_i - ^B\vec{CD}_i

# Columna 1
DE_1 = P_P + PF_1 + FE_1 - OA_1 - AB_1 - BC_1 - CD_1

# Columna 2
DE_2 = P_P + PF_2 + FE_2 - OA_2 - AB_2 - BC_2 - CD_2

# Columna 3
DE_3 = P_P + PF_3 + FE_3 - OA_3 - AB_3 - BC_3 - CD_3

^B\vec{DE}_1  = \left[\begin{matrix}\frac{\sqrt{3} a}{2} + \frac{c}{2} - \frac{s}{2} + x\\- \frac{a}{2} - \frac{\sqrt{3} c}{6} + \frac{\sqrt{3} s}{6} + y\\\epsilon + b - l_{1} + z\end{matrix}\right]

^B\vec{DE}_2 = \left[\begin{matrix}x\\a + \frac{\sqrt{3} c}{3} - \frac{\sqrt{3} s}{3} + y\\\epsilon + b - l_{2} + z\end{matrix}\right]

^B\vec{DE}_3 = \left[\begin{matrix}- \frac{\sqrt{3} a}{2} - \frac{c}{2} + \frac{s}{2} + x\\- \frac{a}{2} - \frac{\sqrt{3} c}{6} + \frac{\sqrt{3} s}{6} + y\\\epsilon + b - l_{3} + z\end{matrix}\right]

No conocemos los vectores ^B\vec{DE}_i pero sabemos que su longitud es fija y vale L así que sacamos el módulo y lo igualamos a L

L = sym.Symbol("L", positive=True)

eq_1 = sym.Eq(L**2, DE_1.dot(DE_1))
eq_2 = sym.Eq(L**2, DE_2.dot(DE_2))
eq_3 = sym.Eq(L**2, DE_3.dot(DE_3))

Obteniendo las ecuaciones:

L^{2} = \left(\epsilon + b - l_{1} + z\right)^{2} + \left(- \frac{a}{2} - \frac{\sqrt{3} c}{6} + \frac{\sqrt{3} s}{6} + y\right)^{2} + \left(\frac{\sqrt{3} a}{2} + \frac{c}{2} - \frac{s}{2} + x\right)^{2}

L^{2} = x^{2} + \left(\epsilon + b - l_{2} + z\right)^{2} + \left(a + \frac{\sqrt{3} c}{3} - \frac{\sqrt{3} s}{3} + y\right)^{2}

$L^{2} = \left(\epsilon + b – l_{3} + z\right)^{2} + \left(- \frac{a}{2} – \frac{\sqrt{3} c}{6} + \frac{\sqrt{3} s}{6} + y\right)^{2} + \left(- \frac{\sqrt{3} a}{2} – \frac{c}{2} + \frac{s}{2} + x\right)^{2}$

Donde

Variable Descripción
\epsilon Distancia vertical entre la estructura y la cama caliente, esta distancia puede variar ligeramente debido al proceso de nivelación
a Distancia entre el punto medio del centro de las guías verticales y el punto medio de los extremos superiores de las juntas esféricas
b Distancia entre el plano forado por los puntos F_i y la punta del extrusor P
c Lado del triángulo formado por los puntos E_i
l_i Distancia vertical del carro
s Distancia entre columnas
L Longitud de los brazos
x, y, z Coordenadas de los puntos de la herramienta
l1 = sym.solve(eq_1,l_1)
l2 = sym.solve(eq_2,l_2)
l3 = sym.solve(eq_3,l_3)

Las ecuaciones que describen la cinemática inversa son las siguientes:

l_1 = \left [ \epsilon + b + z - \frac{\sqrt{3}}{3} \sqrt{3 L^{2} - 3 a^{2} - 2 \sqrt{3} a c + 2 \sqrt{3} a s - 3 \sqrt{3} a x + 3 a y - c^{2} + 2 c s - 3 c x + \sqrt{3} c y - s^{2} + 3 s x - \sqrt{3} s y - 3 x^{2} - 3 y^{2}}, \quad \epsilon + b + z + \frac{\sqrt{3}}{3} \sqrt{3 L^{2} - 3 a^{2} - 2 \sqrt{3} a c + 2 \sqrt{3} a s - 3 \sqrt{3} a x + 3 a y - c^{2} + 2 c s - 3 c x + \sqrt{3} c y - s^{2} + 3 s x - \sqrt{3} s y - 3 x^{2} - 3 y^{2}}\right ]

l_2 = \left [ \epsilon + b + z - \frac{1}{3} \sqrt{9 L^{2} - 9 a^{2} - 6 \sqrt{3} a c + 6 \sqrt{3} a s - 18 a y - 3 c^{2} + 6 c s - 6 \sqrt{3} c y - 3 s^{2} + 6 \sqrt{3} s y - 9 x^{2} - 9 y^{2}}, \quad \epsilon + b + z + \frac{1}{3} \sqrt{9 L^{2} - 9 a^{2} - 6 \sqrt{3} a c + 6 \sqrt{3} a s - 18 a y - 3 c^{2} + 6 c s - 6 \sqrt{3} c y - 3 s^{2} + 6 \sqrt{3} s y - 9 x^{2} - 9 y^{2}}\right ]

l_3 = \left [ \epsilon + b + z - \frac{\sqrt{3}}{3} \sqrt{3 L^{2} - 3 a^{2} - 2 \sqrt{3} a c + 2 \sqrt{3} a s + 3 \sqrt{3} a x + 3 a y - c^{2} + 2 c s + 3 c x + \sqrt{3} c y - s^{2} - 3 s x - \sqrt{3} s y - 3 x^{2} - 3 y^{2}}, \quad \epsilon + b + z + \frac{\sqrt{3}}{3} \sqrt{3 L^{2} - 3 a^{2} - 2 \sqrt{3} a c + 2 \sqrt{3} a s + 3 \sqrt{3} a x + 3 a y - c^{2} + 2 c s + 3 c x + \sqrt{3} c y - s^{2} - 3 s x - \sqrt{3} s y - 3 x^{2} - 3 y^{2}}\right ]

Las dos soluciones que nos ha dado corresponden a los casos de la plataforma móvil encima de los carros (l_i menor) y el de la plataforma móvil debajo de los carros (l_i mayor). Así que solo tomamos la segunda solución.

l1 = l1[1]
l2 = l2[1]
l3 = l3[1]

l_1 = \epsilon + b + z + \frac{\sqrt{3}}{3} \sqrt{3 L^{2} - 3 a^{2} - 2 \sqrt{3} a c + 2 \sqrt{3} a s - 3 \sqrt{3} a x + 3 a y - c^{2} + 2 c s - 3 c x + \sqrt{3} c y - s^{2} + 3 s x - \sqrt{3} s y - 3 x^{2} - 3 y^{2}}

l_2 = \epsilon + b + z + \frac{1}{3} \sqrt{9 L^{2} - 9 a^{2} - 6 \sqrt{3} a c + 6 \sqrt{3} a s - 18 a y - 3 c^{2} + 6 c s - 6 \sqrt{3} c y - 3 s^{2} + 6 \sqrt{3} s y - 9 x^{2} - 9 y^{2}}

l_3 = \epsilon + b + z + \frac{\sqrt{3}}{3} \sqrt{3 L^{2} - 3 a^{2} - 2 \sqrt{3} a c + 2 \sqrt{3} a s + 3 \sqrt{3} a x + 3 a y - c^{2} + 2 c s + 3 c x + \sqrt{3} c y - s^{2} - 3 s x - \sqrt{3} s y - 3 x^{2} - 3 y^{2}}

 

 

 

 

Licencia Creative Commons
Diseño y construcción de una impresora delta por Eduardo Vieira se distribuye bajo una Licencia Creative Commons Atribución 4.0 Internacional.
Basada en la obra “The Delta Parallel Robot: Kinematics Solutions” de R.L. Williams II en http://www.ohio.edu/people/williar4/html/pdf/DeltaKin.pdf.
Bibliografía: Fundamentos De Robótica de: Antonio Barrientos, Luis F. Penin, Carlos Balaguer y Rafael Aracil. 2da Ed (2007).

Advertisements

CINEMÁTICA DIRECTA Y CINEMÁTICA INVERSA DE UN ROBOT DELTA ANGULAR Y DE UN ROBOT DELTA LINEAL

En esta primera entrada estudiaremos la cinemática directa de un robot delta angular y escribiremos código en python para la simulación.

El Problema

Se tiene el robot delta angular de la figura 1 (adjunto al final del post), se desea resolver el problema cinemático directo y el problema cinemático

Delta_angular

Figura 1: Robot delta angular

El problema cinemático directo

Comencemos resolviendo el problema cinemático directo. Esto es, obtener las ecuaciones que determinan la posición de la herramienta o plataforma a partir de las coordenadas articulares del robot, en este caso los tres ángulos entre la base y cada uno de los brazos superiores.

Entendiendo la geometría

Antes de comenzar detengámonos en analizar la geometría del robot delta de la figura 2.

Delta_angular_acotada

Figura 2: Robot Acotado

 

En la parte superior tenemos una placa de forma triangular que se encuentra fija, a este elemento lo llamaremos base y fijaremos un sistema de referencia en el punto B el cual se ubica en el centro del triángulo base. El sistema de referencia de la base viene dado por los vectores X_B, Y_B y Z_B. Llamaremos a los brazos 1, 2 y 3, el eslabón superior tiene una longitud L_s y el eslabón inferior una longitud de L_i para cada brazo. Consideraremos los ángulos de rotación entre la base y los eslabones superiores \theta_1, \theta_2 y \theta_3. Denotaremos H al punto donde se ubica la herramienta o plataforma cuya ubicación queremos obtener, ubicamos allí un sistema de referencia de coordenadas X_H, Y_H y Z_H que por la configuración de las barras inferiores, las cuales forman un paralelogramo, no rotarán respecto a al sistema de referencia de la base B. Finalmente llamaremos A_i, B_i y C_i (i=1,2,3) a las articulaciones.

tabla1

Tabla 1: Parámetros del robot

Grados de libertad

Podemos calcular el número de grados de libertad usando la fórmula de Grübler:

NGDL = \lambda (n - j - 1) + \sum_{i=1}^{j} f_i    (1)

Donde:
\lambda: Grados de libertad del espacio de trabajo (3 en el plano, 6 en el espacio).
n: Número de eslabones (incluyendo la base).
j: Número de articulaciones.
f_i: Grados de libertad de la articulación i.

Aplicando la ecuación:
NGDL = 6 (17-21-1) + 21 = -9

Lo cual es incorrecto,para hacer que la fórmula funcione debemos tratar las barras paralelas como un solo eslabón por brazo.

NGDL = 6 (14-15-1) + 15 = 3

Planteamiento geométrico

Si observamos de nuevo la figura 2 podemos establecer la siguiente igualdad vectorial

^{B}\vec{A_i}+^B\vec{L_{si}}+^B\vec{L_{ii}} = ^B\vec{P_P} + ^B_P\vec{R} ^P\vec{P_i}    (2)

Como no existe rotación entre los sistemas en B y en H tenemos que ^B_P\vec{R}=1 quedando entonces

^{B}\vec{A_i}+^B\vec{L_{si}}+^B\vec{L_{ii}} = ^B\vec{P_P} + ^P\vec{C_i}    (3)

^B\vec{L_{ii}}= ^B\vec{P_P} + ^P\vec{C_i}-^B\vec{L_{si}} -^{B}\vec{A_i}    (4)

El vector ^B\vec{P_P} determina la posición de la herramienta respecto a B y tiene coordenadas

^B\vec{P_P} = \left[ X_H, Y_H, Z_H \right]

Para determinar ^{B}\vec{A_i} detallemos la figura 3.

Base

Figura 3: Base

Con el uso de trigonometría podemos determinar las coordenadas de los ^{B}\vec{A_i}

^{B}\vec{A_1} = [0, -{{\sqrt{3}}\over{6}} S_B, 0]

^{B}\vec{A_2} = [{{1}\over{4}} S_B, {{\sqrt{3}}\over{12}} S_B, 0]

^{B}\vec{A_3} = [ -{{1}\over{4}} S_B, {{\sqrt{3}}\over{12}} S_B, 0]

Para determinar los vectores ^P\vec{C_i} lo hacemos desde la representación de la plataforma móvil (figura 4).

 

Herramienta

Figura 4: Plataforma móvil

Entonces las coordenadas de los ^P\vec{C_i} serán:

^P\vec{C_1} = [0, -{{\sqrt{3}}\over{3}} S_H, 0]

^P\vec{C_2} = [{{S_H}\over{2}}, {{\sqrt{3}}\over{6}} S_H, 0]

^P\vec{C_3} = [-{{S_H}\over{2}}, {{\sqrt{3}}\over{6}} S_H, 0]

Para determinar ^B\vec{L_{si}} debemos tomar en cuenta que tenemos su módulo L_s y su ángulo con respecto a la plataforma \theta_i

brazo.png

Figura 5: Eslabón superior

^B\vec{L_{s1}} = [0, -L_s cos(\theta_1), -L_s sin(\theta_1)]

^B\vec{L_{s2}} = [{{\sqrt{3}}\over{2}} L_s cos(\theta_2), {{1}\over{2}} L_s cos(\theta_2), -L_s sin(\theta_2)]

^B\vec{L_{s3}} = [-{{\sqrt{3}}\over{2}} L_s cos(\theta_3), {{1}\over{2}} L_s cos(\theta_3), {{1}\over{2}} L_s sin(\theta_3)]

Sustituyendo en la ecuación (4) obtenemos:

ecuaciones

No conocemos el valor de ^B\vec{L_{ii}} pero sabemos que su magnitud es L_i, así que tomamos la norma de las expresiones anteriores obteniendo

ecuaciones_mod.png

Para resolver este sistema llevaremos las ecuaciones anteriores a la forma

r^2 = (x -a_x)^2+(y-a_y)^2+(z-a_z)^2

Que corresponde a la representación espacial de una esfera. Siendo entonces nuestro problema la intersección de las 3 esferas.

esf.png

tabla_const.png

Tabla 2: Constantes a, b y c

Código en Python para la solución interactiva del problema

Se he escrito el siguiente código en Python 3 para la solución del problema cinemático directo de manera interactiva. Además se reprsenta gráficamente la solución.

Si quieres ejecutar y experimentar con el código interactivo puedes copiar el código y ejecutarlo en un Jupyter Notebook en https://cloud.sagemath.com/ seleccionando el kernel Anaconda Python 3.

from scipy.optimize import root
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from ipywidgets import interact, FloatSlider, interactive
from IPython.display import display
%matplotlib inline
#plt.style.use('default') # descomentar en /uncomment in: matplotlib 1.5.1


def interact(Sh,Sb,Ls,Li,theta1,theta2,theta3):
f = np.zeros(3)
a = np.zeros(3)
b = np.zeros(3)
c = np.zeros(3)
p = np.zeros(3)
a[0] = 0
a[1] = -np.sqrt(3)*Sh/3+Ls*np.cos(theta1)-np.sqrt(3)*Sb/6
a[2] = Ls*np.sin(theta1)
b[0] = Sh/2-(np.sqrt(3)/2)*Ls*np.cos(theta2)-Sb/4
b[1] = (1/6)*np.sqrt(3)*Sh-Ls*np.cos(theta2)/2-(1/12)*np.sqrt(3)*Sb
b[2] = Ls*np.sin(theta2)
c[0] = -Sh/2+np.sqrt(3)*Ls*np.cos(theta3)/2+Sb/4
c[1] = (1/6)*np.sqrt(3)*Sh-Ls*np.cos(theta3)/2-(1/12)*np.sqrt(3)*Sb
c[2] = Ls*np.sin(theta3)


def f(x):
return [
(x[0] + a[0])**2 + (x[1] + a[1])**2 + (x[2] + a[2])**2 - Li**2,
(x[0] + b[0])**2 + (x[1] + b[1])**2 + (x[2] + b[2])**2 - Li**2,
(x[0] + c[0])**2 + (x[1] + c[1])**2 + (x[2] + c[2])**2 - Li**2
]
def jac(x):
return np.array([[2*(a[0]+x[0]),2*(a[1]+x[1]),2*(a[1]+x[1])],
[2*(b[0]+x[0]),2*(b[1]+x[1]),2*(b[1]+x[1])],
[2*(c[0]+x[0]),2*(c[1]+x[1]),2*(c[1]+x[1])]])

sol = root(f, [0.0, 0.0, -0.5], jac=jac)
fig = plt.figure(figsize=(10,10))
ax = fig.gca(projection='3d',aspect=1,xlim=[-1,1],ylim=[-1,1])
ax.scatter(sol.x[0],sol.x[1],sol.x[2])
ax.plot([0,0,0,sol.x[0],sol.x[0]],
[0,-np.sqrt(3)/6*Sb,-np.sqrt(3)/6*Sb-Ls*np.cos(theta1),sol.x[1]-np.sqrt(3)/3*Sh,sol.x[1]],
[0,0,-Ls*np.sin(theta1),sol.x[2],sol.x[2]],'r')
ax.plot([0,Sb/4,(1/4)*Sb+np.sqrt(3)*Ls*np.cos(theta2)/2,sol.x[0]+Sh/2,sol.x[0]],
[0,np.sqrt(3)/12*Sb,np.sqrt(3)*Sb/12+Ls*np.cos(theta2)/2,sol.x[1]+np.sqrt(3)*Sh/6,sol.x[1]],
[0,0,-Ls*np.sin(theta2),sol.x[2],sol.x[2]],'g')
ax.plot([0,-(1/4)*Sb,-Sb/4-np.sqrt(3)*Ls*np.cos(theta3)/2,sol.x[0]-Sh/2,sol.x[0]],
[0,np.sqrt(3)*Sb/12,np.sqrt(3)*Sb/12+Ls*np.cos(theta3)/2,sol.x[1]+np.sqrt(3)*Sh/6,sol.x[1]],
[0,0,-Ls*np.sin(theta3),sol.x[2],sol.x[2]],'k')
print("xp=",sol.x[0]," yp=",sol.x[1]," zp=",sol.x[2])

theta1_slider = FloatSlider(min=0, max=np.pi/2, step=0.1, value=np.pi/6, description='Angulo Theta1')
theta2_slider = FloatSlider(min=0, max=np.pi/2, step=0.1, value=np.pi/6, description='Angulo Theta2')
theta3_slider = FloatSlider(min=0, max=np.pi/2, step=0.1, value=np.pi/6, description='Angulo Theta3')
Sh_slider = FloatSlider(min=0.05, max=1, step=0.05, value=0.1, description='Lado triángulo herramienta')
Sb_slider = FloatSlider(min=0.05, max=1, step=0.05, value=0.1, description='Lado triángulo base')
Ls_slider = FloatSlider(min=0.1, max=2, step=0.1, value=0.2, description='Long. eslabón superior')
Li_slider = FloatSlider(min=0.1, max=2, step=0.1, value=0.3, description='Long. eslabón inferior')
w=interactive(interact,Sh=0.1,Sb=0.1,Ls=0.2,Li=0.3,theta1=theta1_slider,theta2=theta2_slider,theta3=theta3_slider)
display(w)

Produciendo una salida como la que se muestra en la siguiente captura:

 

Captura de pantalla de 2016-02-05 12:33:50

 

 

 

Licencia Creative Commons
CINEMÁTICA DIRECTA Y CINEMÁTICA INVERSA DE UN ROBOT DELTA ANGULAR Y DE UN ROBOT DELTA LINEAL por Eduardo Vieira se distribuye bajo una Licencia Creative Commons Atribución 4.0 Internacional.
Basada en la obra “The Delta Parallel Robot: Kinematics Solutions” de R.L. Williams II en http://www.ohio.edu/people/williar4/html/pdf/DeltaKin.pdf.