Forward Kinematics

February 4, 2016

ˆ Kinematics is the relationships between the positions, velocities and accelerations of

ˆ The problem we’re eventually trying to solve is: we know where we want our robot
is, what position are the motors in? This in Inverse Kinematics, and is important
because we can use the same technique to solve: we know what position we want to be
in, what position would the motors be in? Once we know that, we can compare where
the motors are now to where the motors should be, and make them so.

ˆ To understand that, we first need to understand the problem of: We know what position
the motors are in, where is the arm? This is know as the forward kinematics problem.

– We will focus on the positions.

ˆ Serial link manipulator is a series of links which connects the hand of the robot to the
base.

– Each link is connected to the next by an actuated joint (i.e. one that the robot
can move).
– The relationship between neighboring links can be described with (yet another)
homogeneous transformation matrix, denoted A.
– We use a series of A matrices to describe the transform from the base of the robot
to the hand of the manipulator- This is called the forward kinematic transform.

ˆ This transform is part of a closed equation involving the position transform from the
previous discussion on space:
R
TH = R TL1 L1 TL2 L2 TL3 · · · Ln−1 TLn
Lm−1
we’ll abbreviate each TLm as Am .

ˆ In order to calculate the hand position, we must:

– Set up coordinate frames for each link in the hand,

1
 – Generate A matrices for each transformation between the frames,
– Multiply the A matrices to generate the right hand side of the above equation,
– Solve the equation for the various pieces we need: the position of the hand frame,
as well as its orientation.

ˆ The method of setting up coordinated frames is somewhat arbitrary, but we’ll use one
standard (Paul, 1981).

ˆ Setup:

– Define: Link- a rigid piece of the robot arm that connects two joints. It therefore
maintains a fixed relationship between the joints at its ends.
– Define: Joint- a connection between two links which allows the links to either
rotate of translate w/r/t each other. When they rotate, it is called a revolute
joint, when they translate, its called a prismatic- no idea why. We will mostly
discuss revolute joints cause that’s what we have in our robots. But note that the
fingers are prismatic.
– Define: Base- a link that does not move w/r/t the robot frame.
– Define: Proximal- of two things (links, joints etc.) the one closer to the robot
base in the chain of links.
– Define: Distal - of two things (links, joints etc.) the one farther from the robot
base in the chain of links.
– Define: Joint Axis- the axis around which the revolute joint turns.
– Start at the base, number the links from 0 to n. The base is 0.
– Number the joints from 1 to n+1. The endpoint of the robot arm is n+1.
– Base frame will be located at joint 1. Why? Why not the bottom of the base?
– z-axis of the base frame is the joint axis of joint 1.
– x-axis of the base frame should be in the direction you want to make the start
position of link 1.

ˆ Repeatedly assign coordinate frame to each link:

– To locate the origin of the frame:

2
 * if the joint axes do intersect, then locate the origin at the intersection of the
joint axes.
– Make the z axis the distal joint axis. What direction should the z be? It doesn’t
really matter, but its best if we’re consistent as a class. We’ll start with z pointing
up, and talk about other cases later.
– If the 2 joint axes do not intersect, then make the x axis coincident with the the
common normal to the 2 joint axes of the link. Make x point away from the
proximal joint.
– If the joint axes intersect, then make the x axis perpendicular to the plane defined
by the 2 joint axes.
– Make the y axis correct using the right hand rule.
– At the hand/gripper set a coordinate frame at the center of the gripper area (the
grip location)
* make the z-axis parallel to the z-axis of the previous coordinate frame.
* make the z-y plane parallel to the plane of the hand.
* Set x-axis so that is is parallel and in the same direction as the previous
x-axis.

ˆ See some examples.

3
ˆ Look at the examples above and note that the relation between two adjacent coordinate
frames can be described by the following parameters:

4
 – ln - the length parameter. The distance between the two z axes. Note that if the
z axes intersect, then ln = 0.
– dn - the distance parameter. The distance between the the two x axes. Often this
too can be 0.
– θn - the link angle. The angle between link n-1 and link n.
– αn - the twist. The angle of twist between the two joints in the link itself.

ˆ We define the A matrices using these parameters:

– An = Rot(z, θ)T rans(0, 0, d)T rans(l, 0, 0)Rot(x, α).

   
cos(Θ) − sin(Θ) 0 0 1 0 0 l 1 0 0 0
 sin(Θ) cos(Θ)
 0 0  0 1
  0 0   0 cos(α) − sin(α)
  0 

 0 0 1 0  0 0 1 d   0 sin(α) cos(α) 0 
0 0 0 1 0 0 0 1 0 0 0 1
 
cos(Θ) − sin(Θ) cos(α) sin(Θ) sin(α) l cos(Θ)
 sin(Θ) cos(Θ) cos(α) − cos(Θ) sin(α) l sin(Θ) 
 
 0 sin(α) cos(α) d 
0 0 0 1
– Finally, we multiply the A matrices together to get the transformation T: T =
A1 A2 A3 ...An

ˆ Next the question becomes, given a T calculated by multiplying the A matrices, can
we determine the position and orientation of the end of the arm?

– given a T:  
xx yx zx px
 xy yy zy py 
 
 xz yz zz pz 
0 0 0 1
calculated by the product of A’s,
– recall that T is also the product of translation and rotation from the reference
frame:
T = T rans(x, y, z)Rot(z, φ)Rot(y, β)Rot(x, ψ).
which is:
 
cos(φ) cos(β) cos(φ) sin(β) sin(ψ) − sin(φ) cos(ψ) cos(φ) sin(β) cos(ψ) + sin(φ) sin(ψ) px
 sin(φ) cos(β) sin(φ) sin(β) sin(ψ) + cos(φ) cos(ψ) sin(φ) sin(β) cos(ψ) − cos(φ) sin(ψ) py 
 
 − sin(β) cos(β) sin(ψ) cos(β) cos(ψ) pz 
0 0 0 1

5
 We set our 2 versions of T equal to each other and solve for the position p as well
as φ, β, and ψ.

β = arcsin(−xz )
yz
ψ = arcsin( )
cos(β)
xy
φ = arcsin( )
cos(β)

– And p is just the rightmost column.