Robot Arm: Inverse Kinematics Pick & Place

Forward is easy, inverse is the hard direction

A robot arm is a chain of rigid links joined by rotating joints. Given every joint angle, computing where the end effector (the gripper) ends up is a straightforward chain of rotations and translations — forward kinematics — one matrix multiplication per joint, always exactly solvable, always exactly one answer. Inverse kinematics asks the opposite question: given where you want the gripper, what joint angles get it there? That is the hard direction. For anything past a two-link planar arm there is generally no closed-form formula, there can be multiple valid solutions (elbow-up versus elbow-down), and some target poses have none at all because they lie outside the arm's reach.

Inverse kinematics: finding the angles

Given a desired end position, inverse kinematics calculates the required joint angles. This is typically done using iterative methods, such as Newton-Raphson, to solve the system of equations derived from the forward kinematic model. The goal is to minimize the error between the actual and desired positions.

T_effector = T_1(theta_1) . T_2(theta_2) . ... . T_n(theta_n)
// each T_i is a 4x4 homogeneous rotation+translation matrix
// built from that joint's Denavit-Hartenberg parameters

Cyclic Coordinate Descent

Instead of solving that chain analytically, Cyclic Coordinate Descent (CCD) solves it one joint at a time, iteratively, which is why it is the go-to method for arms with more than a couple of links or with joint limits that break closed-form solutions. Starting from the joint closest to the gripper and working back toward the base, each pass:

for joint j = last .. first: v1 = (end_effector_pos - joint[j].pos), normalized v2 = (target_pos - joint[j].pos), normalized angle = angle_between(v1, v2) rotate joint j by `angle` around its axis, toward v2 clamp joint j to its mechanical limits recompute forward kinematics for every joint after j repeat the whole sweep until the effector is within tolerance of target, or a max iteration count is hit Each single-joint rotation is a closed-form, one-line calculation — rotate this one joint by exactly the angle that best points the current end effector at the target, given everything else fixed — so a full sweep across n joints costs only O(n) work, and a handful of sweeps (rarely more than 10-15) typically converges to sub-millimetre accuracy. That is dramatically cheaper than the alternative, Jacobian-based IK, which builds and pseudo-inverts an n-by-3 (or n-by-6, with orientation) matrix of partial derivatives every single iteration — more numerically robust near certain configurations, but far more expensive per step.

for joint j = last .. first:
  v1 = (end_effector_pos - joint[j].pos), normalized
  v2 = (target_pos        - joint[j].pos), normalized
  angle = angle_between(v1, v2)
  rotate joint j by `angle` around its axis, toward v2
  clamp joint j to its mechanical limits
  recompute forward kinematics for every joint after j
repeat the whole sweep until the effector is within tolerance of target,
or a max iteration count is hit

Обмеження суглобів, надлишок та недосяжні цілі

Реальні суглоби не обертаються вільно — плече чи лікоть мають жорсткі механічні обмеження — тому кожен цикл CCD закріплює запропонований кут у межах дозволеного діапазону суглоба перед переходом до наступного, а також оскільки це демонструє елегантність CCD: оскільки воно незалежно обробляє кожен суглоб під час пробіжки, обмеження одного суглоба не коштує додаткових зусиль, тоді як розв’язувач Якобі потрібно було б скласти межі у лінійну систему як обмеження, що набагато складніше.

Рука з більшою кількістю суглобів, ніж 3 (лише позиція) або 6 (позиція + орієнтація) ступенів свободи, необхідних для визначення цільового положення, є кінематично надлишковою — нескінченна кількість конфігурацій суглобів досягають однієї й тієї ж позиції захватної клешні, що саме пояснює, чому людське лікоть може розвернутися дугою, а рука залишається абсолютно непорушно на столі. CCD не вибирає серед цих рішень з будь-якою перевагою (на відміну від методу Якобі з другочним об’єктом нульового простору, який може, наприклад, віддавати перевагу конфігураціям, які уникають перешкод); воно просто сходиться до будь-якого локального рішення, яке знаходить його першим за допомогою жадібного пробігу суглоб по суглобу, що послідовно ініційовано поточним положенням руки.

Коли ціль знаходиться поза загальним досяжним радіусом руки (далі, ніж сума всіх довжин зв’язків), не існує жодної конфігурації, і CCD збігається замість цього до найближчої досяжної точки вздовж лінії до цілі — рука повністю розтягується та вказує на напрямок цілі якомога ближче, що є правильним і очікуваним поводженням для неможливого запиту.

Від розв'язання IK до послідовності збору та відкладання

Автоматизоване завдання збору та відкладання об’єктів складає ланцюг кількох розв’язувань IK разом: переміщення над об’єктом, опускання на висоту захоплення, закриття захватної клепсидри, витягування, переміщення над призначенням, опускання, звільнення, витягування. Кожна точка шляху є незалежним розв’язанням IK; уникнення перешкод вкладається шаром, додаючи проміжні точки шляху, які обходять відомі перешкоди, а не змінюють розв’язувач IK, що саме пояснює цю структуру — точки шляху плюс швидке розв’язання для кожної точки шляху — як стандартну структуру реальних контролерів збору та відкладання на виробничій ділянці, а не лише цієї симуляції.

Часті запитання

Спробуйте наживо

Усе, що вище, працює прямо у вашому браузері — відкрийте Robot Arm Pick & Place і змінюйте параметри під час роботи. Нічого не встановлюється, нічого не завантажується на сервер, уся модель живе в одній вкладці.