Vectors, Matrices, Rotations€¦ · Vectors, Matrices, Rotations. You want to put your hand on the...
31
Vectors, Matrices, Rotations
Transcript of Vectors, Matrices, Rotations€¦ · Vectors, Matrices, Rotations. You want to put your hand on the...
Vectors, Matrices, Rotations
You want to put your hand on the cup…
• Suppose your eyes tell you where the mug is and its orientation in the robot base frame (big assumption)
• In order to put your hand on the object, you want to align the coordinate frame of your hand w/ that of the object
• This kind of problem makes representation of pose important...
Why are we studying this?
Representing Position: Vectors
Representing Position: vectors
x
y
p
5
2
=2
5p (“column” vector)
[ ]52=p (“row” vector)
x
y
z
=2
5
2
p
p
Representing Position: vectors
• Vectors are a way to transform between two different reference frames w/ the same orientation
• The prefix superscript denotes the reference frame in which the vector should be understood
=2
5pb
=0
0pp
Same point, two different reference frames
p
5
2
b
px
bx
by
py
Representing Position: vectors
• Note that I am denoting the axes as orthogonal unit basis vectors
bx A vector of length one pointing in the direction of the base frame x axis
y axisby
py p frame y axis
This means “perpendicular”
p
5
2
b
px
bx
by
py
What is this unit vector you speak of?
22yx aaa +=
Vector length/magnitude:a
1ˆ =aDefinition of unit vector:
=
y
x
a
aaThese are the elements of a:
5
2
xb ˆ
yb ˆ
22ˆ
yx aa
aa
+=You can turn a into a unit vector of the
same direction this way:
yyxx bababa +=⋅
)cos(θba=
And what does orthogonal mean?
Unit vectors are orthogonal iff (if and only if) the dot product is zero:
xp ˆ is orthogonal to iff
First, define the dot product:
0=⋅ba when: 0=a0=b
( ) 0cos =θ
or,
or, a
b
θ
xp ˆ
yp ˆ
yp ˆ 0ˆˆ =⋅ yx pp
A couple of other random things
bbb yxp ˆ2ˆ5 +=
5
2
b xb ˆ
yb ˆ
x
y
zx
y
z
right-handed coordinate frame
left-handed coordinate frame
Vectors are elements of nR
The importance of differencing two vectors
errb
effb
objectb xxx =−
The eff needs to make a Cartesian displacement of this much to reach the object
objectbxerr
bxeff
bx
The importance of differencing two vectors
errb
effb
objectb xxx =−
bbx
by
effeffx
effy
objectobjectx
objecty
err
The eff needs to make a Cartesian displacement of this much to reach the object
Representing Orientation: Rotation Matrices
• The reference frame of the hand and the object have different orientations
• We want to represent and difference orientations just like we did for positions…
=
333231
232221
131211
aaa
aaa
aaa
A
=
332313
322212
312111
aaa
aaa
aaaTA
333231
232221
131211
aaa
aaa
aaa
=2
5p [ ]25=Tp
Before we go there – review of matrix transpose
( )TTT BABA =Important property:
=
2221
1211
aa
aaA
=
2221
1211
bb
bbB
and matrix multiplication…
++++
=
=
2222122121221121
2212121121121111
2221
1211
2221
1211
babababa
babababa
bb
bb
aa
aaAB
[ ] bab
baabababa T
y
xyxyyxx =
=+=⋅
Can represent dot product as a matrix multiply:
Same point - different reference frames
xa ˆ
p
5
2
ya ˆ yb ˆ
xb ˆ
8.3
8.3
=2
5pa
=
8.3
8.3pb
• for the moment, assume that there is no difference in position…
a
b
)cos()cos(ˆˆ θθ bbabal ==⋅=
θ
yyxx bababa +=⋅
)cos(θba=
Another important use of the dot product: projection
l
p
5
2
8.3
8.3
B-frame’s x axis written in A frame
B-frame’s y axis written in A frame
Same point - different reference frames
xa
ya
ba x
ba y
( ) xbaa
ba pppx ==⋅ θcosˆ
xa
pa
5
2
ya
8.3
8.3
ba x
B-frame’s x axis written in A frame
ba y B-frame’s y axis written
in A frame
Same point - different reference frames
θ
pxp ab
ax
b ⋅= ˆ
5
2
8.3
8.3
B-frame’s x axis written in A frame
B-frame’s y axis written in A frame
pyp ab
ay
b ⋅= ˆ
Same point - different reference frames
xa
pa
ya
ba x
ba y
pxp AB
Ax
B ⋅= ˆ
pyp AB
Ay
B ⋅= ˆ xA
p
5
2
yA
8.3
8.3
BA x
BA y
py
x
py
px
py
pxp A
TB
A
TB
A
ATB
A
ATB
A
AB
A
AB
AB
=
=
⋅⋅
=ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
Rotation matrix
pRp ATB
AB =
py
xp A
TB
A
TB
AB
=
ˆ
ˆ
Same point - different reference frames
pRp ATB
AB =py
xp A
TB
A
TB
AB
=
ˆ
ˆ
The rotation matrix
By the same reasoning:
From last page:
pRp BTA
BA =py
xp B
TA
B
TA
BA
=
ˆ
ˆ
The rotation matrix
( )BAB
AB
A yxR ˆˆ=
==
TA
B
TA
BTA
BB
A
y
xRR
ˆ
ˆ
=
2221
1211
rr
rrRBA
and
BA x B
A y
=
2221
1211
rr
rrRBA
TA
B xTA
B y
The rotation matrix can be understood as:
1. Columns of vectors of B in A reference frame, OR
2. Rows of column vectors A in B reference frame
The rotation matrix
p
ya
ba x
ba y
p
ab y
xb ˆ
yb ˆ
( )BAB
AB
A yxR ˆˆ=
=
TA
B
TA
B
BA
y
xR
ˆ
ˆ
ab x
Example 1: rotation matrix
xa ˆ
ya ˆ
xb ˆ
yb ˆ
θ
θ
( ) ( ) ( )( ) ( )
−==
θθθθ
cossin
sincosˆˆ b
ab
ab
a yxR( )( )
=
θθ
sin
cosˆb
a x
( )( )
−=
θθ
cos
sinˆb
a y( ) ( )( ) ( )
−
=θθθθ
cossin
sincosa
bR
Example 2: rotation matrix
yA
zA
xA45
zB
xB
yB
( )BAB
AB
AB
A zyxR ˆˆˆ=
−
−
=
21
21
21
21
0
0
1
0
0BAR
−−=
21
21
21
21
0
010
0
BAR
Example 3: rotation matrix
ax
ay
θ
φ
az
bzbx
by
−−−
=
−=
+
+
+
φφ
φθθφθ
φθθφθ
φφ
φθθφθ
φθθφθ
π
π
π
cs
ssccs
scscc
ss
csccs
ccscc
Rca
002
2
2
Rotations about x, y, z
( )( ) ( )( ) ( )
−=
100
0cossin
0sincos
αααα
αzR
( )( ) ( )
( ) ( )
−=
ββ
βββ
cos0sin
010
sin0cos
yR
( ) ( ) ( )( ) ( )
−=
γγγγγ
cossin0
sincos0
001
xR
These rotation matrices encode the basis vectors of the after-rotation reference frame in terms of the before-rotation reference frame
Remember those double-angle formulas…
( ) ( ) ( ) ( ) ( )φθφθφθ sincoscossinsin ±=±
( ) ( ) ( ) ( ) ( )φθφθφθ sinsincoscoscos =±
Example 1: composition of rotation matrices
CB
BA
CA RRR =
p
ya ˆ
xb ˆ
yb ˆ
xa ˆ
yc ˆ
xc ˆ
1θ2θ
( ) ( )( ) ( )
( ) ( )( ) ( )
−+−−−
=
−
−=
21212121
21212121
22
22
11
11
cossin
sincos
cossin
sincos
ssccsccs
csscssccRca
θθθθ
θθθθ
−=
1212
1212
cs
sc
Example 2: composition of rotation matrices
ax
ay
θ
φ
az
bx
cx
−=
100
0
0
θθ
θθ
cs
sc
Rba
−=
−=
−−
−−
φφ
φφ
φφ
φφ
cs
sc
cs
sc
Rcb
0
010
0
0
010
0
Example 2: composition of rotation matrices
ax
ay
θ
φ
az
bx
cx
−−−
=
−
−==
φφ
φθθφθ
φθθφθ
φφ
φφ
θθ
θθ
cs
ssccs
scscc
cs
sc
cs
sc
RRR cbb
ac
a
00
010
0
100
0
0
