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 , Z0 ) at the supporting base
with Z 0 axis lying 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 Zi & Zi-1 or at the
intersection of common normal between the Zi & Zi-1 axes and the Zi
axis.
• Establish Xi axis. Establish X i  (Zi1  Zi ) / Zi1  Zi or along the
common normal between the Zi-1 & Zi axes when they are parallel.
• Establish Yi axis. Assign Yi  (Zi  X i ) / Zi  X i to complete the
right-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 di : the distance from the origin of the (i1) 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  i : the 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
Coordinate
C i
 S
 i
 0

 0
 C i S i
S i S i
C i C i
 S i C i
S i
0
C i
0
The City College of New York
ai C i 
ai S i 
di 

1 
6
Review
• Kinematics Equations
– chain product of successive coordinate transformation
matrices of Ti i 1
– T0n specifies the location of the n-th coordinate frame
w.r.t. the base coordinate system
T0n  T01T12 Tnn1
Orientation
matrix
 R0n

0
P0n  n s a P0n 


1  0 0 0 1 
Position
vector
The City College of New York
7
Review
• Forward Kinematics
• Kinematics Transformation
Matrix
 px 
1 
p 
 
 nx
 y
 2
n
 pz 
 3 
 y
  
T

 
 4 
 nz
 
 5 

 
 
0
 6 
 
Why use Euler angle representation?
What is
a tan2( y, x)
sx
sy
sz
0
ax
ay
az
0
px 
p y 
pz 

1
?
 0    90
 

90    180
  a tan 2( y, x)  



180




90

  90    0
The City College of New York
for  x and  y
for  x and  y
for  x and  y
for  x and  y
8
Review
• Yaw-Pitch-Roll Representation
1
z ,
R T  Ry, Rx,
T  Rz , Ry , Rx,
 C
  S

 0

 0
S
C
0
0
0
0
1
0
0
0
0

1
 nx
n
 y
 nz

0
sx
sy
sz
0
ax
ay
az
0
0  C
0  0


 S
0
 
1  0
XX
 C  nx  S  n y
 S  n  C  n  S  s  C  s
x
y
x
y


nz
XX

0
0

 C SS SC 0
 0

C


S

0


 S CS CC 0


0
0
0
1


0 S
1 0
0 C
0 0
0
0
0

1
XX
 S  a x  C  a y
XX
0
1 0
 0 C

0 S

0 0
0
 S
C
0
0
0
0

1
0
0
0

1
(Equation A)
The City College of New York
9
Review
• Compare LHS and RHS of Equation A, we have:
 sin   nx  cos  ny  0
  a tan2(ny , nx )
cos   nx  sin   n y  cos 

nz   sin 

  a tan2(nz , cos  nx  sin   n y )
  sin   s x  cos  s y  cos

 sin   ax  cos  a y   sin
  a tan2(sin  ax  cos  ay , sin   sx  cos  s y )
The City College of New York
10
Inverse Kinematics
• Transformation Matrix
 nx
n
T06   y
 nz

0
sx
sy
sz
0
ax
ay
az
0
px 
p y 
 T01T12T23T34T45T56
pz 

1
1 
 
 2
 3 
 
 4 
 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
n
T06   y
 nz

0
sx
sy
sz
0
ax
ay
az
0
px 
p y 
 T01T12T23T34T45T56
pz 

1
(T01 )1T06  T12T23T34T45T56  T16
X
X
T16  
X

0
X
X
X
0
X
X
X
0
C1 p x  S1 p y   X
 X
 pz
 
 S1 p x  C1 p y   X
 
1
 0
The City College of New York
X
X
X
0
X
X
X
0
S 2  d 3 
 C 2  d 3 
0.1 

1

12
Example
• Solving the inverse kinematics of Stanford arm
 Sin1  px  cos1  py  0.1
cos1  px  sin 1  py  sin 2  d3
 pz   cos2  d3
Equation (1)
Equation (2)
Equation (3)
In Equ. (1), let
px  r  cos ,
p y  r  sin  ,
r
p p ,
2
x
2
y
  a tan2(
py
px

sin(  1 )  0.1

0
.
1
r
sin   cos1  sin 1  cos 


r
2
cos(  1 )   1  (0.1 / r ) 

py
0.1
1  a tan2( )  a tan2(
)
2
2
px
 r  0.1
pz
cos1 px  sin 1 p y
d3 
 2  a tan2(
)
cos 2
pz
The City College of New York
)
13
Example
• Solving the inverse kinematics of Stanford arm
X
X
(T34 ) 1 (T23 ) 1 (T12 ) 1 (T01 ) 1T06  T45T56  
X

From term (3,3)
0
X
X
X
0
S 5
 C 5
0
0
0
0
0

1
 S4[C2 (C1ax  S1ay )  S2az ]  C4 (S1ax  C1ay )  0
S 5
 S1ax  C1a y
 5  a tan 2(
)
 4  a tan2(
)
C 5
C 2 (C1ax  S1a y )  S 2 az
From term (1,3), (2,3)
S5  C 4 (C 2 (C1ax  S1a y )  S 2 az ]  S 4 (S1ax  C1a y )

C5  S 2 (C1ax  S1a y )  C 2 az )

The City College of New York
14
Example
• Solving the inverse kinematics of Stanford arm
C 6
 S
(T45 ) 1 (T34 ) 1 (T23 ) 1 (T12 ) 1 (T01 ) 1T06  T56   6
 0

 0
 S 6
C 6
0
0
0
0
1
0
0
0
0

1
S 6  C5{C 4 [C 2 (C1sx  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 sz ]

C 6   S 4 [C 2 (C1sx  S1s y )  S 2 sz ]  C 4 ( S1sx  C1s y )

S 6
 6  a tan 2(
)
C 6
The City College of New York
15
Jacobian Matrix
1 
 
 2
 3 
 
 4 
 5 
 
 6 
Forward
Kinematics
Inverse
Joint Space
x 
y
 
z 
 
 
 
 
 
Task Space
1 
 
 2 
3 
 
 4 
 
 5
6 
Jacobian
Matrix
 x 
 y 
 
 z 
 
 x 
 y 
 
 z 
Jacobian Matrix: Relationship between joint
space velocity with task space velocity
The City College of New York
16
Jacobian Matrix
Forward kinematics
x 
 h1 (q1 , q2 , , q6 ) 
 q1 
y
h (q , q , , q )
q 
6 
2
 
 2 1 2
 
z 
 h3 (q1 , q2 , , q6 ) 
 q3 


h
(
)
 

  61 

h
(
q
,
q
,

,
q
)
q
6 
 
 4 1 2
 4
 
 h5 (q1 , q2 ,, q6 ) 
 q5 
 


 
h
(
q
,
q
,

,
q
)
 
q
 61
 6 
6 
 6 1 2
 x 
 y 
 
 z 
 
 x 
 y 
 
 z 
 q1 
q 
 dh(q )   2 
 dq    

 6n
 
q n  n1
Y61  h(qn1 )
d
dh(q) dq dh(q)

Y61  h(qn1 ) 

q
dt
dq dt
dq
Y61  J 6n qn1
dh(q )
J
dq
The City College of New York
17
Jacobian Matrix
 x 
 y 
 q1 
 
 z   dh(q )  q 2 
 
  

 x   dq  6n   
 
 y 
q n  n1
 
 z 
 dh(q) 

J  
 dq  6n
Jacobian is a function of
q, it is not a constant!
 h1
 q
 1
 h2
  q1
 
 h
 6
 q1
The City College of New York
h1
q2
h2
q2

h6
q2
h1 

qn 

h2 

qn 

 
h6 


qn  6n
18
Jacobian Matrix
Forward Kinematics
n s a p 
T 

0 0 0 1  44
6
0
 h1 (q ) 
h (q )
Y61  h(q )   2 
  


 h6 (q ) 
Y61  J 6n qn1
 x   h1 ( q ) 
p   y   h2 ( q ) 
 z   h3 ( q ) 
 (q )  h4 (q )
{n, s, a}   (q )    h5 (q ) 
 (q ) h6 (q ) 
 x 
 y 
 
 z  V 

Y   
 x   
 y 
 
 z 
Linear velocity Angular velocity
 x 
V   y 
 z 
The City College of New York
  
 
   
 
 
19
Example
• 2-DOF planar robot arm
– Given l1, l2 , Find: Jacobian
 x  l1 cos1  l2 cos(1   2 )  h1 (1 , 2 ) 
 y    l sin   l sin(   )   h ( , )
  1
1
2
1
2 
 2 1 2 
1 
 x 

Y     J 

 y 
 2 
 h1
 
J  1
 h2
 1
(x , y)
- 2
l2
1 l1
h1 
 2   l1 sin 1  l2 sin(1   2 )  l2 sin(1   2 )

h2   l1 cos1  l2 cos(1   2 ) l2 cos(1   2 ) 
 2 
The City College of New York
20
Jacobian Matrix
•Physical Interpretation
 J11
J
Y  Jq   21
 

 J 61
J12
J 22

J 62
 J16   q 

q 

 J 26   q 
 

  q 
  q 
 J 66  q 
 x   J11q1  J12 q 2    J16 q6 
 y   J q  J q    J q 
22 2
26 6 
   21 1
 z   J 31q1  J 32 q 2    J 36 q6 

   



J
q

J
q



J
q

42 2
46 6 
   41 1
   J 51q1  J 52 q 2    J 56 q6 

  
   J 61q1  J 62 q 2    J 66 q6 
1
2
3
4
5
6
How each individual joint
space velocity contribute
to task space velocity.
The City College of New York
21
Jacobian Matrix
• Inverse Jacobian
 J11
J
Y  Jq   21
 

 J 61
J12
J 22

 J16   q 

q 

 J 26   q 
 

  q 
 q 
 J 66  q 
1
q5
2
3
4
5
J 62
6
q  J Y
1
q1
• 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 )
J 

l
cos


l
cos(



)
l
cos(



)
1
2
1
2
2
1
2 
 1
determinant(J)=0
2  0
Det(J)=0
Not full rank
V
(x , y)
l2
Y
2 =0
1 l1
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:
– Example:
 AT [ AAT ]1

A   A1
[ AT A]1 AT

1 0 2
3
1  1 0 x   2


 
mn
mn
mn
x  Ab  1 / 9[5,13,16]T
1 1 
1 4 
1
5 1 
1




T
T 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
Tasks
Task Plan
Action Plan
Path Plan
Trajectory
Plan
Controller
Robot
• Path planning
– Geometric path
– Issues: obstacle avoidance, shortest
path
• Trajectory planning,
– “interpolate” or “approximate” the
desired path by a class of polynomial
functions and generates a sequence of
time-based “control set points” for the
control of manipulator from the initial
configuration to its destination.
Sensor
The City College of New York
25
Trajectory Planning
(continuity,
smoothness)
Path
constraints
joint space
Path
specification
Trajectory
Planner
{q (t ), q (t ), q(t )}
or
sequence of control set points
along desired trajectory
{ p(t ), v(t ), a(t )}
cartesian space
The City College of New York
26
Trajectory planning
Joint i
Final
q(tf)
q(t2)
• Path Profile
• Velocity Profile
Set down
q(t1)
q(t0)
Lift-off
Initial
• Acceleration Profile
t0
t1
t2
tf Time
Speed
t0
Acceleration
t0
The City College of New York
t1
t1
t2
t2
tf
Time
tf Time
27
The boundary conditions
1)
2)
3)
4)
5)
6)
7)
8)
9)
10)
11)
12)
13)
14)
Initial position
Initial velocity
Initial acceleration
Lift-off position
Continuity in position at t1
Continuity in velocity at t1
Continuity in acceleration at t1
Set-down position
Continuity in position at t2
Continuity in velocity at t2
Continuity in acceleration at t2
Final position
Final velocity
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
a13t13   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
z
z
y
y
x
z
x
y
x
y
x
The City College of New York
34