Forward Kinematics 1
Forward Kinematics
1
Links and Joints
2
n joints, n + 1 links
link 0 is fixed (the base)
joint i connects link i – 1 to link i
link i moves when joint i is actuated
joint 1
joint 2
joint 3 joint 4 joint n-1joint n
link 0
link 1
link 2
link 3
link n-1
link n
.................
prismatic
revolute
i
i
id
q
Forward Kinematics
3
given the joint variables and dimensions of the links what is
the position and orientation of the end effector?
2
1
a1
a2
x0
y0
p0 ?
Forward Kinematics
4
because the base frame and frame 1 have the same
orientation, we can sum the coordinates to find the position
of the end effector in the base frame
2
1
a1
a2
x0
y0
( a1 cos 1 , a1 sin 1 )
1
x1
y1( a2 cos (1 + 2),
a2 sin (1 + 2) )
(a1 cos 1 + a2 cos (1 + 2),
a1 sin 1 + a2 sin (1 + 2) )
Forward Kinematics
5
from earlier in the course
2
1
a1
a2
x0
y01
x2 = (cos (1 + 2),
sin (1 + 2) )
y2 = (-sin (1 + 2),
cos (1 + 2) )
x2y2
p0 = (a1 cos 1 + a2 cos (1 + 2),
a1 sin 1 + a2 sin (1 + 2) )
Frames
6
2
1
a1
a2
x0
y0 x1
y1
x2y2
Forward Kinematics
7
using transformation matrices
11 ,,
0
1 axz DRT
22 ,,
1
2 axz DRT
1
2
0
1
0
2 TTT
Links and Joints
8
n joints, n + 1 links
link 0 is fixed (the base)
joint i connects link i – 1 to link i
link i moves when joint i is actuated
joint 1
joint 2
joint 3 joint 4 joint n-1joint n
link 0
link 1
link 2
link 3
link n-1
link n
.................
prismatic
revolute
i
i
id
q
Forward Kinematics
9
attach a frame {i} to link i
all points on link i are constant when expressed in {i}
if joint i is actuated then frame {i} moves relative to frame {i - 1}
motion is described by the rigid transformation
the state of joint i is a function of its joint variable qi (i.e., is a function of qi)
this makes it easy to find the last frame with respect to the
base frame
1i
iT
)(11 ii
i
i
i qTT
12
3
1
2
0
1
0 nnn TTTTT
Forward Kinematics
10
more generally
the forward kinematics problem has been reduced to matrix
multiplication
jiji
ji
T
I
TTT
Ti
j
j
j
i
j
i
i
i
j
if
if
if
1
11
21
Forward Kinematics
11
Denavit J and Hartenberg RS, “A kinematic notation for lower-
pair mechanisms based on matrices.” Trans ASME J. Appl. Mech,
23:215–221, 1955
described a convention for standardizing the attachment of frames
on links of a serial linkage
common convention for attaching reference frames on links of
a serial manipulator and computing the transformations
between frames
Denavit-Hartenberg
12
iiii xaxdzz
i
i RTTRT ,,,,1
1000
0 i
i
i
dcs
sascccs
casscsc
ii
iiiiii
iiiiii
anglejoint
offsetlink
link twist
lengthlink
i
i
i
i
d
a
Denavit-Hartenberg
13
Denavit-Hartenberg
14
notice the form of the rotation component
this does not look like it can represent arbitrary rotations
can the DH convention actually describe every physically
possible link configuration?
ii
iiiii
iiiii
cs
scccs
sscsc
0
Denavit-Hartenberg
15
yes, but we must choose the orientation and position of the
frames in a certain way
(DH1)
(DH2)
claim: if DH1 and DH2 are true then there exists unique
numbers
1ˆˆ ii zx
1ˆ intersects ˆ ii zx
,,,,0
1 such that ,,, xaxdzz RDDRTda
Denavit-Hartenberg
16
proof: on blackboard in class
Denavit-Hartenberg
17
DH Parameters
18
ai : link length
distance between zi-1 and zi measured along xi
i : link twist
angle between zi-1 and zi measured about xi
di : link offset
distance between oi-1 to the intersection of xi and zi-1 measured
along zi-1
i : joint angle
angle between xi-1 and xi measured about zi-1
Example with Frames Already Placed
19
Step 5: Find the DH parameters
20
Link ai i di i
1 0 0 d1 1*
2 0 -90 d2* 0
3 0 0 d3* 0
* joint variable
Denavit-Hartenberg Forward Kinematics
21
RPP cylindrical manipulator
http://strobotics.com/cylindrical-format-robot.htm
http://strobotics.com/cylindrical-format-robot.htm
Denavit-Hartenberg Forward Kinematics
22
How do we place the frames?
Step 1: Choose the z-axis for each frame
23
recall the DH transformation matrix
iiii xaxdzz
i
i RTTRT ,,,,1
1000
0 i
i
i
dcs
sascccs
casscsc
ii
iiiiii
iiiiii
1ˆ iix
1ˆ iiy
1ˆ iiz
Step 1: Choose the z-axis for each frame
24
axis of actuation for joint i+1
iẑ
link i link ilink i+1 link i+1
joint i+1 joint i+1
iẑ
iẑ
Step 1: Choose the z-axis for each frame
25
Warning: the picture is deceiving. We do not yet know the origin of the
frames; all we know at this point is that each zi points along a joint axis
Step 2: Establish frame {0}
26
place the origin o0 anywhere on z0 often the choice of location is obvious
choose x0 and y0 so that {0} is right-handed
often the choice of directions is obvious
Step 2: Establish frame {0}
27
Step 3: Iteratively construct {1}, {2}, ... {n-1}
28
using frame {i-1} construct frame {i}
DH1: xi is perpendicular to zi-1
DH2: xi intersects zi-1
3 cases to consider depending on the relationship between zi-1and zi
Step 3: Iteratively construct {1}, {2}, ... {n-1}
29
Case 1
zi-1 and zi are not coplanar (skew)
i angle from zi-1 to zi measured about xi
iẑ
1ˆiz
(out of page)shortest line between
and1
ˆiz iẑ
ix̂
io point of intersection
ia
Step 3: Iteratively construct {1}, {2}, ... {n-1}
30
Case 2
zi-1 and zi are parallel ( i = 0 )
notice that this choice results in di = 0
iẑ1ˆ iz
ix̂
io point of intersection
1ioia
Step 3: Iteratively construct {1}, {2}, ... {n-1}
31
Case 3
zi-1 and zi intersect ( ai = 0 )
iẑ
1ˆiz
ix̂io point of intersection
(out of page)
Step 3: Iteratively construct {1}, {2}, ... {n-1}
32
Step 3: Iteratively construct {1}, {2}, ... {n-1}
33
Step 4: Place the end effector frame
34
“approach”
“sliding”
“normal”
Step 4: Place the end effector frame
35
Step 5: Find the DH parameters
36
ai : distance between zi-1 and zi measured along xi
i : angle between zi-1 and zi measured about xi di : distance between oi-1 to the intersection of xi and zi-1
measured along zi-1
i : angle between xi-1 and xi measured about zi-1
Step 5: Find the DH parameters
37
Link ai i di i
1 0 0 d1 1*
2 0 -90 d2* 0
3 0 0 d3* 0
* joint variable
More Denavit-Hartenberg Examples
38
Step 5: Find the DH parameters
39
Link ai i di i
1 0 0 d1 1*
2 0 -90 d2* 0
3 0 0 d3* 0
* joint variable
Step 6: Compute the transformation
40
once the DH parameters are known, it is easy to construct
the overall transformation
Link ai i di i
1 0 0 d1 1*
2 0 -90 d2* 0
3 0 0 d3* 0
* joint variable
1000
100
00
00
1
11
11
,,,,
0
1 1111 d
cs
sc
RTTRT xaxdzz
Step 6: Compute the transformation
41
Link ai i di i
1 0 0 d1 1*
2 0 -90 d2* 0
3 0 0 d3* 0
* joint variable
1000
010
0100
0001
2
,,,,
1
2 2222 dRTTRT xaxdzz
Step 6: Compute the transformation
42
Link ai i di i
1 0 0 d1 1*
2 0 -90 d2* 0
3 0 0 d3* 0
* joint variable
1000
100
0010
0001
3
,,,,
2
3 3333 dRTTRT xaxdzz
Step 6: Compute the transformation
43
1000
010
0
0
21
3111
3111
2
3
1
2
0
1
0
3dd
dccs
dssc
TTTT
Spherical Wrist
44
Spherical Wrist
45
Spherical Wrist: Step 1
46
Spherical Wrist: Step 2
47
Spherical Wrist: Step 2
48
Spherical Wrist: Step 4
49
Step 5: DH Parameters
50
Link ai i di i
4 0 -90 0 4*
5 0 90 0 5*
6 0 0 d6 6*
* joint variable
Step 6: Compute the transformation
51
1000
6556565
654546465464654
654546465464654
5
6
4
5
3
4
3
6dccsscs
dssssccscsscccs
dscsccssccssccc
TTTT
RPP + Spherical Wrist
52
RPP + Spherical Wrist
53
1000
333231
232221
131211
3
6
0
3
0
6
z
y
x
drrr
drrr
drrr
TTT
21654
651641654111
dddssd
csssscccccr
z
Stanford Manipulator + Spherical Wrist
54
Link ai i di i
1 0 -90 0 1*
2 0 90 d2 2*
3 0 0 d3* 0
4 0 -90 0 4*
5 0 90 0 5*
6 0 0 d6 6*
* joint variable
SCARA + 1DOF Wrist
55
Link ai i di i
1 a1 0 d1 1*
2 a2 180 0 2*
3 0 0 d3* 0
4 0 0 d4 4* * joint variable