Chapter 2

Kinematics
 Contents
• Definitions,
• Transformations,
• Properties of transformation matrices,
• Forward kinematics,
• Matlab and the robotics toolbox,
• Inverse kinematics
 Robot kinematics
• Kinematics –study of motion with out regard
to the force generating it
• In robots
– Forward kinematics
• Deals with determination of position and orientation of
end effector as a function of joint positions
– Inverse kinematics
• Deals with determination of joint positions for a given
end effector position and orientation
 Forward kinematics
• Description of objects in space
– Needs six parameters
• 3 for rotation or orientation
• 3 for position
– For 3D space manipulation, 6DOF robot is
required
– Most industrial robots are 5 or 6DOF
• To describe position and orientation of a
link in space, a coordinate frame is attached
to the link
 Forward kinematics
• Mechanical structure and notation
y
Link i

Link i-1 x

Revolute link

prismatic
 Mappings
• Rotations
– Given the coordinates of a point in frame {1}, find
the coordinate of the vector in another frame {2}
which is rotated by a given angle wrt frame {1}

2
P 1 R 2 P

 x.u x.v x.w  1 0 0  c  s 0

1
R 2   y.u y.v y.w  1
R 2  0 c  s 1
R 2  s c 0
 z.u z.v z.w  0 s c   0 0 1
 Mappings
• Translation
– Given coordinate of vector or point p in frame {1},
find its coordinates in frame {2} which is translated
by a distance d

2
P 1 P1 D 2
 Mappings
• Combined motion
– Given coordinate of vector or point p in frame {1},
find its coordinates in frame {2} which is rotated
by angles() about x,y and z respectively and
translated by a distance d

1
P1 T2 2 P
 x.u x.v x.w dx 
 y.u y.v y.w dy 
1
T2   
 z.u z.v z.w dz 
 
 0 0 0 1 
Example: use of transformations
• The coordinates of point p in frame {1} are [3
2 1], find its coordinates in frame {2} which is
rotated by 45 degree about Z axis
Ans =[0.707 3.535 1]

y1
P*
 Forward kinematics
• Naming convention
– Base is link 0
– End effector is link n
– Joint between base and link 1 is joint 1
– Joint between n-1 link and end effector is joint n
 Forward kinematics

• Twisted link
– Link length is distance between common
normal to z-axis of the joints
– Link twist is the angle between the two z-axis
• Straight link

i

ai
 DH algorithm or notation
• The definition of a manipulator with four
parameters and the method of assigning
frames and axis is first proposed by Denavit-
Hartenberg in 1955
Axis i-2, joint
i-1 Axis i-1, joint
i, zi-1 Zi
i

ai
C D
A di
Xi-1
B i
 Rule for frame assignment
• Zi axis is aligned with axis i, its assignment is
arbitrary
• Xi axis is perpendicular to axis of Zi-1 and Zi
and points away from axis Zi-1
• Origin of ith coordinate frame is located at the
intersection of axis of joint i+1 and common
normal between (i-1) and i
• Yi is assigned to complete the RH rule
 Joint parameters

• Link length ai- distance measured along xi axis from point

of intersection of xi axis with zi-1 axis to the origin of
frame i
• Link twist – angle between zi-1 and zi axes measured
about xi axis
• Joint distance di- distance measured along zi-1 axis from
the origin of frame i-1 to the point of intersection of xi axis
with zi-1
• Joint angle – angle between xi-1 and xi axes measured
about the zi-1 axis in RH sense
 Kinematic relation ship between adjacent
joints
• The transformation of frame i-1 to frame I
consists of
– A rotation about zi-1 by an angle i
– Translation along zi-1 by a distance di
– Translation by distance ai along xi axis
– Rotation by an angle i about xi axis
 Composite transformation
• The total transformation will be, product of
the transformation matrices

i 1
Ti  Tz i Tz d i Tx a i Tz  i 
C  SC SS aC
 S CC  CS aS 
i 1
Ti   
0 S C d 
 
 0 0 0 1 
Complete transformation matrix of a robot

• The complete transformation matrix of a robot

gives, the location and orientation of the end
effector with respect to the base
• It is found by multiplication of transformation
matrix of adjacent frames
0
Tn  0 T1 1 T2 2 T3 ...n 1 Tn
• Above equation is known as Kinematic model
of robot
 Example – 2DOF planar robot
• For the following two DOF robot, solve the
forward kinematics

x  L1 cos( 1 )  L 2 cos(1   2 )
y  L1 sin(1 )  L 2 sin(1   2 )
 Frame assignment

zo z1 z2
 Kinematic parameters
Link a d  
1 L1 0 1 90
2 L2 0 2 90

C1 0  S1 0 C12 0  S12 0

 S1 S 0
0  C1 0 T2   12
0  C12
T1    0 1 0 0
0 1 0 0
   
0 0 0 1  0 0 0 1
 Inverse kinematics
• Given
– Length of each link
– Position and orientation of end effector
• Required
– Angle of each joint
 Forward kinematics: Find coordinate of yellow dot with
respect to base frame
Y3

Y4
3

Y2 2 X3 3
2 X2
X4

H = Rz(1 ) * Tx1(l1) * Rz(2 ) * Tx2(l2) * Rz(3 ) * Tx3(l3)

Y0
1
This takes you from the X0Y0 frame to the X4Y4 frame.
X1

1
The position of the yellow dot relative to the X4Y4 frame
Y1

X0
is (0,0).

Inverse kinematics is given (x4, y4), find the angles y3,y2 and y1
 A Simple Example
Revolute and Prismatic Joints Combined
Finding :
y
θ  arctan( )
x
More Specifically:
(x , y)
y arctan2() specifies that it’s in the
θ  arctan 2( ) first quadrant
x
Y

S
1 Finding S:
X

S  (x 2  y 2 )
Inverse Kinematics of a Two Link Manipulator

(x , y) Given: l1, l2 , x , y

2 Find: 1 , 2
l2
Redundancy:
A unique solution to this problem
does not exist. Notice, that using the
1
l1 “givens” two solutions are possible.
Sometimes no solution is possible.

l2 (x , y)

l2
l1 l1
The Geometric Solution
(x , y) Using the Law of Cosines:
l2
2 c 2  a 2  b 2  2ab cos C
2 2
( x 2  y 2 )  l1  l2  2l1l2 cos(180  θ 2 )
cos(180  θ 2 )   cos(θ 2 )
2 2
l1 x 2  y 2  l1  l2
 cos(θ 2 ) 
2l1l2
1
 x 2  y 2  l12  l2 2 
θ 2  arccos 

 2l1l2 
Using the Law of Sines:
Redundant since 2 could be in the
sin B sin C
 first or fourth quadrant.
b c
sin θ1 sin(180  θ 2 ) sin(θ 2 )
  Redundancy caused since 2 has two possible
l2 x 2  y2 x 2  y2 values

θ1  θ1  α  l sin(θ )  y
θ1  arcsin 2 2 
 arctan 2 
 y  x 2  y2  x
α  arctan 2   
x
The Algebraic Solution
l2 (x , y) 
2

c 1  cos θ 1
c 1  2  cos( θ 2  θ 1 )
l1 (1) x  l1 c 1  l 2 c 1  2
(2) y  l1 s 1  l 2 sin 1 2
1
(3) θ  θ 1  θ 2

(1) 2  (2) 2  x 2  y 2 
 2 2 2
  2 2 2
 l1 c1  l2 (c1 2 ) 2  2l1l2 c1 (c1 2 )  l1 s1  l2 (sin 1 2 ) 2  2l1l2 s1 (sin 1 2 ) 
 l1  l2  2l1l2 c1 (c1 2 )  s1 (sin 1 2 ) 
2 2

2 2
 l1  l2  2l1l2 c 2 Only Unknown Note:
 x 2  y 2  l12  l2 2  cos(ab)  (cosa)(cosb) (sina)(sinb)
 θ 2  arccos 
2 l l  sin(ab)  (cosa)(sinb) (cosb)(sina)
 1 2 
 x  l1 c 1  l 2 c 1  2 Note:
 l1 c 1  l 2 c 1 c 2  l 2 s 1 s 2 cos(ab)  (cosa)(cosb) (sina)(sinb)
 c 1 ( l1  l 2 c 2 )  s 1 ( l 2 s 2 ) sin(ab)  (cosa)(sinb) (cosb)(sina)

y  l1 s 1  l 2 sin 1  2 We know what 2 is from the previous

 l1 s 1  l 2 s 1 c 2  l 2 s 2 c 1 slide. We need to solve for 1 . Now
 c 1 ( l 2 s 2 )  s 1 ( l1  l 2 c 2 ) we have two equations and two
unknowns (sin 1 and cos 1 )
x  s1 ( l 2 s 2 )
c1 
( l1  l 2 c 2 )
x  s1 ( l 2 s 2 ) Substituting for c1 and simplifying
y ( l 2 s 2 )  s 1 ( l1  l 2 c 2 ) many times
( l1  l 2 c 2 )


1
( l1  l 2 c 2 )

x l 2 s 2  s 1 ( l1 2  l 2 2  2 l1 l 2 c 2 )  Notice this is the law of cosines
and can be replaced by x2+ y2

y ( l1  l 2 c 2 )  x l 2 s 2  y ( l1  l 2 c 2 )  x l 2 s 2 
s1  θ 1  arcsin  

x2 y2  x2 y2 
 For a general manipulator
• For an n DOF robot
n x ox ax dx 
n
n oy ay d y 
0
Tn q1 , q 2, ..., q n   j1 Tj   y
j1 n z oz az dz 
 
0 0 0 1

• Given the 12 elements of the matrix on right

side, find the n angels q1 to qn
• For 6 DOF robot, there are 12 equations and 6
unknowns
For a general robotic manipulator
• Conditions for existence of solution
– The point or location of the end effector has to be
with in the reachable workspace
– The orientation given should be a dexterous
workspace
• Closed form solution
• Iterative or numerical solution
 Closed form solution
• Guidelines to obtain closed form solution
– A) look for equations involving only one joint variable, solve
this equation and obtain the expression for that variable
– B) look for pair or set of equations which could be reduced to
one joint variable by application of algebraic or trig identities
– C) use arc tangent (Atan2(x,y)) function instead of arc sine or
arc cosine.
– D) solutions in terms of position variables are better than
orientation variables
– E) use inverse transform approach
 Inverse kinematics of articulated robot

• Consider the articulated robot with 3DOF

 Articulated robot
• Using DH algorithm
Link a  d  q C S
1 0 90 0 1 1 0 1
2 L2 0 0 2 2 1 0
C 1

3 L3 0 0 3 3 1 0

C1 0 S1 0
S C 2  S2 0 L 2C2 
0  C1 0 S
0
T1   1 C2 0 L 2S2 
0 1 0 0
1
T2   2
  0 0 1 0 
0 0 0 1  
0 0 0 1 
 Articulated robot
• Using DH algorithm
C 3  S3 0 L 3C 3 
S C3 0 L 3S3 
2
T3   3
0 0 1 0 
 
0 0 0 1 

C1C 23  C1S23 S1 C1 L 3C 23  L 2 C 2 
S C  S1S23  C1 S1 L 3C 23  L 2 C 2 
0
T3   1 23
 S23 C 23 0 L3S23  L 2S2 
 
 0 0 0 1 
 Articulated manipulator
• Let the end effector position and orientation
be
 0.35 0.866 0.35 0.11 
 0.6 0.5  0.6  0.18
T 
 0.7 0 0.7 0.21 
 
 0 0 0 1 

• Find the three joint angles

 Articulated joint
• Solutions
– Step 1) apply guide line a, b, c and d

C1C 23  C1S23 S1 C1 L 3C 23  L 2 C 2   r11 r12 r13 r14 

S C  S1S23  C1 S1 L 3C 23  L 2C 2  r21 r22 r23 r24 
0
T3   1 23 
 S23 C 23 0 L 3S23  L 2S2  r31 r32 r33 r34 
   
 0 0 0 1  0 0 0 1

S1 r24  0.18
 
C1 r14 0.11

– Which gives 1=-58.57 degree

 Articulated manipulator
• For the other two angles, use guideline e
0
T1 1 T2 2 T3  T
0 1
T1 T2  T T3  
2 1

• Use elements, (1,4), (2,4) and (3,4) which are

functions of 1 and 2 only
L 2 C 2 C 2   L 3 r11  r14
L 2S1C 2  L 3 r21  r24
L 2S2  L 3 r31  r34
 Articulated joint
• Squaring the first two equations and adding
them and using the trigonometric identity
L 2C2    L3r11  r14 2   L3r21  r24 2

• Dividing the third equation by this one

S2  L 3 r31  r34

C2   L3r11  r14 2   L3r21  r24 2
 Articulated joint
• The third angle is obtained by dividing (3,1) by
(3,2) and equating to right side
C1C 23  C1S23 S1 C1 L 3C 23  L 2 C 2   r11 r12 r13 r14 
S C  S1S23  C1 S1 L 3C 23  L 2 C 2  r21 r22 r23 r24 
0
T3   1 23 
 S23 C 23 0 L3S23  L 2S2  r31 r32 r33 r34 
   
 0 0 0 1  0 0 0 1

S23 r31
• This gives and hence

C 23 r32
2+3= atan(r31,r32)