Introduction to ROBOTICS

Inverse Kinematics
Jacobian Matrix
Trajectory Planning
Dr. Jizhong Xiao
Department of Electrical Engineering
City College of New York
jxiao@ccny.cuny.edu

The City College of New York 1

 Outline

• Review
– Kinematics Model
– Inverse Kinematics
• Example
• Jacobian Matrix
– Singularity
• Trajectory Planning

The City College of New York 2

 Review
• Steps to derive kinematics model:
– Assign D-H coordinates frames
– Find link parameters
– Transformation matrices of adjacent joints
– Calculate kinematics matrix
– When necessary, Euler angle representation

The City College of New York 3

 Denavit-Hartenberg Convention
• Number the joints from 1 to n starting with the base and ending with
the end-effector.
• Establish the base coordinate system. Establish a right-handed
orthonormal coordinate system ( X 0 , Y0 , Z 0 at ) the supporting base with
axis Zlying
0 along the axis of motion of joint 1.
• Establish joint axis. Align the Zi with the axis of motion (rotary or
sliding) of joint i+1.
• Establish the origin of the ith coordinate system. Locate the origin of
the ith coordinate at the intersection of the Z i & Zi-1 or at the
intersection of common normal between the Z i & Zi-1 axes and the Zi
axis.
• Establish Xi axis. Establish X i  ( Z i 1  Z i ) / Z i 1  Z i or along the
common normal between the Zi-1 & Zi axes when they are parallel.
• Establish Y axis. Assign Yi  ( Z i  X i ) / Z i  X i to complete the right-
i
handed coordinate system.
• Find the link and joint parameters
The City College of New York 4
 Review
• Link and Joint Parameters
– Joint angle :i the angle of rotation from the Xi-1 axis to the
Xi axis about the Zi-1 axis. It is the joint variable if joint i is
rotary.
– Joint distance d: ithe 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.
– Link length a:i 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.
– Link twist angle : ithe angle of rotation from the Zi-1 axis to
the Zi axis about the Xi axis.
The City College of New York 5
 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 

The City College of New York 6

 Review
• Kinematics Equations
– chain product of successive coordinate transformation
i
matrices of Ti 1
– T0n specifies the location of the n-th coordinate frame
w.r.t. the base coordinate system

T0n  T01T12  Tnn1

Orientation
 R0n P0n  n s a P0n 
matrix
  
0 1  0 0 0 1 
Position
vector

The City College of New York 7

 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 for  x and  y
  a tan 2( y, x)    
  180     90 for  x and  y
  90    0 for  x and y
The City College of New York 8
 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 0 n
 C
 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  nx  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 
The City College of New York 9
 Review
• Compare LHS and RHS of Equation A, we have:

 sin   nx  cos   n y  0   a tan 2(n y , nx )

cos   nx  sin   n y  cos 


 nz   sin    a tan 2(n z , cos   n x  sin   n y )

  sin   s x  cos   s y  cos


 sin   a x  cos   a y   sin

  a tan 2(sin   a x  cos   a y , sin   s x  cos  s y )

The City College of New York 10
 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)

The City College of New York 11

 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 

The City College of New York 12

 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
2 2
py
p x  r  cos  , p y  r  sin  , r p p ,
x y   a tan 2( )
px
sin(  1 )  0.1 
sin   cos1  sin 1  cos   0.1  r 
r 2
cos(  1 )   1  (0.1 / r ) 
py 0.1
1  a tan 2( )  a tan 2( )
px 2
 r  0.1 2

cos 1 p x  sin 1 p y pz
 2  a tan 2( ) d3 
pz cos  2
The City College of New York 13
 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 )

The City College of New York 14

 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

The City College of New York 15

 Jacobian Matrix

1  x  1 
 
Forward  x 
y    y 
 2    2  Jacobian  
 3  z  3  Matrix  z 
  Kinematics      
 4     4   x 
 5       y 
     5  
 6  Inverse   6   z 

Joint Space Task Space Jacobian Matrix: Relationship between joint

space velocity with task space velocity

The City College of New York 16

 Jacobian Matrix
Forward kinematics
x   q1   h1 (q1 , q2 , , q6 ) 
y q  h (q , q , , q )
    2  2 1 2 6 
z   q3   h3 (q1 , q2 , , q6 )  Y61  h(qn1 )
   h ( )
  61   

  q
 4 h (
 4 1 2q , q ,  , q )
6 
   q5   h5 (q1 , q2 , , q6 ) 
     
  q
 6  h ( q
 6 1 2 , q ,  , q )
6  61

 d dh(q ) dq dh(q )
Y61  h(qn1 )   q
dt dq dt dq
 x 
 y   q 1 
  q 
 z   dh(q )   2 
   dq     Y61  J 6n q n1
 x    6n
 y    dh(q)
  q n  n1 J
 z  dq
The City College of New York 17
 Jacobian Matrix
 x 
 y 
   q 1 
 z   dh(q )  q 2  Jacobian is a function of
    
 q, it is not a constant!
 x   dq  6n   
 y   
 q n  n1
 
 z   h1 h1 h1 
 q 
q2 qn 
 1 
 dh(q )   h2 h2

h2 
J      q1 q2 qn 
 dq  6n     
 h h6 h6 
 6  
 q1 q2 qn  6n

The City College of New York 18

 Jacobian Matrix
Forward Kinematics
 x   h1 (q ) 
p   y   h2 (q )
6n s a p  z   h3 (q ) 
T 
1  44
0
0 0 0  (q )  h4 (q)
 h1 (q)  {n, s, a}   (q )    h5 (q ) 
h (q )  (q ) h6 (q ) 
Y61  h(q )   2 
    x 
   y  Linear velocity Angular velocity
 
h6 (q )   z  V   

Y   
 x 
 
 x   V   y     
Y61  J 6n q n1  y 
 
   z   
 z 
The City College of New York 19
 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 

The City College of New York 20

 Jacobian Matrix
•Physical Interpretation
 q 1 
 J11 J12  J16  q 
J J 22  J 26 
 2
 q 3 
Y  Jq   21  
     q 4 
 q 5 
   
 J 61 J 62  J 66   q 6 

 x   J11q 1  J12 q 2    J16 q 6 

 y   J q  J q    J q 
   21 1 22 2 26 6 

 z   J 31q 1  J 32 q 2    J 36 q 6  How each individual joint

     space velocity contribute

   41 1 J q  J q    J q
46 6 
42 2 to task space velocity.
   J 51q 1  J 52 q 2    J 56 q 6 
   
   J 61q 1  J 62 q 2    J 66 q 6 

The City College of New York 21

 Jacobian Matrix
• Inverse Jacobian
 q 1  q5
 J11 J12  J16  q 
J J 22  J 26 
 2
 q 3 
Y  Jq   21  
     q 4 
 q 5 
   
 J 61 J 62  J 66  q 6 
q1
q  J Y
1

• Singularity
– rank(J)<min{6,n}, Jacobian Matrix is less than full rank
– Jacobian is non-invertable
– Boundary Singularities: occur when the tool tip is on the surface
of the work envelop.
– Interior Singularities: occur inside the work envelope when two or
more of the axes of the robot form a straight line, i.e., collinear

The City College of New York 22

 Quiz
• Find the singularity configuration of the 2-DOF
planar robot arm
 x  
Y     J  1 
 y  
 2 
 l1 sin 1  l2 sin(1   2 )  l2 sin(1   2 ) V
J   (x , y)
 1l cos 1  l 2 cos(1   2 ) l 2 cos(1   )
2 

determinant(J)=0 Not full rank

l2
Y
2  0  2 =0

Det(J)=0  l11

x
The City College of New York 23
 Jacobian Matrix
• Pseudoinverse
– Let A be an mxn matrix, and let A be the pseudoinverse
of A. If A is of full rank, then A can be computed as:
 AT [ AAT ]1 mn

A   A1 mn
[ AT A]1 AT mn

– Example:
1 0 2 3 x  A b  1 / 9[5,13,16]T
1  1 0 x   2
   
1 1  1 1 4 
 T T 1   5 1  1 
A  A [ AA ]  0  1    1  5
1 2  9 
2 0  4  2
The City College of New York 24
 Robot Motion Planning
T asks
T ask P lan
• Path planning
– Geometric path
A ction P lan – Issues: obstacle avoidance, shortest
path
P ath P lan
• Trajectory planning,
T rajectory – “interpolate” or “approximate” the
P lan
desired path by a class of polynomial
C ontroller
functions and generates a sequence of
time-based “control set points” for the
R obot control of manipulator from the initial
configuration to its destination.
S ensor

The City College of New York 25

 Trajectory Planning

(continuity,
sm oothness)
P ath
constraints

joint space
{q (t ), q (t ), q(t )}
P ath T rajectory sequence of control set points
or
specification P lanner along desired trajectory

{ p(t ), v(t ), a(t )}

cartesian space

The City College of New York 26

 Trajectory planning
Joint i
F inal
q(tf)
q(t2)

• Path Profile S et dow n

• Velocity Profile q(t1)

Lift-off
Initial
q(t0)

• Acceleration Profile
t0 t1 t2 tf T im e
S peed

t0 t1 t2 tf T im e
A cceleration

t0 t1 t2 tf T im e

The City College of New York 27

 The boundary conditions
1) Initial position
2) Initial velocity
3) Initial acceleration
4) Lift-off position
5) Continuity in position at t1
6) Continuity in velocity at t1
7) Continuity in acceleration at t1
8) Set-down position
9) Continuity in position at t2
10) Continuity in velocity at t2
11) Continuity in acceleration at t2
12) Final position
13) Final velocity
14) Final acceleration

The City College of New York 28

 Requirements
• Initial Position
– Position (given)
– Velocity (given, normally zero)
– Acceleration (given, normally zero)
• Final Position
– Position (given)
– Velocity (given, normally zero)
– Acceleration (given, normally zero)

The City College of New York 29

 Requirements
• Intermediate positions
– set-down position (given)
– set-down position (continuous with previous
trajectory segment)
– Velocity (continuous with previous trajectory
segment)
– Acceleration (continuous with previous
trajectory segment)

The City College of New York 30

 Requirements
• Intermediate positions
– Lift-off position (given)
– Lift-off position (continuous with previous
trajectory segment)
– Velocity (continuous with previous trajectory
segment)
– Acceleration (continuous with previous
trajectory segment)

The City College of New York 31

 Trajectory Planning
• n-th order polynomial, must satisfy 14 conditions,
• 13-th order polynomial
a13t 13    a2t 2  a1t  a0  0
• 4-3-4 trajectory
h1 (t )  a14t 4  a13t 3  a12t 2  a12t  a10 t0t1, 5 unknow

h2 (t )  a23t 3  a22t 2  a21t  a20 t1t2, 4 unknow

hn (t )  an 4t 4  an 3t 3  an 2t 2  an 2t  an 0 t2tf, 5 unknow

• 3-5-3 trajectory

The City College of New York 32

 How to solve the parameters
• Handout in the class

The City College of New York 33

 Thank you!
Homework 3 posted on the web.

Next class: Robot Dynamics

z
y
z
y x
z z
x y
y x

x
The City College of New York 34