ROBOTICS

Kinematics
Lecture 6
Erbil International University
Computer Science Department
2nd Term -4th Grade

Dr. Wasan Adnan Hashim

Kinematics studies the motion of bodies
 Review
• Link and Joint Parameters
– Joint angle  i : the angle of rotation from the X axis to the
i-1
Xi axis about the Zi-1 axis. It is the joint variable if joint i is
rotary.
di
– Joint distance : the distance from the origin of the (i-1)
coordinate system to the intersection of the Zi-1 axis and the
Xi axis along the Zi-1 axis. It is the joint variable if joint i is
prismatic. ai
– Link length : the distance from the intersection of the Zi-1
axis and the Xi axis to the origin of the ith coordinate system
along the Xi axis. i
– Link twist angle : the angle of rotation from the Zi-1 axis3 to
 ROBOT KINEMATICS
• Kinematics is the science of motion
• Kinematics is the relationships
between the positions, velocities, and
accelerations of the links of a
manipulator, where a manipulator is
an arm, finger, or leg
 An Example - The PUMA 560

2
3

4
1
There are two more
joints on the end
effector (the
The PUMA 560 has SIX revolute joints gripper)
A revolute joint has ONE degree of freedom ( 1 DOF) that is
defined by its angle
 HOMOGENEOUS
TRANSFORMATIONS
• [0 0 0 w]T : if w is not equal to 0, this
defines the origin
• A transformation is a 4 by 4 matrix
• Rotation of  about the x axis
• Rotation about the y axis
• Rotation about the z axis
• Translation
 Transformations
• General coordinate transformation from x’ to x
is x = Bx’ + p , where B is a rotation matrix and p
is a translation vector
• More conveniently, one can create an augmented
matrix which allows the above equation to be
expressed as x = A x’.
• Coordinate transformations of multilink systems
are represented as
x0 = A01 A12A23. . .A(n-1)(n)xn
 Homogeneous Matrices in 3D
H is a 4x4 matrix that can describe a translation, rotation, or both in one matrix
O
1 0 0 Px 
Y
0 1 0 Py 
N H
P
0 0 1 Pz 
X A  
Translation without rotation 0 0 0 1
Z

Y
n x ox ax 0
n oy ay 0
H y
O
N
n z oz az 0
 
X 0 0 0 1
Rotation part:
Z Rotation without translation Could be rotation around z-axis,
A x-axis, y-axis or a combination of
the three.
 Homogeneous Continued….
V N 
 O The (n,o,a) position of a point relative to the current
V
V XY
 H A  coordinate frame you are in.
V 
 
 1 

n x ox ax Px  V N 
n   O
oy ay Py  V  V X  n x V N  o x V O  a x V A  Px
V XY  y
n z oz az Pz  V A 
  
0 0 0 1   1 

The rotation and translation part can be combined into a single homogeneous
matrix IF and ONLY IF both are relative to the same coordinate frame.
 The Homogeneous Matrix is a concatenation of numerous
translations and rotations
J
I W N 
Y N  O
W 
W A 
 
T
P
X
A O
K
Z
One more variation on finding H:

H= (Rotate so that the X-axis is aligned with T)

* ( Translate along the new t-axis by || T || (magnitude of T))
* ( Rotate so that the t-axis is aligned with P)
* ( Translate along the p-axis by || P || )
* ( Rotate so that the p-axis is aligned with the O-axis)

This method might seem a bit confusing, but it’s actually an easier way to
solve our problem given the information we have. Here is an example…
 We are interested in two kinematics topics
Forward Kinematics (angles to position)
What you are given: The length of each link
The angle of each joint

What you can find: The position of any point

(i.e. it’s (x, y, z) coordinates

Inverse Kinematics (position to angles)

What you are given: The length of each link
The position of some point on the robot

What you can find: The angles of each joint needed to

obtain that position
 Forward vs. Inverse Kinematics
• Forward Kinematics
– Compute configuration (pose) given individual
DOF values
• Good for simulation
• Inverse Kinematics
– Compute individual DOF values that result in
specified end effector position
• Good for control
 What is Inverse Kinematics?
• Forward Kinematics

?
2 3
1 End Effector

Base  
x  f(θ)
 What is Inverse Kinematics?
• Inverse Kinematics

2 3
1 End Effector

Base  1 
θ  f ( x)
Forward Kinematics
 Review
• D-H transformation matrix for adjacent coordinate
frames, i and i-1.
– The position and orientation of the i-th frame coordinate can be
expressed in the (i-1)th frame by the following 4 successive
elementary transformations:
Source coordinate

Ti i 1  T ( zi 1 , d i ) R ( zi 1 ,  i )T ( xi , ai ) R ( xi ,  i )
Reference C i  C  i S i S i S i ai C i 
Coordinate  S C  i C i  S i C  i ai S i 
 i
 0 S i C i di 
 
 0 0 0 1 
17
 Review
• Forward Kinematics • Kinematics Transformation
1   px  Matrix
  p 
 2  y  nx sx ax px 
 3   pz  n sy ay p y 
   T   y
 4     nz sz az pz 
 5     
    0 0 0 1 
 6   
Why use Euler angle representation?
What is a tan 2( y, x) ?
 0    90 for  x and y
 
90    180  x and y

for
  a tan 2( y, x)  
  180 
    90 
for  x and y
  90    0 for  x and y
18
 Review
• Yaw-Pitch-Roll Representation
1
T  Rz , R y , Rx , R T  R y , Rx ,
z ,
 C S 0 0  nx sx ax 0   C 0 S 0 1 0 0 0
 S 0 n
 C 0
 y sy ay 0  0 1 0 0 0 C
  S 0

 0 0 1 0  nz sz az 0    S 0 C 0 0 S C 0
       
 0 0 0 1 0 0 0 1  0 0 0 1 0 0 0 1

 C   n x  S  n y XX XX 0
  S  n  C   n  S  s  C   s  S  a x  C   a y 0
 x y x y

 nz XX XX 0
 
 0 0 0 1
 C SS SC 0
 0 C   S 0 
  (Equation A)
 S CS CC 0
 
 0 0 0 1  19
 ROBOTICS
Kinematics
Lecture 7
Erbil International University
Computer Science Department
2nd Term -4th Grade

Dr. Wasan Adnan Hashim

 The Situation:
You have a robotic arm that
starts out aligned with the xo-axis.
You tell the first link to move by 1
and the second link to move by 2.

The Quest:
What is the position of the
end of the robotic arm?
Solution:
1. Geometric Approach
This might be the easiest solution for the simple situation. However,
notice that the angles are measured relative to the direction of the previous
link. (The first link is the exception. The angle is measured relative to it’s
initial position.) For robots with more links and whose arm extends into 3
dimensions the geometry gets much more tedious.
2. Algebraic Approach
Involves coordinate transformations.
 Example Problem:
You have a three link arm that starts out aligned in the x-axis. Each
link has lengths l1, l2, l3, respectively. You tell the first one to move by 1 , and
so on as the diagram suggests. Find the Homogeneous matrix to get the
position of the yellow dot in the X0Y0 frame.
Y3

3

Y2 2 X3 3
2 X2

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

Y0
1 i.e. Rotating by 1 will put you in the X1Y1 frame.
Translate in the along the X1 axis by l1.
X1

1 Rotating by 2 will put you in the X2Y2 frame.

Y1

X0 and so on until you are in the X3Y3 frame.

The position of the yellow dot relative to the X3Y3 frame is

(l1, 0). Multiplying H by that position vector will give you the
coordinates of the yellow point relative the the X0Y0 frame.
 Slight variation on the last solution:
Make the yellow dot the origin of a new coordinate X4Y4 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
Y1

X0 The position of the yellow dot relative to the X4Y4 frame

is (0,0).
X 0 
Y  0 
   H  Notice that multiplying by the (0,0,0,1) vector will
Z 0  equal the last column of the H matrix.
   
1
  1 
Inverse Kinematics
From Position to Angles
 two-joint robotic arm and given the desired co-ordinate,
the problem reduces to finding the two angles involved.
The first angle is between the first arm and the ground.
The second angle is between the first arm and the second
arm
 Inverse Kinematics
• Transformation Matrix 1 
 
 nx sx ax px   2
n sy ay p y   3 
T06   y  T01T12T23T34T45T56  
 nz

sz az pz 
  4 
0 0 0 1  5 
 
 6 
Robot dependent, Solutions not unique
Systematic closed-form solution in general is not available

Special cases make the closed-form arm solution possible:

1. Three adjacent joint axes intersecting (PUMA, Stanford)
2. Three adjacent joint axes parallel to one another (MINIMOVER)
26
 Example
• Solving the inverse kinematics of Stanford arm
 nx sx ax px 
n sy ay p y 
T06   y  T01T12T23T34T45T56
 nz sz az pz 
 
0 0 0 1

(T01 ) 1T06  T12T23T34T45T56  T16

X X X C1 p x  S1 p y   X X X S 2  d 3 
X X X  pz  X X X  C 2  d 3 
T16    
X X X  S1 p x  C1 p y   X X X 0.1 
   
0 0 0 1  0 0 0 1 
27
 Example
• Solving the inverse kinematics of Stanford arm
 Sin 1  p x  cos 1  p y  0.1 Equation (1)

cos 1  p x  sin 1  p y  sin  2  d 3 Equation (2)

 p z   cos  2  d 3 Equation (3)

In Equ. (1), let
py
p x  r  cos  , p y  r  sin  , r p p ,
2
x
2
y   a tan 2( )
px
sin(  1 )  0.1 
sin   cos 1  sin 1  cos   0.1  r 
r cos(  1 )   1  (0.1 / r ) 
2

py 0 .1
1  a tan 2( )  a tan 2( )
px  r  0 .1
2 2

cos 1 p x  sin 1 p y pz
 2  a tan 2( ) d3 
pz cos  2 28
 Example
• Solving the inverse kinematics of Stanford arm
X X S 5 0
X X  C 5 0
(T34 ) 1 (T23 ) 1 (T12 ) 1 (T01 ) 1T06  T45T56  
X X 0 0
 
From term (3,3) 0 0 0 1

 S 4 [C 2 (C1a x  S1a y )  S 2 a z ]  C 4 ( S1a x  C1a y )  0

 S1a x  C1a y S 5
 4  a tan 2( )  5  a tan 2( )
C 2 (C1a x  S1a y )  S 2 a z C 5
From term (1,3), (2,3)
S 5  C 4 (C 2 (C1a x  S1a y )  S 2 a z ]  S 4 ( S1a x  C1a y )

 C 5  S 2 (C1a x  S1a y )  C 2 a z )
29
 Example
• Solving the inverse kinematics of Stanford arm
C 6  S 6 0 0
 S C 6 0 0
(T45 ) 1 (T34 ) 1 (T23 ) 1 (T12 ) 1 (T01 ) 1T06  T56   6
 0 0 1 0
 
 0 0 0 1

S 6  C 5{C 4 [C 2 (C1s x  S1s y )  S 2 s z ]  S 4 ( S1s x  C1s y )}  S 5 [ S 2 (C1s x  S1s y )  C 2 s z ]

 C 6   S 4 [C 2 (C1s x  S1s y )  S 2 s z ]  C 4 ( S1s x  C1s y )

S 6
 6  a tan 2( )
C 6
30
 ROBOTICS
Kinematics
Lecture 8
Erbil International University
Computer Science Department
2nd Term -4th Grade

Dr. Wasan Adnan Hashim

 A Simple Example

Revolute and
Finding :
Prismatic Joints
y
Combined θ  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 Cosines:
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
 Example
• 2-DOF planar robot arm (x , y)
– Given l1, l2 , Find: Jacobian
- 2
 x  l1 cos 1  l2 cos(1   2 )  h1 (1 , 2 )  l2
 y    l sin   l sin(   )   h ( , )
  1 1 2 1 2   2 1 2 

  x  1 
Y     J   l1

 y   2 
1

 h1 h1 
   2   l1 sin 1  l2 sin(1   2 )  l2 sin(1   2 )
J  1 
 h2 h2   l1 cos 1  l2 cos(1   2 ) l2 cos(1   2 ) 
 1  2 
35
 Hierarchical Kinematic Modeling
• A family of parent-child spatial relationships are
functionally defined
– Moon/Earth/Sun movements
– Articulations of a humanoid

• Limb connectivity is built

into model (joints) and
animation is easier
 A General Transformation in D-H
Convention
D-H transformation for adjacent coordinate frames:
i 1
T i
 Tz , Tz ,d Tx ,aTx , I 44
 The Link Parameters
ai = the distance from zi to zi+1.
measured along xi.

ai = the angle between zi and zi+1.

measured about xi.

di = the distance from xi-1 to xi.

measured along z i.

qi = the angle between xi-1 to xi.

measured about z i
Example:SCARA Robot
 The Concept of
CONFIGURATION SPACE
Notation:
 Example
Suppose a drive robot is commanded to execute the following
motions:
• Straight 0.77 meters
• Right turn for 36 degrees
• Straight 1.16 meters
• Left turn for 60 degrees

• Assume that the world coordinate frame has its origin at the
starting position with the y axis pointing forwards and the x axis
pointing to the robot’s right.
• Assume the robot coordinate frame is coincident with the world
frame at the starting point.