Velocidad y matriz jacobiana de un brazo

La matriz jacobiana dice cuánto se mueve la mano cuando una articulación gira un poco. Con ella se pasa de las velocidades de las articulaciones a la velocidad de la mano.

Puedes estimarla sin fórmulas: mueves un ángulo una cantidad muy pequeña y mides cuánto se mueve la mano. Todo es una simulación.

Gira un ángulo una cantidad muy pequeña, de 0,01 grados, y deja el otro quieto. La mano se mueve un poco. Divide ese movimiento entre lo que giraste y obtienes cuánto se mueve la mano por radián.

Haz lo mismo con el otro ángulo. Salen cuatro números: el cambio de x y de y por cada ángulo. Esa tabla es el jacobiano.

docker exec jupyter python3 -c "import numpy as np; h = 0.001; print(round((10 * np.cos(h) + 6 * np.cos(h + np.pi / 2) - 6 * np.cos(np.pi / 2) - 10) / h, 2))"

La velocidad horizontal de la mano es la primera fila del jacobiano por las dos velocidades angulares. La vertical es la segunda fila.

Se multiplica cada número de la fila por su velocidad y se suman. Las velocidades angulares van en radianes por segundo.

docker exec jupyter python3 -c "import numpy as np; print(np.array([[-6, -6], [10, 0]]) @ np.array([1, 0]))"

import math

def hand(l1, l2, a1, a2):
    x = l1 * math.cos(math.radians(a1)) + l2 * math.cos(math.radians(a1 + a2))
    y = l1 * math.sin(math.radians(a1)) + l2 * math.sin(math.radians(a1 + a2))
    return [x, y]

h = 0.01  # un paso pequeño, en grados
more = hand(10, 6, 0 + h, 90)
less = hand(10, 6, 0 - h, 90)
step = 2 * math.radians(h)
dx = (more[0] - less[0]) / step
dy = (more[1] - less[1]) / step
print("columna del ángulo 1:", round(dx, 2), round(dy, 2))
# Cambia el 90 del ángulo 2 por 0 y mira cómo cambia la columna.

Estira el brazo sobre el eje x. Girar cualquier articulación mueve la mano solo hacia arriba o hacia abajo, nunca hacia fuera ni hacia dentro.

Esa pose es una singularidad. El determinante del jacobiano vale 0 y el brazo no puede mover la mano en una dirección.

Estima a mano el jacobiano en la pose (90, 0) con eslabones de 10 y 6 cm. Mueve cada ángulo un paso pequeño y mide cuánto cambia la mano.

Después calcula el determinante y decide si esa pose es una singularidad.