Coordenadas, ángulos y transformaciones

01 · Punto de partida

Dónde está cada parte

Hasta ahora controlamos ejes por separado o coordinados: pasos, ángulos de servo, vueltas de rueda. Esas son magnitudes de los actuadores. Pero una tarea se formula en otro lenguaje: «llevar la pinza a este punto», «girar hacia aquella pared», «esquivar lo que ve el sensor». Para pasar de un lenguaje al otro hace falta geometría.

Este tema reúne las herramientas mínimas para describir posiciones y orientaciones en el plano y cambiar de sistema de referencia. Son la base de la cinemática de brazos (temas 21 a 24) y de la odometría y navegación de robots móviles (temas 25 a 28).

Nos limitamos al plano (2D), que cubre el brazo de dos articulaciones y el robot diferencial. Al final indicamos qué cambia en 3D.

02 · Un punto, muchas descripciones

Sistemas de referencia

Un sistema de referencia (o marco) es un origen y dos direcciones perpendiculares, los ejes x e y. Las coordenadas de un punto solo tienen sentido respecto de un marco: el mismo punto físico tiene coordenadas distintas en cada uno.

La convención que usaremos es la habitual en robótica: ejes x e y a derechas (si x apunta a la derecha, y apunta hacia arriba), y ángulos positivos en sentido antihorario, con 0° sobre el eje x. No es la única: las imágenes y las pantallas suelen tener el eje y hacia abajo, y muchas hojas de datos miden el rumbo desde el norte en sentido horario. Verificá siempre la convención de cada dato.

La pose de un robot en el plano es el conjunto (x, y, θ): dónde está su origen respecto del mundo y hacia dónde apunta su eje x. Es el dato que convierte coordenadas locales en coordenadas del mundo.

03 · Medir giros

Ángulos y convenciones

Un ángulo puede darse en grados o en radianes: 180° = π rad, es decir, rad = grados · π/180. Las funciones sin(), cos() y atan2() de Arduino y de Python trabajan en radianes. Llamar a sin(30) pensando en grados da el seno de 30 radianes (unos −0,988), no 0,5. Arduino ofrece DEG_TO_RAD y RAD_TO_DEG para convertir.

Dos ángulos que difieren en 360° describen la misma orientación. Por eso conviene mantener los ángulos en un rango fijo, por ejemplo (−180°, 180°], y aplicar la normalización después de cada resta o suma:

La normalización es especialmente importante en la diferencia angular. Si el robot mira a 350° y el objetivo está a 10°, la resta directa da −340°: giraría casi una vuelta a la derecha. Normalizada, la diferencia es +20°: un giro corto a la izquierda. Un controlador que no normaliza gira en el sentido largo o, peor, oscila al cruzar el límite.

Para obtener el ángulo de un vector (dx, dy) se usa atan2(dy, dx), no atan(dy/dx). La función atan2 recibe los dos componentes, distingue los cuatro cuadrantes y no divide por cero cuando dx = 0. Devuelve un ángulo en (−π, π]. Para el vector nulo, el ángulo no está definido y hay que decidir qué hacer.

Un servo (tema 15) usa su propia escala, por ejemplo 0° a 180° con el centro en 90°. La relación entre el ángulo geométrico del mecanismo y la orden al servo es una calibración: una resta de un desfase y, a veces, un cambio de signo. Conviene dejarla en un único lugar del código.

04 · Direcciones y distancias

Vectores en el plano

Un vector a = (ax, ay) representa un desplazamiento o una dirección. Se suman y restan componente a componente; la resta q − p es el vector que va de p a q. Su módulo es |a| = √(ax² + ay²) y su ángulo es atan2(ay, ax).

Dos productos cumplen funciones distintas. Con θ el ángulo de a a b:

• Producto punto: mide cuánto apuntan en la misma dirección. Es cero si son perpendiculares y positivo si el ángulo es menor que 90°. Con un vector unitario, da la proyección del otro sobre su dirección.
• Producto cruz en 2D (un número): su signo indica de qué lado queda b respecto de a. Positivo, a la izquierda (antihorario); negativo, a la derecha. Es útil para decidir hacia dónde girar sin calcular ángulos.

Pasar de polares a cartesianas es x = r·cos θ, y = r·sen θ; la vuelta usa r = √(x²+y²) y θ = atan2(y, x). Una lectura de distancia d de un sensor montado con ángulo φ es, en el marco del robot, el punto (d·cos φ, d·sen φ) si el sensor está en el origen.

05 · Girar un punto

Rotación en 2D

Rotar un punto (x, y) un ángulo θ alrededor del origen, en sentido antihorario, da:

En forma matricial, p' = R(θ) · p con R(θ) = [[cos θ, −sen θ], [sen θ, cos θ]]. Tres propiedades sirven para verificar el código:

• Conserva distancias y ángulos: el módulo de p' es igual al de p.
• Se compone sumando ángulos: R(α) · R(β) = R(α + β). En el plano el orden de dos rotaciones no importa.
• La inversa es la traspuesta: R(−θ) = R(θ)T. Deshacer una rotación es rotar el ángulo opuesto.

Comprobación rápida: rotar 90° el punto (1, 0) da (0, 1). Rotar 90° el punto (2, 1) da (−1, 2).

Hay dos lecturas de la misma fórmula. Una activa: mueve el punto dentro de un mismo marco. Otra pasiva: el punto no se mueve y lo que cambia es el marco; las coordenadas se transforman con la rotación opuesta. Conviene tener claro cuál se usa; en este tema la fórmula se aplica a coordenadas locales de un marco girado para obtener las del marco exterior.

06 · Mover el origen

Traslación y pose

Una traslación suma un vector fijo. Si un marco local tiene su origen en (tx, ty) y está girado un ángulo θ respecto del exterior, un punto p dado en coordenadas locales queda, en las del exterior:

El orden importa: primero se rota, luego se traslada. Las coordenadas locales se giran alrededor del origen del robot y solo después se les suma la posición del robot. Invertir el orden (trasladar y rotar) equivale a girar también la posición del robot alrededor del origen del mundo, y da un punto distinto.

Ejemplo: robot en (1, 0) con θ = 90° y un punto local (1, 0), un metro delante del robot. Correcto: rotar da (0, 1), sumar la posición da (1, 1). Orden invertido: (1,0)+(1,0) = (2,0), rotar da (0, 2): un punto que está a un metro del robot en la dirección equivocada.

La traslación sola no cambia la orientación; la rotación sola no cambia la posición del origen. Una transformación que combina ambas, y conserva distancias, se llama rígida. Es lo que describe la pose de un cuerpo rígido como el chasis o un eslabón.

07 · Cadenas de marcos

Componer e invertir transformaciones

Un sensor montado en el robot tiene su propia pose respecto del robot. Para pasar un punto del marco del sensor al mundo se encadenan dos transformaciones: sensor → robot, y robot → mundo. En código se evalúa de adentro hacia afuera, aplicando primero la más cercana al punto.

Una forma ordenada de manejar la composición es la matriz homogénea de 3×3, que junta rotación y traslación en una sola matriz:

Con ella, componer es multiplicar matrices: Tmundo←sensor = Tmundo←robot · Trobot←sensor. A diferencia de las rotaciones planas, el producto de transformaciones con traslación no es conmutativo: el orden cambia el resultado. Es la base de la cinemática directa de un brazo: cada eslabón agrega su rotación y su traslación a la cadena.

Para ir en sentido contrario, del mundo al marco local, se usa la transformación inversa: se resta primero la traslación y luego se rota por el ángulo opuesto:

Aplicar la directa y después la inversa debe devolver el punto original: es una de las mejores pruebas automáticas de un módulo de transformaciones.

08 · Lo que suele salir mal

Errores típicos

• Grados donde se esperan radianes (o al revés). El resultado parece «casi bien» o totalmente absurdo, según el ángulo.
• Orden de operaciones: trasladar antes de rotar, o multiplicar transformaciones en el orden equivocado.
• Falta de normalización: diferencias de ángulo de más de 180°, saltos al cruzar ±180° y giros largos innecesarios.
• Sentido de giro y ejes invertidos: el eje y de una imagen apunta hacia abajo; un motor montado en espejo gira al revés para el mismo comando.
• Usar atan(y/x) en vez de atan2(y, x): pierde el cuadrante y falla con x = 0.
• Mezclar unidades: metros con milímetros, o cuentas de encoder con distancia, sin conversión.
• Acumulación numérica: componer muchas rotaciones pequeñas en una matriz va degradando su ortogonalidad. Es mejor guardar el ángulo, sumarlo, normalizarlo y reconstruir la matriz cuando haga falta.
• Marco del dato desconocido: una medición sin marco asociado es ambigua. Anotá en el código en qué marco está cada variable.

Una buena práctica es nombrar las variables con su marco: pMundo, pRobot, pSensor. Los errores de marco se ven a simple vista cuando se mezclan.

09 · Código para la placa

Transformaciones en Arduino

Este sketch no necesita cables: calcula y muestra por el monitor serie una cadena de transformaciones. Usa un robot en (1,0; 0,5) m con θ = 30°, un sensor montado 10 cm adelante y girado 90° a la izquierda, y un obstáculo medido a 0,5 m sobre el eje x del sensor.

struct Punto { float x, y; };
struct Pose { float x, y, grados; };

Punto aMundo(Punto p, Pose pose) {
  const float t = pose.grados * DEG_TO_RAD;
  const float c = cos(t), s = sin(t);
  return { pose.x + c * p.x - s * p.y, pose.y + s * p.x + c * p.y };
}

Punto aLocal(Punto q, Pose pose) {
  const float t = pose.grados * DEG_TO_RAD;
  const float c = cos(t), s = sin(t);
  const float dx = q.x - pose.x, dy = q.y - pose.y;
  return { c * dx + s * dy, -s * dx + c * dy };
}

Pose componer(Pose a, Pose b) {
  const Punto p = aMundo({ b.x, b.y }, a);
  return { p.x, p.y, a.grados + b.grados };
}

float normalizar(float grados) {            // devuelve un valor en (-180, 180]
  while (grados > 180.0) grados -= 360.0;
  while (grados <= -180.0) grados += 360.0;
  return grados;
}

float limpio(float v) { return (fabs(v) < 1e-4) ? 0.0 : v; }

void mostrar(const char *nombre, Punto p) {
  Serial.print(nombre);
  Serial.print(" x="); Serial.print(limpio(p.x), 4);
  Serial.print(" y="); Serial.println(limpio(p.y), 4);
}

void setup() {
  Serial.begin(115200);
  const Pose robot = { 1.0, 0.5, 30.0 };
  const Pose sensor = { 0.10, 0.0, 90.0 };    // 10 cm adelante, mirando a la izquierda
  const Punto medida = { 0.5, 0.0 };           // obstáculo a 0,5 m sobre el eje x del sensor

  const Pose sensorMundo = componer(robot, sensor);
  const Punto obstaculo = aMundo(medida, sensorMundo);
  mostrar("Sensor en el mundo:", { sensorMundo.x, sensorMundo.y });
  mostrar("Obstaculo en el mundo:", obstaculo);
  mostrar("Ida y vuelta al sensor:", aLocal(obstaculo, sensorMundo));

  const float rumbo = atan2(obstaculo.y - robot.y, obstaculo.x - robot.x) * RAD_TO_DEG;
  Serial.print("Rumbo al obstaculo: "); Serial.println(rumbo, 1);
  Serial.print("Giro necesario: "); Serial.println(normalizar(rumbo - robot.grados), 1);
  Serial.print("Giro de 350 a 10 grados: "); Serial.println(normalizar(10.0 - 350.0), 1);
}

void loop() {}

La salida esperada, con las cifras de la prueba equivalente en Python, es: sensor en el mundo x=1,0866 y=0,5500; obstáculo en el mundo x=0,8366 y=0,9830; ida y vuelta al sensor x=0,5000 y=0,0000; rumbo al obstáculo 108,7; giro necesario 78,7; y el giro de 350 a 10 grados, 20,0 (no −340).

Las estructuras Punto y Pose evitan arreglos sueltos y hacen explícito qué es cada cosa. limpio() elimina el «−0,0000» que aparece por redondeo de coma flotante. Con float de 32 bits, esperá precisión de unos 6 o 7 dígitos: sobra para posiciones en metros, pero conviene no acumular miles de operaciones sobre la misma variable.

Los datos de la pose provendrían, en un robot real, de la odometría (tema 27); las medidas, de un sensor de distancia (tema 6). Aquí se fijan para poder verificar el cálculo a mano antes de conectar algo.

10 · Exploración interactiva

Laboratorios: marcos y ángulos

Con el robot en (2,0; 1,2) m a 30° y el punto local (0,4; 0,2), el punto cae en (2,246; 1,573) en el mundo, con rumbo 56,6° desde el robot. El orden invertido lo ubicaría en (1,378; 2,412): un error de 1,21 m. Girá el robot a 0° y el error desaparece, porque la rotación es la identidad y ambos órdenes coinciden; con ángulos grandes el error crece mucho.

Con 350° y 10°, la resta directa da −340°, pero el giro corto es +20°. Probá 10° y 190°: la diferencia es 180° y ambos sentidos son igual de largos; el programa debe elegir uno. Para ángulos cercanos entre sí, resta y normalización coinciden. El producto cruz (el valor «sen θ») da el signo del giro sin pasar por ángulos.

11 · Código ejecutable

Cadena de sistemas en Python

Repetimos la cadena sensor → robot → mundo del sketch, comprobamos que la inversa devuelve el punto original y calculamos el giro necesario hacia el obstáculo. Sirve de referencia para validar el código del microcontrolador.

import math

def a_mundo(p, pose):
    x, y, th = pose
    c, s = math.cos(math.radians(th)), math.sin(math.radians(th))
    return (x + c * p[0] - s * p[1], y + s * p[0] + c * p[1])

def a_local(q, pose):
    x, y, th = pose
    c, s = math.cos(math.radians(th)), math.sin(math.radians(th))
    dx, dy = q[0] - x, q[1] - y
    return (c * dx + s * dy, -s * dx + c * dy)

def componer(pose_a, pose_b):
    x, y = a_mundo((pose_b[0], pose_b[1]), pose_a)
    return (x, y, pose_a[2] + pose_b[2])

def limpio(v):
    return 0.0 if abs(v) < 1e-12 else v

def normalizar(angulo):
    a = (angulo + 180) % 360 - 180
    return 180 if a == -180 else a

robot = (1.0, 0.5, 30)
sensor = (0.10, 0.0, 90)
medida = (0.5, 0.0)

sensor_mundo = componer(robot, sensor)
obstaculo = a_mundo(medida, sensor_mundo)
vuelta = tuple(limpio(v) for v in a_local(obstaculo, sensor_mundo))
print(f"Sensor en el mundo: x={sensor_mundo[0]:.4f} y={sensor_mundo[1]:.4f} theta={sensor_mundo[2]}")
print(f"Obstaculo en el mundo: x={obstaculo[0]:.4f} y={obstaculo[1]:.4f}")
print(f"Ida y vuelta al sensor: x={vuelta[0]:.4f} y={vuelta[1]:.4f}")

dx, dy = obstaculo[0] - robot[0], obstaculo[1] - robot[1]
rumbo = math.degrees(math.atan2(dy, dx))
print(f"Rumbo al obstaculo: {rumbo:.1f} grados; giro necesario: {normalizar(rumbo - robot[2]):.1f} grados")
print(f"Giro de 350 a 10 grados: ingenuo={10 - 350}, normalizado={normalizar(10 - 350)}")
print(f"Giro de 10 a 190 grados: {normalizar(190 - 10)}")

El sensor queda en (1,0866; 0,5500) con orientación 120° (30° del robot más 90° del montaje). El obstáculo cae en (0,8366; 0,9830). La ida y vuelta devuelve (0,5; 0) y el giro hacia él es de 78,7° a la izquierda.

Cambiá el ángulo de montaje del sensor a −90° y observá hacia qué lado cae el obstáculo. Probá también una medida de (0,5; 0) con un robot a 0°: el resultado debe ser el punto sumado a la posición, sin rotación.

12 · Método y diagnóstico

Pruebas y diagnóstico

1. Probá casos con resultado conocido: rotaciones de 0°, 90° y 180°; puntos en los ejes; el origen.
2. Verificá la ida y vuelta: aLocal(aMundo(p)) = p para varias poses, con tolerancia pequeña.
3. Comprobá que las rotaciones conservan la distancia al origen.
4. Probá la normalización en los bordes: −180°, 180°, 540°, −540°.
5. Con hardware, medí una pose real con una regla y un transportador y comparala con la calculada.
6. Anotá el marco de cada variable y revisá que cada operación mezcle solo variables del mismo marco.
7. Dibujá el resultado (en papel o en un simulador): un error de marco se ve de inmediato.

13 · Práctica guiada

Predecí antes de comprobar

1. Grados y radianes

Convertí 135° a radianes y π/6 rad a grados. ¿Qué devuelve sin(30) en Arduino y por qué?

2. Rotación de un punto

Rotá 90° antihorario el punto (2, 1) alrededor del origen.

3. Local a mundo

Un robot está en (3, 1) con θ = 90°. Un punto está a 0,5 m delante de él, en (0,5; 0) locales. ¿Dónde está en el mundo?

4. Mundo a local

Con el mismo robot, ¿qué coordenadas locales tiene el punto (3, 2) del mundo?

5. Giro corto

El robot mira a 170° y el objetivo está a −170°. ¿Cuánto y hacia dónde conviene girar?

6. Orden de operaciones

Robot en (1, 0) con θ = 90° y un punto local (1, 0). Calculá el resultado correcto y el que da trasladar antes de rotar.

14 · Síntesis

Ideas para recordar

¿Listo para el tema 21?

Antes de avanzar, tomá un robot con un sensor montado y dibujá los tres marcos (mundo, robot, sensor). Escribí la cadena de transformaciones que lleva una medida al mundo y verificá a mano un caso con ángulos de 0° y 90°.

En el próximo tema aplicamos estas herramientas a un brazo: la cinemática directa de un brazo de dos articulaciones, de los ángulos de las articulaciones a la posición del extremo.