Cinemática inversa de un brazo robótico planar de dos eslabones

La cinemática directa es fácil: dados los ángulos de las articulaciones, multiplicas las transformaciones de los eslabones y sabes dónde está la herramienta. La cinemática inversa va en sentido contrario —dada una posición objetivo, encuentra ángulos de articulación que lleguen a ella— y ahí es donde viven los problemas interesantes. Un brazo planar de dos eslabones es el ejemplo más pequeño que muestra todos ellos. Explóralo en el Robot Arm Simulator.

El espacio de trabajo alcanzable

Con longitudes de eslabón L₁ y L₂, la herramienta solo puede alcanzar puntos dentro de un anillo. El radio exterior es L₁ + L₂ (brazo completamente estirado) y el radio interior es |L₁ − L₂| (brazo completamente plegado). Cualquier cosa fuera del anillo está demasiado lejos; cualquier cosa dentro del hueco está demasiado cerca. Ningún ángulo de articulación puede alcanzar esos puntos, así que la respuesta correcta es sencillamente «inalcanzable».

Dos soluciones: codo arriba y codo abajo

Para la mayoría de los objetivos dentro del anillo, el codo puede doblarse de dos maneras y aun así colocar la herramienta en el mismo punto. Son las configuraciones codo arriba y codo abajo. Provienen de un ± en el ángulo del teorema del coseno para la segunda articulación:

cos(θ₂) = (x² + y² − L₁² − L₂²) / (2·L₁·L₂)
θ₂ = ± acos(cos(θ₂))        # + is one elbow, − is the other
θ₁ = atan2(y, x) − atan2(L₂·sin θ₂, L₁ + L₂·cos θ₂)

Como hay dos respuestas válidas, la cinemática inversa no es una función única: es una elección. Los robots eligen la configuración que mantiene las articulaciones lejos de sus límites, que minimiza el movimiento desde la pose actual, o que evita un obstáculo.

Singularidades

En los bordes del espacio de trabajo —brazo completamente estirado o completamente plegado— las dos soluciones se funden en una. Estas son las singularidades, y cerca de ellas un pequeño movimiento de la herramienta puede exigir un movimiento enorme de las articulaciones. Los planificadores frenan o rodean las poses singulares porque el brazo pierde brevemente la capacidad de mover la herramienta en una dirección.

¿Cuándo no hay solución?

Un objetivo no tiene solución cuando cae fuera del anillo alcanzable, cuando exigiría un ángulo de articulación que el hardware prohíbe, o cuando exige una orientación que las articulaciones disponibles no pueden producir. Un buen simulador distingue estos casos honestamente en vez de engancharse a la pose más cercana y fingir que ha tenido éxito.

Solvers analíticos frente a numéricos

Para dos o tres eslabones la geometría tiene solución en forma cerrada (analítica), así que calculas los ángulos de articulación directamente, exactamente y rápido. Eso hace que la IK analítica sea ideal para enseñar y para testear. Los brazos redundantes o completamente espaciales normalmente no tienen forma cerrada y necesitan solvers numéricos que iteran hacia una solución — más generales, pero más lentos y más difíciles de razonar porque pueden pasar por alto soluciones o atascarse cerca de las singularidades.

El solver, escrito con seguridad

Las ecuaciones de arriba son correctas y seguirán produciendo NaN en hardware real. Aquí está la misma matemática con cada fallo manejado explícitamente:

enum IkStatus { IK_OK, IK_TOO_FAR, IK_TOO_CLOSE, IK_JOINT_LIMIT, IK_SINGULAR };

struct IkResult {
  IkStatus status;
  float theta1, theta2;      // radians
};

IkResult solveIk(float x, float y, float L1, float L2, bool elbowUp) {
  IkResult r{};

  const float r2 = x * x + y * y;
  const float reach = sqrtf(r2);

  // 1. Reachability FIRST, before any trigonometry.
  //    A small epsilon keeps a target exactly on the boundary from
  //    producing an acos argument of 1.0000001 through rounding.
  const float eps = 1e-4f;
  if (reach > L1 + L2 - eps)          { r.status = IK_TOO_FAR;   return r; }
  if (reach < fabsf(L1 - L2) + eps)   { r.status = IK_TOO_CLOSE; return r; }

  // 2. Now acos is guaranteed to be in range.
  float c2 = (r2 - L1 * L1 - L2 * L2) / (2.0f * L1 * L2);
  c2 = constrain(c2, -1.0f, 1.0f);              // belt and braces
  const float s2 = sqrtf(1.0f - c2 * c2);

  r.theta2 = elbowUp ? atan2f(s2, c2) : atan2f(-s2, c2);

  // 3. atan2, never atan — this is what gets all four quadrants right.
  r.theta1 = atan2f(y, x) - atan2f(L2 * sinf(r.theta2), L1 + L2 * cosf(r.theta2));

  // 4. Warn near the boundary, where the arm is ill-conditioned.
  if (reach > (L1 + L2) * 0.95f) { r.status = IK_SINGULAR; return r; }

  // 5. Mechanical limits are part of the answer, not an afterthought.
  if (r.theta1 < T1_MIN || r.theta1 > T1_MAX ||
      r.theta2 < T2_MIN || r.theta2 > T2_MAX) { r.status = IK_JOINT_LIMIT; return r; }

  r.status = IK_OK;
  return r;
}

Cinco detalles separan esto de la versión de libro de texto, y cada uno corresponde a un fallo real:

Alcanzabilidad antes que trigonometría. Si el objetivo está más allá de L1 + L2, el teorema del coseno produce un argumento fuera del intervalo −1 a 1 y acos devuelve NaN. Ese NaN fluye después a la orden de servo como un ángulo basura — y por eso un objetivo inalcanzable tan a menudo produce un movimiento violento en vez de una negativa a moverse.

El épsilon. Un objetivo calculado para caer exactamente en el límite puede, tras el redondeo en coma flotante, dar c2 = 1.0000001. Probar la alcanzabilidad con un margen pequeño lo detecta antes que acos.

atan2f, no atan. El atan a secas toma un único cociente y no puede distinguir un objetivo delante de su espejo detrás, así que el brazo funciona en dos cuadrantes y se refleja en los otros dos. Es el bug más común en una primera implementación de IK.

Un aviso de singularidad en vez de una negativa. Cerca de la extensión completa la solución sigue siendo válida pero mal condicionada — un pequeño movimiento de la herramienta exige un movimiento enorme de las articulaciones. Marcarlo permite que quien llama frene o lo rodee.

Los límites de articulación como estado. Un brazo cuyo hombro no puede bajar de −10° tiene objetivos geométricamente alcanzables y mecánicamente imposibles. Recortar en silencio produce un brazo que va a cerca de algún sitio y reporta éxito.

Elegir entre las dos soluciones

Las matemáticas te dan ambas ramas y no pueden decirte cuál usar. Eso es una decisión de robótica, y hay cuatro reglas comunes:

El movimiento mínimo es el valor por defecto correcto, y el motivo es lo que pasa sin él: si el solver cambia de rama en silencio a mitad de una trayectoria, el brazo da la vuelta con un movimiento grande, rápido y completamente no planificado. En un brazo real eso esaites y puede ser destructivo.

// Pick the branch closest to where the arm already is.
IkResult up   = solveIk(x, y, L1, L2, true);
IkResult down = solveIk(x, y, L1, L2, false);

if (up.status == IK_OK && down.status == IK_OK) {
  const float costUp   = fabsf(up.theta1   - cur1) + fabsf(up.theta2   - cur2);
  const float costDown = fabsf(down.theta1 - cur1) + fabsf(down.theta2 - cur2);
  return (costUp <= costDown) ? up : down;
}
return (up.status == IK_OK) ? up : down;

Las rectas necesitan IK en cada paso

Interpolar entre dos poses en espacio de articulaciones — mover cada articulación linealmente desde su ángulo inicial hasta su ángulo final — es simple y hace que la herramienta siga una curva. Para muchas tareas eso está bien. Para cualquier cosa que tenga que desplazarse en línea recta, interpolas en espacio cartesiano y resuelves IK en cada paso:

void moveStraight(float x0, float y0, float x1, float y1, int steps) {
  for (int i = 1; i <= steps; i++) {
    const float t = (float)i / steps;
    const float x = x0 + (x1 - x0) * t;
    const float y = y0 + (y1 - y0) * t;

    IkResult r = solveClosest(x, y);
    if (r.status != IK_OK) { abortMove(r.status); return; }   // do not skip the point
    commandJoints(r.theta1, r.theta2);
  }
}

Dos cosas se hacen visibles la primera vez que haces esto. La velocidad de articulación no es constante aunque la velocidad de la herramienta sí lo sea — cerca del borde del espacio de trabajo las articulaciones tienen que moverse mucho más rápido para el mismo movimiento de la herramienta. Y una recta puede pasar por una región que el brazo no puede alcanzar aunque los dos extremos estén bien, y por eso importa la comprobación de estado dentro del bucle y no solo en los extremos.

De ángulos de articulación a cuentas de servo

El solver devuelve ángulos de articulación en un marco matemático. Un servo no sabe nada de ese marco: su cero está donde se montó el horquilla, y puede girar al revés.

struct Joint {
  uint8_t channel;
  float   offsetDeg;   // servo angle when the joint is at mathematical zero
  int8_t  sign;        // +1 or -1
  float   minDeg, maxDeg;
};

int servoAngle(const Joint &j, float jointRad) {
  const float deg = jointRad * 57.2957795f;
  const float clamped = constrain(deg, j.minDeg, j.maxDeg);
  return (int)(j.offsetDeg + j.sign * clamped);
}

Esa struct es el puente entero entre la cinemática y el hardware, y mantenerla explícita es lo que permite distinguir un fallo del solver de uno de calibración. Un brazo que se mueve suave y aterriza lejos del objetivo es casi siempre esto, no las matemáticas.

Resolución de problemas

Pruébalo tú mismo

En el Robot Arm Simulator, arrastra el objetivo y mira cómo el brazo lo resuelve en tiempo real. Cambia la rama del codo para ver la solución alternativa, empuja el objetivo fuera del anillo para ver un «inalcanzable» honesto, y acércate al borde del espacio de trabajo para notar una singularidad.