The City College of New York

1

Jizhong Xiao
Department of Electrical Engineering
City College of New York
jxiao@ccny.cuny.edu
Inverse Kinematics
Jacobian Matrix
Trajectory Planning
Introduction to ROBOTICS
The City College of New York
2
Outline

Review
Kinematics Model
Inverse Kinematics
Example
Jacobian Matrix
Singularity
Trajectory Planning


The City College of New York
3
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
4
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 at the supporting base
with axis lying along the axis of motion of joint 1.
Establish joint axis. Align the Z
i
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
& Z
i-1
or at the
intersection of common normal between the Z
i
& Z
i-1
axes and the Z
i

axis.
Establish X
i
axis. Establish or along the
common normal between the Z
i-1
& Z
i
axes when they are parallel.
Establish Y
i
axis. Assign to complete the
right-handed coordinate system.
Find the link and joint parameters
) , , (
0 0 0
Z Y X
i i i i i
Z Z Z Z X =
1 1
/ ) (
i i i i i
X Z X Z Y + = / ) (
0
Z
The City College of New York
5
Review
Link and Joint Parameters
Joint angle : the angle of rotation from the X
i-1
axis to
the X
i
axis about the Z
i-1
axis. It is the joint variable if
joint i is rotary.
Joint distance : the distance from the origin of the (i-
1) coordinate system to the intersection of the Z
i-1
axis
and the X
i
axis along the Z
i-1
axis. It is the joint
variable if joint i is prismatic.
Link length : the distance from the intersection of the
Z
i-1
axis and the X
i
axis to the origin of the ith
coordinate system along the X
i
axis.
Link twist angle : the angle of rotation from the Z
i-1

axis to the Z
i
axis about the X
i
axis.
i
u
i
d
i
a
i
o
The City College of New York
6
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:



(
(
(
(

=
=

1 0 0 0
0
) , ( ) , ( ) , ( ) , (
1 1 1
i i i
i i i i i i i
i i i i i i i
i i i i i i i i
i
i
d C S
S a C S C C S
C a S S S C C
x R a x T z R d z T T
o o
u u o u o u
u u o u o u
o u
Source coordinate
Reference
Coordinate
The City College of New York
7
Review
Kinematics Equations
chain product of successive coordinate transformation
matrices of
specifies the location of the n-th coordinate frame
w.r.t. the base coordinate system

(

=
(

=
=

1 0 0 0 1 0
0 0 0
1
2
1
1
0 0
n n n
n
n
n
P a s n P R
T T T T
i
i
T
1
n
T
0
Orientation
matrix
Position
vector
The City College of New York
8
Review
Forward Kinematics






(
(
(
(
(
(
(
(

(
(
(
(
(
(
(
(

u
|
u
u
u
u
u
u
z
y
x
p
p
p
6
5
4
3
2
1
(
(
(
(

=
1 0 0 0
z z z z
y y y y
x x x x
p a s n
p a s n
p a s n
T
Kinematics Transformation
Matrix

+ s s
s s
+ s s
+ + s s
= =
y and x f or
y and x f or
y and x f or
y and x f or
x y a




0 90
90 180
180 90
90 0
) , ( 2 tan
u
u
u
u
u
Why use Euler angle representation?
) , ( 2 tan x y a
What is ?
The City College of New York
9
Review
(
(
(
(

=
1 0 0 0
0
0 0
0
u u u

u u u
C C S C S
S C
C S S S C
u | , , , x y z
R R R T =
u | , ,
1
, x y z
R R T R =

(
(
(
(

1 0 0 0
0 1 0 0
0 0
0 0
| |
| |
C S
S C
(
(
(
(

=
1 0 0 0
0 0
0 0 1 0
0 0
u u
u u
C S
S C
(
(
(
(

1 0 0 0
0 0
0 0
0 0 0 1


C S
S C
(
(
(
(

1 0 0 0
0
0
0
z z z
y y y
x x x
a s n
a s n
a s n
(Equation A)
Yaw-Pitch-Roll Representation
(
(
(
(

+ + +
+
1 0 0 0
0
0
0
XX XX n
a C a S s C s S n C n S
XX XX n S n C
z
y x y x y x
y x
| | | | | |
| |
The City College of New York
10
Review
0 cos sin = +
y x
n n | |

=
= +
u
u | |
sin
cos sin cos
z
y x
n
n n

= +
= +
| |
| |
sin cos sin
cos cos sin
y x
y x
a a
s s
Compare LHS and RHS of Equation A, we have:
) , ( 2 tan
x y
n n a = |
) sin cos , ( 2 tan
y z z
n n n a + = | | u
) cos sin , cos (sin 2 tan
y x y x
s s a a a + = | | | |
The City College of New York
11
Inverse Kinematics
(
(
(
(
(
(
(
(

6
5
4
3
2
1
u
u
u
u
u
u
6
5
5
4
4
3
3
2
2
1
1
0
6
0
1 0 0 0
T T T T T T
p a s n
p a s n
p a s n
T
z z z z
y y y y
x x x x
=
(
(
(
(

=
Transformation Matrix
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)
Robot dependent, Solutions not unique
Systematic closed-form solution in general is not available

The City College of New York
12
Example
Solving the inverse kinematics of Stanford arm
6
5
5
4
4
3
3
2
2
1
1
0
6
0
1 0 0 0
T T T T T T
p a s n
p a s n
p a s n
T
z z z z
y y y y
x x x x
=
(
(
(
(

=
6
1
6
5
5
4
4
3
3
2
2
1
6
0
1 1
0
) ( T T T T T T T T = =

(
(
(
(

+

+
=
1 0 0 0
1 1
1 1
6
1
y x
z
y x
p C p S X X X
p X X X
p S p C X X X
T
u u
u u
(
(
(
(

=
1 0 0 0
1 . 0
3 2
3 2
X X X
d C X X X
d S X X X
u
u
The City College of New York
13
Example
Solving the inverse kinematics of Stanford arm
3 2 1 1
sin sin cos d p p
y x
= + u u u
1 . 0 cos
1 1
= +
y x
p p Sin u u
3 2
cos d p
z
= u
Equation (1)
Equation (2)
Equation (3)
In Equ. (1), let
) ( 2 tan , , sin , cos
2 2
x
y
y x y x
p
p
a p p r r p r p = + = = = o o o

=
=
=
2
1
1
1 1
) / 1 . 0 ( 1 ) cos(
1 . 0
) sin(
1 . 0
cos sin cos sin
r
r
r
u o
u o
o u u o
)
1 . 0
1 . 0
( 2 tan ) ( 2 tan
2 2
1

=
r
a
p
p
a
x
y
u
)
sin cos
( 2 tan
1 1
2
z
y x
p
p p
a
u u
u
+
=
2
3
cosu
z
p
d =
The City College of New York
14
Example
Solving the inverse kinematics of Stanford arm
)
) (
( 2 tan
2 1 1 2
1 1
4
z y x
y x
a S a S a C C
a C a S
a
u u u u
u u
u
+
+
=
(
(
(
(

= =

1 0 0 0
0 0
0
0
) ( ) ( ) ( ) (
5
5
6
5
5
4
6
0
1 1
0
1 2
1
1 3
2
1 4
3
X X
C X X
S X X
T T T T T T T
u
u
0 ) ( ] ) ( [
1 1 4 2 1 1 2 4
= + + +
y x z y x
a C a S C a S a S a C C S u u u u u u u u

+ + =
+ + + =
) ) (
) ( ] ) ( (
2 1 1 2 5
1 1 4 2 1 1 2 4 5
z y x
y x z y x
a C a S a C S C
a C a S S a S a S a C C C S
u u u u u
u u u u u u u u u
From term (3,3)
From term (1,3), (2,3)
) ( 2 tan
5
5
5
u
u
u
C
S
a =
The City College of New York
15
Example
Solving the inverse kinematics of Stanford arm

+ + + =
+ + + + + + =
) ( ] ) ( [
] ) ( [ )} ( ] ) ( [ {
1 1 4 2 1 1 2 4 6
2 1 1 2 5 1 1 4 2 1 1 2 4 5 6
y x z y x
z y x y x z y x
s C s S C s S s S s C C S C
s C s S s C S S s C s S S s S s S s C C C C S
u u u u u u u u u
u u u u u u u u u u u u u u u
(
(
(
(

= =

1 0 0 0
0 1 0 0
0 0
0 0
) ( ) ( ) ( ) ( ) (
6 6
6 6
6
5
6
0
1 1
0
1 2
1
1 3
2
1 4
3
1 5
4
u u
u u
C S
S C
T T T T T T T
) ( 2 tan
6
6
6
u
u
u
C
S
a =
The City College of New York
16
Jacobian Matrix
(
(
(
(
(
(
(
(

6
5
4
3
2
1
u
u
u
u
u
u
(
(
(
(
(
(
(
(

6
5
4
3
2
1
u
u
u
u
u
u

(
(
(
(
(
(
(
(

u
|
z
y
x
Joint Space Task Space
Forward
Inverse
Kinematics
Jacobian Matrix: Relationship between joint
space velocity with task space velocity
(
(
(
(
(
(
(
(

z
y
x
z
y
x
e
e
e

Jacobian
Matrix
The City College of New York
17
Jacobian Matrix
(
(
(
(
(
(
(
(

u
|
z
y
x
1 6
6
5
4
3
2
1
) (

(
(
(
(
(
(
(
(

=
q
q
q
q
q
q
h
1 6
6 2 1 6
6 2 1 5
6 2 1 4
6 2 1 3
6 2 1 2
6 2 1 1
) , , , (
) , , , (
) , , , (
) , , , (
) , , , (
) , , , (

(
(
(
(
(
(
(
(

=
q q q h
q q q h
q q q h
q q q h
q q q h
q q q h

Forward kinematics
) (
1 1 6
=
n
q h Y
q
dq
q dh
dt
dq
dq
q dh
q h
dt
d
Y
n

) ( ) (
) (
1 1 6
= = =

(
(
(
(
(
(
(
(

z
y
x
z
y
x
e
e
e

1
2
1
6
) (

(
(
(
(

=
n
n
n
q
q
q
dq
q dh

1 6 1 6
=
n n
q J Y

dq
q dh
J
) (
=
The City College of New York
18
Jacobian Matrix
(
(
(
(
(
(
(
(

z
y
x
z
y
x
e
e
e

1
2
1
6
) (

(
(
(
(

=
n
n
n
q
q
q
dq
q dh

n
n
n
n
n
q
h
q
h
q
h
q
h
q
h
q
h
q
h
q
h
q
h
dq
q dh
J

(
(
(
(
(
(
(
(

c
c
c
c
c
c
c
c
c
c
c
c
c
c
c
c
c
c
=
|
|
.
|

\
|
=
6
6
2
6
1
6
2
2
2
1
2
1
2
1
1
1
6
) (

Jacobian is a function of
q, it is not a constant!
The City College of New York
19
Jacobian Matrix
4 4
6
0
1 0 0 0

(

=
p a s n
T
(
(
(

=
(
(
(

=
) (
) (
) (
3
2
1
q h
q h
q h
z
y
x
p
(
(
(

=
(
(
(

) (
) (
) (
) (
) (
) (
} , , {
6
5
4
q h
q h
q h
q
q
q
a s n

u
|
(

O
=
(
(
(
(
(
(
(
(

=
V
z
y
x
Y
z
y
x
e
e
e

Forward Kinematics
(
(
(

=
z
y
x
V

(
(
(

= O

u
|

(
(
(
(

= =

) (
) (
) (
) (
6
2
1
1 6
q h
q h
q h
q h Y

1 6 1 6
=
n n
q J Y

Linear velocity Angular velocity

The City College of New York
20
Example
2-DOF planar robot arm
Given l
1
, l
2
, Find: J acobian
u
2
u
1
(x , y)
l
2

l
1

(

=
(

+ +
+ +
=
(

) , (
) , (
) sin( sin
) cos( cos
2 1 2
2 1 1
2 1 2 1 1
2 1 2 1 1
u u
u u
u u u
u u u
h
h
l l
l l
y
x
(

+ + +
+ +
=
(
(
(
(

c
c
c
c
c
c
c
c
=
) cos( ) cos( cos
) sin( ) sin( sin
2 1 2 2 1 2 1 1
2 1 2 2 1 2 1 1
2
2
1
2
2
1
1
1
u u u u u
u u u u u
u u
u u
l l l
l l l
h h
h h
J
(

=
(

=
2
1
u
u

J
y
x
Y
The City College of New York
21
Jacobian Matrix
(
(
(
(
(
(
(
(

+ + +
+ + +
+ + +
+ + +
+ + +
+ + +
=
(
(
(
(
(
(
(
(

6 66 2 62 1 61
6 56 2 52 1 51
6 46 2 42 1 41
6 36 2 32 1 31
6 26 2 22 1 21
6 16 2 12 1 11
q J q J q J
q J q J q J
q J q J q J
q J q J q J
q J q J q J
q J q J q J
z
y
x

u
|
(
(
(
(

= =
66 62 61
26 22 21
16 12 11
J J J
J J J
J J J
q J Y

(
(
(
(
(
(
(
(

6
5
4
3
2
1
q
q
q
q
q
q

Physical Interpretation
How each individual joint
space velocity contribute
to task space velocity.
The City College of New York
22
Jacobian Matrix
Inverse Jacobian




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

(
(
(
(

= =
66 62 61
26 22 21
16 12 11
J J J
J J J
J J J
q J Y

(
(
(
(
(
(
(
(

6
5
4
3
2
1
q
q
q
q
q
q

Y J q

1
=
5
q
1
q
The City College of New York
23
Quiz
Find the singularity configuration of the 2-DOF
planar robot arm
(

+ + +
+ +
=
) cos( ) cos( cos
) sin( ) sin( sin
2 1 2 2 1 2 1 1
2 1 2 2 1 2 1 1
u u u u u
u u u u u
l l l
l l l
J
(

=
(

=
2
1
u
u

J
y
x
Y
u
2
u
1
(x , y)
l
2

l
1

x
Y
=0
V
determinant(J)=0 Not full rank
0
2
= u
Det(J)=0
The City College of New York
24
Jacobian Matrix
Pseudoinverse
Let A be an mxn matrix, and let be the pseudoinverse
of A. If A is of full rank, then can be computed as:



Example:

+
A
+
A

>
=
s
=

+
n m A A A
n m A
n m AA A
A
T T
T T
1
1
1
] [
] [
(

=
(

2
3
0 1 1
2 0 1
x
(
(
(

=
(

(
(
(

= =

+
2 4
5 1
4 1
9
1
2 1
1 5
0 2
1 0
1 1
] [
1
1 T T
AA A A
T
b A x ] 16 , 13 , 5 [ 9 / 1 = =
+
The City College of New York
25
Robot Motion Planning
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.

Task Plan
Action Plan
Path Plan
Trajectory
Plan
Controller
Sensor
Robot
Tasks
The City College of New York
26
Trajectory Planning
sequence of control set points
along desired trajectory
(continuity,
smoothness)
Trajectory
Planner
Path
constraints
Path
specification
)} ( ), ( ), ( { t q t q t q

)} ( ), ( ), ( { t a t v t p
joint space
cartesian space
or
The City College of New York
27
Trajectory planning
Path Profile

Velocity Profile

Acceleration Profile

t0 t1 t2 tf
Time
q(t0)
q(t1)
q(t2)
q(tf)
Initial
Lift-off
Set down
Final
Joint i
t0 t1 t2 tf Time
Speed
t0 t1 t2 tf Time
Acceleration
The City College of New York
28
The boundary conditions
1) Initial position
2) Initial velocity
3) Initial acceleration
4) Lift-off position
5) Continuity in position at t
1
6) Continuity in velocity at t
1
7) Continuity in acceleration at t
1
8) Set-down position
9) Continuity in position at t
2
10) Continuity in velocity at t
2
11) Continuity in acceleration at t
2
12) Final position
13) Final velocity
14) Final acceleration
The City College of New York
29
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
30
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
31
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
32
Trajectory Planning
n-th order polynomial, must satisfy 14 conditions,
13-th order polynomial

4-3-4 trajectory




3-5-3 trajectory
0
0 1
2
2
13
13
= + + + + a t a t a t a
0 2
2
2
3
3
4
4
20 21
2
22
3
23 2
10 12
2
12
3
13
4
14 1
) (
) (
) (
n n n n n n
a t a t a t a t a t h
a t a t a t a t h
a t a t a t a t a t h
+ + + + =
+ + + =
+ + + + = t0t1, 5 unknow
t1t2, 4 unknow
t2tf, 5 unknow
The City College of New York
33
How to solve the parameters
Handout in the class
The City College of New York
34
Thank you!
x
y
z
x
y
z
x
y
z
x
z
y
Homework 3 posted on the web.
Due: Oct. 7, 2008
No Class on Sept. 30, 2008.
Next class (Oct. 7): Robot Dynamics