Matrix Methods in Kinematics - School of Engineeringmoreno/Kinematics/ME 5150 Kinematics...
Transcript of Matrix Methods in Kinematics - School of Engineeringmoreno/Kinematics/ME 5150 Kinematics...
1
Matrix Methods in Kinematics
Rigid Body Rotation Matrices
Rigid Body – points have same relative position
Displacement = Rotation and Translation
Angular rotations described about
1. Right hand Cartesian axes (x,y,z)
2. Arbitrary Axis
3. Euler Angles
Vector Methods with Matrix Notation
2
Matrix Methods in Kinematics
Holonomic Systems Holonomic Constraints
Relations between co-ordinates and possibly time)
0),,..,( 21 tqqqf n
Rigid body
ir
jrijL
xyz displacement is not order dependent
3
Matrix Methods in Kinematics
Rigid body rotations
Rotations not commutative (order dependent)
z
y
x
4
Matrix Methods in Kinematics
1. Rotate about Z (α)
zv
yv
xv
zv
yv
xv
zvzv
vvv
vvv
yxy
yxx
1
1
1
2
2
2
12
112
112
100
0cossin
0sincos
cossin
sincos
Rotation matrix
Components in the fixed system
x-y
5
Matrix Methods in Kinematics
Rotation about three Cartesian axes
z
y
x
z
y
x
v
v
v
v
v
v
1
1
1
2
2
2
100
0cossin
0sincos
z
y
x
z
y
x
v
v
v
v
v
v
1
1
1
'2
'2
'2
cos0sin
010
sin0cos
z
y
x
z
y
x
v
v
v
v
v
v
1
1
1
"2
"2
"2
cossin0
sincos0
001
y
z
x
X,Y,Z axes fixed in space
'2v 2v
''2v
1v
6
Matrix Methods in Kinematics
Plane rotation (2D) (rotation about z)
Spatial Rotation – sequence z,y,x
1,2 vRv z
1,,1,,,2 vRvRRRv zyx
100
0cossin
0sincos
cos0sin
010
sin0cos
cossin0
sincos0
001
7
Matrix Methods in Kinematics
1,,1,,,2 vRvRRRv zyx
12 v
CCCSSSCCSCSS
SCSSSCCSSCCS
SCSCC
v
X
Y
Z
dependentsequence
axesfixedabout
.3
.2
.1
etcSC ,sin,cos
8
Matrix Methods in Kinematics
2. Rotation about axis u (φ)
• Decompose to rotations about x, y, z
• Rotate u about z and then back again
+/- show directions
x
y
z
u
1,1,,,,,2 vRvRRRRRv uyxzxy
xu
yu
zu
cos0sin
010
sin0cos
cossin0
sincos0
001
1v
9
Matrix Methods in Kinematics
y
),,,,,(2
1,1,,,,,2
cscscsfv
vRvRRRRRv uyxzxy
z
x
zx
zzx
zx
xy
u
u
uu
uuu
uu
uu
coscos
sincos
coscos
sinsin
22
22
22
x
z
u
xu
yu
zu
1v
22
zx uu
10
Matrix Methods in Kinematics
1,1,,,,,2 vRvRRRRRv uyxzxy
1
2
2
2
2 v
CVuSuVuuSuVuu
SuVuuCVuSuVuu
SuVuuSuVuuCVu
v
zxzyyzx
xzyyzyx
yzxzyxx
cos,sin
cos1
),,,(,
CS
V
uuufR zyxu
spaceinfixedu
ofdiruuu zyx
)(
cos,,
11
Matrix Methods in Kinematics
3. Euler Angles
Displacement of a rigid body in terms
of three relative displacement angles
Each rotation about an axis whose
location depends on preceding
rotation
anglespin
anglenutation
angleprecision
rotationzxz
"
'"","'','
'',',,
zabout
xaboutthenzyxzyx
zaboutbyzyxzyx
3
2
1
• 4 sets of axes initially coincident
• xyz fixed in the body
14
Matrix Methods in Kinematics
z-x-z
x-y-x
y-z-y
z-y-z
x-z-x
y-x-y
x-y-z
y-z-x
z-x-y
x-z-y
z-y-x
y-x-z
12 different possible rotation sets
15
Matrix Methods in Kinematics
zxz
z
T
zxz
T
z
T
xzxz
T
z
T
xzxzz
T
zxzx
zxz
RRRR
RRRRRRRRRR
RRRRRR
RRRR
RRRR
,,,,,
,,,,,,,,,,,
,,,,,",
,,,',
,',",,,
basesdifferentoperatorsame
similarareAandB
APPB
tiontransformasimilarity
,
,
1
16
Matrix Methods in Kinematics
CCSSS
SCCCCSSSCCCS
SSCCSSCSCSCC
R
getcaseeitherRRRR
axesfixedinitialabouttiontransformasimilaritywithor
RRRR
zxz
zxz
,,
,,,,,
,',",,,
:
)(
etcR z
100
0cossin
0sincos
,
17
Matrix Methods in Kinematics
Euler Angles
3
2
1
1
2
3
Angular Velocity Vectors
Rigid
body
18
Matrix Methods in Kinematics
Rotation of Rigid Body in 2d Cartesian Space
ppRqRqalso
ppqRq
pqRpq
pq
pq
pq
pq
yy
xx
yy
xx
11
11
11
11
11
cossin
sincos
p1 q1
Fixed x-y
Vector form
specifiedqwhenqfind
knownandppif
1
1,
19
Matrix Methods in Kinematics
1100
)cossin(cossin
)sincos(sincos
1
cossincossin
sincossincos
cossin
sincos
cossin
sincos
1
1
1
11
11
1111
1111
1
1
1
1
z
y
x
yxy
yxx
z
y
x
yyxyxy
xyxyxx
y
x
y
x
y
x
y
x
q
q
q
ppp
ppp
q
q
q
pppqqq
pppqqq
p
p
p
p
q
q
q
q
1 2
20
Matrix Methods in Kinematics
1100
)cossin(cossin
)sincos(sincos
1
1
1
11
11
y
x
yxy
yxx
y
x
q
q
ppp
ppp
q
q
11
1100
)(
1
1
1
1
1
qD
q
q
qpRpR
q
q
y
x
y
x
3x3 D = plane displacement matrix
21
Matrix Methods in Kinematics
Spatial (3D) Rigid Body Displacement
Replace
ppqRq
pqRpq
RorRorRwithR u
11,,
11,,
,,,,,
1100011
11,,,,1,,
qpRpRqD
q
Cartesian u Axis Euler
4x4 matrix (3D) new original
Using Cartesian
22
Matrix Methods in Kinematics
1
1000
(
1
)(
1
1
1
1,1,
11,1
11
1
,
z
y
x
uu
u
u
q
q
q
pRsupR
qz
qy
qx
pqRsupq
pqRpq
suppreplace
uaxisalongrotationRuse
Screw displacement matrix
Screw displacement matrix
p1
u
s
p=p1+su
q
q1
x
z
y
23
Matrix Methods in Kinematics
1100011
11,,,,1,,
qpRpRqD
q
12 non constant elements
3D space= 6 DOF, 6 elements are dependent
To Define D: euler angles, dir cos, points on body, etc…
24
Matrix Methods in Kinematics
Example Displacement of a point
Moving with a rigid body
2112 ppRqRq
732.3
0.4
2
3
11
13
60cos60sin
60sin60cos
60cos60sin
60sin60cos
2
2
2
2
2
2
11
11
2
2
y
x
y
x
y
x
yy
xx
y
x
q
q
q
q
p
p
pq
pq
q
q
25
1
732.3
0.4
1
1
3
100
634.05.0866.
366.3866.5.0
1
1100
)60cos160sin12(60cos60sin
)60sin160cos13(60sin60cos
1
1100
)cossin(cossin
)sincos(sincos
1
2
2
1
1
2
2
1
1
112
112
2
2
2
y
x
y
x
y
x
y
x
yxy
yxx
z
y
x
q
q
q
q
q
q
q
q
ppp
ppp
q
q
q
3x3 Displacement matrix
Matrix Methods in Kinematics
26
Matrix Methods in Kinematics
Finite Rotation Pole – plane rotation about p0
With new position vectors
p1=p2=p0
po
27
Matrix Methods in Kinematics
Displacement matrix D now written as:
1
732.3
0.4
1
1
3
100
634.05.0866.
366.3866.5.0
1100
)cossin(cossin
)sincos(sincos
1
1
1
000
000
2
2
y
x
yxy
yxx
y
x
q
q
ppp
ppp
q
q
Previously
p1 and p2 in
q2 is the
same point Original
displacement
matrix
28
Matrix Methods in Kinematics
634.0
366.3
cos1sin
sincos1
634.0cossin
366.3sincos
0
0
1212
1212
000
000
y
x
yxy
yxx
p
p
ppp
ppp
232.3
134.1
0
0
y
x
p
p
29
Matrix Methods in Kinematics
HW #5 Salute
Z
Y
X
shoulder
p1=elbow
q1=tip of finger
p
q
30◦
Use Cartesian angles to find [D]
Treat as 3 independent rotations
30
Matrix Methods in Kinematics
HW #5 Salute
Z
Y
X
shoulder
p1=elbow
q1=tip of finger
p
q
30◦
z-y-z rotation
Z
Y
X
180
Z
X
Changed axes
notation
30
3
2
1
Y move origins for
2,3 rotations
31
Matrix Methods in Kinematics
Finding the Displacement Matrix by Inversion
x
y
C1
A1
B1
B2
C2
A2 1,2,61,5,1
1,1,71,6,2
1,1,51,4,2
21
21
21
CC
BB
AA
Known points
32
Matrix Methods in Kinematics
Displacement Matrix by Inversion
1
1
1
1
1
1
1
333231
232221
131211
1
11
111
y
x
y
x
y
x
y
x
y
x
q
q
q
q
D
q
q
Dq
q
aaa
aaa
aaa
q
q
qDq
33
Matrix Methods in Kinematics
1
111
564
122
111
211
675
111
564
122
111
211
675
D
D
d a
D=d*a-1
2D Planar motion
x
y
z
q q1
34
Matrix Methods in Kinematics
ij
ji
ij M
AAA
AAA
AAA
AadjA
AadjA
A
)1(
333231
232221
131211
332313
322212
312111
1
ith row=ith column
α=co factor
Mij= minor of A
Finding the inverse of A (by hand – the long way)
adj - adjoint
35
Matrix Methods in Kinematics
d =
5 7 6
1 1 2
1 1 1
0 1 1
-1 0 3
0 0 1
100
)cossin(cossin
)sincos(sincos
112
112
yxy
yxx
ppp
ppp
a =
2 2 1
4 6 5
1 1 1
inv(a)
ans =
0.5000 -0.5000 2.0000
0.5000 0.5000 -3.0000
-1.0000 0 2.0000
90
Using MATLAB
Displacement matrix
D=d*a-1
36
Matrix Methods in Kinematics
132.1725
11032.2
1015
3
3
3
c
b
a
12015
1035
1015
2
2
2
c
b
a
11520
1350
1150
1
1
1
c
b
a
X
Y
1a
2b
2c
3a2a
3c
3b
1b
1c
30
HW 5 by Inversion
Using My arm
37
Matrix Methods in Kinematics
By Inverse method – position 1 2
111
153515
2000
111
2000
153515
12D
2a 2b 2c 1a 1b 1c
-0.0500 0.0500 1.7500
0 -0.0500 -0.7500
0.0500 0 0
D =
0 1.0000 0
-1.0000 0 0
0 0 1.0000
inv
12D
90
38
Matrix Methods in Kinematics
By Inverse method – position 2 3
111
2000
153515
111
32.17100
2532.215
23D
3a3b 3c
2a 2b 2c
0.0500 0.0500 1.7500
-0.0500 0 -0.7500
0 -0.0500 0
inv
23D -0.8660 0.5000 -27.9900
-0.5000 -0.8660 -7.5000
0 0 1.0000
150
39
Matrix Methods in Kinematics
1
10
32.2
1
35
0
100
5.75.866.
99.27866.5.
133
122313
Db
DDD240