2D Inverse Kinematics Implementation

Question

I am trying to implement Inverse Kinematics on a 2D arm(made up of three sticks with joints). I am able to rotate the lowest arm to the desired position. Now, I have some questi

Answer 1

I'll give it a shot, but since my Robotics are two decades in the past, take it with a grain of salt.

The way I learned it, every joint was described by its own rotation matrix, defined relative to its current position and orientation. The coordinate of the whole arm's endpoint was then calculated by combining the rotation matrices together.

This achieved exactly the effect you are looking for: you could move only one joint (change its orientation), and all the other joints followed automatically.

You won't have much chance in getting around matrices here - in fact, if you use homogeneous coordinates, all joint calculations (rotations as well as translations) can be modeled with matrix multiplications. The advantage is that the full arm position can then be described with a single matrix (plus the arm's origin).

With this transformation matrix, you can tackle the inverse kinematic problem: since the transformation matrix' elements will depend on the angles of the joints, you can treat the whole calculation 'endpoint = startpoint x transformation' as a system of equations, and with startpoint and endpoint known, you can solve this system to determine the unknown angles. The difficulty herein lies that the equation may not be solvable, or that there are multiple solutions.

I don't quite understand your second question, though - what are you looking for?

Answer 2

1. Instead of a rotation matrix, the rotation can be represented by its angle or by a complex number of the unit circle, but it's the same thing really. More importantly, you need a representation T of rigid body transformations, so that you can write stuff like t1 * t2 * t3 to compute the position and orientation of the third link.

2. Use atan2 to compute the angle between the vectors.

As the following Python example shows, those two things are enough to build a small IK solver.

from gameobjects.vector2 import Vector2 as V
from matrix33 import Matrix33 as T
from math import sin, cos, atan2, pi
import random

The gameobjects library does not have 2D transformations, so you have to write matrix33 yourself. Its interface is just like gameobjects.matrix44.

Define the forward kinematics function for the transformation from one joint to the next. We assume the joint rotates by angle and is followed by a fixed transformation joint:

def fk_joint(joint, angle): return T.rotation(angle) * joint

The transformation of the tool is tool == fk(joints, q) where joints are the fixed transformations and q are the joint angles:

def fk(joints, q):
    prev = T.identity()
    for i, joint in enumerate(joints):
        prev = prev * fk_joint(joint, q[i])
    return prev

If the base of the arm has an offset, replace the T.identity() transformation.

The OP is solving the IK problem for position by cyclic coordinate descent. The idea is to move the tool closer to the goal position by adjusting one joint variable at a time. Let q be the angle of a joint and prev be the transformation of the base of the joint. The joint should be rotated by the angle between the vectors to the tool and goal positions:

def ccd_step(q, prev, tool, goal):
    a = tool.get_position() - prev.get_position()
    b = goal - prev.get_position()
    return q + atan2(b.get_y(), b.get_x()) - atan2(a.get_y(), a.get_x())

Traverse the joints and update the tool configuration for every change of a joint value:

def ccd_sweep(joints, tool, q, goal):
    prev = T.identity()
    for i, joint in enumerate(joints):
        next = prev * fk_joint(joint, q[i])
        q[i] = ccd_step(q[i], prev, tool, goal)
        prev = prev * fk_joint(joint, q[i])
        tool = prev * next.get_inverse() * tool
    return prev

Note that fk() and ccd_sweep() are the same for 3D; you just have to rewrite fk_joint() and ccd_step().

Construct an arm with n identical links and run cnt iterations of the CCD sweep, starting from a random arm configuration q:

def ccd_demo(n, cnt):
    q = [random.uniform(-pi, pi) for i in range(n)]
    joints = [T.translation(0, 1)] * n
    tool = fk(joints, q)
    goal = V(0.9, 0.75)  # Some arbitrary goal.
    print "i     Error"
    for i in range(cnt):
        tool = ccd_sweep(joints, tool, q, goal)
        error = (tool.get_position() - goal).get_length()
        print "%d  %e" % (i, error)

We can try out the solver and compare the rate of convergence for different numbers of links:

>>> ccd_demo(3, 7)
i     Error
0  1.671521e-03
1  8.849190e-05
2  4.704854e-06
3  2.500868e-07
4  1.329354e-08
5  7.066271e-10
6  3.756145e-11
>>> ccd_demo(20, 7)
i     Error
0  1.504538e-01
1  1.189107e-04
2  8.508951e-08
3  6.089372e-11
4  4.485040e-14
5  2.601336e-15
6  2.504777e-15

Answer 3

The DH (Denavit-Hartenberg) notation is part of the solution. It helps you collect a succinct set of values that describe the mechanics of your robot such as link length and joint type.

From there it becomes easier to calculate forward kinematics. The first think you have to understand is how to translate a coordinate frame from one place to another coordinate frame. For example, given your robot (or the DH table of it), what is the set of rotations and translations you have to apply to one coordinate frame (the world for example) to know the location of a point (or vector) in the robot's wrist coordinate frame.

As you may already know, homogeneous transform matrices are very useful for such transformations. They are 4x4 matrices that encapsulate rotation and translation. Another very useful property of those matrices is that if you have two coordinate frames linked and defined by some rotation and translation, if you multiply the two matrices together, then you just need to multiply your transformation target by the product of that multiplication.

So the DH table will help you build that matrix.

Inverse kinematics is a bit more complicated though and depends on your application. The complication arises from having multiple solutions for the same problem. The greater the number of DOF, the greater the number of solutions.

Think about your arm. Pinch something solid around you. You can move your arm to several locations in the space and still keep your pinching vector unchanged. Solving the inverse kinematics problem involves deciding which solution to choose as well.

Answer 4

In robotics we most often use DH parameters for the forward and reverse kinematics. Wikipedia has a nice introduction.