﻿ Project - Inverse Kinematics

# Overview

This project is to add inverse kinematics to my game engine. Inverse kinematics is useful for controlling the movement of arm-like objects, such as a robot arm. For example, it can be used by the AI to determine exactly the positioning of the segments of a robot arm so that the end segment of the arm is touching a certain point. It can also be used to make an enemy's AI point a rocket launcher at the player.

Eventually the goal is for entire characters' bodies to be controlled by inverse kinematics, allowing for dynamic gameplay where characters (especially giant robots) can attempt to smack or grab the player or point weapons at them, and the player can shoot back at the arm segments, knocking them away.

# Algorithm

## Arm state

The algorithm for inverse kinematics is very elegant. It states that given a desired end goal for the system, one can compute the required configuration of the end state of the system. For the case of moving a robot arm to touch a specified point in space, the desired goal for the system would be

f(θ₁, θ₂, .. θₙ) = robotArmEndPoint = E = targetPoint
where each θ corresponds to a single degree of freedom of the arm (a single arm segment can have multiple degrees of freedom). The end point of the robot arm is a function of the transformations of each segment relative to its connections. Since the length of the segments is generally constant, the end point of the arm is just a function of the angles between each segment for each degree of freedom. E = targetPoint when the goal is satisfied. ## Inverse

While we have an equation for the current endpoint of the arm, we would like to be able to go the other direction -- having a target point for the end of the arm, what would the set of angles between each segment for each degree of freedom be.

f'(targetPoint) = (θ₁, θ₂, .. θₙ)
where f' is the inverse of the original function computing the endpoint of the robot arm. This inversion function allows to compute a set of angles given a target point.

### Approximating the inverse function

In the majority of cases, there is no easy way to calculate the exact inverse function for this. Instead the inverse function will be approximated. To make inverting the function easier, the function can be put in a matrix, and the matrix inverted. We will actually be inverting the derivative of the function. This matrix will end up being the Jacobian of our original function. An approximation for f(θ₁, θ₂, .. θₙ) can be computed by taking the derivative of f(θ₁, θ₂, .. θₙ) around the point of the current values of (θ₁, θ₂, .. θₙ). This will give us a linear approximation of how the function changes near (θ₁, θ₂, .. θₙ) for the current values of theta (or a single term Taylor Series). This is because knowing its derivative, a delta for theta can be input into the derivative and its result added to the current value of f(θ₁, θ₂, .. θₙ).

### Approximating the derivative of f(θ₁, θ₂, .. θₙ)

It is too difficult for me and a lot of other people to find the exact derivative of f(θ₁, θ₂, .. θₙ). Instead it can be approximated by, for each θ, calculate the change in E in f(θ₁, θ₂, .. θₙ) = E when applying a small epsilon to each θ, one at a time. This gives us the approximate change in the function with respect to each θ input. Since f is a function of a vector as well as outputs a vector, the resulting derivative will be a matrix #### Image taken from Wikipedia

termed a Jacobian. Each row is the partial derivative of f for a single output component of E (in this case, an X, Y, or Z component of the point) with respect to each θ input variable of f. Each component of the partial derivative is computed as a number by varying the respective input variable and calculating the delta of that component of E relative to its current value, and the new value when adding the θ epsilon to a single element of the input vector of f. Since the output vector E is a vector with 3 elements (x, y, z), the resulting Jacobian will have 3 rows. It will have 1 column for each degree of freedom in the entire IK system.

### Inverting the Jacobian matrix

Having computed the approximate derivative of f(θ₁, θ₂, .. θₙ) around the neighborhood of the current θ values of the robot arm, we can invert this matrix to get the approximate derivative of (θ₁, θ₂, .. θₙ) = f'(targetPoint). The inverse of the Jacobian matrix can be computed using singular value decomposition.

## Computing the target values for (θ₁, θ₂, .. θₙ)

### Deltas

Since we have the inverse of the derivative, we will compute the distance from our current state to the goal state -- in this case

goalDistance = targetPoint - currentEndPoint
A diagonal matrix built from goalDistance can be multiplied with the SVD Jacobian inverse to compute a list of angle deltas. These angle deltas are the difference between the current angle of each segment degree of freedom and the target angle between each segment degree of freedom. Because this inverse derivative matrix is generally only valid for a small neighborhood around the current state, the resulting angle deltas can be multiplied by a small alpha factor to decrease the change. The algorithm can then be run several more iterations until the difference between the target and the goal is less than some desired error.

# Other Jacobians

Different f(θ₁, θ₂, .. θₙ) can be used to encode more interesting goals for the robot arm. The above position goal solved for a function outputting an x,y,z coordinate.

## Distance goal

Instead of solving for moving the arm to a specific point, it is possible to solve for moving the arm a specific distance away from a target by having f(θ₁, θ₂, .. θₙ) = distanceFromTarget where distanceFromTarget is a scalar. The Jacobian in this case will only have a single row (but still 1 column for each degree of freedom in the entire robot arm).

## Distance goal and face target

More interesting than a simple distance goal, is to have the robot arm hover a specified distance from a target, as well as have the end segment face the target. This is useful when the end of the robot arm is also a rocket launcher, and you want the AI to point the rocket launcher at the player but also maintain a distance far enough away from the player so it doesn't blow itself up.

The Jacobian for the distance and facing goal will still have the distance scalar. In addition, it will have another component which is the dot product between the vector from the end segment to the target point, and the vector along the central axis of the end segment (the actual column will 1 - dotProduct). When the segment is facing the target, the dot product between its central axis and the angle between it and the target will be 1 (or almost 1). This will make the column in the Jacobian storing 1 - dotProduct to be 0, allowing it to be solved for the same way distance is.

# Full robot body control with inverse kinematics

Inverse kinematics is not limited to solving for the configuration of a single arm with one endpoint. An arbitrary number of goal states can be encoded as rows in the Jacobian matrix.

Segments in the robot arm are not limited to only having a single child. A single segment can branch off into multiple segments, such as fingers on a hand, allowing the robot hand to grip an object with all five fingers. Calculating the derivative estimate is done as a tree traversal; the configuration for satisfying the goal state of each branch is solved for in the same invocation. More complex constructions than fingers on a hand can be created -- the entire skeleton of a robot can have its body configuration solved for at once.

The following video demonstrates a simple robot arm with a claw at the end. Both branches of the claw, and the root segments above it are all part of the same inverse kinematics system, and both fingers of the claw have their endpoints and facing angles as rows in the same Jacobian, such that after running the IK algorithm once, the entire system, including both claw fingers, is in a valid state.

# Integrating IK into the physics engine

The demo video above shows the robot arm being moved by setting the angles between each segment directly from the results of the inverse kinematics calculations. It skips the force and torque applications and therefore is not affected by the physics engine. For more interesting gameplay, the movement of the arm should instead take the results of the IK calculations and use them to apply specific forces and torques to the segments so that external forces can interact with or interrupt the movement of the arm (such as the player shooting rockets at the arm to knock it away from smacking them).

More coming soon.