6-DOF маніпулятор у Three.js

1. Налаштування сцени

Стандартний шаблон Three.js: перспективна камера, рендерер і допоміжна сітка, щоб обертання суглобів було легко оцінювати візуально. Камера розташована під кутом, дивлячись униз на базу руки — так само, як ви б налаштували кадр для реальної робочої комірки.

import * as THREE from 'three';
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';

const scene = new THREE.Scene();
scene.background = new THREE.Color(0x0d1117);

const camera = new THREE.PerspectiveCamera(45, innerWidth / innerHeight, 0.1, 100);
camera.position.set(4, 3, 5);

const renderer = new THREE.WebGLRenderer({ antialias: true });
renderer.setSize(innerWidth, innerHeight);
document.body.appendChild(renderer.domElement);

const controls = new OrbitControls(camera, renderer.domElement);
controls.target.set(0, 1, 0);

scene.add(new THREE.GridHelper(10, 20, 0x334155, 0x1e293b));
scene.add(new THREE.HemisphereLight(0xffffff, 0x223344, 1.2));
const dir = new THREE.DirectionalLight(0xffffff, 1.5);
dir.position.set(3, 5, 2);
scene.add(dir);

2. Визначення ланцюга суглобів

Спочатку описуємо руку як дані: шість суглобів, кожен із довжиною ланки (відстань до наступного суглоба вздовж локальної осі Z після обертання) та віссю обертання, за назвами база/плече/лікоть/ зап'ястя, подібними до реальних промислових рук:

const JOINTS = [
  { name: 'base',     axis: 'y', length: 0.4 },
  { name: 'shoulder', axis: 'x', length: 1.2 },
  { name: 'elbow',    axis: 'x', length: 1.0 },
  { name: 'wristRoll',  axis: 'z', length: 0.25 },
  { name: 'wristPitch', axis: 'x', length: 0.25 },
  { name: 'wristYaw',   axis: 'y', length: 0.2 },
];

3. Побудова ієрархії Object3D

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

function buildArm(joints) {
  const root = new THREE.Group();
  const jointGroups = [];
  let parent = root;

  for (const j of joints) {
    const jointGroup = new THREE.Group(); // обертання цієї Group і Є кутом суглоба
    parent.add(jointGroup);
    jointGroups.push(jointGroup);

    // візуальна ланка: box або циліндр від цього суглоба до наступного
    const linkMesh = new THREE.Mesh(
      new THREE.CylinderGeometry(0.06, 0.06, j.length, 12),
      new THREE.MeshStandardMaterial({ color: 0xfb923c })
    );
    linkMesh.position.set(0, j.length / 2, 0); // циліндр тягнеться вздовж локальної +Y
    linkMesh.rotation.z = 0;
    jointGroup.add(linkMesh);

    // Group НАСТУПНОГО суглоба зміщена на довжину цієї ланки вздовж локальної Y
    const linkEnd = new THREE.Group();
    linkEnd.position.set(0, j.length, 0);
    jointGroup.add(linkEnd);

    parent = linkEnd; // наступний суглоб приєднується сюди
  }
  return { root, jointGroups, endEffector: parent };
}

const { root, jointGroups, endEffector } = buildArm(JOINTS);
scene.add(root);

4. Пряма кінематика через світові матриці

Оскільки endEffector — реальний вузол у графі сцени, Three.js уже обчислив для вас пряму кінематику щоразу, коли оновлює світові матриці. Вам ніколи не потрібно вручну множити ланцюг матриць Денавіта-Хартенберга — просто зчитайте їх назад:

function getEndEffectorPose() {
  endEffector.updateWorldMatrix(true, false); // оновити цей вузол + предків
  const position = new THREE.Vector3();
  const quaternion = new THREE.Quaternion();
  endEffector.matrixWorld.decompose(position, quaternion, new THREE.Vector3());
  return { position, quaternion };
}

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

5. Повзунки суглобів і живе зчитування позиції

Шість HTML-повзунків діапазону, по одному на суглоб, кожен пише прямо в локальне обертання цього суглоба навколо його осі:

JOINTS.forEach((j, i) => {
  const slider = document.querySelector(`#joint-${i}`);
  slider.addEventListener('input', () => {
    const angle = THREE.MathUtils.degToRad(Number(slider.value));
    jointGroups[i].rotation[j.axis] = angle;

    const { position } = getEndEffectorPose();
    document.querySelector('#readout').textContent =
      `x=${position.x.toFixed(2)} y=${position.y.toFixed(2)} z=${position.z.toFixed(2)}`;
  });
});

6. Додавання захвата

Захват — це просто ще один дочірній елемент endEffector — два меша пальців, що відкриваються й закриваються, переміщуючись вздовж локальної осі X, повністю успадковуючи накопичену позицію та орієнтацію зап'ястя:

function buildGripper() {
  const group = new THREE.Group();
  const fingerGeo = new THREE.BoxGeometry(0.04, 0.18, 0.04);
  const mat = new THREE.MeshStandardMaterial({ color: 0x34d399 });

  const left = new THREE.Mesh(fingerGeo, mat);
  left.position.set(-0.05, 0.09, 0);
  const right = left.clone();
  right.position.x = 0.05;

  group.add(left, right);
  group.userData.fingers = [left, right];
  return group;
}

const gripper = buildGripper();
endEffector.add(gripper);

function setGripperOpening(t) { // t: 0 (закрито) .. 1 (відкрито)
  const [left, right] = gripper.userData.fingers;
  left.position.x = -0.02 - t * 0.05;
  right.position.x = 0.02 + t * 0.05;
}

7. Простий розв'язувач IK на основі якобіана

Щоб рука могла переслідувати цільову точку, яку можна перетягувати, обчислюємо чисельний якобіан методом скінченних різниць (злегка збурюємо кожен кут суглоба, дивимося, наскільки зсунувся кінцевий ефектор), а не виводимо його аналітично вручну — набагато менше коду, і працює для будь-якого ланцюга суглобів без модифікацій:

function solveIK(target, iterations = 12, alpha = 0.6) {
  const EPS = 1e-4;
  for (let iter = 0; iter < iterations; iter++) {
    const { position: current } = getEndEffectorPose();
    const error = new THREE.Vector3().subVectors(target, current);
    if (error.length() < 0.01) break; // збіглося

    for (let i = 0; i < jointGroups.length; i++) {
      const axis = JOINTS[i].axis;
      const original = jointGroups[i].rotation[axis];

      // збурюємо цей суглоб, вимірюємо результуюче зміщення кінцевого ефектора
      jointGroups[i].rotation[axis] = original + EPS;
      const { position: perturbed } = getEndEffectorPose();
      jointGroups[i].rotation[axis] = original; // відновлюємо

      const jCol = new THREE.Vector3().subVectors(perturbed, current).divideScalar(EPS);
      const dTheta = jCol.dot(error); // оновлення transpose якобіана для цього суглоба
      jointGroups[i].rotation[axis] = original + alpha * dTheta;
    }
  }
}

8. Цикл анімації

Насамкінець об'єднуємо все: щокадру, якщо активний режим IK, підштовхуємо суглоби до цілі, потім рендеримо.

let ikTarget = new THREE.Vector3(1.2, 1.5, 0.8);
let ikEnabled = true;

function animate() {
  requestAnimationFrame(animate);

  if (ikEnabled) solveIK(ikTarget);

  controls.update();
  renderer.render(scene, camera);
}
animate();

Звідси природні розширення: обмеження кожного суглоба реалістичними межами (наприклад, ±170° для бази, ±120° для ліктя), заміна скінченнорізницевого якобіана на аналітичний із нашого порівняння Jacobian IK vs FABRIK, або керування рукою вздовж повної траєкторії з кватерніонною SLERP між орієнтаціями контрольних точок.