[Download] Rev-Chapter-3

Chapter 3 Manipulator Kinematics

1

Chapter Lecture Notes for
Manipulator
Kinematics
A Geometrical Introduction to
Forward
kinematics Robotics and Manipulation
Inverse
Kinematics

Manipulator
Richard Murray and Zexiang Li and Shankar S. Sastry
Jacobian CRC Press
Redundant
Manipulators

Reference
Zexiang Li and Yuanqing Wu

ECE, Hong Kong University of Science & Technology

May ,


2

Chapter Manipulator Kinematics
Chapter
Manipulator
Kinematics
Forward kinematics
Forward
kinematics

Inverse
Kinematics
Inverse Kinematics
Manipulator
Jacobian
Manipulator Jacobian
Redundant
Manipulators

Reference
Redundant Manipulators

Reference


3.1 Forward kinematics
3

Chapter
Manipulator
Kinematics

Forward
kinematics

Inverse (a) Adept Cobra i600 (SCARA) (b) Forward kinematics of SCARA
Kinematics Figure 3.1
Manipulator Lower Pair Joints:
revolute joint S ↦ SO( ) prismatic joint ℝ ↦ T( )
Jacobian

Redundant

R T
Manipulators

Reference
Li Li+ Li Li+

Forward kinematics:

L
joint L
joint n Ln
base: stationary Link : ﬁrst movable link Link n: end-eﬀector


4
◻ Joint space:
Revolute joint: S , θ i ∈ S or θ i ∈ [−π, π]
Chapter Prismatic joint: ℝ
Q ∶ S × ⋯ × S ×ℝ × ⋯ × ℝ
Manipulator
Kinematics Joint space:
Forward
kinematics
no. of R joint no. of P joint

Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


4
◻ Joint space:
Q ∶ S × ⋯ × S ×ℝ × ⋯ × ℝ
Manipulator
Forward
kinematics

Q ∶S ×S ×S ×ℝ
Inverse
Adept
Q = Γ ∶S ×⋯×S
Kinematics

Manipulator Elbow
Jacobian

Redundant
Manipulators

Reference


4
◻ Joint space:
Q ∶ S × ⋯ × S ×ℝ × ⋯ × ℝ
Manipulator
Forward
kinematics

Q ∶S ×S ×S ×ℝ
Inverse
Adept
Q = Γ ∶S ×⋯×S
Kinematics

Manipulator Elbow
Jacobian

Redundant

θ = ( , ,..., ) ∈ Q
Manipulators
Reference (nominal) joint config:
gst ( ) ∈ SE( )
Reference
Reference (nominal) end-effector config:


4
◻ Joint space:
Q ∶ S × ⋯ × S ×ℝ × ⋯ × ℝ
Manipulator
Forward
kinematics

Q ∶S ×S ×S ×ℝ
Inverse
Adept
Q = Γ ∶S ×⋯×S
Kinematics

Manipulator Elbow
Jacobian

Redundant

θ = ( , ,..., ) ∈ Q
Manipulators
Reference (nominal) joint config:
gst ( ) ∈ SE( )
Reference
Reference (nominal) end-effector config:

Arbitrary configuration gst (θ):

gst ∶ θ ∈ Q ↦ gst (θ) ∈ SE( )


5
◻ Classical Approach:
Chapter
gst (θ , θ ) = gst (θ ) ⋅ gl l ⋅ gl t
Manipulator
Kinematics
Disadvantage: A coordinate frame needed for each link
Forward
kinematics

Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


5
◻ Classical Approach:
Chapter
gst (θ , θ ) = gst (θ ) ⋅ gl l ⋅ gl t
Manipulator
Kinematics
Disadvantage: A coordinate frame needed for each link
◻ The product of exponentials formula:
Forward
kinematics

Inverse
Kinematics
Consider Fig 3.2.
Manipulator
Jacobian
θ2
Redundant
Manipulators θ1

Reference L2 T

L1

S

Figure 3.2: A two degree of freedom manipulator
(Continues next slide)


6

Step 1: Rotating about ω by θ
ξ = −ωω× q
Chapter
Manipulator
Kinematics

gst (θ ) = e ξ θ ⋅ gst ( )
ˆ
Forward
kinematics

Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


6

ξ = −ωω× q
Chapter
Manipulator
Kinematics

gst (θ ) = e ξ θ ⋅ gst ( )
ˆ
Forward
kinematics

Inverse Step 2: Rotating about ω by θ
ξ = −ωω× q
Kinematics

gst (θ , θ ) = e ξ θ ⋅ e ξ θ ⋅ gst ( )
Manipulator
Jacobian ˆ ˆ
Redundant
Manipulators
o set
Reference θ ∶ ( , ) ↦ ( , θ ) ↦ (θ , θ )



7

What if another route is taken?
Chapter

θ ∶ ( , ) ↦ (θ , ) ↦ (θ , θ )
Manipulator
Kinematics

Forward
kinematics

Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


7

What if another route is taken?
Chapter

θ ∶ ( , ) ↦ (θ , ) ↦ (θ , θ )
Manipulator
Kinematics

Forward
kinematics
ξ = −ωω× q
Inverse
Kinematics

gst (θ ) = e ξ θ ⋅ gst ( )
Manipulator ˆ
Jacobian

Redundant
Manipulators

Reference
Step 2: Rotating about ω′ by θ
Let e ξ θ = R p
ˆ

ω′ = R ⋅ ω
q′ = p + R ⋅ q


8

−ω′ × q′ −R ω RT (p + R q )
ξ′ = = ˆ
Chapter
Manipulator
Kinematics ω′ Rω
Forward
= R pRˆ −ω × q = Ad ˆ ⋅ ξ ⇒ ξ ′ = eξ θ ⋅ ξ ⋅ e− ξ θ
ˆ ˆ ˆ ˆ
kinematics
R ω eξ θ
Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


8

−ω′ × q′ −R ω RT (p + R q )
ξ′ = = ˆ
Chapter
Manipulator
Kinematics ω′ Rω
Forward
= R pRˆ −ω × q = Ad ˆ ⋅ ξ ⇒ ξ ′ = eξ θ ⋅ ξ ⋅ e− ξ θ
ˆ ˆ ˆ ˆ
kinematics
R ω eξ θ
Inverse
Kinematics

Manipulator

gst (θ , θ ) = eξ ⋅ eξ θ ⋅ gst ( )
Jacobian ˆ′ θ ˆ
Redundant

= ee ⋅ eξ θ ⋅ gst ( )
ˆ ˆ θ
Manipulators ξ θ ⋅ ξ θ ⋅e− ξ
ˆ ˆ
Reference

= eξ θ ⋅ eξ ⋅ e− ξ θ ⋅ eξ θ ⋅ gst ( )
ˆ ˆθ ˆ ˆ

= eξ θ ⋅ eξ ⋅ gst ( )
ˆ ˆθ

Independent of the route taken


9
◻ Procedure for forward kinematic map:
Chapter Identify a nominal conﬁguration:
Manipulator

Θ = (θ , . . . , θ n ) = , gst ( ) ≜ gst (θ , . . . , θ n )
Kinematics

Forward
kinematics

Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


9
Manipulator

Θ = (θ , . . . , θ n ) = , gst ( ) ≜ gst (θ , . . . , θ n )
Kinematics

Forward
kinematics

Inverse
Kinematics
Simpliﬁcation of forward kinematics mapping:
ξi = −ωωi qi
Manipulator ×
Jacobian Revolute joint:
Redundant
Manipulators
Choose qi s.t. ξi is simple.
vi
ξi =
Reference
Prismatic joint:


9
Manipulator

Θ = (θ , . . . , θ n ) = , gst ( ) ≜ gst (θ , . . . , θ n )
Kinematics

Forward
kinematics

Inverse
Kinematics
Simpliﬁcation of forward kinematics mapping:
ξi = −ωωi qi
Manipulator ×
Jacobian Revolute joint:
Redundant
Manipulators
Choose qi s.t. ξi is simple.
vi
ξi =
Reference
Prismatic joint:

Write gst (θ) = e ξ θ . . . e ξn θ n ⋅ gst ( ) (product of exponential
ˆ ˆ

mapping)


10
Example: SCARA manipulator
θ3

⎡ ⎤
⎢ ⎥
Chapter θ2

gst ( ) = ⎢ I l +l ⎥
Manipulator θ1 l2

⎢ ⎥
θ4
Kinematics

⎢ ⎥
l1

l
⎣ ⎦
Forward
kinematics z T

ω =ω =ω =
l0
q1 S q2 q3
Inverse y

Kinematics x

Manipulator
Jacobian
Figure 3.3
Redundant
Manipulators

Reference


10
θ3

⎡ ⎤
⎢ ⎥
Chapter θ2

gst ( ) = ⎢ I l +l ⎥
Manipulator θ1 l2

⎢ ⎥
θ4
Kinematics

⎢ ⎥
l1

l
⎣ ⎦
Forward
kinematics z T

ω =ω =ω =
l0
q1 S q2 q3
Inverse y

Kinematics x

Manipulator
Jacobian
Figure 3.3

q = ,q = ,q = l +l
Redundant
Manipulators l
Reference


10
θ3

⎡ ⎤
⎢ ⎥
Chapter θ2

gst ( ) = ⎢ I l +l ⎥
Manipulator θ1 l2

⎢ ⎥
θ4
Kinematics

⎢ ⎥
l1

l
⎣ ⎦
Forward
kinematics z T

ω =ω =ω =
l0
q1 S q2 q3
Inverse y

Kinematics x

Manipulator
Jacobian
Figure 3.3

q = ,q = ,q = l +l
Redundant
Manipulators l
Reference

⎡ ⎤ ⎡ l ⎤ ⎡ l +l ⎤ ⎡ ⎤
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⇒ξ =⎢ ⎥, ξ = ⎢ ⎥, ξ = ⎢ ⎥, ξ = ⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦


11

gst (θ) = eξ θ ⋅ eξ ⋅ eξ ⋅ eξ ⋅ gst ( ) = R(θ) p(θ)
ˆ ˆ θ ˆθ ˆ θ
Chapter
Manipulator
Kinematics

Forward
kinematics

Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


11

gst (θ) = eξ θ ⋅ eξ ⋅ eξ ⋅ eξ ⋅ gst ( ) = R(θ) p(θ)
ˆ ˆ θ ˆθ ˆ θ
Chapter

⎡ c −s ⎤ ⎡ ls ⎤
⎥ ξ θ ⎢ c −s
Manipulator

⎢ l s ⎥,
=⎢ s ⎥, e ˆ = ⎢ s ⎥
Kinematics

⎢ ⎥ ⎢ ⎥
e
ˆ
ξθ c c
⎢ ⎥ ⎢ ⎥
Forward

⎣ ⎦ ⎣ ⎦
kinematics

⎡ c −s (l s )s ⎤ ⎡ ⎤
⎢ ⎥ ˆ
Inverse

⎢ (l s )v ⎥, eξ θ = ⎢ I ⎥
Kinematics

=⎢ s ⎥ ⎢ ⎥
⎢ ⎥
ˆ c
⎢ ⎥
ξ θ
e
⎢ ⎥
Manipulator

⎢ ⎥
θ
⎣ ⎦
Jacobian

Redundant ⎣ ⎦
Manipulators

Reference


11

gst (θ) = eξ θ ⋅ eξ θ ⋅ eξ θ ⋅ eξ θ ⋅ gst ( ) = R(θ) p(θ)
ˆ ˆ ˆ ˆ
Chapter

⎡ c −s ⎤ ⎡ ls ⎤
⎥ ξ θ ⎢ c −s
Manipulator

⎢ s l s ⎥,
⎢ ⎥, e ˆ = ⎢ s ⎥
Kinematics

=⎢ c
⎥ ⎢
c
⎥
ˆ
ξθ
e
⎢ ⎥ ⎢ ⎥
Forward

⎣ ⎦ ⎣ ⎦
kinematics

⎡ c −s (l s )s ⎤ ⎡ ⎤
⎢ ⎥ ˆ
Inverse

⎢ s (l s )v ⎥, eξ θ = ⎢ I ⎥
Kinematics

=⎢ ⎥ ⎢ ⎥
⎢ ⎥
ˆ c
⎢ ⎥
ξ θ
e
⎢ ⎥
Manipulator

⎢ ⎥
θ
⎣ ⎦
Jacobian

⎣ ⎦
⎡ −s −l s − l s ⎤
Redundant

⎢ c ⎥
⎢ l c +l c ⎥
Manipulators

gst (θ) = ⎢ s c ⎥
⎢ l +θ ⎥
Reference

⎢ ⎥
⎣ ⎦
in which, c = cos(θ + θ + θ ) and c = cos(θ + θ ).


12
Example: Elbow manipulator
Chapter
Manipulator ξ1 ξ4
ξ2 ξ3 ξ5

⎡ ⎤
Kinematics

⎢ l +l ⎥
l1 l2

gst ( ) = ⎢ I ⎥
Forward q1 q2 ξ6

⎢ ⎥
kinematics T

⎢ l ⎥
⎣ ⎦
Inverse
Kinematics l0

Manipulator S
Jacobian

Redundant
Figure 3.4
⎡ ⎤ ⎡ ⎤
Manipulators

⎢ − × ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
Reference

⎢ ⎥ ⎢ ⎥
ξ =⎢ l ⎥=⎢ ⎥,
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦


13
⎡ − ⎤ ⎡ ⎤ ⎡ ⎤
⎢ − × ⎥ ⎢ −l ⎥ ⎢ −l ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢ l ⎥
ξ =⎢ −
l ⎥ = ⎢ − ⎥, ξ = ⎢ − ⎥,
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
Chapter

⎢ ⎥ ⎢ ⎥ ⎢ ⎥
Manipulator

⎣ ⎦ ⎣ ⎦ ⎣ ⎦
Kinematics

⎡ l +l ⎤ ⎡ ⎤ ⎡ −l ⎤
⎢ ⎥ ⎢ −l ⎥ ⎢ ⎥
Forward

⎢ ⎥ ⎢ ⎥ ⎢ ⎥
kinematics

⎢ ⎥ ⎢ l +l ⎥ ⎢ ⎥
ξ =⎢ ⎥, ξ = ⎢ − ⎥, ξ = ⎢ ⎥
Inverse

⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
Kinematics

⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦ ⎣ ⎦
Manipulator
Jacobian

Redundant

→ gst (θ , . . . θ ) = eξ θ . . . eξ ⋅ gst ( ) =
Manipulators
ˆ ˆ θ R(θ) p(θ)
Reference

⎡ −s (l c + l c ) ⎤
⎢ ⎥
p(θ) = ⎢ c (l c + l c ) ⎥ , R(θ) = [rij ]
⎢ l −l s −l s ⎥
⎣ ⎦



14

in which, r = c (c c − s c s ) + s (s s c + s c c s + c s s )
Chapter r = −c (s c c + c s ) + s s s
r = c (−c s s − (c c s + c s )s ) + (c c − c s s )s
Manipulator
Kinematics

r = c (c s + c c s ) − (c c s + (c c c − s s )s )s
Forward
kinematics

r = c (c c c − s s ) − c s s
Inverse
Kinematics

Manipulator
Jacobian
r = c (c c s + (c c c − s s )s ) + (c s + c c s )s
Redundant r = −(c s s ) − (c c − c s s )s
r = −(c c s ) − c s
Manipulators

Reference

r = c (c c − c s s ) − s s s


14

in which, r = c (c c − s c s ) + s (s s c + s c c s + c s s )
Chapter r = −c (s c c + c s ) + s s s
r = c (−c s s − (c c s + c s )s ) + (c c − c s s )s
Manipulator
Kinematics

r = c (c s + c c s ) − (c c s + (c c c − s s )s )s
Forward
kinematics

r = c (c c c − s s ) − c s s
Inverse
Kinematics

Manipulator
Jacobian
r = c (c c s + (c c c − s s )s ) + (c s + c c s )s
Redundant r = −(c s s ) − (c c − c s s )s
r = −(c c s ) − c s
Manipulators

Reference

r = c (c c − c s s ) − s s s

Simplify forward Kinematics Map:
Choose base frame or ref. Conﬁg. s.t. gst ( ) = I


15

◻ Manipulator Workspace:
Chapter
Manipulator
W = {gst (θ) ∀θ ∈ Q} ⊂ SE( )
Kinematics

Forward
Reachable Workspace:
kinematics

Inverse
Kinematics
WR = {p(θ) ∀θ ∈ Q} ⊂ ℝ
Manipulator
Jacobian

Redundant
Manipulators

Reference


15

◻ Manipulator Workspace:
Chapter
Manipulator
W = {gst (θ) ∀θ ∈ Q} ⊂ SE( )
Kinematics

Forward
Reachable Workspace:
kinematics

Inverse
Kinematics
WR = {p(θ) ∀θ ∈ Q} ⊂ ℝ
Manipulator
Jacobian

Redundant Dextrous Workspace:
Manipulators

WD = {p ∈ ℝ ∀R ∈ SO( ), ∃θ, gst (θ) = (p, R)}
Reference


16
Example: A planar serial 3-bar linkage
Chapter
(a) Workspace calculation: θ3

g = (x, y, ϕ)
θ2
Manipulator l2
l3

x =l c +l c +l c
Kinematics

y =l s +l s +l s
θ1
l1
Forward
kinematics

ϕ=θ +θ +θ
(a) (b)
Inverse
Kinematics
(b) Construction of Workspace:
Manipulator
Jacobian (c) Reachable Workspace: l1 l2 l3
2l3 2l3

Redundant (d) Dextrous Workspace:
Manipulators
(c) (d)

Reference
Figure 3.5


16
Example: A planar serial 3-bar linkage
Chapter
(a) Workspace calculation: θ3

g = (x, y, ϕ)
θ2
Manipulator l2
l3

x =l c +l c +l c
Kinematics

y =l s +l s +l s
θ1
l1
Forward
kinematics

ϕ=θ +θ +θ
(a) (b)
Inverse
Kinematics
(b) Construction of Workspace:
Manipulator
Jacobian (c) Reachable Workspace: l1 l2 l3
2l3 2l3

Redundant (d) Dextrous Workspace:
Manipulators
(c) (d)

Reference
Figure 3.5

◻ R manipulator with maximum
workspace (Paden):
Elbow manipulator and its kinematics inverse.
† End of Section †


3.2 Inverse Kinematics
17
Deﬁnition: Inverse kinematics
Given g ∈ SE( ), ﬁnd θ ∈ Q s.t.
gst (θ) = g, where gst ∶ Q ↦ SE( )
Chapter
Manipulator
Kinematics

Forward
kinematics

Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


17
Deﬁnition: Inverse kinematics
Given g ∈ SE( ), ﬁnd θ ∈ Q s.t.
gst (θ) = g, where gst ∶ Q ↦ SE( )
Chapter
Manipulator
Kinematics

Forward
kinematics
Example: A planar example
Inverse
Kinematics (x, y) (x, y)

Manipulator l2
Jacobian θ2
θ2 r
Redundant
y α
Manipulators l1 β
Reference θ1 φ θ1
B
x

(a) (b)
Figure 3.6
x = l cos θ + l cos (θ + θ )
y = l sin θ + l sin (θ + θ )
Given (x, y), solve for (θ , θ ).


18
Review:
Chapter
Polar Coordinates:
(r, ϕ), r = x +y
Manipulator
Kinematics

Forward
Law of cosines:
θ = π ± α, α = cos−
kinematics
l +l −r
Inverse

Flip solution: π + α
Kinematics
ll

Manipulator

r +l −l
Jacobian

Redundant
Manipulators θ = atan (y, x) ± β, β = cos−
lr
Reference


18
Review:
Chapter
Polar Coordinates:
(r, ϕ), r = x +y
Manipulator
Kinematics

Forward
Law of cosines:
θ = π ± α, α = cos−
kinematics
l +l −r
Inverse

Flip solution: π + α
Kinematics
ll

Manipulator

r +l −l
Jacobian

Redundant
Manipulators θ = atan (y, x) ± β, β = cos−
lr
Reference

Hight Lights:
Subproblems
Each has zero, one or two solutions!


19
◻ Paden-Kahan Subproblems:
Chapter Subproblem 1: Rotation about a single axis
Manipulator
Kinematics
Let ξ be a zero-pitch twist, with unit magnitude and two points
p, q ∈ ℝ . Find θ s.t. eξθ p = q
Forward
kinematics
ˆ

Inverse
Kinematics

Manipulator p
Jacobian
u
Redundant u′
Manipulators r θ ξ θ
Reference v′
v
q

(a) (b)
Figure 3.6


19
◻ Paden-Kahan Subproblems:
Chapter Subproblem 1: Rotation about a single axis
Manipulator
Kinematics
Let ξ be a zero-pitch twist, with unit magnitude and two points
p, q ∈ ℝ . Find θ s.t. eξθ p = q
Forward
kinematics
ˆ

Inverse
Kinematics

Manipulator p
Jacobian
u
Redundant u′
Manipulators r θ ξ θ
Reference v′
v
q

(a) (b)
Figure 3.6
Solution: Let r ∈ l ξ , deﬁne u = p − r, v = q − r, eξθ r = r
ˆ



20
Moreover,

Chapter
Manipulator ⇒ eξθ p = q ⇒ eξθ (p − r) = q − r ⇒
ˆ ˆ eωθ
ˆ
∗ u
=
v
Kinematics
u v
w u=w v
Forward

⇒ eωθ u = v
kinematics T T
ˆ
Inverse
Kinematics
u = v
Manipulator
Jacobian
p
Redundant
Manipulators
u
u′
Reference r θ ξ θ
v′
v
q

(a) (b)
Figure 3.6


21
u′ = (I − ωωT )u, v′ = (I − ωωT )v

u′ = v′
Chapter
Manipulator

ωT u = ωT v
Kinematics The solution exists only if
Forward
kinematics

Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


21
u′ = (I − ωωT )u, v′ = (I − ωωT )v

u′ = v′
Chapter
Manipulator

ωT u = ωT v
Forward
kinematics

If u′ ≠ , then
Inverse
Kinematics

Manipulator

u′ × v′ = ω sin θ u′ v′
Jacobian

Redundant
Manipulators
u′ ⋅ v′ = cos θ u′ v′
Reference

⇒ θ = atan (ωT (u′ × v′ ), u′T v′ )


21
u′ = (I − ωωT )u, v′ = (I − ωωT )v

u′ = v′
Chapter
Manipulator

ωT u = ωT v
Forward
kinematics

If u′ ≠ , then
Inverse
Kinematics

Manipulator

u′ × v′ = ω sin θ u′ v′
Jacobian

Redundant
Manipulators
u′ ⋅ v′ = cos θ u′ v′
Reference

⇒ θ = atan (ωT (u′ × v′ ), u′T v′ )

If u′ = , ⇒ Inﬁnite number of solutions!


22
Subproblem 2: Rotation about two subsequent axes
Chapter Let ξ and ξ be two zero-pitch, unit magnitude twists, with
intersecting axes, and p, q ∈ R . ﬁnd θ and θ s.t.
Manipulator
Kinematics

eξ θ eξ θ p = q.
Forward ˆ ˆ
kinematics

Inverse
Kinematics p
ξ2
Manipulator
Jacobian θ2
Redundant
Manipulators
c
Reference

r θ1
ξ1

q

Figure 3.6


23
Solution: If two axes of ξ and ξ coincide, then we get:
Subproblem 1: θ + θ = θ
Chapter
Manipulator
Kinematics

Forward
kinematics

Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


23
Chapter
Manipulator
If the two axes are not parallel, ω × ω ≠ , then, let c satisfy:
p = c = e− ξ θ q
Kinematics ˆθ ˆ
Forward eξ
kinematics

Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


23
Chapter
Manipulator
p = c = e− ξ θ q
Kinematics ˆθ ˆ
Forward eξ
Set r ∈ l ξ ∩ l ξ
kinematics

Inverse

p − r = c − r = e− ξ θ (q − r), ⇒ eω u = z = e−ω θ v
Kinematics
ˆθ ˆ
Manipulator eξ ˆ θ ˆ
Jacobian
u z v
ω u=ω z
Redundant

= z = v
Manipulators T T

ωT v = ωT z
Reference ⇒ , u


23
Chapter
Manipulator
p = c = e− ξ θ q
Kinematics ˆθ ˆ
Forward eξ
Set r ∈ l ξ ∩ l ξ
kinematics

Inverse

p − r = c − r = e− ξ θ (q − r), ⇒ eω u = z = e−ω θ v
Kinematics
ˆθ ˆ
Manipulator eξ ˆ θ ˆ
Jacobian
u z v
ω u=ω z
Redundant

= z = v
Manipulators T T

ωT v = ωT z
Reference ⇒ , u

As ω , ω and ω × ω are linearly independent,
z = αω + βω + γ(ω × ω )
⇒ z = α + β + αβωT ω + γ ω × ω


24
⎧
⎪α
⎪ =
(ωT ω )ωT u−ωT v
ωT u = αωT ω + β ⎪
⇒⎨ (ωT ω ) −
ω v = α + βω ω ⎪β
⎪ =
⎪
Chapter T T (ωT ω )ωT v−ωT u
⎩
Manipulator
Kinematics (ωT ω ) −
Forward
kinematics

Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


24
⎧
⎪α
⎪ =
⇒⎨ (ωT ω ) −
⎪ =
⎪
⎩
Manipulator

− α − β − αβωT ω
Forward

= u ⇒γ = (∗)
kinematics u
ω ×ω
Inverse
z
Kinematics

Manipulator (∗) has zero, one or two solution(s):
Jacobian

eξ θ p = c
Redundant ˆ
Manipulators
Given z ⇒ c ⇒
eξ θ p = c
ˆ
Reference

for θ and θ
Two solutions when the two circles intersect.


24
⎧
⎪α
⎪ =
⇒⎨ (ωT ω ) −
⎪ =
⎪
⎩
Manipulator

Forward

= u ⇒γ = (∗)
kinematics u
ω ×ω
Inverse
z
Kinematics

Jacobian

eξ θ p = c
Redundant ˆ
Manipulators
Given z ⇒ c ⇒
eξ θ p = c
ˆ
Reference

for θ and θ
One solution when they are tangent


24
⎧
⎪α
⎪ =
⇒⎨ (ωT ω ) −
⎪ =
⎪
⎩
Manipulator

Forward

= u ⇒γ = (∗)
kinematics u
ω ×ω
Inverse
z
Kinematics

Jacobian

eξ θ p = c
Redundant ˆ
Manipulators
Given z ⇒ c ⇒
eξ θ p = c
ˆ
Reference

for θ and θ
One solution when they are tangent
Zero solution when they do not intersect


25
Subproblem 3: Rotation to a given point
Chapter Given a zero-pitch twist ξ, with unit magnitude and p, q ∈ ℝ ,
ﬁnd θ s.t. q − eξθ p = δ
Manipulator
Kinematics
ˆ

Forward
kinematics

Inverse
Kinematics
p
Manipulator u u′
Jacobian r θ ξ θ0 θ
Redundant
Manipulators ωθ ′
e u
Reference v v′
δ δ′

q
ω T (p−q)

Figure 3.7


25
Subproblem 3: Rotation to a given point
Chapter Given a zero-pitch twist ξ, with unit magnitude and p, q ∈ ℝ ,
ﬁnd θ s.t. q − eξθ p = δ
Manipulator
Kinematics
ˆ

Forward

Deﬁne: u = p − r, v = q − r, v − eωθ u
ˆ
=δ
kinematics

Inverse
Kinematics
p

u = u − ωω u
′
Manipulator u u′
T
Jacobian r θ

v′ = v − ωωT v
ξ θ0 θ
Redundant
Manipulators ωθ ′
e u
Reference v v′
δ δ′

q
ω T (p−q)

Figure 3.7
⇒ u = u′ + ωωT u, v = v′ + ωωT v, δ ′ = δ − ωT (p − q)


26

Chapter
Manipulator (v′ + ωωT v) − eωθ (u′ + ωωT u)
ˆ
=δ ⇒
v − e u + ωωT (v − u) =δ
Kinematics
′ ωθ ′
ˆ
Forward
kinematics

Inverse ωωT (q−p)
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


26

Chapter
Manipulator (v′ + ωωT v) − eωθ (u′ + ωωT u)
ˆ
=δ ⇒
v − e u + ωωT (v − u) =δ
Kinematics
′ ωθ ′
ˆ
Forward
kinematics

Inverse ωωT (q−p)
Kinematics

Manipulator

v′ − eωθ u′ = δ − ωT (p − q) = δ′ ,
Jacobian ˆ
Redundant

θ = atan (ωT (u′ × v′ ), u′T v′ ),
Manipulators

ϕ = θ − θ ⇒ u′ + v′ − u′ ⋅ v′ cos ϕ = δ ′ ,
Reference

u′ + v′ − δ ′
θ = θ ± cos− (∗)
u′ ⋅ v′
Zero, one or two solutions!


27
◻ Solving inverse kinematics using
Chapter
subproblems:
Manipulator
Kinematics
Technique 1: Eliminate the dependence on a joint
eξθ p = p, if p ∈ l ξ . Given eξ θ eξ θ eξ θ = g, select p ∈ l ξ , p ∉ l ξ
Forward ˆ ˆ ˆ ˆ
kinematics

Inverse or l ξ , then:
gp = eξ θ eξ
Kinematics ˆ ˆθ
Manipulator p
Jacobian

Redundant
Manipulators

Reference


27
◻ Solving inverse kinematics using
Chapter
subproblems:
Manipulator
Kinematics
Technique 1: Eliminate the dependence on a joint
eξθ p = p, if p ∈ l ξ . Given eξ θ eξ θ eξ θ = g, select p ∈ l ξ , p ∉ l ξ
Forward ˆ ˆ ˆ ˆ
kinematics

Inverse or l ξ , then:
gp = eξ θ eξ
Kinematics ˆ ˆθ
Manipulator p
Jacobian

Redundant
Manipulators
Technique 2: subtract a common point
Reference

= g, q ∈ l ξ ∩ l ξ ⇒ eξ θ eξ p − q = gp − q ⇒
ˆ ˆθ ˆθ ˆ ˆθ ˆθ
eξ θ eξ eξ ˆ ˆ eξ

p − q = gp − q
ˆθ
eξ


28
Example: Elbow manipulator
Chapter
Manipulator
ξ1 ξ4
Kinematics ξ2 ξ3 ξ5
Forward
l1 l2
kinematics pb ξ6
qw
Inverse
Kinematics

Manipulator l0
Jacobian
S
Redundant
Manipulators

Reference Figure 3.7

gst (θ) = eξ θ eξ gst ( ) = gd
ˆ ˆθ ˆθ ˆ θ ˆθ ˆ θ
eξ eξ eξ eξ

= gd ⋅ gst ( ) = g
−
ˆ ˆ θ ˆθ ˆ θ ˆθ ˆ θ
⇒ eξ θ eξ eξ eξ eξ eξ



29

Step 1: Solve for θ

qω = g ⋅ qω
Chapter ˆ ˆ θ
Manipulator Let eξ θ . . . eξ
Kinematics

qω = g ⋅ qω
Forward ˆ ˆθ ˆθ
kinematics ⇒ eξ θ eξ eξ
Inverse
Kinematics Subtract pb from g qω :
Manipulator

(eξ qω − pb ) = g qω − pb
Jacobian ˆ ˆθ ˆθ
Redundant
eξ θ eξ
qω − pb ≜ δ ← Subproblem 3
Manipulators
ˆθ
Reference ⇒ eξ


29

Step 1: Solve for θ

qω = g ⋅ qω
Chapter ˆ ˆ θ
Manipulator Let eξ θ . . . eξ
Kinematics

qω = g ⋅ qω
Forward ˆ ˆθ ˆθ
kinematics ⇒ eξ θ eξ eξ
Inverse
Kinematics Subtract pb from g qω :
Manipulator

(eξ qω − pb ) = g qω − pb
Jacobian ˆ ˆθ ˆθ
Redundant
eξ θ eξ
qω − pb ≜ δ ← Subproblem 3
Manipulators
ˆθ
Reference ⇒ eξ

Step 2: Given θ , solve for θ , θ

(eξ qω ) = g qω , Subproblem 2 ⇒ θ , θ
ˆ ˆθ ˆθ
eξ θ eξ


30

Step 3: Given θ , θ , θ , solve θ , θ

= e− ξ
Chapter
e− ξ e− ξ θ g
ˆ θ ˆθ ˆ θ ˆθ ˆθ ˆ
Manipulator
Kinematics
eξ eξ eξ
Forward g

let p ∈ l ξ , p ∉ l ξ or l ξ , e eξ p = g p,
kinematics
ˆ
ξ θ ˆθ
Inverse
Kinematics
Subproblem 2 ⇒ θ and θ .
Manipulator
Jacobian

Redundant
Manipulators

Reference


30

Step 3: Given θ , θ , θ , solve θ , θ

= e− ξ
Chapter
e− ξ e− ξ θ g
ˆ θ ˆθ ˆ θ ˆθ ˆθ ˆ
Manipulator
Kinematics
eξ eξ eξ
Forward g

let p ∈ l ξ , p ∉ l ξ or l ξ , e eξ p = g p,
kinematics
ˆ
ξ θ ˆθ
Inverse
Kinematics
Subproblem 2 ⇒ θ and θ .
Step 4: Given (θ , . . . , θ ), solve for θ
Manipulator
Jacobian

Redundant

eξ θ = (eξ θ . . . eξ θ )− ⋅ g ≜ g
Manipulators ˆ ˆ ˆ

Let p ∉ l ξ ⇒ eξ θ p = g ⋅ p = q ⇐ Subproblem 1
Reference
ˆ

Maximum of solutions:


31
Example: Inverse Kinematics of SCARA
⎡ ⎤
θ3

⎢ l +l ⎥
Chapter

gst ( ) = ⎢ ⎥
θ2

⎢ ⎥
Manipulator

⎢ ⎥
Kinematics l2
l
⎣ ⎦
θ1
θ4
Forward l1

gst (θ) = e ξ θ . . . e ξ θ gst ( )
kinematics
ˆ ˆ

⎡ cϕ −sϕ ⎤
Inverse
z

⎢ ⎥
Kinematics l0 T
x
⎢ ⎥
q1 S q2 q3

= ⎢ sϕ cϕ ⎥ ≜ gd
y
Manipulator y
⎢ ⎥
x

⎢ ⎥
Jacobian
z
⎣ ⎦
Redundant
Figure 3.8
Manipulators

⎡ ⎤ −l s − l s
⎢ ⎥
Reference

p=⎢ ⎥ ⇒ p(θ) =
x
⎢ ⎥ l c +l c = y ⇒θ =z−l
⎢ ⎥ l +θ z
⎣ ⎦
= gd gst ( )e− ξ ≜g
ˆ ˆ θ ˆ θ ˆ θ
eξ θ eξ eξ −



32

Let p ∈ l ξ , q ∈ l ξ ⇒ e ξ θ e ξ p = g p,
ˆ ˆ θ

Chapter

e ξ θ (e ξ p − q) = g p − q ,
Manipulator ˆ ˆ θ
Kinematics

p − q = δ ← Subproblem 3 to get θ
Forward ˆ
ξ θ
kinematics e
Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


32

Let p ∈ l ξ , q ∈ l ξ ⇒ e ξ θ e ξ p = g p,
ˆ ˆ θ

Chapter

e ξ θ (e ξ p − q) = g p − q ,
Manipulator ˆ ˆ θ
Kinematics

p − q = δ ← Subproblem 3 to get θ
Forward ˆ
ξ θ
kinematics e
Inverse

⇒ e ξ θ (e ξ p) = g p ⇒ θ ← Subproblem 1 to get θ
Kinematics ˆ ˆ θ
Manipulator

= e− ξ e− ξ θ gd gst ( )e− ξ ≜g
Jacobian ˆ ˆ θ ˆ ˆ θ
ξ θ −
Redundant
⇒e
p = g p, p ∉ l ξ ← Subproblem 1 to get θ
Manipulators ˆ
ξ θ
e
Reference
There are a maximum of two solutions!



33
Example: ABB IRB4400
θ
x
Chapter θ
l l
Manipulator z
Kinematics l T
θ θ y
Forward
kinematics z y
l
Inverse l
S θ
Kinematics θ q
x
Manipulator
Jacobian

Redundant
Manipulators

Reference
−
ω = , ω = −ω = −ω = ,ω = ω =

l l l +l
q = ,q = ,q = , pw ∶= q = q = q =
l l +l


34
⎡ l +l +l ⎤
⎢ − ⎥ qi × ω i
gst ( ) = ⎢
⎢ l +l
⎥ , ξi =
⎥
⎢ ⎥
ωi
Chapter ⎣ ⎦
gst (θ) = e ξ θ e ξ gst ( ) ∶= gd
Manipulator
ˆ ˆ θ ˆ θ ˆ θ ˆ θ ˆ θ
Kinematics
eξ eξ eξ eξ
Forward
kinematics

Inverse
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


34
⎡ l +l +l ⎤
⎢ − ⎥ qi × ω i
gst ( ) = ⎢
⎢ l +l
⎥ , ξi =
⎥
⎢ ⎥
ωi
Chapter ⎣ ⎦
Manipulator
Kinematics
eξ eξ eξ eξ
Forward

pw = gd pw =∶ q ⇒ e ξ pw = e− ξ θ q
kinematics
ˆ ˆ θ ˆ θ ˆ θ ˆ θ ˆ
Inverse eξ θ eξ eξ eξ
Kinematics qx
⇒ =[ ] ⋅ e− ξ θ q = cos θ qy − sin θ qx , q =
ˆ
Manipulator qy
Jacobian qz
Redundant ⇒ θ = tan− (qy qx )
Manipulators

Reference


34
⎡ l +l +l ⎤
⎢ − ⎥ qi × ω i
gst ( ) = ⎢
⎢ l +l
⎥ , ξi =
⎥
⎢ ⎥
ωi
Chapter ⎣ ⎦
Manipulator
Kinematics
eξ eξ eξ eξ
Forward

kinematics
Kinematics qx
ˆ
Manipulator qy
Jacobian qz
Manipulators

pw − q = e− ξ θ q − q =∶ δ ← Subproblem 3 to get θ
ˆ θ ˆ
Reference
eξ
(e pw ) = e
ˆ
ξ θ ˆ
ξ θ ˆ
−ξ θ
e q ← Subproblem 1 to get θ


34
⎡ l +l +l ⎤
⎢ − ⎥ qi × ω i
gst ( ) = ⎢
⎢ l +l
⎥ , ξi =
⎥
⎢ ⎥
ωi
Chapter ⎣ ⎦
Manipulator
Kinematics
eξ eξ eξ eξ
Forward

kinematics
Kinematics qx
ˆ
Manipulator qy
Jacobian qz
Manipulators

pw − q = e− ξ θ q − q =∶ δ ← Subproblem 3 to get θ
ˆ θ ˆ
Reference
eξ
(e pw ) = e
ˆ
ξ θ ˆ
ξ θ ˆ
−ξ θ
e q ← Subproblem 1 to get θ

= e− ξ e− ξ θ gd gst ( ) =∶ g
ˆ θ ˆ θ ˆ θ ˆ θ ˆ θ ˆ
eξ eξ eξ e− ξ −

Use subproblem 1,2 to solve for θ , θ , θ


3.3 Manipulator Jacobian
35
Given gst ∶ Q → SE( ),
θ(t) = (θ (t) . . . θ n (t))T → eξ θ . . . eξn θ n gst ( )
ˆ ˆ
Chapter

and θ(t) = (θ (t) . . . θ n (t))T ,
Manipulator
Kinematics ˙ ˙ ˙
Forward
kinematics

Inverse
What is the velocity of the tool frame?
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


35
Given gst ∶ Q → SE( ),
θ(t) = (θ (t) . . . θ n (t))T → eξ θ . . . eξn θ n gst ( )
ˆ ˆ
Chapter

and θ(t) = (θ (t) . . . θ n (t))T ,
Manipulator
Kinematics ˙ ˙ ˙
Forward
kinematics

Inverse
What is the velocity of the tool frame?
Kinematics
n
Vst = gst (θ)gst (θ) = ( θ i )gst (θ)
Manipulator
ˆs ˙ − ∂gst ˙ −
Jacobian

Redundant i= ∂θ i
Manipulators n n
= ( g (θ))θ i ⇒ Vst = (
g (θ))∨ θ i
∂gst − ˙ s ∂gst − ˙
Reference
i= ∂θ i st i= ∂θ i st
⎡ θ ⎤
⎢ ˙ ⎥
= [( gst (θ))∨ , . . . , ( gst (θ))∨ ] ⎢ ⎥
∂gst − ∂gst −
⎢ ⎥
⎢ θn ⎥
⎣ ⎦
∂θ ∂θ n ˙
Jst (θ)∈ℝ
s ×n



36
gst (θ) = e gst ( )
ˆ
ξθ ˆ
ξn θ n
...e
∂gst −
gst (θ) = ( ξ e ξ θ . . . e ξn θ n gst ( ))(gst (θ))− = ξ ⇒
ˆ ˆ ˆ ˆ
Chapter
Manipulator
Kinematics
∂θ
∂gst −
( g (θ))∨ = ξ
Forward
kinematics

Inverse
∂θ st
Kinematics

Manipulator
Jacobian

Redundant
Manipulators

Reference


36
gst (θ) = e gst ( )
ˆ
ξθ ˆ
ξn θ n
...e
∂gst −
gst (θ) = ( ξ e ξ θ . . . e ξn θ n gst ( ))(gst (θ))− = ξ ⇒
ˆ ˆ ˆ ˆ
Chapter
Manipulator
Kinematics
∂θ
∂gst −
( g (θ))∨ = ξ
Forward
kinematics

Inverse
∂θ st
∂gst −
gst (θ) = (e ξ θ ξ e ξ θ . . . e ξn θ n gst ( ))(gst (θ))−
Kinematics
ˆ ˆ ˆ ˆ
Manipulator
Jacobian ∂θ
= e ξ θ ξ e ξ θ . . . e ξn θ n gst ( )gst (θ) = e ξ θ ξ e−ξ θ ≜ ξ′
−
Redundant ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ
Manipulators

∂gst −
( g (θ))∨ = Ade ξˆ θ ξ = ξ′ . . . . . .
Reference

∂θ st

[Download] Rev-Chapter-3

Recommended

Recommended

More Related Content

What's hot

What's hot (20)

Viewers also liked

Viewers also liked (12)

Similar to [Download] Rev-Chapter-3

Similar to [Download] Rev-Chapter-3 (7)

More from ATC, ECE, Hong Kong University of Science and Technology

More from ATC, ECE, Hong Kong University of Science and Technology (6)

Recently uploaded

Recently uploaded (20)

[Download] Rev-Chapter-3