Introduction to ROBOTICS

Inverse Kinematics
Jacobian Matrix
Trajectory Planning
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 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 d i : 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.
Link length ai : 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 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 Tnn1

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 Ry , 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 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 SS SC 0
0 C S 0
(Equation A)
S CS CC 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(nz , cos nz sin n y )

sin sx cos s y cos

sin ax cos a y sin

a tan 2(sin ax 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 C1 p x S1 p y X X X S 2 d 3
X X X pz X X X C 2 d 3
T16
X X X S1 p x C1 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
Sin1 p x cos 1 p y 0.1 Equation (1)

cos 1 p x sin 1 p y sin 2 d 3 Equation (2)

pz cos 2 d3 Equation (3)

In Equ. (1), let
py
px 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
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 (C1a x S1a y ) S 2 a z ] C 4 ( S1a x C1a y ) 0

S1a x C1a y S 5
4 a tan 2( ) 5 a tan 2( )
C 2 (C1a x S1a y ) S 2 a z C 5
From term (1,3), (2,3)
S5 C 4 (C 2 (C1ax S1a y ) S 2 az ] S 4 (S1ax C1a y )

C5 S 2 (C1ax S1a y ) C 2 az )

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 C5{C 4 [C 2 (C1sx S1s y ) S 2 sz ] S 4 (S1sx C1s y )} S5 [ S 2 (C1sx S1s y ) C 2 sz ]

C 6 S 4 [C 2 (C1sx S1s y ) S 2 sz ] C 4 (S1sx C1s 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
3 z 3
Jacobian
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 ) Y61 h(qn1 )
h ( 61
)

q
4 h (
4 1 2q , q , , q )
6
q5 h5 (q1 , q2 , , q6 )

6
q 6 1 2
h ( q , q , , q 6
) 61

d dh(q) dq dh(q)
Y61 h(qn1 ) q
dt dq dt dq
x
y q1
q
z dh(q) 2
dq Y61 J 6n q n1
x 6n
y J
dh(q)

z
q n n1 dq
The City College of New York 17
 Jacobian Matrix
x
y
q1
z dh(q) q 2
Jacobian is a function of

x dq 6n
q, it is not a constant!
y
q n n1

z h1 h1 h1
q
q2 qn
1
dh(q) h2 h2

h2
J q1 q2 qn
dq 6n
h h6 h6
6
q1 q2 qn 6n
The City College of New York 18
 Jacobian Matrix
Forward Kinematics
x h1 (q)
p y h2 (q)
n s a p z h3 (q)
T
6

1 44
0
0 0 0 (q) h4 (q)
h1 (q) {n, s, a} (q) h5 (q)
h ( q ) (q) h6 (q)
Y61 h(q) 2
x
y

Linear velocity Angular velocity
h6 (q) z V

Y x

x V y
Y61 J 6n q n1 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 cos1 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 1 l1

y 2

h1 h1
2 l1 sin 1 l2 sin( 1 2 ) l2 sin( 1 2 )
J 1
h2 h2 l1 cos1 l2 cos(1 2 ) l2 cos(1 2 )
1 2

The City College of New York 20

 Jacobian Matrix
Physical Interpretation
J11 J12 J16 q 1

J
q
J 22 J 26 q
2

Y Jq
21

3

q 4

q 5

J 61 J 62 J 66 q 6

x J11q1 J12q 2 J16q6

y J q J q J q
21 1 22 2 26 6

z J 31q1 J 32q 2 J 36q6 How each individual joint

space velocity contribute

41 1 J q J q
42 2 J q
46 6 to task space velocity.
J 51q1 J 52q 2 J 56q6

J 61q1 J 62q 2 J 66q6
The City College of New York 21
 Jacobian Matrix
Inverse Jacobian
q5
J11 J12 J16 q
1

J
q
J 22 J 26 q
2

Y Jq 21
3

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 )
J
V (x , y)
1
l cos 1 l 2 cos(1 2 ) l 2 cos( 1 2
)

l2
determinant(J)=0 Not full rank
Y
2 0 2 =0
Det(J)=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:
AT [ AAT ]1 mn

A A1 mn
[ AT A]1 AT mn

Example:
1 0 2 3 x Ab 1 / 9[5,13,16]T
1 1 0 x 2

1 1 1 1 4
T 1 5 1 1
A A [ AA ] 0 1
T
1 5
1 2 9
2 0 4 2
The City College of New York 24
 Robot Motion Planning
Tasks
Task Plan
Path planning
Geometric path
Action Plan Issues: obstacle avoidance, shortest
path
Path Plan
Trajectory planning,
Trajectory interpolate or approximate the
Plan
desired path by a class of polynomial
functions and generates a sequence of
Controller
time-based control set points for the
Robot
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
{q (t ), q (t ), q(t )}
Path Trajectory sequence of control set points
or
specification Planner 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 Set down

Velocity Profile q(t1)

Lift-off
Initial
q(t0)

Acceleration Profile
t0 t1 t2 tf Time
Speed

t0 t1 t2 tf Time
Acceleration

t0 t1 t2 tf Time

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 t0t1, 5 unknow

h2 (t ) a23t 3 a22t 2 a21t a20 t1t2, 4 unknow

hn (t ) an 4t 4 an 3t 3 an 2t 2 an 2t an 0 t2tf, 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.
Due: Oct. 7, 2008
No Class on Sept. 30, 2008.
Next class (Oct. 7): RobotzDynamics
y
z
y x
z
z x y
y x

x
The City College of New York 34