Skip to content

Instantly share code, notes, and snippets.

@cychitivav
Last active September 10, 2022 17:15
Show Gist options
  • Save cychitivav/bd09e438ed1615e4ef8768061765115e to your computer and use it in GitHub Desktop.
Save cychitivav/bd09e438ed1615e4ef8768061765115e to your computer and use it in GitHub Desktop.
Inverse kinematics problem of a RPR robot

Robot RPR

  1. ¿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.

  2. 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} $$

  3. 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$
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment