Cinemática inversa de un brazo de dos articulaciones

01 · Punto de partida

El problema inverso

En el tema 21 fuimos de los ángulos al punto: una cuenta directa con respuesta única. Una tarea real se plantea al revés: «llevá la pinza a (12; 6) cm». Hay que encontrar los ángulos θ1 y θ2 que dejan el extremo ahí. Es la cinemática inversa.

Para un brazo plano de dos eslabones existe una solución cerrada, es decir, una fórmula sin iteraciones. Lo delicado no es la fórmula sino lo que ocurre alrededor de ella:

• el punto puede estar fuera del alcance: no hay solución;
• en general hay dos soluciones, con el codo a cada lado;
• en los bordes del alcance las dos se juntan en una sola;
• en un caso particular hay infinitas;
• la solución geométrica puede violar los límites del mecanismo.

Seguimos en lazo abierto: los ángulos calculados son las órdenes, no la posición medida (tema 15). El resultado es tan bueno como la calibración y las longitudes del modelo (tema 21).

02 · Existencia

¿Se puede llegar? El alcance

El extremo está siempre a distancia r = √(x² + y²) de la base, y esa distancia solo depende del codo (tema 21): r² = L1² + L2² + 2·L1·L2·cos θ2. Como el coseno está entre −1 y 1, el punto es alcanzable únicamente si:

Con L1 = 10 cm y L2 = 8 cm, la zona alcanzable es una corona circular entre 2 y 18 cm de la base. El punto (15; 10) está a 18,03 cm: queda 0,03 cm fuera. El punto (1; 1) está a 1,41 cm, dentro del hueco central: tampoco se alcanza. El origen (0; 0) está dentro del hueco y no es alcanzable, salvo que L1 = L2.

La figura muestra la clave geométrica: conocidos los tres lados (L1, L2 y r), el triángulo queda determinado salvo por su reflejo respecto de la recta base–extremo. Esos dos triángulos son las dos soluciones.

03 · Primer paso

Primero el codo: la ley de los cosenos

Despejando cos θ2 de la fórmula de r²:

El valor D es la prueba de alcance: si |D| > 1, el punto no se alcanza. Si está dentro, hay dos ángulos con ese coseno, +θ2 y −θ2. Conviene obtenerlos con atan2 en lugar de acos:

La función atan2 devuelve el ángulo correcto en todo el rango (−180°, 180°] y trata sin problemas los extremos. El signo «+» da θ2 > 0 y el «−» da θ2 < 0.

Un detalle numérico importante: por redondeo, un punto que en teoría está justo sobre el borde puede dar D = 1,0000001. Un programa ingenuo declararía «sin solución» o calcularía la raíz de un número negativo. Se usa una tolerancia: se acepta un exceso diminuto y se recorta D a [−1, 1] antes de seguir.

04 · Segundo paso

Después la base

Con θ2 conocido, el ángulo de la base es la dirección del objetivo menos el ángulo que el segundo eslabón «desvía» al extremo respecto de la dirección del primero:

El primer término es el rumbo hacia el objetivo (tema 20). El segundo es el ángulo en la base entre la recta al objetivo y el primer eslabón, que sale de las componentes del segundo eslabón en el marco del primero: L2 sen θ2 perpendicular y L1 + L2 cos θ2 a lo largo.

Resolvamos el ejemplo del tema 21: objetivo (10,731; 12,727), L1 = 10, L2 = 8. r² = 277,1, D = (277,1 − 164)/160 = 0,707, y θ2 = ±45°. Con +45°: θ1 = 49,86° − atan2(5,657; 15,657) = 49,86° − 19,89° ≈ 30°, justo el par de partida. Con −45°: θ1 = 49,86° + 19,89° ≈ 69,7°. Ambos pares llevan al mismo punto.

Siempre se normaliza θ1 a (−180°, 180°] (tema 20): la resta de dos atan2 puede salir de ese rango.

05 · Los bordes

Casos especiales

El último caso es el más traicionable. Con eslabones iguales el brazo puede plegarse sobre sí mismo hasta que el extremo coincide con la base, y en esa posición cualquier θ1 sirve. Además atan2(0, 0) no está definido: el programa debe detectar el caso y conservar el último θ1 en lugar de usar un valor arbitrario.

Cuando no hay solución, el programa debe decidirlo explícitamente: rechazar el objetivo, o llevarlo al punto alcanzable más cercano (el mismo rumbo, con r recortado al borde), y avisar. Lo que no debe hacer es calcular un ángulo con un número fuera de rango y mandarlo a un servo.

06 · Dos respuestas, una acción

Elegir entre dos soluciones

Cuando hay dos soluciones válidas en geometría, hay que elegir una. Criterios habituales, en este orden:

1. Límites del mecanismo. Descartar las que violen el rango de algún servo, un tope o una zona prohibida (suelo, base, otros objetos). Es el criterio más fuerte y deja a veces una sola opción; el tema 23 lo desarrolla.
2. Continuidad. Elegir la solución más cercana a la pose actual, por ejemplo la que minimiza |Δθ1| + |Δθ2|. Evita que el brazo «salte» de un codo al otro, un movimiento grande, rápido y potencialmente peligroso.
3. Convención de la tarea. Preferir siempre «codo arriba» si hay obstáculos abajo, o por la geometría de la herramienta.
4. Margen respecto de los límites. Entre dos válidas, preferir la más alejada de los topes.

Con nuestra convención, θ2 > 0 deja el codo del lado derecho de la recta base→objetivo (si el brazo apunta hacia +x, por debajo de ella) y θ2 < 0 del lado izquierdo (por encima). Los nombres «codo arriba» y «codo abajo» dependen de cómo esté montado el plano del brazo; conviene definirlos por el signo de θ2.

Cambiar de codo implica pasar por la posición extendida o plegada, donde ambas ramas se tocan. Un robot no debería cruzar de una a otra en medio de una trayectoria si no es necesario.

07 · Cerrar el círculo

Verificar con la cinemática directa

La mejor prueba de la inversa es la directa: se aplica directa(inversa(x, y)) y se compara con el objetivo. Con doble precisión, el error es del orden de 10⁻¹⁴ cm; con float de 32 bits en un microcontrolador, del orden de 10⁻⁵ a 10⁻⁴ cm. Si el error es mayor, hay un signo, una convención o una unidad equivocados.

La verificación sirve para cuatro cosas:

• probar el código durante el desarrollo, con una grilla de objetivos;
• comprobar en tiempo de ejecución que no se envió una orden absurda (error mayor que una tolerancia → rechazar);
• medir cuánto se aparta el recorte de un objetivo fuera de alcance;
• comparar el modelo con el brazo real: llevalo al punto y medí con una regla; la diferencia es calibración y mecánica, no matemática.

La inversa no sabe de torque, de colisiones ni de holgura. Es geometría pura: un resultado correcto no garantiza un movimiento ejecutable.

08 · Donde el brazo se complica

Cerca del borde: la singularidad

Con el brazo casi extendido o casi plegado, un movimiento radial pequeño del extremo exige un cambio grande de θ2. Derivando D(r) = cos θ2 respecto de r:

El denominador se anula cuando D = ±1, es decir, en los bordes: la sensibilidad crece sin límite. Esos bordes son singularidades. Con nuestros eslabones y a 12 cm de la base, avanzar 1 mm radialmente pide cambiar el codo unos 0,87°; a 17,9 cm, unos 6,1°, siete veces más. Para avanzar a 5 cm/s a esa distancia, el codo necesitaría girar unos 300 °/s.

Consecuencias prácticas:

• un servo no puede seguir velocidades angulares arbitrarias (tema 16): cerca del borde, la trayectoria pedida no es ejecutable;
• el ruido en el objetivo se amplifica: θ2 «tiembla» aunque el punto casi no se mueva;
• las dos ramas se acercan y una pequeña perturbación puede intercambiarlas;
• el esfuerzo en la articulación base aumenta con el brazo extendido (tema 21).

Por eso suele convenirse una zona de trabajo con un margen respecto del borde, por ejemplo operar entre 10 % y 90 % del rango de r, y recortar los objetivos que lo exceden.

09 · Mover el extremo en línea recta

Seguir una recta punto a punto

En el tema 19, interpolar los ángulos hacía que los ejes llegaran juntos; para un brazo, un camino recto entre ángulos no es una recta del extremo. Para que el extremo recorra una recta en el espacio se divide la recta en puntos intermedios, se resuelve la inversa en cada uno y se envían los ángulos resultantes con una cadencia constante.

Al hacerlo hay que cuidar:

• Continuidad de rama. En cada punto, elegir la solución más cercana a la anterior; si no, la trayectoria puede saltar de un codo al otro.
• Que todos los puntos sean alcanzables. Dos extremos dentro del alcance no garantizan que el segmento entre ellos lo esté (puede cruzar el hueco central).
• Velocidades articulares. Una velocidad constante del extremo no es una velocidad constante de los ángulos; cerca del borde crece. Verificá que cada ángulo respete su límite de velocidad y de aceleración (temas 16 y 17).
• Densidad de puntos. Muy pocos puntos hacen que el extremo se aparte de la recta entre ellos.

Es el uso más frecuente de la inversa en un brazo: no un punto aislado, sino una secuencia de puntos que forman el camino.

10 · Código para la placa

Cinemática inversa en Arduino

Este sketch no necesita servos conectados. Recibe un objetivo x y en centímetros, decide si es alcanzable, calcula las dos soluciones, indica si cada una cumple los límites del mecanismo, verifica el resultado con la cinemática directa y muestra la orden de servo equivalente. Los límites son los del montaje del tema 21: θ1 de 0° a 180° y θ2 de −135° a 135°.

const float L1 = 10.0, L2 = 8.0;                 // cm, entre ejes
const float TH1_MIN = 0.0,    TH1_MAX = 180.0;   // límites validados (grados)
const float TH2_MIN = -135.0, TH2_MAX = 135.0;
const float SERVO2_CENTRO = 90.0;

struct Solucion { bool existe; float th1, th2; };

float normalizar(float g) {
  while (g > 180.0) g -= 360.0;
  while (g <= -180.0) g += 360.0;
  return g;
}

// signo = +1: theta2 positivo; signo = -1: theta2 negativo
Solucion inversa(float x, float y, int signo) {
  float d = (x * x + y * y - L1 * L1 - L2 * L2) / (2.0 * L1 * L2);
  if (d > 1.0 + 1e-6 || d < -1.0 - 1e-6) return { false, 0, 0 };   // fuera de alcance
  d = constrain(d, -1.0, 1.0);                                      // absorbe el redondeo
  const float t2 = atan2(signo * sqrt(1.0 - d * d), d);
  const float t1 = atan2(y, x) - atan2(L2 * sin(t2), L1 + L2 * cos(t2));
  return { true, normalizar(t1 * RAD_TO_DEG), t2 * RAD_TO_DEG };
}

bool dentroDeLimites(Solucion s) {
  return s.th1 >= TH1_MIN && s.th1 <= TH1_MAX && s.th2 >= TH2_MIN && s.th2 <= TH2_MAX;
}

void informar(const char *nombre, float x, float y, Solucion s) {
  Serial.print(nombre);
  if (!s.existe) { Serial.println(": sin solucion"); return; }
  const float a = s.th1 * DEG_TO_RAD, b = (s.th1 + s.th2) * DEG_TO_RAD;
  const float ex = L1 * cos(a) + L2 * cos(b) - x, ey = L1 * sin(a) + L2 * sin(b) - y;
  Serial.print(": th1="); Serial.print(s.th1, 2);
  Serial.print(" th2="); Serial.print(s.th2, 2);
  Serial.print(dentroDeLimites(s) ? " [dentro de limites]" : " [FUERA DE LIMITES]");
  Serial.print(" error de vuelta="); Serial.print(sqrt(ex * ex + ey * ey), 4);
  Serial.print(" cm | servo2="); Serial.println(s.th2 + SERVO2_CENTRO, 1);
}

void setup() {
  Serial.begin(115200);
  Serial.setTimeout(100);
  Serial.println("Escribi 'x y' en cm, por ejemplo: 10.731 12.727");
}

void loop() {
  if (Serial.available() == 0) return;
  const float x = Serial.parseFloat();
  const float y = Serial.parseFloat();
  while (Serial.available() > 0) Serial.read();

  const float r = sqrt(x * x + y * y);
  Serial.print("r="); Serial.print(r, 3);
  Serial.print(" cm (alcance "); Serial.print(fabs(L1 - L2), 1);
  Serial.print(" a "); Serial.print(L1 + L2, 1); Serial.println(" cm)");
  informar("theta2 positivo", x, y, inversa(x, y, +1));
  informar("theta2 negativo", x, y, inversa(x, y, -1));
}

Probá 10.731 12.727: las dos soluciones son th1=30 th2=45 y th1=69.73 th2=-45, con un error de vuelta del orden de 10⁻⁵ cm o menos (precisión de float), y ambas dentro de límites. Con 12 6, una de las dos queda FUERA DE LIMITES (su th1 es negativo). Con 18 0, ambas ramas dan th2=0. Con 20 0 o 0 0, el programa informa sin solucion. Con 2 0 una rama da th2=180 y la otra -180: la misma pose, fuera del límite de ±135°.

Detalles del código: la tolerancia de 1e-6 en D absorbe el redondeo, constrain() evita una raíz de un negativo, y atan2 se usa en lugar de acos. Como en el tema 21, parseFloat() espera con un tiempo límite; es aceptable aquí porque el sketch solo calcula. Enviá siempre los dos números.

Antes de mover un servo real con estos ángulos, aplicá los criterios de elección de la sección 6, llevá el brazo con una rampa de velocidad (tema 16) y no ordenes una solución marcada fuera de límites. Si el objetivo cae fuera del alcance, el brazo debe quedarse donde está, no ir «lo más cerca posible» sin una decisión explícita.

11 · Exploración interactiva

Laboratorios: soluciones y sensibilidad

Con el objetivo inicial (10,7; 12,7) hay dos soluciones: (29,7°; 45,7°) y (70,1°; −45,7°). Llevá x a 20: queda fuera de alcance. Ponelo en (12; 6): una de las ramas tiene θ₁ = −9,8°, fuera del límite, y solo queda la de (63,0°; −84,3°). Ahora (10; −5): una rama exige θ₁ = −70,5° y la otra θ₁ = 17,4°. Con L₂ = 10 y el objetivo (0; 0) aparecen infinitas soluciones.

A 12 cm el codo gira unos 0,87° por milímetro (43 °/s para 5 cm/s). A 15 cm, 1,2°/mm. A 17,5 cm, 2,7°/mm, y a 17,9 cm, más de 6°/mm. Algo parecido pasa cerca de los 2 cm: la curva se vuelve vertical en ambos extremos.

12 · Código ejecutable

Resolver y verificar en Python

Resolvemos varios objetivos —incluidos los casos de borde y los inalcanzables—, verificamos cada solución con la cinemática directa, medimos el error de ida y vuelta en una grilla de puntos y seguimos una recta eligiendo la solución más cercana a la anterior.

import math

L1, L2 = 10.0, 8.0

def directa(t1, t2):
    a, b = math.radians(t1), math.radians(t1 + t2)
    return (L1 * math.cos(a) + L2 * math.cos(b), L1 * math.sin(a) + L2 * math.sin(b))

def normalizar(a):
    a = (a + 180) % 360 - 180
    return (180 if a == -180 else round(a, 9)) + 0.0

def inversa(x, y):
    d = (x * x + y * y - L1 ** 2 - L2 ** 2) / (2 * L1 * L2)
    if abs(d) > 1 + 1e-9:
        return []
    d = max(-1.0, min(1.0, d))
    s = math.sqrt(1 - d * d)
    soluciones = []
    for signo in ((1, -1) if s > 1e-9 else (1,)):
        t2 = math.atan2(signo * s, d)
        t1 = math.atan2(y, x) - math.atan2(L2 * math.sin(t2), L1 + L2 * math.cos(t2))
        soluciones.append((normalizar(math.degrees(t1)), round(math.degrees(t2), 9) + 0.0))
    return soluciones

objetivos = [(10.731, 12.727), (8, 10), (18, 0), (2, 0), (20, 0), (0, 0)]
for x, y in objetivos:
    sols = inversa(x, y)
    if not sols:
        print(f"({x}, {y}): sin solucion (r={math.hypot(x, y):.2f} cm, alcance 2 a 18 cm)")
        continue
    for t1, t2 in sols:
        ex, ey = directa(t1, t2)
        print(f"({x}, {y}): th1={t1:7.2f} th2={t2:7.2f}  error de vuelta={math.hypot(ex - x, ey - y):.1e} cm")

peor = 0.0
for i in range(-34, 35):
    for j in range(-34, 35):
        x, y = i * 0.5, j * 0.5
        for t1, t2 in inversa(x, y):
            ex, ey = directa(t1, t2)
            peor = max(peor, math.hypot(ex - x, ey - y))
print(f"Error maximo de ida y vuelta en la grilla: {peor:.1e} cm")

actual = None
print("Trayectoria recta de (12, 3) a (6, 12), eligiendo la solucion mas cercana:")
for k in range(5):
    u = k / 4
    x, y = 12 + (6 - 12) * u, 3 + (12 - 3) * u
    sols = inversa(x, y)
    if actual is None:
        elegida = min(sols, key=lambda s: s[1] < 0)      # empieza con theta2 > 0
    else:
        elegida = min(sols, key=lambda s: abs(s[0] - actual[0]) + abs(s[1] - actual[1]))
    actual = elegida
    print(f"  ({x:5.2f}, {y:5.2f})  th1={elegida[0]:7.2f}  th2={elegida[1]:7.2f}")

Para (10,731; 12,727) salen (30°; 45°) y (69,73°; −45°). (8; 10) da (12,68°; 90°) y (90°; −90°). (18; 0) y (2; 0) tienen una sola solución, y (20; 0) y (0; 0) no tienen ninguna. El error de ida y vuelta en la grilla es del orden de 10⁻¹⁴ cm. La recta de (12; 3) a (6; 12) se recorre sin saltos de rama: θ₂ pasa de 93,9° a 99,6° y baja a 84,3°.

La rama con θ₂ positivo arranca con θ₁ de unos −26°, fuera del límite de 0° a 180° del sketch; la otra rama de ese punto sí sería utilizable. Elegir la rama es una decisión de la aplicación, no de la fórmula. Probá agregar el filtro de límites en inversa() y fijá la rama de partida con él.

13 · Método y diagnóstico

Pruebas y diagnóstico

1. Probá primero con puntos que sepas resolver a mano (brazo extendido, plegado, vertical-horizontal).
2. Aplicá la directa a cada resultado y exigí un error pequeño; probá una grilla completa de objetivos.
3. Probá los bordes: r igual al alcance máximo, al mínimo, y un poco fuera de ambos.
4. Probá (0, 0) y el caso L1 = L2 para ver que no se produzcan NaN ni saltos.
5. Comprobá que se rechazan objetivos inalcanzables en lugar de enviarlos a los servos.
6. Con el brazo armado, llevá la pinza a puntos medidos con una regla y compará, a baja velocidad y sin carga.
7. Seguí una recta corta y verificá que la elección de rama es continua.

14 · Práctica guiada

Predecí antes de comprobar

1. ¿Alcanzable?

Con L₁ = 10 cm y L₂ = 8 cm, decidí si son alcanzables (15; 10), (12; 6) y (1; 1).

2. El valor D

Para el objetivo (14; 6,928), calculá D y θ2.

3. Las dos soluciones

Obtené los dos pares de ángulos para (10,731; 12,727).

4. Los bordes

¿Cuántas soluciones tienen (18; 0) y (2; 0)? ¿Qué ángulos?

5. Límites del mecanismo

Para el objetivo (10; −5) hay dos soluciones: (−70,5°; 104,1°) y (17,4°; −104,1°). Con θ₁ entre 0° y 180° y θ₂ entre −135° y 135°, ¿cuál se usa?

6. Sensibilidad

Comparando r = 12 cm y r = 17,9 cm, ¿cuántas veces más debe girar el codo por milímetro radial? ¿Qué velocidad pide a 5 cm/s en el segundo caso?

7. Continuidad

El brazo está en (30°; 45°) y se pide el objetivo (10,731; 12,727), que tiene las soluciones (30°; 45°) y (69,7°; −45°). ¿Cuál conviene y por qué?

15 · Síntesis

Ideas para recordar

¿Listo para el tema 23?

Antes de avanzar, elegí dos longitudes y los rangos de ambos servos. Dibujá la zona que el extremo puede cubrir y marcá un punto que sea alcanzable en geometría pero no con tus límites. Pensá cómo avisaría tu programa de ese caso.

En el próximo tema estudiamos el espacio de trabajo y los límites articulares: alcance, singularidades y restricciones mecánicas.