Sie sind auf Seite 1von 76

Universiti Kuala Lumpur Malaysia France Institute

FAB30703

Forward Kinematics - 1

Originally prepared by: Prof Engr Dr Ishkandar Baharin Head of Campus & Dean UniKL MFI

P Orientation Representations •Euler Angle I •Euler Angle II •Yaw-Pitch-Roll DH Parameters ⎡ X ⎤
P
Orientation Representations
•Euler Angle I
•Euler Angle II
•Yaw-Pitch-Roll
DH Parameters
X ⎤
q
1
Joint
Y ⎥
q
Position and Orientation
of the end-effector
2
variables
Direct Kinematics
Z ⎥
q =⎢
q
3
P
= ⎢
M
ψ ⎥
θ ⎥
q
⎦ ⎥
n
φ ⎦
Inverse Kinematics
Universiti Kuala Lumpur Malaysia France Institute

Universiti Kuala Lumpur Malaysia France Institute

Kinematics Frame Assignment using Denavit-Hartenberg Convention

Finding the DH Parameters for our Robots

Coordinate Transformations End- effector Z Robot Base Supply Table X Goal Y X Universiti Kuala
Coordinate Transformations
End-
effector
Z
Robot Base
Supply
Table
X
Goal
Y
X
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute

Coordinate Transformations

End-

Malaysia France Institute Coordinate Transformations End- effector Supply End - Effector Goal T Table Robot Forward

effector

France Institute Coordinate Transformations End- effector Supply End - Effector Goal T Table Robot Forward

Supply

Institute Coordinate Transformations End- effector Supply End - Effector Goal T Table Robot Forward Kinematics Robot
Institute Coordinate Transformations End- effector Supply End - Effector Goal T Table Robot Forward Kinematics Robot

End -Effector Goal T

Table

Coordinate Transformations End- effector Supply End - Effector Goal T Table Robot Forward Kinematics Robot Base

Robot Forward Kinematics

Robot Base

Goal

Universiti Kuala Lumpur Malaysia France Institute

Coordinate Transformations

Kuala Lumpur Malaysia France Institute Coordinate Transformations Proof this equation !!! Robot forward kinematic model
Kuala Lumpur Malaysia France Institute Coordinate Transformations Proof this equation !!! Robot forward kinematic model
Kuala Lumpur Malaysia France Institute Coordinate Transformations Proof this equation !!! Robot forward kinematic model

Proof this equation !!!

Kuala Lumpur Malaysia France Institute Coordinate Transformations Proof this equation !!! Robot forward kinematic model
Kuala Lumpur Malaysia France Institute Coordinate Transformations Proof this equation !!! Robot forward kinematic model

Robot forward kinematic model

Manipulator Forward Kinematics • Motion is composition of elementary motions for each link End-effector Base
Manipulator Forward Kinematics
• Motion is composition of elementary
motions for each link
End-effector
Base
Universiti Kuala Lumpur Malaysia France Institute

Universiti Kuala Lumpur Malaysia France Institute

Relative Pose between 2 links

Link i Link i-1
Link i
Link i-1
Relative Pose between 2 links • • • Frames can be chosen arbitrarily Denavit-Hartenberg convention
Relative Pose between 2 links
Frames can be chosen arbitrarily
Denavit-Hartenberg convention is used to assign
frames
Iterative process (start at base, assign frames
for each link from base to end-effector)
Universiti Kuala Lumpur Malaysia France Institute
DH Frame assignment • Frame {i} moves with link i when joint i is actuated
DH Frame assignment
• Frame {i} moves with link i when joint i is actuated
• Z i axis is along joint axis i+1
Joint axis i+1
• Z i is axis of actuation for joint i+1
Imagine Link i does’nt move
Z
i-1
Z
Link i
i
Link i+1
moves
Link
i+1
F{i}
Link i-1
Universiti Kuala Lumpur Malaysia France Institute

Universiti Kuala Lumpur Malaysia France Institute

DH convention: Assign Z axes

Use actuation as a guide

– Prismatic – joint slides along z i

– Revolute – joint rotates around z i

Establish base frame {0}:

– Nearly arbitrary

Start at base and assign frames 1,…,N

– Pick x-axis and origin

– y-axis chosen to form a right hand system

DH Frame assignment • Frame {1} moves with link 1 when joint 1 is actuated
DH Frame assignment
• Frame {1} moves with link 1 when joint 1 is actuated
• Z 1 axis is along joint axis 2
Joint axis 2
• Z 1 is axis of actuation for joint 2
Imagine there is a joint here to move link 2,
called joint 2
Imagine Link 1 doesn’t move
Z
0
Z
Link 1
1
Link 2 moves
Link
2
F{1}
Lets try for first Frame, where i=1; F{1}
Link 0
Universiti Kuala Lumpur Malaysia France Institute
Robot Base • Often base is “given” or some fixed point on the work-table is
Robot Base
• Often base is “given” or
some fixed point on the
work-table is used.
• z 0 is along joint axis 1
Origin:
– any point on z 0 for origin
Modified DH:
– {0} is defined to be
completely co-incident
with the reference system
{1}, when the variable
joint parameter, d 1 or θ 1 ,
is zero.
Universiti Kuala Lumpur Malaysia France Institute

Universiti Kuala Lumpur Malaysia France Institute

DH convention: Assign X axes

Start at base and assign frames 1,…,N

– Pick x-axis and origin

– y-axis chosen to form a right hand system

Consider 3 cases for z i-1 and z i :

– Not-coplanar

– Parallel

– Intersect

• Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute

DH convention: x axis

z i-1 and z i are not-coplanar

• Common normal to axes is the “link” axis

• Intersection with z i is origin

z i-1 z i X i F{i-1}
z i-1
z i
X i
F{i-1}

“Link Axis”

F{i}

Origin of Frame {i}

Usually, x i points from frame i-1 to i

• Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute

DH convention: x axis

z i and z i-1 are parallel

• Infinitely many common normals

• Pick one to be the “link” axis

• Choose normal that passes through origin of frame {i-1} pointing toward z i

• Origin is intersection of x i with z i

X i F{i-1} F{i}
X i
F{i-1}
F{i}

z

toward z i • Origin is intersection of x i with z i X i F{i-1}

i-1

Link i

toward z i • Origin is intersection of x i with z i X i F{i-1}

z i

toward z i • Origin is intersection of x i with z i X i F{i-1}

Origin of Frame {i}

DH convention: x axis z If joint axes z i-1 and z i intersect, x
DH convention: x axis
z
If joint axes z i-1 and z i
intersect, x i is normal to the
plane containing the axes
i
x i = ±(z i-1 × z i )
Origin of Frame {i}
link i
x i = ±(z i-1 × z i )
X
i
Universiti Kuala Lumpur Malaysia France Institute

z i-1

DH convention: Origin non-coplanar Z Origin of frame {i} is placed at intersection of joint
DH convention: Origin non-coplanar Z
Origin of frame {i} is placed at intersection of joint axis and
link axis
Joint Axis
z
i
Origin of F{i}
O
Link Axis
i
F{i}
x
i
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute

DH convention: y axis

• Y i is chosen to make a right hand frame

Z i Y i F{i-1} x i points from frame i-1 to i F{i} x
Z i
Y i
F{i-1}
x i points from
frame i-1 to i
F{i}
x i
DH convention: Origin parallel Z • z i and z i-1 are parallel • Origin
DH convention: Origin parallel Z
• z i and z i-1 are parallel
• Origin is intersection of x i with z i
z
i-1
z
i
F{i-1}
F{i}
x
i
Universiti Kuala Lumpur Malaysia France Institute
DH convention: x axis - parallel Z • z i and z i-1 are parallel
DH convention: x axis - parallel Z
• z i and z i-1 are parallel
• Origin is intersection of x i with z i
• Yi is chosen to make a right hand frame
y
z
i
i-1
z
i
x
i
Universiti Kuala Lumpur Malaysia France Institute

Universiti Kuala Lumpur Malaysia France Institute

DH convention: origin

If joint axes intersect, the origin of frame {i} is usually placed at intersection of
If joint axes intersect, the
origin of frame {i} is
usually placed at
intersection of the joint
axes
z i
O i
F{i}
link i

x

i

i-1

z i-1 Universiti Kuala Lumpur Malaysia France Institute
z
i-1
Universiti Kuala Lumpur Malaysia France Institute

DH convention: y axis

Y i is chosen to make a right hand frame z i O i F{i}
Y i is chosen to make a
right hand frame
z i
O i
F{i}
y i
link i
x
i
End-Effector Frame • Frame to which the gripper is attached – Sometimes {n} is used
End-Effector Frame
Frame to which
the gripper is
attached
Sometimes {n} is
used
Z
n
X
n
Z
e
F{e}
X
e
denoted by {e} (or
{n+1} in many
texts)
– Often simple
translation along
X n axis (Link n
axis) or the Last
Axis !!!
Universiti Kuala Lumpur Malaysia France Institute
End-Effector Frame • Frame to which the gripper is attached – – denoted by {e}
End-Effector Frame
• Frame to which the
gripper is attached –
– denoted by {e} (or
{n+1} in many texts)
– Often simple
translation along X n
axis
Z
4
• Often:
– Origin between
grippers
– Z points outward
(approach)
y
e
F{e}
– Y points along pinch
direction (sliding)
– X points normal
x
z
e
e
Universiti Kuala Lumpur Malaysia France Institute
Link Parameters α i+1 Z Z’ i i Z i-1 Z i+1 Link i α
Link Parameters
α
i+1
Z
Z’ i
i
Z
i-1
Z
i+1
Link i
α
i
a
i+1
a
i
Universiti Kuala Lumpur Malaysia France Institute
Joint Parameters θ i Link i d i+1 θ i+1 d i θ i Universiti
Joint Parameters
θ
i
Link i
d
i+1
θ
i+1
d
i
θ
i
Universiti Kuala Lumpur Malaysia France Institute
Original DH -1 Frame is placed at distal end of link x i screw motion
Original DH
-1
Frame is placed at
distal end of link
x i screw motion
z i-1 screw motion
Universiti Kuala Lumpur Malaysia France Institute
DH Frames and Parameters Class Test – You Try !!! Universiti Kuala Lumpur Malaysia France
DH Frames and Parameters
Class Test – You Try !!!
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute

Robot Revolute Joint DH

Universiti Kuala Lumpur Malaysia France Institute Robot Revolute Joint DH Class Test – You Try !!!

Class Test – You Try !!!

Prismatic Joint DH Class Test – You Try !!! Universiti Kuala Lumpur Malaysia France Institute
Prismatic Joint DH
Class Test – You Try !!!
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute

Link Transformations

• Described by 4 parameters:

α i : twist

– a i : link length

– d i : joint offset

θ i : joint angle

• Joint variable is d i or θ i

• Build Table with values for each link:

Link

Var

θ

d

α

a

1

θ

1

θ

1

0

90

o

L

1

2

d

2

0

d

2

0

0

M

M

M

M

M

M

Link Transformations • Described by 4 parameters: – α i : twist – a i
Link Transformations
• Described by 4 parameters:
– α i : twist
– a i : link length
– d i : joint offset
– θ i : joint angle
• Joint variable is d i or θ i
• Link Transformation is
z
i-1 screw motion
x
screw motion
i
Universiti Kuala Lumpur Malaysia France Institute
A-matrices A i = contains only one variable or Universiti Kuala Lumpur Malaysia France Institute
A-matrices
A i =
contains only one variable
or
Universiti Kuala Lumpur Malaysia France Institute
Original DH -1 Frame is placed at distal end of link z i-1 screw motion
Original DH
-1
Frame is placed at
distal end of link
z
i-1 screw motion
x i screw motion
Universiti Kuala Lumpur Malaysia France Institute
Modified DH Z Z i+1 i Z i+2 Link i Link i+1 z i y
Modified DH
Z
Z
i+1
i
Z i+2
Link i
Link i+1
z i
y i
x
Frame is placed at
proximal end of link i
i
x
i-1 screw motion
z
screw motion
i
Universiti Kuala Lumpur Malaysia France Institute
Modified DH – text figure Universiti Kuala Lumpur Malaysia France Institute
Modified DH – text figure
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute

DH Example: “academic manipulator”

•3 revolute joints •Shown in home position

joint 1

Follow Right-Hand-Rule !!!

R Link 2 joint 2 joint 3 L 1
R
Link 2
joint 2
joint 3
L 1

L 2

Link 3

joints •Shown in home position joint 1 Follow Right-Hand-Rule !!! R Link 2 joint 2 joint

Link 1

Universiti Kuala Lumpur Malaysia France Institute

DH Example: “academic manipulator”

Z i is axis of actuation for joint i+1 (The Distal Frame !!!) Start from Base, where i=0

Z 0 Z Z 0 and Z 1 are not co-planar 1 and Z 2
Z
0
Z
Z
0 and Z 1 are not co-planar
1 and Z 2 are parallel
θ
1
Joint 1
Joint 2
Joint 3
θ
θ
3
Z
2
1
Z
2

Universiti Kuala Lumpur Malaysia France Institute

DH Example: “academic manipulator”

Z 0 and Z 1 are not co-planar: Use x 0 is the common normal
Z
0 and Z 1 are not co-planar:
Use x 0 is the common normal
Z
0
θ
1
x
0
θ
θ
3
Z
2
Z
1
Z
3
2
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute

DH Example: “academic manipulator”

Z 0 and Z 1 are not co-planar: Use x 0 is the common normal
Z 0 and Z 1 are not co-planar:
Use x 0 is the common normal
Z
0
θ
1
x
1
x
0
θ
θ
3
Z
2
Z
1
Z
3
2
Z 1 and Z 2 are parallel :
x 1 is selected as the common
normal that lies along the center
of the link
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute

DH Example: “academic manipulator”

Z 0 and Z 1 are not co-planar: x 0 is the common normal Z
Z 0 and Z 1 are not co-planar:
x 0 is the common normal
Z
0
θ
1
x 1
x
x 2
3
x
0
θ
θ
3
Z
2
Z
1
Z
3
2
Z 2 and Z 3 are parallel :
x 2 is selected as the common
normal that lies along the center of
the link

Universiti Kuala Lumpur Malaysia France Institute

DH Example: “academic manipulator”

Shown with joints in non-zero positions

F{3} Z x 0 z 3 θ F{2} 2 Link 1 x 2 F{1} x
F{3}
Z
x
0
z
3
θ
F{2}
2
Link 1
x
2
F{1}
x
Link 2
1
Z
θ
2
x
1
0
Link 3

Z 1

3

Observe that frame i moves with link i

Universiti Kuala Lumpur Malaysia France Institute

DH Example: “academic manipulator”

Link lengths given

α 1 = 90 o (rotate by 90 o around x 0 to align Z 0 and Z 1 )

R

Z 0 L 1 L 2 x 1 x 2 x 3 α x 1
Z
0
L 1
L 2
x 1
x 2
x 3
α
x
1
0
Z 1
Z 3
Z 2
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute

DH Example: “academic manipulator”

R Z 0 L 1 L 2 θ 1 Link 1 x 1 Link 2
R
Z
0
L 1
L 2
θ
1
Link 1
x
1
Link 2
x
x
2
Link 3
3
α
x
1
0
θ
θ
3
Z
2
Z
1
3
Z 2

Build table

Link

Var

 

θ d

α

a

1

θ 1

θ 1

0

90

o

R

2

θ 2

θ 2

0

0

L

1

3

θ 3

θ 3

0

0

L

2

Universiti Kuala Lumpur Malaysia France Institute

DH Example: “academic manipulator”

Link

Var

θ

d

α

a

1

θ 1

θ 1

0

90

o

R

2

θ 2

θ 2

0

0

L

1

3

θ 3

θ 3

0

0

L

2

1 0 90 o R 2 θ 2 θ 2 0 0 L 1 3 θ

Universiti Kuala Lumpur Malaysia France Institute

DH Example: “academic

manipulator”
manipulator”
Universiti Kuala Lumpur Malaysia France Institute DH Example: “academic manipulator”
Universiti Kuala Lumpur Malaysia France Institute DH Example: “academic manipulator”
DH Example: “academic manipulator” z x 0 3 z 3 θ θ 3 2 x
DH Example: “academic manipulator”
z
x
0
3
z
3
θ
θ 3
2
x
2
x
1
z
θ
2
x
1
0
z
1
x 1 axis expressed
wrt {0}
y 1 axis expressed
wrt {0}
z 1 axis expressed
wrt {0}
Origin of {1}
w.r.t. {0}
Universiti Kuala Lumpur Malaysia France Institute
DH Example: “academic manipulator” z x 0 3 z 3 θ θ 3 2 x
DH Example: “academic manipulator”
z
x
0
3
z
3
θ
θ 3
2
x
2
x
1
z
θ
2
x
1
0
z
1
x 2 axis expressed
wrt {1}
y 2 axis expressed
wrt {1}
z 2 axis expressed
wrt {1}
Origin of {2}
w.r.t. {1}
Universiti Kuala Lumpur Malaysia France Institute
DH Example: “academic manipulator” z x 0 3 z 3 θ 2 x 2 x
DH Example: “academic manipulator”
z
x
0
3
z
3
θ
2
x
2
x
1
z
θ
2
x
1
0
z
1
x 3 axis expressed
wrt {2}
y 3 axis expressed
wrt {2}
z 3 axis expressed
wrt {2}
Origin of {3}
w.r.t. {2}
Universiti Kuala Lumpur Malaysia France Institute
DH Example: “academic manipulator” where Universiti Kuala Lumpur Malaysia France Institute
DH Example: “academic manipulator”
where
Universiti Kuala Lumpur Malaysia France Institute

Universiti Kuala Lumpur Malaysia France Institute

DH Example: “academic manipulator” – alternate end-effector frame

Z

i is axis of actuation for joint i+1

Z 0 Z Z 0 and Z 1 are not co-planar 1 and Z 2
Z
0
Z
Z
0 and Z 1 are not co-planar
1 and Z 2 are parallel
θ
1
Pick this z 3
θ
θ
3
Z
2
1
Z
2
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute

DH Example: “academic manipulator” – alternate end-effector frame

Z 0

y

2 θ 1 x 1 x 2 x 0 1 θ
2
θ 1
x 1
x 2
x 0
1
θ

α

Z 1

θ

2

Z 2

3

end-effector frame Z 0 y 2 θ 1 x 1 x 2 x 0 1 θ
end-effector frame Z 0 y 2 θ 1 x 1 x 2 x 0 1 θ
end-effector frame Z 0 y 2 θ 1 x 1 x 2 x 0 1 θ

3

Z Would need to rotate about y 2 here!
Z
Would need
to rotate
about y 2
here!
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute

DH Example: “academic manipulator” – alternate end-effector frame

Z

x’ 2 θ 1 x 1 x 2 x 0 1
x’ 2
θ 1
x 1
x 2
x 0
1

α

Z 1

θ

2

θ

3

0

frame Z x’ 2 θ 1 x 1 x 2 x 0 1 α Z 1
frame Z x’ 2 θ 1 x 1 x 2 x 0 1 α Z 1
Z 3 Solution: Add “offset” to rotation about z 2 (θ 3 +90 o )
Z 3
Solution: Add
“offset” to
rotation about z 2
(θ 3 +90 o )
DH Example: “academic manipulator” – alternate end-effector frame Z 0 x’ 2 L x 2
DH Example: “academic manipulator” –
alternate end-effector frame
Z
0
x’ 2
L
x
2
3
θ
1
x
1
x
2
α
x
1
0
Z
3
θ
θ
3
Z
2
1
Z
2
Now can rotate
about x’ to
align z 2 and z 3
Universiti Kuala Lumpur Malaysia France Institute
DH Example: “academic manipulator” – alternate end-effector frame Link Var θ d α a 90
DH Example: “academic manipulator” –
alternate end-effector frame
Link
Var
θ
d
α
a
90
o
1
θ
0
R
1
θ 1
2
θ
0
0
L
2
θ 2
1
o
3
θ
90
θ 3 +90 o
0
0
3
e
-
L
2
Universiti Kuala Lumpur Malaysia France Institute
DH Example: “academic manipulator” – alternate end-effector frame x 3 R Z 0 x’ 2
DH Example: “academic manipulator” –
alternate end-effector frame
x
3
R
Z
0
x’ 2
L
L
Z
1
2
3
θ
1
x
1
x
2
α
x
1
0
θ
θ
3
Z
2
1
Z
2
Link
Var
θ
d
α
a
90
o
1
0
R
θ 1
θ 1
2
0
0
L
θ 2
θ 2
1
o
3
0
90
θ 3
θ 3 +90 o
0
Universiti Kuala Lumpur Malaysia France Institute
DH Example: “academic manipulator” – alternate end-effector frame x 3 R Z 0 x’ 2
DH Example: “academic manipulator” –
alternate end-effector frame
x
3
R
Z
0
x’ 2
Z
L
L
2
3
1
θ
1
x
1
x
2
α
x
1
0
Z
3
θ
θ
3
Z
2
1
Z
2
Universiti Kuala Lumpur Malaysia France Institute
DH Example: “academic manipulator” – alternate end-effector frame x 3 R Z 0 x’ 2
DH Example: “academic manipulator” –
alternate end-effector frame
x
3
R
Z
0
x’ 2
L
Z
L
3
1
2
θ
1
x
1
x
2
α
x
1
0
Z
3
θ
θ
3
Z
2
1
Z
2
Universiti Kuala Lumpur Malaysia France Institute
DH Example: “academic manipulator” – alternate end-effector frame x 3 R Z 0 x’ 2
DH Example: “academic manipulator” –
alternate end-effector frame
x
3
R
Z
0
x’ 2
L
Z
L
1
2
3
θ
1
x
1
x
2
α
x
1
0
Z
3
θ
θ
3
Z
2
1
Z
2
Universiti Kuala Lumpur Malaysia France Institute

Universiti Kuala Lumpur Malaysia France Institute

Universiti Kuala Lumpur Malaysia France Institute Class Test – Your Turn !!!!
Universiti Kuala Lumpur Malaysia France Institute Class Test – Your Turn !!!!
Universiti Kuala Lumpur Malaysia France Institute Class Test – Your Turn !!!!

Class Test – Your Turn !!!!

Universiti Kuala Lumpur Malaysia France Institute Class Test – Your Turn !!!!
Universiti Kuala Lumpur Malaysia France Institute Class Test – Your Turn !!!!
Universiti Kuala Lumpur Malaysia France Institute Class Test – Your Turn !!!!
Universiti Kuala Lumpur Malaysia France Institute Class Test – Your Turn !!!!
The Situation: You have a robotic arm that starts out aligned with the x o
The Situation:
You have a robotic arm that starts
out aligned with the x o -axis.
You tell the first link to move by
θ
1
and the second link to move by
θ
2
The Question:
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.
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute
Universiti Kuala Lumpur Malaysia France Institute

Example Problem:

You have a three link arm that starts out aligned in the x-axis. Each link

has lengths l 1 , l 2 , l 3 , respectively. You tell the first one to move by

so on as the diagram suggests. Find the Homogeneous matrix to get the

position of the yellow dot in the

θ 1 , and

X

0

Y

0

frame.

Y

1

Y 3 θ 3 Y θ X X 3 2 2 2 l 3 l
Y
3
θ
3
Y
θ
X
X
3
2
2
2
l
3
l
2
Y
0
X l
1 1
θ
1
X
0
Y 3 θ 3 ⎡ l ⎤ 3 Y θ X X 3 ⎢ ⎥
Y
3
θ
3
l
3
Y
θ
X
X
3
2
2
2
3
P
0
3
= ⎢
l
0
3
⎥ ⎦
l
2
3
Vector
P
is in the X 3 -Y 3 frame.
3
Y
0
l
3
Find Vector
P
X
in the X 0 -Y 0 frame ?.
1
3
1
Y
θ
1
1
Solution:
X
0
Rotating by
θ 1 will put you in the
X 1 frame.
Y
1
Translating along the
X 1 axis by l 1 .
Rotating by
will put you in the
X
2 frame, and so on until you are in the
Y
θ
2
2
X
3 Y
frame. The position of the yellow dot relative to the
X
Y
frame is
3
3
3
(l 3 , 0). Multiplying H by that position vector will give you the
coordinates of the yellow point relative the the
frame.
X
0 Y
0
3
H
= R
(
θ ⋅⋅ θ ⋅
)
TlR
()
(
)
Tl
()
R
(
θ
)
0
ZX 1
11
Z
2
X
2
2
Z
3
3
33
P
=
HP
0
3
0
Universiti Kuala Lumpur Malaysia France Institute
Slight variation on the last solution: Make the yellow dot the origin of a new
Slight variation on the last solution:
Make the yellow dot the origin of a new coordinate
X
Y
frame
4
4
Y
3
Y
4
θ
3
Y
θ
X
X
3
2
2
2
X
l
4
3
l
2
Y
0
X l
1 1
H
= R θ ⋅⋅TlR θ ⋅Tl ⋅ R θ ⋅Tl
(
)
()
(
)
()
(
)
()
ZX
1
11
Z
2
X
2
2
ZX
3
3
3
Y
θ
1
1
This takes you from the
X
Y
frame to the
X
Y
frame.
X
4
4
0
0
0
The position of the yellow dot relative to the
X
Y
4
4
frame is (0,0).
⎡ X ⎤
⎡ 0
Y
0
=
H
Z
0
Notice that multiplying by the (0,0,0,1) vector will equal
the last column of the H matrix.
1
1
Universiti Kuala Lumpur Malaysia France Institute

Forward KinematicTransformation

LL Robot:

Let us consider a Cartesian LL robot

y

LL Robot: Let us consider a Cartesian LL robot y J1(x1, y1) L 2 L 1
J1(x1, y1) L 2 L 1
J1(x1, y1)
L
2
L
1

L 3

(x, y)

J2(x2,y2)

LL robot y J1(x1, y1) L 2 L 1 L 3 (x, y) J2(x2,y2) x Joints

x

Joints J1 and J2 are linear joints with links of variable lengths L1 and L2. Let joint J1 be denoted by (x1 y1) and joint J2 by (x2, y2).

From geometry, we can easily get

the following:

x 2 =x 1 +L 2

y 2 = y 1

Forward KinematicTransformation

These relations can be represented in homogeneous matrix form:

or

x y 1

⎥= 010

1 0

00

1

2

L ⎤ ⎡ x

⋅ ⎢ y 1

2

2

1

1

X 2 =T 1 X 1

Forward KinematicTransformation

where

X

x

= ⎢ y 1

2

2

T

21

1 0

00

L

2

= 010

1

x

1

X y ⎥ ⎣ 1

1

1

If the end-effector point is denoted by (x, y), then:

x = x 2

y =

y 2 - L 3

Forward KinematicTransformation

therefore:

or

and

x

y

1

⎥ =

10

00

0

1

⎤ ⎡ x

1

0

2

2

L ⎥ ⋅ ⎢ y

2

1

X = T 2 X 2

T LL = T 2 T 1

T LL =

1 0

0

1

00

L

L

1

2

Forward KinematicTransformation

RR Robot:

Let θ and α be the rotations at joints J 1 and J 2 respectively. Let J 1 and J 2 have the coordinates of (x 1 , y 1 ) and (x 2 , y 2 ), respectively.

J 2 ( x 2 y 2 ) - L 2 L 3 J 1
J 2
( x
2
y 2 )
-
L
2
L
3
J
1
x
y
(
)
( x
y
)
1
1
L
1
x

y

One can write the following

from the geometry:

x 2 = x 1 +L 2 cos(θ) y 2 = y 1 +L 2 sin(θ)

Forward KinematicTransformation

In matrix form:

x ⎤ ⎢ y 1

2

2

⎥= ⎢ ⎡ 0 1 L

1 0

L

2

2

00

or

X 2 = T 1 X 1

On the other end:

x = x 2 +L 3 cos(α-θ)

y = y 2 - L 3 sin(α-θ)

θ ⎥⋅ ⎡ ⎢

θ ⎤

⎤ ⎥ ⎣ 1

x

y

1

1

cos( )

sin( )

1

Forward KinematicTransformation

In matrix form:

⎡ ⎤ ⎡ 1 0 L cos(

y ⎥= ⎢ 0 1

1

α − θ ⎥⋅ ⎡ ⎢

α − θ ⎤

)

)

x

2

y

x

2

2

1

L

2

sin(

1

00

or

X = T 2 X 2

Combining the two equation gives:

X = T 2 (T 1 X 1 ) = T RR X 1

Forward KinematicTransformation

where

T RR = T 2 T 1

1 0 T RR = ⎢ ⎣ 00

0

1

L

2

L

2

cos( )

θ +

sin( )

θ −

L

L

1

2

2

αθ − ⎤

)

)

αθ − ⎥

cos(

sin(

Forward KinematicTransformation

TL Robot:

Let α be the rotation at twisting joint J 1 and L 2 be the variable link length at linear joint J 2 .

z

y J ( x 2 y 2 ) 2 ( x y ) L 2
y
J
( x
2
y
2
)
2
( x
y )
L
2
J
1
( x
1
y
1
)
x

One can write that:

x = x 2 + L 2 cos(α)

y = y 2 + L 2 sin(α)

Forward KinematicTransformation

In matrix form:

⎡ ⎤

x

y ⎥= ⎡ ⎢ 0 1 L sin( α ) ⎥⋅ ⎡ ⎢ ⎥

1

1

cos( α )

1

1 0

L

2

2

x

2

2

y

00

or

X = T TL X 2

Universiti Kuala Lumpur Malaysia France Institute

Universiti Kuala Lumpur Malaysia France Institute q =⎢

q =⎢


⎣ ⎢

q

q

3

2

1

q

M

q

n

Link

Var

θ

d

α

a

1

θ

1

θ

1

0

90

o

L

1

2

d

2

0

d

2

0

0

M

M

M

M

M

M

T =

n

T

0

=

1

TT

01

2

⋅⋅

T

n

n

P = Tq

X

Y

Z

ψ

θ

φ

=

⎣ ⎢

TTT

1,1

1,2

1,3

T

2,1

T

3,1

L

T

4,1

T

5,1

T

6,1

L

L

LL

LL

1

P = Tq

T

1,

n

M

M

M

1

T

1,

M

M

M

TT

5,

n

1

5,

TT

6,

n

1

6,

n

n

n

⎤⎡

⎥⎢

⎥⎢

⎥⎢

⎥⎢

⎥⎢

⎥⎢

⎥⎢

⎥⎢ ⎦⎣

q

q

q

M

1

2

3

q

n 1

q

n

P

X

Y

Z

= ⎢

ψ

θ

φ