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 questions:

1. How can I make the upper arm move alongwith the third so the end point of the arm reaches the desired point. Do I need to use the rotation matrices for both and if yes can someone give me some example or an help and is there any other possibl;e way to do this without rotation matrices???

2. The lowest arm only moves in one direction. I tried google it, they are saying that cross product of two vectors give the direction for the arm but this is for 3D. I am using 2D and cross product of two 2D vectors give a scalar. So, how can I determine its direction???

Plz guys any help would be appreciated....

Thanks in advance Vikram

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