Loading [MathJax]/jax/output/HTML-CSS/jax.js
Skip to main content
Library homepage
 

Text Color

Text Size

 

Margin Size

 

Font Type

Enable Dyslexic Font
Mathematics LibreTexts

15.2: 2D Forward Kinematics

( \newcommand{\kernel}{\mathrm{null}\,}\)

Login with LibreOne to run this code cell interactively.

If you have already signed in, please refresh the page.

%matplotlib inline
import matplotlib.pylab as plt
import numpy as np
from ipywidgets import interact
import sympy as sym
sym.init_printing(True)
%matplotlib inline
import matplotlib.pylab as plt
import numpy as np
from ipywidgets import interact
import sympy as sym
sym.init_printing(True)
Robot diagram.

This robot can move in the xy plane. We can represent the configuration of the robot in its “Joint Space” by knowing the two joint angles or [a1,a2]. However what we would like is to represent the location of the end of the robot (often called the “end effector” or “hand”) in “world” coordinates (i.e. xy coordinate system).

Today, we will use Linear Algebra and simple transformation matrices to try and calculate how to go from “joint” coordinates to “world” coordinates.

Login with LibreOne to run this code cell interactively.

If you have already signed in, please refresh the page.

from IPython.display import YouTubeVideo
YouTubeVideo("aCohcLYrYcY",width=640,height=360, cc_load_policy=True)
from IPython.display import YouTubeVideo
YouTubeVideo("aCohcLYrYcY",width=640,height=360, cc_load_policy=True)

Single axis Robot

The following code draws a simple single axis (single joint) robot with its joint centered at the origin and its initial angle of zero with an robot arm length of 4 “units” long.

Login with LibreOne to run this code cell interactively.

If you have already signed in, please refresh the page.

plt.scatter(4,0, s=200, facecolors='none', edgecolors='r') #plot end effector
plt.scatter(0,0, s=200, facecolors='r', edgecolors='r') # plot origin
plt.plot([0,4],[0,0]) #plot blue line for arm
plt.axis('square')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])
# 'Run' this cell to see the output
plt.scatter(4,0, s=200, facecolors='none', edgecolors='r') #plot end effector
plt.scatter(0,0, s=200, facecolors='r', edgecolors='r') # plot origin
plt.plot([0,4],[0,0]) #plot blue line for arm
plt.axis('square')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])
# 'Run' this cell to see the output

A 2D rotation matrix around the origin is defined as the following:

[xendyend]=[cos(a)sin(a)sin(a)cos(qa][xstartystart]

The following rotation matrix will rotate the point 45o around the origin:

Login with LibreOne to run this code cell interactively.

If you have already signed in, please refresh the page.

p = [[4],[0]]

a1=np.pi/4

R = np.matrix([[np.cos(a1), -np.sin(a1)], [np.sin(a1), np.cos(a1)]])

p2 = R*p

x1 = p2[0,0]
y1 = p2[1,0]

plt.scatter(x1,y1, s=200, facecolors='none', edgecolors='r') #plot end effector
plt.scatter(0,0, s=200, facecolors='r', edgecolors='r') # plot origin
plt.plot([0,x1],[0,y1]) #plot blue line for arm
plt.axis('square')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])
sym.Matrix(R)
p = [[4],[0]]

a1=np.pi/4

R = np.matrix([[np.cos(a1), -np.sin(a1)], [np.sin(a1), np.cos(a1)]])

p2 = R*p

x1 = p2[0,0]
y1 = p2[1,0]

plt.scatter(x1,y1, s=200, facecolors='none', edgecolors='r') #plot end effector
plt.scatter(0,0, s=200, facecolors='r', edgecolors='r') # plot origin
plt.plot([0,x1],[0,y1]) #plot blue line for arm
plt.axis('square')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])
sym.Matrix(R)

The following code uses the Jupyter interact function and numpy to make an interactive view of the above. This lets us change the value of the rotation motor and see how it changes the robot. The input to the function is the axis angle and the output is the xy coordinates.

Note

It can take some time for the interaction to catch up. Try moving the slider slowly…

Login with LibreOne to run this code cell interactively.

If you have already signed in, please refresh the page.

def Robot_Simulator(q1=0):
    a1 = q1/180  * np.pi
    p0 = np.matrix([4,0]).T
    p = p0
    J1 = np.matrix([[np.cos(a1), -np.sin(a1)], [np.sin(a1), np.cos(a1)]]) 
    p = np.concatenate( ( J1*p, np.matrix([0,0]).T), axis=1 )
    
    plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=200, facecolors='none', edgecolors='r')
    plt.scatter(0,0, s=200, facecolors='r', edgecolors='r')
    plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
    plt.axis('square')
    plt.xlim([-5.5,5.5])
    plt.ylim([-5.5,5.5])
    ax = plt.gca()
    plt.show()
    return([ p[0,0], p[1,0] ])
    
p = interact(Robot_Simulator, q1=(-180,180,2));
def Robot_Simulator(q1=0):
    a1 = q1/180  * np.pi
    p0 = np.matrix([4,0]).T
    p = p0
    J1 = np.matrix([[np.cos(a1), -np.sin(a1)], [np.sin(a1), np.cos(a1)]]) 
    p = np.concatenate( ( J1*p, np.matrix([0,0]).T), axis=1 )
    
    plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=200, facecolors='none', edgecolors='r')
    plt.scatter(0,0, s=200, facecolors='r', edgecolors='r')
    plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
    plt.axis('square')
    plt.xlim([-5.5,5.5])
    plt.ylim([-5.5,5.5])
    ax = plt.gca()
    plt.show()
    return([ p[0,0], p[1,0] ])
    
p = interact(Robot_Simulator, q1=(-180,180,2));
0
Do This

Inspect the above code.

Question

Which line of the code defines the transformation matrix for the robot joint?

Question

Which line of code defines the length of the robot arm?

Multi axis robot

Now, let’s consider the robot from the video:

Robot diagram from the previous video.

Notice it has two joints a1 and a2 and offset lengths of d1, d2 and de. The joint space for this robot is just its angles [a1,a2]. However, what we want is to know the location of end effector point pe at the gripper in the “world” reference frame, which the bottom most axies “on the ground”.

At each joint, we can define a reference frame that rotates and then transforms the origin to the ealier joint. The forward transformation matrices capture the relationship between the reference frames of different links of the robot.

For example, We can move from the base motor, or p1, reference frame to the world, or pw, reference frame using the following equations:

pw=[cos(a1)sin(a1)sin(a1)cos(a1)]p1+[0d1].

The equation shown above are a little tricky to work with because the [0d1] part makes the equation non-linear (if you don’t believe me? Remember the rules for making a function linear and apply them and see for yourself). However, there is an easy trick in Linear Algebra to convert the above to one big linear matrix. This trick requires us to keep an extra 1 (one) for each point but makes the math work out nicely. Basically the trick works as follows:

[xwyw1]=[cos(a1)sin(a1)0sin(a1)cos(a1)d1001][x1y11]

Let’s call the transformation matrix for Joint 1, J1, and we can rewrite the equations as follows:

p1=J1p2

Do This

On your own, write out the above matrix multiplication and convince yourself that it is the same as the one above.

Now, we can move from the p2 reference frame to the p1 reference frame basically the same equation:

[x1y11]=[cos(a2)sin(a2)d2sin(a2)cos(a2)0001][x2y21]

For the last step we can do a simple linear transpose from the end effector pe reference frame to the p2 reference frame:

[x2y21]=[10d2010001][xeye1]

If we call each transformation matrix J1,J2,Je then hopefully you can see that we can string these transformation matrices together such that we get a single transform from the end effector all the way back to the world coordinates as follows:

pw=J1J2Jepe

Let’s see what this looks like in Python. I am going to use numpy. The plotting gets a little awkward but hopefully it makes sense.

First, lets initialize the variables to some discreet numbers:

Login with LibreOne to run this code cell interactively.

If you have already signed in, please refresh the page.

%matplotlib inline
import matplotlib.pylab as plt
import numpy as np
from ipywidgets import interact

#Inicial state
a1 = 0
a2 = 0

#Lenths of the offsets
d1 = 0.5
d2 = 3
de = 3
%matplotlib inline
import matplotlib.pylab as plt
import numpy as np
from ipywidgets import interact

#Inicial state
a1 = 0
a2 = 0

#Lenths of the offsets
d1 = 0.5
d2 = 3
de = 3

Next, I am going to define a set of points in the end effector coordinate system. These points are picked to form a sort of “C” shaped designed to look sort of like an end effector. I will plot them to help show you what I mean:

Login with LibreOne to run this code cell interactively.

If you have already signed in, please refresh the page.

#Points needed to define a square
pe = np.matrix([[1, 0.5, 1],[0,0.5,1],[0,-0.5, 1],[1,-0.5, 1],[0,-0.5, 1],[0,0, 1]]).T
p = pe

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.axis('scaled')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])
# 'Run' this cell to see the output
#Points needed to define a square
pe = np.matrix([[1, 0.5, 1],[0,0.5,1],[0,-0.5, 1],[1,-0.5, 1],[0,-0.5, 1],[0,0, 1]]).T
p = pe

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.axis('scaled')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])
# 'Run' this cell to see the output

The next step is to apply the Je transformation matrix to the gripper points which will put them in the p2 coordinate system. Once the points are transposed the code concatenates the origin (0,0) onto the list of points so we can part of the robot arm in the plot:

Login with LibreOne to run this code cell interactively.

If you have already signed in, please refresh the page.

Je = np.matrix([[1, 0, de], 
                [0, 1, 0], 
                [0,0,1]]) 

p = np.concatenate( ( Je*p, np.matrix([0,0,1]).T), axis=1 )

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.axis('scaled')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])
Je = np.matrix([[1, 0, de], 
                [0, 1, 0], 
                [0,0,1]]) 

p = np.concatenate( ( Je*p, np.matrix([0,0,1]).T), axis=1 )

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.axis('scaled')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])

We do this again. Apply the J2 transformation matrix to put the points into the p1 coordinate system, concatenate the origin and plot the results.

Login with LibreOne to run this code cell interactively.

If you have already signed in, please refresh the page.

J2 = np.matrix([[np.cos(a2), -np.sin(a2), 0], 
                [np.sin(a2), np.cos(a2), d2], 
                [0,0,1]]) 

p = np.concatenate( ( J2*p, np.matrix([0,0,1]).T), axis=1 )

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.axis('scaled')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])
J2 = np.matrix([[np.cos(a2), -np.sin(a2), 0], 
                [np.sin(a2), np.cos(a2), d2], 
                [0,0,1]]) 

p = np.concatenate( ( J2*p, np.matrix([0,0,1]).T), axis=1 )

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.axis('scaled')
plt.xlim([-5.5,5.5])
plt.ylim([-5.5,5.5])

We do it yet again. Apply the J1 transformation matrix which will put the points in the pw coordinate system, concatenate the origin and plot the results. The result is a skeletal frame representing our robot.

Login with LibreOne to run this code cell interactively.

If you have already signed in, please refresh the page.

J1 = np.matrix([[np.cos(a1), -np.sin(a1), 0], 
                [np.sin(a1), np.cos(a1), d1], 
                [0,0,1]]) 

p = np.concatenate( ( J1*p, np.matrix([0,0,1]).T), axis=1 )

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.axis('scaled')
plt.xlim([-8,8])
plt.ylim([-8,8])
J1 = np.matrix([[np.cos(a1), -np.sin(a1), 0], 
                [np.sin(a1), np.cos(a1), d1], 
                [0,0,1]]) 

p = np.concatenate( ( J1*p, np.matrix([0,0,1]).T), axis=1 )

plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
plt.axis('scaled')
plt.xlim([-8,8])
plt.ylim([-8,8])
Do This

Modify the rotation variables a1 and a2 in the above code and see if the new robot configuration looks right.

HINT make sure your angles are in radians.

The following is the same code as above but put into an interactive function to make the code easier to play with:

Login with LibreOne to run this code cell interactively.

If you have already signed in, please refresh the page.

from ipywidgets import interact

def Robot_Simulator(q1=0,q2=-0):
    a1 = q1/180  * np.pi
    a2 = q2/180  * np.pi

    d1 = 0.5
    d2 = 3
    de = 3
    
    target = np.matrix([-3,2, 1])
    print(target)

    pe = np.matrix([[1, 0.5, 1],[0,0.5,1],[0,-0.5, 1],[1,-0.5, 1],[0,-0.5, 1],[0,0, 1]]).T

    Je = np.matrix([[1, 0, de], 
                    [0, 1, 0], 
                    [0,0,1]]) 
    p = np.concatenate( ( Je*pe, np.matrix([0,0,1]).T), axis=1 )    

    J2 = np.matrix([[np.cos(a2), -np.sin(a2), 0], 
                    [np.sin(a2), np.cos(a2), d2], 
                    [0,0,1]]) 
    p = np.concatenate( ( J2*p, np.matrix([0,0,1]).T), axis=1 )

    J1 = np.matrix([[np.cos(a1), -np.sin(a1), 0], 
                    [np.sin(a1), np.cos(a1), d1], 
                    [0,0,1]]) 
    p = np.concatenate( ( J1*p, np.matrix([0,0,1]).T), axis=1 )


    plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
    plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
    plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
    plt.plot(target[0,0], target[0,1],'*')
    plt.axis('scaled')
    plt.xlim([-8,8])
    plt.ylim([-8,8])

    plt.show()
    
target = interact(Robot_Simulator, q1=(-180,180), q2=(-180,180));
from ipywidgets import interact

def Robot_Simulator(q1=0,q2=-0):
    a1 = q1/180  * np.pi
    a2 = q2/180  * np.pi

    d1 = 0.5
    d2 = 3
    de = 3
    
    target = np.matrix([-3,2, 1])
    print(target)

    pe = np.matrix([[1, 0.5, 1],[0,0.5,1],[0,-0.5, 1],[1,-0.5, 1],[0,-0.5, 1],[0,0, 1]]).T

    Je = np.matrix([[1, 0, de], 
                    [0, 1, 0], 
                    [0,0,1]]) 
    p = np.concatenate( ( Je*pe, np.matrix([0,0,1]).T), axis=1 )    

    J2 = np.matrix([[np.cos(a2), -np.sin(a2), 0], 
                    [np.sin(a2), np.cos(a2), d2], 
                    [0,0,1]]) 
    p = np.concatenate( ( J2*p, np.matrix([0,0,1]).T), axis=1 )

    J1 = np.matrix([[np.cos(a1), -np.sin(a1), 0], 
                    [np.sin(a1), np.cos(a1), d1], 
                    [0,0,1]]) 
    p = np.concatenate( ( J1*p, np.matrix([0,0,1]).T), axis=1 )


    plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
    plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
    plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
    plt.plot(target[0,0], target[0,1],'*')
    plt.axis('scaled')
    plt.xlim([-8,8])
    plt.ylim([-8,8])

    plt.show()
    
target = interact(Robot_Simulator, q1=(-180,180), q2=(-180,180));
0
0
Note

If the interact plot is really choppy on your machine this is because the function is coninuously updating as you move the sliders. Consider replacing the interact function with interact_manual to eliminate the continuous updates. You will have to change the import code in line 1.

Question

Move the above robot so that the end effector is “gripping” the target (yellow/orangeish star). Notice that there is more than one point in the “joint space” that gives the same answer. This is the reverse Kinematic problem (which is harder). We know the point we want but we need to find the joints that put the robot at that point.

Do This

The code in the following cell is cut and pasted from above. Modify the code to add a third Joint to the robot.

Login with LibreOne to run this code cell interactively.

If you have already signed in, please refresh the page.

from ipywidgets import interact

def Robot_Simulator(q1=0,q2=-0):
    a1 = q1/180  * np.pi
    a2 = q2/180  * np.pi
    #####Start your code here #####    

    #####End of your code here#####     

    d1 = 0.5
    d2 = 3
    de = 3
    #####Start your code here #####    

    #####End of your code here#####     
    
    
    target = np.matrix([-3,2, 1])
    print(target)

    pe = np.matrix([[1, 0.5, 1],[0,0.5,1],[0,-0.5, 1],[1,-0.5, 1],[0,-0.5, 1],[0,0, 1]]).T

    Je = np.matrix([[1, 0, de], 
                    [0, 1, 0], 
                    [0,0,1]]) 
    p = np.concatenate( ( Je*pe, np.matrix([0,0,1]).T), axis=1 )    

    #####Start your code here #####    

    #####End of your code here#####     
    
    J2 = np.matrix([[np.cos(a2), -np.sin(a2), 0], 
                    [np.sin(a2), np.cos(a2), d2], 
                    [0,0,1]]) 
    p = np.concatenate( ( J2*p, np.matrix([0,0,1]).T), axis=1 )

    J1 = np.matrix([[np.cos(a1), -np.sin(a1), 0], 
                    [np.sin(a1), np.cos(a1), d1], 
                    [0,0,1]]) 
    
    p = np.concatenate( ( J1*p, np.matrix([0,0,1]).T), axis=1 )


    plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
    plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
    plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
    plt.plot(target[0,0], target[0,1],'*')
    plt.axis('scaled')
    plt.xlim([-8,8])
    plt.ylim([-8,8])

    plt.show()
    
target = interact(Robot_Simulator, q1=(-180,180), q2=(-180,180));
from ipywidgets import interact

def Robot_Simulator(q1=0,q2=-0):
    a1 = q1/180  * np.pi
    a2 = q2/180  * np.pi
    #####Start your code here #####    

    #####End of your code here#####     

    d1 = 0.5
    d2 = 3
    de = 3
    #####Start your code here #####    

    #####End of your code here#####     
    
    
    target = np.matrix([-3,2, 1])
    print(target)

    pe = np.matrix([[1, 0.5, 1],[0,0.5,1],[0,-0.5, 1],[1,-0.5, 1],[0,-0.5, 1],[0,0, 1]]).T

    Je = np.matrix([[1, 0, de], 
                    [0, 1, 0], 
                    [0,0,1]]) 
    p = np.concatenate( ( Je*pe, np.matrix([0,0,1]).T), axis=1 )    

    #####Start your code here #####    

    #####End of your code here#####     
    
    J2 = np.matrix([[np.cos(a2), -np.sin(a2), 0], 
                    [np.sin(a2), np.cos(a2), d2], 
                    [0,0,1]]) 
    p = np.concatenate( ( J2*p, np.matrix([0,0,1]).T), axis=1 )

    J1 = np.matrix([[np.cos(a1), -np.sin(a1), 0], 
                    [np.sin(a1), np.cos(a1), d1], 
                    [0,0,1]]) 
    
    p = np.concatenate( ( J1*p, np.matrix([0,0,1]).T), axis=1 )


    plt.scatter(p[0,:].tolist()[0],p[1,:].tolist()[0], s=20, facecolors='none', edgecolors='r')
    plt.scatter(0,0, s=20, facecolors='r', edgecolors='r')
    plt.plot(p[0,:].tolist()[0],p[1,:].tolist()[0])
    plt.plot(target[0,0], target[0,1],'*')
    plt.axis('scaled')
    plt.xlim([-8,8])
    plt.ylim([-8,8])

    plt.show()
    
target = interact(Robot_Simulator, q1=(-180,180), q2=(-180,180));
0
0
Question

Do the reverse Kinematics again, and find three angles that place the robot on the star.


This page titled 15.2: 2D Forward Kinematics is shared under a CC BY-NC 4.0 license and was authored, remixed, and/or curated by Dirk Colbry via source content that was edited to the style and standards of the LibreTexts platform.

Support Center

How can we help?