1
Tugino, ST [email protected]
Kinematika Manipulator Robot
1
Jurusan Teknik Elektro STTNASYogyakarta
Pendahuluan Robot Manipulators
Konfigurasi Robot S ifik i R b tSpesifikasi Robot
Jumlah Axis, DOF(Degree Of Freedom)Precision, Repeatability
KinematicsPreliminary
World frame, joint frame, end-effector frameRotation Matrix, composite rotation matrixHomogeneous Matrix
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 2
Homogeneous MatrixDirect kinematics
Denavit-Hartenberg RepresentationExamples
Inverse kinematics
2
Manipulators
Lengan Robot , Robot industriRigid bodies (links) connectedRigid bodies (links) connected by jointsJoints: revolute or prismaticDrive: electric or hydraulic End-effector (tool) mounted on a flange or plate secured to the wrist joint of robot
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 3
j
Manipulators R(revolute) or P( prismatic)
Robot Configuration:
Cartesian: PPP Cylindrical: RPP Spherical: RRP
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 4
SCARA: RRP(Selective Compliance Assembly Robot Arm)
Articulated: RRR
Hand coordinate:n: normal vector; s: sliding vector;
a: approach vector, normal to the
tool mounting plate
3
Robots Cartesian
Cartesian robotArm moves in 3 linearArm moves in 3 linear axes. X,y,z axes
z
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 5
xy
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 6
4
Manipulators
Motion Control MethodsP i t t i t t lPoint to point control
a sequence of discrete pointsspot welding, pick-and-place, loading & unloading
Continuous path controlfollow a prescribed path, controlled-path motionSpray painting Arc welding Gluing
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 7
Spray painting, Arc welding, Gluing
Manipulators
Robot SpecificationsNumber of AxesNumber of Axes
Major axes, (1-3) => Position the wristMinor axes, (4-6) => Orient the toolRedundant, (7-n) => reaching around obstacles, avoiding
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 8
undesirable configurationDegree of Freedom (DOF)WorkspacePayload (load capacity)Precision v.s. Repeatability
Which one is more important?
5
Kinematik
0ykinematics Forward
x
0y
1x1y
kinematics Inverse
sincos
0
0
lylx
==
θθ
θ
l
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 9
0x)/(cos 0
1 lx−=θ
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 10
6
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 11
A Simple Example
Revolute and Prismatic Joints Finding Υ:
y
Y
S
Combined
(x , y)
)xyarctan(θ =
More Specifically:
)xy(2arctanθ = arctan2() specifies that it’s in the
first quadrant
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 12
Υ1
X
SFinding S:
)y(xS 22+=
7
Υ2
(x , y)
Inverse Kinematics of a Two Link Manipulator
Given: l1, l2 , x , y
Find: Υ1, Υ2
Υ1
l2
l1
Redundancy:A unique solution to this problem
does not exist. Notice, that using the “givens” two solutions are possible. Sometimes no solution is possible.
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 13
(x , y)
Preliminary
Robot Reference FramesW ld fWorld frameJoint frameTool frame
x
yz
z
y T
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 14
x
y
W R
PT
8
PreliminaryCoordinate Transformation
Reference coordinate frame OXYZ zOXYZBody-attached frame O’uvw
zyx kji zyxxyz pppP ++=r
y
zP
vw
Point represented in OXYZ:T
zyxxyz pppP ],,[=
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 15
wvu kji wvuuvw pppP ++=r
zyx j zyxxyz pppx
uO, O’
zwyvxu pppppp ===
Point represented in O’uvw:
Two frames coincide ==>
PreliminaryProperties: Dot Product Let and be arbitrary vectors in and be 3R θx y
Mutually perpendicular Unit vectors
Properties of orthonormal coordinate frame
0=⋅ jivv
1||v
the angle from to , thenθcosyxyx =⋅
x y
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 16
00
0
=⋅
=⋅
jkki
ji
vv
vv
1||1||1||
=
=
=
kji
v
v
9
Preliminary
Coordinate TransformationRotation only zRotation only
wvu kji wvuuvw pppP ++=r y
zP
zyx kji zyxxyz pppP ++=r
u
vw
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 17
xuvwxyz RPP = u
How to relate the coordinate in these two frames?
PreliminaryBasic Rotation
, , and represent the projections of xp Pyp zponto OX, OY, OZ axes, respectively
Since
x y z
wvux pppPp wxvxuxx kijiiii ⋅+⋅+⋅=⋅=
wvu kji wvu pppP ++=
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 18
wvuy pppPp wyvyuyy kjjjijj ⋅+⋅+⋅=⋅=
wvuz pppPp wzvzuzz kkjkikk ⋅+⋅+⋅=⋅=
10
wvux pppPp wxvxuxx kijiiii ⋅+⋅+⋅=⋅=
wvuy pppPp wyvyuyy kjjjijj ⋅+⋅+⋅=⋅=
wvuz pppPp wzvzuzz kkjkikk ⋅+⋅+⋅=⋅=
⎥⎤
⎢⎡⎥⎤
⎢⎡ ⋅⋅⋅
⎥⎤
⎢⎡ ux pp wxvxux kijiii
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 19
⎥⎥⎥
⎦⎢⎢⎢
⎣⎥⎥⎥
⎦⎢⎢⎢
⎣ ⋅⋅⋅⋅⋅⋅=
⎥⎥⎥
⎦⎢⎢⎢
⎣ w
v
z
y
pp
pp
wzvzuz
wyvyuy
kkjkikkjjjij
PreliminaryBasic Rotation Matrix
⎥⎤
⎢⎡⎥⎤
⎢⎡ ⋅⋅⋅
⎥⎤
⎢⎡ ux pp wxvxux kijiii
Rotation about x-axis with⎥⎥⎥
⎦⎢⎢⎢
⎣⎥⎥⎥
⎦⎢⎢⎢
⎣ ⋅⋅⋅⋅⋅⋅=
⎥⎥⎥
⎦⎢⎢⎢
⎣ w
v
z
y
pp
pp
wzvzuz
wyvyuy
kkjkikkjjjij
zw
θ
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 20x
y
vP
u
θ⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡−=θθθθθ
CSSCxRot
00
001),(
11
Preliminary
Is it True? Rotation about x axis with θ
θθθθ
cossin0sincos0001
w
v
u
z
y
x
ppp
ppp
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡−=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
z
v
wP
θ
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 21
θθ
θθ
cossin
sincos
wvz
wvy
ux
ppp
ppppp
+=
−==
x
yu
θ
Basic Rotation MatricesRotation about x-axis with
⎥⎥⎤
⎢⎢⎡
−= θθθ SCxRot 0001
),(
θ
Rotation about y-axis with
R t ti b t i ith
⎥⎥⎦⎢
⎢⎣ θθ CS0
0
0100
),(⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
−=
θθ
θθθ
CS
SCyRot
θ
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 22
Rotation about z-axis with
uvwxyz RPP =⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡ −=
10000
),( θθθθ
θ CSSC
zRot
θ
12
PreliminaryBasic Rotation Matrix
⎥⎤
⎢⎡ ⋅⋅⋅ wxvxux kijiii
Obtain the coordinate of from the coordinate of
uvwxyz RPP =⎥⎥⎥
⎦⎢⎢⎢
⎣ ⋅⋅⋅⋅⋅⋅=
wzvzuz
wyvyuy
kkjkikkjjjijR
QPP =
uvwP
xyzP
⎥⎤
⎢⎡⎥⎤
⎢⎡ ⋅⋅⋅
⎥⎤
⎢⎡ xu pp zuyuxu kijiii
Dot products are commutative!
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 23
xyzuvw QPP =
TRRQ == −1
31 IRRRRQR T === − <== 3X3 identity matrix
⎥⎥⎥
⎦⎢⎢⎢
⎣⎥⎥⎥
⎦⎢⎢⎢
⎣ ⋅⋅⋅⋅⋅⋅=
⎥⎥⎥
⎦⎢⎢⎢
⎣ z
y
w
v
pp
pp
zwywxw
zvyvxv
kkjkikkjjjij
Example 2A point is attached to a rotating frame, the frame rotates 60 degree about the OZ axis of the reference frame.Find the coordinates of the point relative to the reference
)2,3,4(=uvwa
Find the coordinates of the point relative to the reference frame after the rotation.
⎤⎡−⎤⎡⎤⎡ −
=
598.040866.05.0
)60,( uvwxyz azRota
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 24
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
2964.4598.0
234
10005.0866.00866.05.0
13
Example 3A point is the coordinate w.r.t. the reference coordinate system, find the corresponding point w.r.t. the rotated OU-V-W coordinate
)2,3,4(=xyza
uvwapoint w.r.t. the rotated OU V W coordinate system if it has been rotated 60 degree about OZ axis.
uvwa
⎤⎡⎤⎡⎤⎡
=
598440866050
)60,( xyzT
uvw azRota
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 25
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡−=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡−=
2964.1
598.4
234
10005.0866.00866.05.0
Composite Rotation MatrixA sequence of finite rotations
matrix multiplications do not commuteprules:
if rotating coordinate O-U-V-W is rotating about principal axis of OXYZ frame, then Pre-multiply the previous (resultant) rotation matrix with an appropriate basic rotation matrixif rotating coordinate OUVW is rotating about its
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 26
if rotating coordinate OUVW is rotating about its own principal axes, then post-multiply the previous (resultant) rotation matrix with an appropriate basic rotation matrix
14
Example 4Find the rotation matrix for the following operations:
axis OUabout Rotation axisOW about Rotation
axis OYabout Rotation
αθφ
⎥⎥⎤
⎢⎢⎡
−+−
=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡−
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡ −
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
=
αθαθθαφαθφαθφαφθφ
ααααθθ
θθ
φφ
φφαθφ
SCCCSCSSSCCSCSSCC
CSSCCS
SCuRotwRotIyRotR
00
001
10000
C0S-010
S0C),(),(),( 3
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 27Post-multiply if rotate about the OUVW axes Pre-multiply if rotate about the OXYZ axes
...Answer ⎥⎥⎦⎢
⎢⎣ −+− αθφαφαφαθφθφ SSSCCSCCSSCS
Example 5Translation along Z-axis with h:
⎥⎤
⎢⎡
00100001
⎥⎤
⎢⎡
⎥⎤
⎢⎡⎥⎤
⎢⎡
⎥⎤
⎢⎡
00100001 ppx uu
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
=
1000100
0010),(
hhzTrans
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
+=
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
=
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣ 111000100
0010
1hp
ppp
hzy
w
v
w
v
y
z P
w
zP
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 28
x
y
u
vw
O, O’hx
y
u
vw
O, O’
15
Example 6Rotation about the X-axis by
⎥⎤
⎢⎡ 0001
⎥⎤
⎢⎡⎥⎤
⎢⎡
⎥⎤
⎢⎡ 0001 upx
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
−=
10000000
),(θθθθ
θCSSC
xRot
z
v
wP
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
−=
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣ 110000000
1w
v
pp
CSSC
zy
θθθθ
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 29x
y
v
u
Homogeneous Transformation
Composite Homogeneous Transformation MatrixMatrixRules:
Transformation (rotation/translation) w.r.t (X,Y,Z) (OLD FRAME), using pre-multiplicationTransformation (rotation/translation) w r t
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 30
Transformation (rotation/translation) w.r.t (U,V,W) (NEW FRAME), using post-multiplication
16
Example 7Find the homogeneous transformation matrix (T) for the following operation:
axis OZabout ofRotation axis OZ along d ofn Translatioaxis OX along a ofn Translatio
axis OXabout Rotation
θ
α
ITTTTT
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 31
:Answer44,,,, ×= ITTTTT xaxdzz αθ
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
=
100000000001
100001000010
001
1000100
00100001
100001000000
ααααθθ
θθ
CSSC
a
dCSSC
Homogeneous Representation
A frame in space (Geometric Interpretation)
z),,( zyx pppP
⎤⎡
y
z
⎥⎥⎤
⎢⎢⎡
yyyy
xxxx
pasnpasn
F
ns
a
⎥⎦
⎤⎢⎣
⎡= ××
101333 PR
F(X’)
(y’)(z’)
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 32
x⎥⎥⎥
⎦⎢⎢⎢
⎣
=
1000zzzz
yyyy
pasnF
Principal axis n w.r.t. the reference coordinate system
17
Homogeneous Transformation
Translation s
a
y
z
ns
a ns
⎥⎤
⎢⎡ +
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
×
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
10001000100010001
xxxxx
zzzz
yyyy
xxxx
z
y
x
new
dpasn
pasnpasnpasn
ddd
F
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 33
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
++
=
1000
zzzzz
yyyyy
dpasndpasn
oldzyxnew FdddTransF ×= ),,(
Orientation RepresentationEuler Angles Representation ( , , )
Many different typesφ θ ψ
Description of Euler angle representations
Euler Angle I Euler Angle II Roll-Pitch-Yaw
Sequence about OZ axis about OZ axis about OX axis
of about OU axis about OV axis about OY axis
φ φθ θ θ
ψ
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 34
of about OU axis about OV axis about OY axis
Rotations about OW axis about OW axis about OZ axisφθ θ θψ ψ
18
Orientation RepresentationEuler Angle I
⎟⎞
⎜⎛
⎟⎞
⎜⎛ − 0010sincos φφ
⎟⎟⎞
⎜⎜⎛ −
=
⎟⎟⎟
⎠
⎞
⎜⎜⎜
⎝
⎛−=
⎟⎟⎟
⎠
⎞
⎜⎜⎜
⎝
⎛=
0cossin0sincos
,cossin0sincos0001
,1000cossin0sincos
'
ϕϕϕϕ
θθθθφφ
φφ
θφ uz
R
RR
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 35
⎟⎟
⎠⎜⎜
⎝
=1000cossin'' ϕϕϕwR
Euler Angle IResultant eulerian rotation matrix:
⎟⎟⎟⎟⎟⎞
⎜⎜⎜⎜⎜⎛
−−
−−
−
=
θφϕφϕφ
θϕθϕφ
ϕφθϕφ
ϕφϕθφ
sincossinsincossin
sinsincoscossin
sincoscossinsin
coscos''' wuz RRRR
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 36
⎟⎟⎟⎟⎟
⎠⎜⎜⎜⎜⎜
⎝
−+−
+
θθϕθϕ
θφθϕφ
ϕφθϕφ
ϕφ
cossincossinsin
sincoscoscoscos
sinsincossincos
cossin
19
Euler Angle II, Animated
zw'=
yv'
φ
θ =v"
w"
v"'ϕ
"'
w"'=
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 37
xu'
u"
u"'
Note the opposite (clockwise) sense of the third rotation, φ.
zw'=
Euler Angle I, Animated
yv'
φv "
w"v'"
ϕ
w'"=
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 38
xu'
θ=u"
u'"
20
Orientation RepresentationMatrix with Euler Angle II
⎞⎛ φφ iii
⎟⎟⎟⎟⎟⎟⎟⎞
⎜⎜⎜⎜⎜⎜⎜⎛
−+
−−
+−
θϕθϕφ
ϕφθϕφ
ϕφ
θφθϕφ
ϕφθϕφ
ϕφ
sinsincoscossin
coscoscoscossin
sincos
sincoscoscossin
cossincoscoscos
sinsin
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 39
⎟⎟⎟
⎠⎜⎜⎜
⎝ −θ
θϕθϕcos
sinsinsincos
Quiz: How to get this matrix ?
Orientation RepresentationDescription of Roll Pitch Yaw
Z
Yϕθ
φ
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 40
X
θ
Quiz: How to get rotation matrix ?
21
Terimakasih
x
yz
x
yz
x
yzz
y
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 41
x
Coordinate Transformations• position vector of P in {B} is transformed to position vector of Pto position vector of P in {A}
• description of {B} as seen from an observer in {A}
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 42
Rotation of {B} with respect to {A}
Translation of the origin of {B} with respect to origin of {A}
22
Coordinate TransformationsTwo Special Cases
1 T l ti l
'oAPBB
APA rrRr +=1. Translation only
Axes of {B} and {A} are parallel
2. Rotation only
1=BAR
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 43
yOrigins of {B} and {A} are coincident
0' =oAr
Homogeneous Representation• Coordinate transformation from {B} to {A}
'oAPBB
APA rrRr +=
• Homogeneous transformation matrix
⎥⎦
⎤⎢⎣
⎡⎥⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡
× 1101 31
' PBoAB
APA rrRr
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 44
g
⎥⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡= ××
× 10101333
31
' PRrRT
oAB
A
BA
Position vector
Rotation matrix
Scaling
23
Homogeneous TransformationSpecial cases1. Translation
2. Rotation
⎥⎦
⎤⎢⎣
⎡=
×
×
10 31
'33
oA
BA rIT
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 45
⎥⎦
⎤⎢⎣
⎡=
×
×
100
31
13BA
BA RT
The Geometric Solution
l
l2Υ2
(x , y) Using the Law of Cosines:
+
−=−−−+=+
−+=
2222
212
22
122
222
)cos(θ)θ180cos()θ180cos(2)(
cos2
ll
llllyx
Cabbac
22
2
l1
Υ1
α
⎟⎟⎠
⎞⎜⎜⎝
⎛ −−+=
−−+=
21
22
21
22
21
21
2arccosθ
2)cos(θ
llllyx
llllyx
2
2
Using the Law of Cosines:
=sinsin
cC
bB Redundant since θ2 could be in the
first or fourth quadrant.
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 46⎟⎠⎞
⎜⎝⎛=
+=
+=
+
−=
xy2arctanα
αθθ
yx)sin(θ
yx)θsin(180θsin
11
222
222
2
1
l
cb
⎟⎠⎞
⎜⎝⎛+
⎟⎟
⎠
⎞
⎜⎜
⎝
⎛
+=
xy2arctan
yx)sin(θarcsinθ
2222
1l
Redundancy caused since θ2 has two possible values
24
The Algebraic Solution
l2Υ2
Υ(x , y)
1221
11
ccx(1))θcos( θc
cos θc
++=
=
+
ll
( ) ( )+++++=
=+=+
++++ 211212
212
22
12
1211212
212
22
12
1
2222
)(sins2)(sins)(cc2)(cc
yx)2((1)
llllllll
l1
Υ1
21
21211
21211
θθθ(3)sinsy(2)ccx(1)
+=+=+=
+
+
llll
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 47
( ) ( )( )
⎟⎟⎠
⎞⎜⎜⎝
⎛ −−+=∴
++=
+++= ++
++++
21
22
21
22
2
2212
22
1
211211212
22
1
21121212112112121211
2yxarccosθ
c2
)(sins)(cc2
)()()()(
llll
llll
llllOnly Unknown
))(sin(cos))(sin(cos)sin(
))(sin(sin))(cos(cos)cos(:
abbaba
bababaNote
+−
+−
−+
+−
=
=
))(sin(cos))(sin(cos)sin(
))(sin(sin))(cos(cos)cos(:
abbaba
bababaNote
+−
+−
−+
+−
=
=
sinsy
)()c(c ccc
ccx
21211
2212211
21221211
21211
ll
slsllsslll
ll
+=
−+=−+=
+=
+
+
We know what θ2 is from the previous
)c(s)s(c cscss
2211221
12221211
llllll++=
++= slide. We need to solve for θ1 . Now we have two equations and two unknowns (sin θ1 and cos θ1 )
221122221
221
221
2211
)c(s)s()c()(xy
)c()(xc
+++
+=
++
=
lllll
slsll
sls
Substituting for c1 and simplifying many times
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 48
( )
2222221
1
2212
22
1122221
yxx)c(ys
)c2(sx)c(
1
+
−+=
++++
=
slll
llllslll
Notice this is the law of cosines and can be replaced by x2+ y2
⎟⎟⎠
⎞⎜⎜⎝
⎛
+
−+= 22
222211 yx
x)c(yarcsinθ slll
25
Homogeneous TransformationComposite Homogeneous Transformation Matrix
1z2z
2y
0x
0z
0y1
0 A2
1 A
1x
1y 2x
?
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 49
21
10
20 AAA =
?i
i A1− Transformation matrix for adjacent coordinate frames
Chain product of successive coordinate transformation matrices
Example 8For the figure shown below, find the 4x4 homogeneous transformation matrices and for i=1, 2, 3, 4, 5
⎥⎤
⎢⎡ xxxx pasn
ii A1−
iA0
c ⎥⎤
⎢⎡− 0001
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
1000zzzz
yyyy
xxxx
pasnpasnp
F
a
b
c
d
e
2z
3y3x
3z
4z
4y4x
5x5y
5z⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
−−+−
=
1000010100
10
dace
A
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
−−−
=0001
100010
21 da
b
A
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 50
0x 0y
0z1x
1y
1z2x
2y
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡+−−
=
10000100
001010
20 ce
b
A
⎥⎦
⎢⎣ 1000
Can you find the answer by observation based on the geometric interpretation of homogeneous transformation matrix?
26
Orientation Representation
⎥⎦
⎤⎢⎣
⎡= ××
101333 PR
F
Rotation matrix representation needs 9 elements to completely describe the orientation of a rotating rigid body. Any easy way?
⎦⎣ 10
Tugino, ST MT Sekolah Tinggi Teknologi Nasional Yogyakarta 51
Euler Angles Representation