Forward Kinematics, Inverse Kinematics, and the M100RAK Robot Arm

The video above shows the M100RAK drawing a rudimentary pie chart onto a pie with a “FoodWriter”, a marker containing food coloring. The pie chart depicts the gender ratio among Google’s staff (women: 30%, men 70%).

The food color marker does not draw well on chocolate, especially in hot weather. Tests with hard-drying royal icing and/or rice paper will follow.

The marker is connected to aluminum piping with a spring. This means that it can draw on uneven surfaces.

The robot drawing itself is not yet optimal. If you compare it to the the perfectly round simulation below, you will see why.

The simulation is made up of points that the robot connects. The two points that are floating above are not a mistake. They make sure that when the pen touches down, it touches down almost vertically. If these points were not there, the marker would leave a trail.

I don’t know yet why the robot draws more of a rectangle than a circle. It might be because the servos are slipping under the weight of the robot parts, or because I am falsely adapting the angles returned by the inverse kinematics function to the machine. The spacing between the gears is relatively wide, this might also negatively influence precision. In the meantime, here is an explanation of Inverse Kinematics and Forward Kinematics and how it relates to the code.

Kinematics & Forward Kinematics: The short explanation

Inverse Kinematics describes equations that produce angles to position a robotic arm on a specific xyz-coordinate. In other words: it is an algorithm (a function for example) that takes an xyz-coordinate as well as the length of the arm segments as parameters and returns the angles of the arm joints.

Forward Kinematics does the opposite. It takes angles and the length of the arm segments as inputs and then outputs an xyz-coordinate.

Inverse Kinematics and Forward Kinematics are used in robotics as well as in computer animation to animate objects in physical and in virtual spaces.

All the code in this project is in Python and requires the SciPy and vPython libraries for math and visualization respectively. It also requires the serial library for interfacing with the SC-32.

The Inverse Kinematics function in Python as well as a visualization function using vPython was written by Yaouen Fily specifically for this project.

Forward Kinematics

In order to get the arm to a certain point, we need an inverse kinematics function. However, to get there, it is easier to first derive the forward kinematics function. We will also need the forward kinematics function to draw the robot in 3D using vPython.

To get started, look at the image below. a) depicts a 3D space. b) depicts a 3D space with an additional direction r. The unit vectors on the xyz axes are $\overrightarrow{ex}$ , $\overrightarrow{ey}$ , $\overrightarrow{ez}$ . Unit vectors are just vectors of length 1. Along r there is an additional unit vector $\overrightarrow{er}$ with angle a1. Why is this vector important to our problem? Assuming that your targetThis could be the direction that the base of the robot arm rotates. This also makes things easier because now, we do not need the x and y axes anymore and can deal with this as a 2D trigonometry problem, not a 3D trigonometry problem in the following steps.

For the next step, look at the image below. It depicts a vector $\overrightarrow{AB}$ of length 1 in a plane. $\overrightarrow{AB}$ makes an angle $\theta$ (theta) with the r axis. The r coordinate of the vector $\overrightarrow{AB}$ is $\cos\theta$ , and its z coordinate is $\sin\theta$ (see wikipedia for more detail). If the vector’s length is not 1, but some length AB, its r and z coordinates are $AB\cos\theta$ and $AB\sin\theta$ instead.

The next image (below) depicts our two axes, the z axis and the r axis as well as the unit vectors. It also depicts two new vectors $\overrightarrow{AB}$ and $\overrightarrow{BC}$ with length l2 and l3. These are the robot arm segments. A and B are the joints. C could be a joint as well if it had another segment attached (in the case of the M100RAK it does have an additional segment). Below are the forward kinematics equations. They give the coordinates of $\overrightarrow{AC}$ as functions of the arm lengths l2, l3 and joint angles a2 and a3.

Screen Shot 2014-07-07 at 3.11.47 PM

The equation below describes the vector $\overrightarrow{AB}$ .

$\overrightarrow{AB} = l_2 \left[ \cos(a_2) \overrightarrow{e}_r + \sin(a_2) \overrightarrow{e}_z \right]$

The next equation describes the vector $\overrightarrow{BC}$ .

$\overrightarrow{BC} = l_3 \left[ \cos(a_2+a_3) \overrightarrow{e}_r + \sin(a_2+a_3) \overrightarrow{e}_z \right]$

However what really interests us is the Vector $\overrightarrow{AC}$ since it tells us where the end of the arm will end up.

$\overrightarrow{AC} = \left[ l_2 \cos(a_2) + l_3 \cos(a_2+a_3) \right] \overrightarrow{e}_r + \left[ l_2 \sin(a_2) + l_3 \sin(a_2+a_3) \right] \overrightarrow{e}_z$

If you are more of a coder than a mathematician, you can see this reflected in parts of the code. Specifically, in the function arms. The function takes angles a1, a2, a3 and a4 as well as the length of the arm segments as inputs and outputs a set of coordinates.

def arms(a1,a2,a3,a4,l2,l3,l4):
	er = cos(a1)*ex + sin(a1)*ey
	u2 = l2 * ( cos(a2)*er + sin(a2)*ez )
	u3 = l3 * ( cos(a2+a3)*er + sin(a2+a3)*ez )
	u4 = l4 * ( cos(a2+a3+a4)*er + sin(a2+a3+a4)*ez )
#	u4 = l4 * er
	return u2,u3,u4

You can also see it in the function draw_robot. In line 5, Yaouen calls invert, the inverse kinematics function to receive the angles of the arms. In line 6 he calls the arms function which does the forward kinematics. Both are necessary to draw the robot in 3D.

def draw_robot(x,y,z,l2,l3,l4):
	# first draw x,y,z axes
	p0 = 10*array([-1,-1,0])
	for e in ex,ey,ez:	vp.arrow(pos=p0,axis=5*e,shaftwidth=0.5,color=e)
	a1,a2,a3,a4 = invert(x,y,z,l2,l3,l4)                  # compute joint angles
	u2,u3,u4 = arms(a1,a2,a3,a4,l2,l3,l4)                    # compute arm vectors
	w = 0.2                                          # arm radius and base thickness
	u0 = w*ez 
	v0 = cos(a1)*ex + sin(a1)*ey                     # "front" direction
	r = 3                                            # radius of the base
	vp.cylinder(pos=-u0,axis=2*u0,radius=r)         # draw base
	vp.cylinder(pos=-u0+0.8*r*v0,axis=2*u0,radius=0.1*r,color=(0.5,0.5,0.5)) # draw grey circle that indicates base direction
	vp.cylinder(pos=(0,0,0),axis=u2,radius=w,color=(0.7,1,0.7)) # draw arm 2
	vp.sphere(pos=u2,radius=w)                                  # draw joint 2
	vp.cylinder(pos=u2,axis=u3,radius=w,color=(0.7,0.7,1))      # draw arm 3
	vp.sphere(pos=u2+u3,radius=w)                               # draw joint 3
	vp.cylinder(pos=u2+u3,axis=u4,radius=w,color=(0.7,0.7,1))   # draw arm 4
	vp.sphere(pos=u2+u3+u4,radius=2*w,color=(0,1,0))            # draw end of arm 4
	vp.sphere(pos=(x,y,z),radius=2*w,color=(1,0,0))             # draw target point
	#the last two lines are doing the same but are there for debugging

Inverse Kinematics

Big picture: In order to get the end of the robot arm to point to an xyz-coordinate of our choice we need an inverse kinematics function. We also need the length of the arm segments. We can then give those two sets of parameters to the function and the function will calculate what angles we have to create between the arm segments (using motors). Below is an explanation of the math that Yaouen used to figure out how to to write the inverse kinematics function in Python. Its all trigonometry. However, this problem could also be solved using other mathematical methods.

Ok. Here we go: Below, you see the 3D space again. As previously said, we want to figure out angles as a function of arm lengths and an xyz coordinate. This coordinate is called C in the drawing below. To get started, we need to determine $r_c$ , $a_1$ , and $z_c$ . We can do this as a 2D problem as shown in b) by defining the point H which is a projection of C onto r.

This is the definition of $\overrightarrow{e}_r$

$\overrightarrow{e}_r = \cos(a_1) \overrightarrow{e}_x + \sin(a_1) \overrightarrow{e}_y$

This is the definition of $\overrightarrow{AH}$

$\overrightarrow{AH} = r_c \overrightarrow{e}_r = x_c \overrightarrow{e}_x + y_c \overrightarrow{e}_y$

$r_c = \sqrt{x_c^2+y_c^2}$

$\cos(a_1)=\frac{x_c}{r_c}$
$\sin(a_1)=\frac{y_c}{r_c}$

$a_1 = \arctan 2 \left( \frac{y_c}{r_c}, \frac{x_c}{r_c} \right)$

Now that we have $a1$ and $r_c$ , the next step will be to determine AC and and $a_3$ .

The distance AC doesn’t depend on $a_2$ , nor on $a_1$ , only on $a_3$ . Therefore computing AC will give us an equation that relates $a_2$ , $l_2$ and $l_3$ to $r_c$ and $z_c$ , that we can then solve for $a_2$ .

First we express AC in terms of the coordinates of C:
$\overrightarrow{AC} = r_c \overrightarrow{e}_r + z_c \overrightarrow{e}_z$

$AC^2 = r_c^2 + z_c^2$

Then in terms of the arms lengths and joint angles, using the forward kinematics expressions from earlier:

To get the last line we used the trigonometric relations (see wikipedia)
$\cos(a)^2+\sin^2(a) = 1$

and
$\cos(a)\cos(b) + \sin(a)\sin(b) = \cos(b-a)$

Comparing the two expressions of $AC^2$ , we get:

Look at the drawing below. It shows $a_3$ , the “elbow” of the robot arm shown in green in a). In a) $a_3$ is negative. $a_3$ could in principle be positive or negative as shown in b). We chose the negative option because we do not want the arm to go below the base (the “shoulder” joint) and violently bang into the table as shown in c) (because the motors and everything else would suffer). Therefore we want B to be above C (B and C are shown in the previous images – not in the ones below).

We want a3 to always be negative because else, the arm would be colliding with a surface below the table as shown in c).

Looking at the the two expressions of AC above, we can write

To disentangle $a_2$ and $a_3$ , we now use the trigonometric formulae:

The result is:

If we now think cos( $a_2$ ) and sin( $a_2$ ) as two independent unknowns, this is a system of two linear equations, which can always be solved, using for example Cramer’s method (see wikipedia).

For the sake of notational brevity, we now introduce

so that our equations can be shortened to:

Screen Shot 2014-07-07 at 5.33.18 PM

On Wikipedia, the unknowns are x and y and the equations are:

and the solution is:

Using the following correspondence table:

We obtain the solution to our equations:

Here is the function that does the inverse kinematics. You will see that it is similar to the math done in this section of the blog post:

# input: coordinates x,y,z of the target point, lengths b,c of the arms
# output: angles a1,a2,a3 of the joints, in degrees
def invert(x,y,z,l2,l3,l4):
s  = "(%g,%g,%g) is out of range." % tuple(around([x,y,z],2))
a1 = arctan2(y,x)
r  = hypot(x,y) - l4
u3 = ( r**2 + z**2 - l2**2 - l3**2 ) / ( 2*l2*l3 )
if abs(u3)>1:    raise Exception(s)
a3 = -arccos(u3)
v  = l2 + l3*u3
w  = -l3 * sqrt(1-u3**2)  # this is sin(a3), >0 assuming 0<a3<pi
a2 = arctan2(v*z-w*r,v*r+w*z)
#    # at this point a2<0 => take the other solution
#    a2 = 2*arctan2(z,r) - a2
#    a3 = -a3
if a2<0 or a2>pi:    raise Exception(s)
a4 = - a2 - a3
#    return r2d(a1,a2,a3)
return a1,a2,a3,a4