Professional Documents
Culture Documents
L3 - Spatial Transformations 2 V1
L3 - Spatial Transformations 2 V1
Spatial Transformations - 2
• Review
– Manipulator Specifications
• Precision, Repeatability
– Homogeneous Matrix
• Denavit-Hartenberg (D-H) Representation
• Kinematics Equations
• Inverse Kinematics
Review
Universiti Kuala Lumpur Malaysia France Institute
Hand coordinate:
⎡ px ⎤ ⎡ i x ⋅ i u i x ⋅ jv i x ⋅ k w ⎤ ⎡ pu ⎤
Pxyz = ⎢⎢ p y ⎥⎥ = ⎢⎢ jy ⋅ i u j y ⋅ jv jy ⋅ k w ⎥⎥ ⎢⎢ pv ⎥⎥ = RPuvw
⎢⎣ p z ⎥⎦ ⎢⎣k z ⋅ i u k z ⋅ jv k z ⋅ k w ⎥⎦ ⎢⎣ pw ⎥⎦
z
Pxyz = RPuvw w
P v
Puvw = QPxyz
Q = R −1 = RT y
u
x
Basic Rotation Matrices
– Rotation about x-axis with θ
Universiti Kuala Lumpur Malaysia France Institute
⎡1 0 0 ⎤
Rot ( x,θ ) = ⎢⎢0 Cθ − Sθ ⎥⎥
⎢⎣0 Sθ Cθ ⎥⎦
– Rotation about y-axis with θ
⎡ Cθ 0 Sθ ⎤
Rot ( y, θ ) = ⎢⎢ 0 1 0 ⎥⎥
⎢⎣− Sθ 0 Cθ ⎥⎦
r = RB B r P + Ar o '
A P A
⎡ A r P ⎤ ⎡ A RB ⎤
A o'
r ⎡ r ⎤
B P
⎢ ⎥=⎢ ⎥⎢ ⎥
⎣ 1 ⎦ ⎣ 01×3 1 ⎦⎣ 1 ⎦
• Homogeneous transformation matrix
⎡ RB r ⎤ ⎡ R3×3
A o'
P3×1 ⎤
A Rotation
TB = ⎢ ⎥=⎢
A matrix
⎥
⎣ 01×3 1 ⎦ ⎣ 0 1 ⎦ Position
vector
Scaling
Review
• Homogeneous Transformation
Universiti Kuala Lumpur Malaysia France Institute
– Special cases
1. Translation
⎡ I 3×3 r ⎤
A o'
A
TB = ⎢ ⎥
⎣01×3 1 ⎦
2. Rotation
⎡ A
RB 03×1 ⎤
A
TB = ⎢ ⎥
⎣ 01×3 1 ⎦
Review
Universiti Kuala Lumpur Malaysia France Institute
– A point in R 3 space
⎡ px ⎤
⎢p ⎥
P = ⎢ y ⎥ Í Homogeneous coordinate of P w.r.t. OXYZ
⎢ pz ⎥
⎢ ⎥ z P( px , p y , pz )
⎣1⎦ a
3
s
– A frame in R space
n
⎡ nx sx ax px ⎤
⎢ y
⎡n s a P ⎤ ⎢n y sy ay p y ⎥⎥
F =⎢ ⎥ =
⎣0 0 0 1 ⎦ ⎢ nz sz az pz ⎥
⎢ ⎥
⎣0 0 0 1⎦ x
Review
Universiti Kuala Lumpur Malaysia France Institute
• Orientation Representation
(Euler Angles)
– Description of Yaw, Pitch, Roll
Z
• A rotation of ψ about the OX
axis ( Rx ,ψ ) -- yaw φ roll
• A rotation of θ about the OY
axis ( R y ,θ ) -- pitch yaw
• A rotation of φ about the OZ ψ Y
axis ( Rz ,φ ) -- roll θ
pitch
X
Quiz 1
• How to get the resultant rotation matrix for YPR?
Universiti Kuala Lumpur Malaysia France Institute
T = Rz ,φ R y ,θ Rx ,ψ
⎡Cφ − Sφ 0 0 ⎤ ⎡ Cθ 0 Sθ 0⎤ ⎡1 0 0 0⎤
⎢ Sφ Cφ 0 0⎥⎥ ⎢⎢ 0 1 0 0⎥⎥ ⎢ 0 Cψ − Sψ 0⎥⎥
=⎢ ⎢
⎢0 0 1 0 ⎥ ⎢ − Sθ 0 Cθ 0⎥ ⎢ 0 Sψ Cψ 0⎥
⎢ ⎥⎢ ⎥ ⎢ ⎥
⎣0 0 0 1⎦ ⎣ 0 0 0 1⎦ ⎣0 0 0 1⎦
Z
φ
ψ Y
θ
X
Quiz 2
• Geometric Interpretation?
Universiti Kuala Lumpur Malaysia France Institute
T =⎢ ⎥
frame w.r.t. OXYZ frame
q = (q1 , q2 , L qn ) z
Joint Position and Orientation
variables of the end-effector
Direct Kinematics
Inverse Kinematics
x
• Inverse Kinematics Y = ( x, y, z , φ , θ ,ψ )
Universiti Kuala Lumpur Malaysia France Institute
• Number the joints from 1 to n starting with the base and ending with
the end-effector.
• Establish the base coordinate system. Establish a right-handed
orthonormal coordinate system ( X 0 , Y0 , Z 0 ) at the supporting base
with Z 0 axis lying along the axis of motion of joint 1.
• Establish joint axis. Align the Zi with the axis of motion (rotary or
sliding) of joint i+1.
• Establish the origin of the ith coordinate system. Locate the origin of
the ith coordinate at the intersection of the Zi & Zi-1 or at the
intersection of common normal between the Zi & Zi-1 axes and the Zi
axis.
• Establish Xi axis. Establish X i = ±( Z i −1 × Z i ) / Z i −1 × Z i or along the
common normal between the Zi-1 & Zi axes when they are parallel.
• Establish Yi axis. Assign Yi = +( Z i × X i ) / Z i × X i to complete the
right-handed coordinate system.
• Find the link and joint parameters
Example I
• 3 Revolute Joints
Universiti Kuala Lumpur Malaysia France Institute
Z3
Z0 Z1
Joint 3
Y0 Z O3 X3
Y1 2
Link 1 Link 2 d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2
a0 a1
Link Coordinate Frames
• Assign Link Coordinate Frames:
Universiti Kuala Lumpur Malaysia France Institute
Link 1 Link 2 d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2
a0 a1
Link Coordinate Frames
– Locate the origin of the ith coordinate at the intersection
Universiti Kuala Lumpur Malaysia France Institute
d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2
a0 a1
Link Coordinate Frames
– Assign Yi = +( Z i × X i ) / Z i × X i to complete the right-
Universiti Kuala Lumpur Malaysia France Institute
d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2
a0 a1
Link and Joint Parameters
• Joint angle θ i : the angle of rotation from the Xi-1 axis to
Universiti Kuala Lumpur Malaysia France Institute
the Xi axis about the Zi-1 axis. It is the joint variable if joint i
is rotary.
• Link twist angle α i : the angle of rotation from the Zi-1 axis
to the Zi axis about the Xi axis.
Example I Z3
Z0 Z1
Universiti Kuala Lumpur Malaysia France Institute
Joint 3
Y0 Z O3 X3
Y1 2
d2
Joint 1
O0 X0 O1 X1 O2 X2 D-H Link Parameter Table
Joint 2
Y2 Joint i αi ai di θi
a0 a1 1 0 a0 0 θ0
1 -90 0 13
θ1 θ2 2 θ2 0 8 0
3 θ3 90 0 -l
Z1
4 θ4 -90 0 8
O1 θ3 5 θ5 90 0 0
X1 Z2 Z6
Y1 6 0 0 t θ6
O
Y3 2 Z Z4
O3 X 2 5 θ 6 Y6
Y2
O
θ 5 θ : angle from X to X
Z0 6 i i-1 i
X 3 Y4 Y5 about Zi-1
O5 X5 X6
O4 Z 3 α i : angle from Zi-1 to Zi
about Xi
X4 θ 4 ofaiZ: distance from intersection
& X to O along Xi
i-1 i i
Source coordinate
T i −i 1 = T ( z i − 1 , d i ) R ( z i − 1 , θ i ) T ( x i , a i ) R ( x i , α i )
⎡C θ i − C α iSθ i Sα iSθ i aiC θ i ⎤
Reference ⎢ Sθ C α iC θ i − S α iC θ i a i S θ i ⎥⎥
Coordinate ⎢ i
⎢ 0 Sα i Cα i di ⎥
⎢ ⎥
⎣ 0 0 0 1 ⎦
Kinematic Equations
Universiti Kuala Lumpur Malaysia France Institute
• Other representations
– reference from, tool frame
T tool
ref =B T H 0
ref 0
n tool
n
⎡θ 1 ⎤ ⎡ px ⎤
⎢θ ⎥ ⎢p ⎥ ⎡nx sx ax px ⎤
⎢ 2⎥ ⎢ y⎥ ⎢n
⎢θ 3 ⎥ ⎢ pz ⎥ sy ay p y ⎥⎥
T = ⎢ y
⎢ ⎥⇒ ⎢ ⎥ ⎢nz sz az pz ⎥
⎢θ 4 ⎥ ⎢φ ⎥ ⎢ ⎥
⎢θ 5 ⎥ ⎢θ ⎥ ⎣0 0 0 1 ⎦
⎢ ⎥ ⎢ ⎥
⎢⎣θ 6 ⎥⎦ ⎢⎣ϕ ⎥⎦
Representing forward kinematics
Universiti Kuala Lumpur Malaysia France Institute
⎢n p y ⎥⎥ az
T0n = ⎢ y
sy ay
ψ = cos ( −1
)
⎢ nz sz az pz ⎥ cos θ
⎢ ⎥ nx
⎣0 0 0 1⎦ φ = cos (
−1
)
cos θ
Problem? Solution is inconsistent and ill-conditioned!!
atan2(y,x)
Universiti Kuala Lumpur Malaysia France Institute
T = Rz ,φ R y ,θ Rx ,ψ
⎡Cφ − Sφ 0 0 ⎤ ⎡ Cθ 0 Sθ 0⎤ ⎡1 0 0 0⎤
⎢ Sφ Cφ 0 0⎥⎥ ⎢⎢ 0 1 0 0⎥⎥ ⎢ 0 Cψ − Sψ 0⎥⎥
=⎢ ⎢
⎢0 0 1 0 ⎥ ⎢ − Sθ 0 Cθ 0⎥ ⎢ 0 Sψ Cψ 0⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎣0 0 0 1⎦ ⎣ 0 0 0 1⎦ ⎣0 0 0 1⎦
⎡nx sx ax 0⎤
⎢n sy ay 0 ⎥⎥
= ⎢ y
⎢nz sz az 0⎥
⎢ ⎥
⎣0 0 0 1⎦
Yaw-Pitch-Roll Representation
−1
Rz ,φ T = R y ,θ Rx ,ψ
Universiti Kuala Lumpur Malaysia France Institute
⎡ Cφ Sφ 0 0⎤ ⎡ n x sx ax 0⎤
⎢− Sφ 0 0⎥⎥ ⎢ n y
⎢ Cφ
⎢ sy ay 0 ⎥⎥
⎢ 0 0 1 0⎥ ⎢ n z sz az 0⎥
⎢ ⎥ ⎢ ⎥
⎣ 0 0 0 1⎦ ⎣ 0 0 0 1⎦
(Equation A)
⎡ Cθ 0 Sθ 0⎤ ⎡1 0 0 0⎤
⎢ 0 ⎢ 0 Cψ 0⎥⎥
1 0 0⎥⎥ ⎢ − Sψ
=⎢
⎢− Sθ 0 Cθ 0⎥ ⎢0 Sψ Cψ 0⎥
⎢ ⎥ ⎢ ⎥
⎣ 0 0 0 1⎦ ⎣0 0 0 1⎦
Yaw-Pitch-Roll Representation
Universiti Kuala Lumpur Malaysia France Institute
Z0 Z1
Joint 3
Y0 Z O3 X3
Y1 2
d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2
a0 a1
Joint i αi ai di θi
1 0 a0 0 θ0
2 -90 a1 0 θ1
3 0 0 d2 θ2
Example
Universiti Kuala Lumpur Malaysia France Institute
⎢ 0 0 1 0 ⎥
2 -90 a1 0 θ1 ⎢ ⎥
⎣ 0 0 0 1 ⎦
⎢ 0 Sα i Cα i di ⎥
⎢ ⎥ − sinθ 2
⎣ 0 0 0 1 ⎦ ⎡cosθ 2 00⎤
⎢sin θ cos θ 2 00 ⎥⎥
T 23= ⎢ 2
q = (q1 , q2 , L qn )
y
• Find the joint variables
which can bring the robot
the desired configuration x
Inverse Kinematics
Universiti Kuala Lumpur Malaysia France Institute
• More difficult
– Systematic closed-form (x , y)
l2
solution in general is not
available l2
l1
– Solution not unique
• Redundant robot
l1
• Elbow-up/elbow-down
configuration
– Robot dependent
Inverse Kinematics
Universiti Kuala Lumpur Malaysia France Institute
xz
y
z
x
x
z
y
x
y
Thank you!