IK FABRIK: з нуля на JavaScript

1. Задача зворотної кінематики

Роботизована рука — це ланцюг жорстких ланок, з’єднаних обертальними суглобами. Пряма кінематика (FK) проста: за кутами суглобів θ₁…θₙ обчислюємо положення кінцевого ефектора. Зворотна кінематика (IK) — це обернена задача: за бажаним положенням кінцевого ефектора знайти кути суглобів, що його дають.

2. Огляд прямої кінематики

Для плоского 2D-ланцюга з n ланок довжин ℓ₁, ℓ₂, …, ℓₙ та кумулятивних кутів α₁, α₂, …, αₙ (кожен виміряний від світової осі x):

Загальний радіус досяжності руки — R = Σᵢ ℓᵢ. Якщо ціль T задовольняє |T − p₀| > R, ціль поза межами досяжності, і рука має максимально витягнутися до неї.

3. Алгоритм FABRIK

FABRIK запропонували Aristidou та Lasenby у 2011 році. Кожна ітерація складається з двох проходів:

Прямий прохід — підтягуємо кінчик до цілі:

Зворотний прохід — повторно закріплюємо корінь:

Повторюємо, доки |p_n − T| < допуск (зазвичай 0.01 px) або не досягнуто максимальної кількості ітерацій (~10). Для досяжних цілей алгоритм гарантовано збігається.

4. Обмеження кутів суглобів

Реальні суглоби мають обмежений діапазон руху. Обмеження застосовуються під час кожного проходу шляхом обтинання вектора напрямку до дозволеного кутового діапазону навколо напрямку попередньої кістки:

Застосування обмежень під час зворотного проходу найстабільніше. Обмеження роблять задачу більше схожою на оптимізацію з обмеженнями — алгоритм може не досягти точної цілі, але осідає в найближчій можливій конфігурації.

5. Розгалужені ланцюги

Розгалужена рука (наприклад, кисть з кількома пальцями) має деревну структуру. FABRIK природно розширюється:

1. Виконати прямий прохід на кожному підланцюзі до його цілі.
2. Усереднити корені підланцюгів, щоб знайти спільну позицію кореня.
3. Виконати зворотний прохід на стовбурі від спільного кореня вниз до справжнього якоря.
4. Повторювати до збіжності.

Крок усереднення за замовчуванням розподіляє напруження порівну між гілками або може бути зваженим за пріоритетом підланцюга (наприклад, домінантна рука отримує більшу вагу).

6. Порівняння з якобіаном

Класичний підхід до IK використовує якобіан J функції FK, що відображає швидкість суглобів на швидкість кінцевого ефектора. Правило оновлення таке:

Методи на основі якобіана збігаються за меншу кількість ітерацій для складних цілей, краще обробляють надлишковість і можуть мінімізувати вторинні цілі (наприклад, уникати перешкод) через проєкцію в нуль-простір. Але вони потребують лінійної алгебри O(n³) на крок і можуть розбігатися поблизу сингулярностей. FABRIK кращий для інтерактивних застосунків реального часу, де надійність важливіша за оптимальність.

7. JavaScript — повна реалізація FABRIK

// Зворотна кінематика FABRIK — демо на 2D Canvas

class FABRIKChain {
  /**
   * @param {number[]} lengths  - довжини ланок [l0, l1, ..., l_{n-1}]
   * @param {{x,y}}   anchor   - фіксована позиція кореня
   */
  constructor(lengths, anchor) {
    this.lengths = lengths;
    this.n = lengths.length;
    // ініціалізуємо суглоби вертикально
    this.joints = [];
    let y = anchor.y;
    for (let i = 0; i <= this.n; i++) {
      this.joints.push({x: anchor.x, y});
      if (i < this.n) y += lengths[i];
    }
    this.anchor = {x: anchor.x, y: anchor.y};
    this.totalLength = lengths.reduce((a, b) => a + b, 0);
    this.constraints = null; // опційно [{min, max}, ...]
  }

  solve(target, maxIter = 10, tol = 0.5) {
    const J = this.joints;
    const L = this.lengths;

    // Перевірка досяжності
    const dist = Math.hypot(target.x - this.anchor.x, target.y - this.anchor.y);
    if (dist > this.totalLength) {
      // Повністю витягуємося до цілі
      for (let i = 0; i < this.n; i++) {
        const dx = target.x - J[i].x, dy = target.y - J[i].y;
        const r = Math.hypot(dx, dy);
        J[i + 1].x = J[i].x + L[i] * dx / r;
        J[i + 1].y = J[i].y + L[i] * dy / r;
      }
      return;
    }

    for (let iter = 0; iter < maxIter; iter++) {
      // --- Прямий прохід ---
      J[this.n].x = target.x; J[this.n].y = target.y;
      for (let i = this.n - 1; i >= 0; i--) {
        const dx = J[i].x - J[i + 1].x;
        const dy = J[i].y - J[i + 1].y;
        const r = Math.hypot(dx, dy);
        J[i].x = J[i + 1].x + L[i] * dx / r;
        J[i].y = J[i + 1].y + L[i] * dy / r;
      }

      // --- Зворотний прохід ---
      J[0].x = this.anchor.x; J[0].y = this.anchor.y;
      for (let i = 0; i < this.n; i++) {
        let dx = J[i + 1].x - J[i].x;
        let dy = J[i + 1].y - J[i].y;
        // застосовуємо кутові обмеження, якщо визначені
        if (this.constraints && this.constraints[i] && i > 0) {
          const prevDx = J[i].x - J[i - 1].x;
          const prevDy = J[i].y - J[i - 1].y;
          const baseAngle = Math.atan2(prevDy, prevDx);
          let relAngle = Math.atan2(dy, dx) - baseAngle;
          const {min, max} = this.constraints[i];
          relAngle = Math.max(min, Math.min(max, relAngle));
          const absAngle = baseAngle + relAngle;
          dx = Math.cos(absAngle); dy = Math.sin(absAngle);
        }
        const r = Math.hypot(dx, dy);
        J[i + 1].x = J[i].x + L[i] * dx / r;
        J[i + 1].y = J[i].y + L[i] * dy / r;
      }

      const err = Math.hypot(J[this.n].x - target.x, J[this.n].y - target.y);
      if (err < tol) break;
    }
  }

  draw(ctx, accent = '#fb923c') {
    const J = this.joints;
    // Ланки
    ctx.beginPath();
    ctx.strokeStyle = accent;
    ctx.lineWidth = 4;
    ctx.lineCap = 'round';
    ctx.moveTo(J[0].x, J[0].y);
    for (let i = 1; i < J.length; i++) ctx.lineTo(J[i].x, J[i].y);
    ctx.stroke();
    // Суглоби
    for (let i = 0; i < J.length; i++) {
      ctx.beginPath();
      ctx.fillStyle = i === 0 ? '#64748b' : i === J.length - 1 ? '#34d399' : accent;
      ctx.arc(J[i].x, J[i].y, i === 0 ? 7 : 5, 0, Math.PI * 2);
      ctx.fill();
    }
  }
}

// Використання: рука, що слідує за мишею
const canvas = document.querySelector('canvas');
const ctx = canvas.getContext('2d');
const arm = new FABRIKChain([60, 50, 40, 30], {x: 200, y: 100});
let mouse = {x: 200, y: 300};
canvas.addEventListener('mousemove', e => {
  const r = canvas.getBoundingClientRect();
  mouse = {x: e.clientX - r.left, y: e.clientY - r.top};
});
function loop() {
  ctx.clearRect(0, 0, canvas.width, canvas.height);
  arm.solve(mouse);
  arm.draw(ctx);
  // малюємо ціль
  ctx.beginPath();
  ctx.fillStyle = '#34d399';
  ctx.arc(mouse.x, mouse.y, 6, 0, Math.PI * 2);
  ctx.fill();
  requestAnimationFrame(loop);
}
loop();

8. Застосування

• Анімація персонажів в іграх: IK використовується в Unreal Engine та Unity для розміщення стоп на нерівному рельєфі, взаємодії «рука-об’єкт» та процедурного коригування тіла.
• Промислова робототехніка: 6-DoF роботизовані руки (KUKA, ABB, Universal Robots) використовують аналітичні розв’язки IK або чисельні методи для точного позиціонування кінцевого ефектора.
• Ретаргетинг захоплення руху: IK відображає положення маркерів на скелет персонажа, забезпечуючи, щоб стопи лишалися на підлозі, а руки дотягувалися до анімованого реквізиту.
• Хірургічні роботи: IK зі строгими обмеженнями суглобів спрямовує інструменти хірургічної системи Da Vinci через вузькі троакарні порти до точних внутрішньотілесних цілей.
• Протезування: стратегії керування на основі IK рухають моторизовані протези рук, перетворюючи сигнали EMG чи BCI на цільові положення.