cychitivav/RPR.md

## RPR.md

      
    Raw
  

              RPR.md
            
          
    Robot RPR


¿Cuántas soluciones posibles de la cinemática inversa existen como máximo?
Rta./
Debido a que la articulación 3 está regida por $L_3$ y la orientación de la herramienta, la posición de esta articulación es única. Por lo tanto, las cantidad de soluciones depende de ¿cuántas posiciones puede tomar la articulación 2 y la 1?. Ahora, suponiendo que no existen limites articulares ni restricciones de movimiento, existen dos posibles soluciones para la cinemática inversa, una con $q_3$ negativo y otra con $q_3$ positivo, como se muestra en la siguiente figura.
 

Configuración 1
Configuración 2


Si existen restricciones de movimiento (como pasaría en la realidad), la articulación 3 no podría pasar por el cilindro de la articulación 2, es decir, $q_3$ no podría ser negativo y solo existiría una solución.


Determinar el modelo cinemático inverso de manera analítica
Rta./
La tabla DH para este robot es:
 

$i$
$\theta_i$
$d_i$
$a_i$
$\alpha_i$


$1$
$q_1$
$0$
$L_1$
$\pi/2$


$2$
$0$
$q_2$
$0$
$-\pi/2$


$3$
$q_3$
$0$
$L_3$
$0$


Por lo tanto, las matrices de transformación homogénea para cada eslabón son:
$$
\begin{gather*}
T_1^0 = \begin{bmatrix}
\cos(q_1) & 0 & \sin(q_1) & L_1\cos(q_1) \
\sin(q_1) & 0 & -\cos(q_1) & L_1\sin(q_1) \
0 & 1 & 0 & 0 \
0 & 0 & 0 & 1
\end{bmatrix}\
T_2^1 = \begin{bmatrix}
1 & 0 & 0 & 0 \
0 & 0 & 1 & 0 \
0 & -1 & 0 & q_2 \
0 & 0 & 0 & 1
\end{bmatrix}\
T_3^2 = \begin{bmatrix}
\cos(q_3) & -\sin(q_3) & 0 & L_3\cos(q_3) \
\sin(q_3) & \cos(q_3) & 0 & L_3\sin(q_3) \
0 & 0 & 1 & 0 \
0 & 0 & 0 & 1
\end{bmatrix}
\end{gather*}
$$
Finalmente, la matriz de transformación homogénea de la base al efector final es:
$$
\begin{gather*}
T_3^0 = \begin{bmatrix}
\cos(q_1+q_3) & -\sin(q_1+q_3) & 0 & L_1\cos(q_1)+L_3\cos(q_1+q_3)+q_2\sin(q_1) \
\sin(q_1+q_3) & \cos(q_1+q_3) & 0 & L_1\sin(q_1)+L_3\sin(q_1+q_3)-q_2\cos(q_1) \
0 & 0 & 1 & 0 \
0 & 0 & 0 & 1
\end{bmatrix}
\end{gather*}
$$
Como es un mecanismo planar, la MTH del efector final está definida por:
$$
\begin{gather*}
T_H^B = \begin{bmatrix}
\cos(\phi) & -\sin(\phi) & 0 & x \
\sin(\phi) & \cos(\phi) & 0 & y \
0 & 0 & 1 & 0 \
0 & 0 & 0 & 1
\end{bmatrix}
\end{gather*}
$$
Por lo tanto:
$$
\begin{gather*}
\phi = q_1 + q_3\
x = L_1\cos(q_1)+L_3\cos(\phi)+q_2\sin(q_1) \
y = L_1\sin(q_1)+L_3\sin(\phi)-q_2\cos(q_1)
\end{gather*}
$$
Elevando al cuadrado y sumando:
$$
\begin{gather*}
x-L_3\cos(\phi) = L_1\cos(q_1)+q_2\sin(q_1) \
y-L_3\sin(\phi) = L_1\sin(q_1)-q_2\cos(q_1)
\end{gather*}
$$
$$
\begin{gather*}
(x-L_3\cos(\phi))^2+(y-L_3\sin(\phi))^2 = (L_1\cos(q_1)+q_2\sin(q_1))^2+(L_1\sin(q_1)-q_2\cos(q_1))^2\
(x-L_3\cos(\phi))^2+(y-L_3\sin(\phi))^2 = L_1^2+q_2^2
\end{gather*}
$$
$$
\mathbf{q_2 = \pm\sqrt{(x-L_3\cos(\phi))^2+(y-L_3\sin(\phi))^2-L_1^2}}
$$
Ahora, despejando el seno
$$
\begin{gather*}
\frac{y-L_3\sin(\phi)+q_2\cos(q_1)}{L_1} = \sin(q_1) \
x-L_3\cos(\phi) = L_1\cos(q_1)+q_2\frac{y-L_3\sin(\phi)+q_2\cos(q_1)}{L_1} \
L_1(x-L_3\cos(\phi)) = L_1^2\cos(q_1)+q_2(y-L_3\sin(\phi))+q_2^2\cos(q_1) \
L_1(x-L_3\cos(\phi))-q_2(y-L_3\sin(\phi)) = (L_1^2+q_2^2)\cos(q_1) \
\cos(q_1) = \frac{L_1(x-L_3\cos(\phi))-q_2(y-L_3\sin(\phi))} {L_1^2+q_2^2}
\end{gather*}
$$
y el coseno
$$
\begin{gather*}
\frac{x-L_3\cos(\phi)-q_2\sin(q_1)}{L_1} = \cos(q_1) \
y-L_3\sin(\phi) = L_1\sin(q_1)-q_2\frac{x-L_3\cos(\phi)-q_2\sin(q_1)}{L_1} \
L_1(y-L_3\sin(\phi)) = L_1^2\sin(q_1)-q_2(x-L_3\cos(\phi))-q_2^2\sin(q_1) \
L_1(y-L_3\sin(\phi))+q_2(x-L_3\cos(\phi)) = (L_1^2+q_2^2)\sin(q_1) \
\sin(q_1) = \frac{L_1(y-L_3\sin(\phi))+q_2(x-L_3\cos(\phi))}{L_1^2+q_2^2}
\end{gather*}
$$
Se obtiene:
$$
\mathbf{q_1 = \arctan\left(\frac{L_1(y-L_3\sin(\phi))+q_2(x-L_3\cos(\phi))}{L_1(x-L_3\cos(\phi))-q_2(y-L_3\sin(\phi))}\right)}
$$
Finalmente
$$
\mathbf{q_3=\phi-q_1}
$$


Asumiendo que $L_1=28$ y $L_3=15$, halle las configuraciones articulares tales que la pose de la herramienta respecto a la base está expresada por $x=93$, $y=67.5$ y $\phi=30\degree$, asumir $\phi$ en sentido positivo antihorario.
Rta./
Utilizando el rtb para python:
import roboticstoolbox as rtb 
import spatialmath.base as stm
import numpy as np
import matplotlib.pyplot as plt


class RPR(rtb.DHRobot):
    def __init__(self, L1=28, L3=15):
        self.L1 = L1
        self.L3 = L3

        self.Links = [rtb.DHLink(a=self.L1, alpha=np.pi/2),
                    rtb.DHLink(alpha=-np.pi/2, sigma=1, qlim=[-100, 100]),
                    rtb.DHLink(a=self.L3)]

        super().__init__(
            self.Links,
            name="Felipe"
        )

    def ikine(self, T, config="up"):
        T = T
        q = np.zeros(3)

        x = T[0, 3]
        y = T[1, 3]
        phi = stm.tr2eul(T)[2]

        if config.lower() == "up":
            q[1] = np.sqrt((x-self.L3*np.cos(phi))**2 +
                        (y-self.L3*np.sin(phi))**2 - self.L1**2)
        elif config.lower() == "down":
            q[1] = -np.sqrt((x-self.L3*np.cos(phi))**2 +
                            (y-self.L3*np.sin(phi))**2 - self.L1**2)
        else:
            raise ValueError("Invalid config")

        q[0] = np.arctan2(self.L1*(y-self.L3*np.sin(phi)) + q[1]*(x-self.L3*np.cos(phi)),
                        self.L1*(x-self.L3*np.cos(phi)) - q[1]*(y-self.L3*np.sin(phi)))

        q[2] = phi-q[0]

        return q


if __name__ == "__main__":
    R = RPR()

    T = stm.transl(93, 67.5, 0)@stm.trotz(np.pi/6)
    q = R.ikine(T, config="down")
    print(q)

    R.teach(q=q)
Se obtiene que para alcanzar esa pose con la configuración codo arriba, las posiciones articulares son:

$q_1 = 110.6\degree$
$q_2 = 96$
$q_3 = -80.6\degree$

Y para la configuración codo abajo:

$q_1 = -36.87\degree$
$q_2 = -96$
$q_3 =66.87\degree$
$i$	$\theta_i$	$d_i$	$a_i$	$\alpha_i$
$1$	$q_1$	$0$	$L_1$	$\pi/2$
$2$	$0$	$q_2$	$0$	$-\pi/2$
$3$	$q_3$	$0$	$L_3$	$0$