Robotic Manipulators: Lecture 4

i
Department of Electrical Engineering

b
. T ale
H . A
.
Dr. H. A. Talebi

o f
Pr

July 17, 2008

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Outline

• Inverse Kinematics
b i
– Workspace

. T ale
. A
– Existence
–
.
Multiple Solutions

f H
–
–
Pro
Algebraic Solutions
Geometric Solutions
– Polynomial Solutions
– Decoupled Manipulators
– Pieper’s Solution

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Inverse Kinematics
• Forward (Direct) Kinematics: Find the position and orientation of the tool
given the joint variables of the manipulators.
• Inverse Kinematics: Given the position and orientation of the tool find the

i
set of joint variables that achieve such configuration.

b
ale
• Placing the tool frame {T } relative to station frame {S} is broken into two
stages
. T
. A
1. Frame transformations are performed to find the wrist frame {W } wrt
H
base frame {B}

o f .
Pr
2. Perform the inverse kinematics to solve for the joint angles

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Solvability
• Inverse Kinematic (IK) mapping is a nonlinear mapping
• Given the numerical value of 0TN , find θ1, · · · , θn.
• For instance for PUMA 560, given the 0T6, find θ1, · · · , θ6.
• For 6 DOF arm (12 equation and 6 unknown).
b i
. T ale
• Among 9 corresponding to rotation, only 3 are independent

A
• 3 from orientation, 3 from position: 6 equation, 6 unknown.

H .
.
• Nonlinear equations: difficult to solve

o f
Pr
– Existence of solution
– multiple solution
– method of solution
• Existence:
– The existence relates to the manipulator’s workspace
– Workspace: set of all points that manipulator can reach
– For a solution to exist the point should be in manipulator’s workspace

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Solvability
• Existence:
– Dexterous workspace: Volume of space that the end effector can reach
with all orientations
– Reachable workspace: Volume of space that the end effector can reach
with at least one orientations
b i
ale
– Example: A two-link planar manipulator

. T
– If l1 = l2 =⇒ Reachable workspace is a disc of radius 2l1 , dexterous

. A
workspace is a single point (the origin)

H
f .
– If l1 6= l2 =⇒ no dexterous workspace; but reachable workspace is a
o
Pr
ring with outer radius is l1 + l2 and the inner radius is |l1 − l2 |
– Inside the reachable workspace, two possible orientations

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Solvability
• joint Limitation
b i
workspace is reduced.
. T ale
– If joints have mechanical limitation and cannot rotate 360-degree, the

H . A
– For example if θ1 has 360-degree motion, but θ2 ’s motion is limited to

f .
0 ≤ θ2 ≤ 180, what is the reachable workspace?
o
Pr
– The reachable workspace has the same volume, but only one orientation
is attainable.
– Manipulator with fewer than 6 DOF cannot achieve general goal position
and orientation in 3D space

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Solvability
• Multiple solution
• Example: three-link planar manipulator

b i
. T ale
H . A
o f .
Pr
– Large dextrous workspace
– Any position within its workspace can be reached by any orientation
– Multiple solution could be problematic (we need to choose one)

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Multiple Solution

b i
. T ale
. A
• The best criteria would be to choose the closest solution
H
f .
• Moving from A to B , choose a path leading to minimum joint movements
o
Pr
• IK consider the present position of the robot
• How can “closest” be interpreted?
• Typical robots have three large link for position and three smaller one for
orientation, so we prefer to move the smaller joints rather then bigger joints
if there is a choice.
• Obstacles force farther solutions
• Useful to calculate all solutions
•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Solvability
• Multiple solution depends on
1. Number of joints of the manipulator
2. Link parameters
3. Joint limitations

b i
ale
• Example: PUMA 560

. T
H . A
o f .
Pr

– There are many solutions to reach a point

– Due to the joint limitations, some solutions might not be accessible.
•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Solvability
• Method of solution: No general algorithm to solve general nonlinear equa-
tions
• A manipulator is said to be solvable if the set of all joint variable associated
to a given position and orientation can be determined.

b i
ale
1. Closed-form solutions
2. Numerical solutions

. T
A
• Closed-form solutions are considered

H .
.
• Algebraic and geometric solution

o f
Pr
• All systems with revolute and prismatic joints having 6 DOF in a single
series chain are solvable.
• Only in special cases it could be solved analytically
• Robots with analytical solutions have many intersecting joints or many αi
are 0 or ±90 degrees.
• Most manipulators designed to have closed-form solutions
• Closed-form solutions exist for decouple manipulator ( three joints inter-
sect)
•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Algebraic Solution
• A three link (RRR) planar manipulator

b i
. T ale
H . A
o f .
Pr

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Algebraic Solution
• Transformation Matrix:
c123 −s123 0 l1c1 + l2c12
 

i
s c 0 l1s1 + l2s12 

b
B
Tw = 0T3  123 123

ale
0 0 1 0 
0 0 0 1

. T
. A
• The goal point is specified as a wrist frame BTw
H
o f .
• For planar manipulator, specified by position (
xy ) and the orientation φ

Pr
• φ: Orientation of the goal point relative to X̂ axis.
• The desired transformation has the following structure:
cφ −sφ 0 x
 
s c 0 y
B
Tw =  φ φ
0 0 1 0
0 0 0 1

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Example: Three-Link Planar manipulator
• Equating the desired and actual transformations, four nonlinear equations
obtained

cφ = c123
sφ = s123
x = l1c1 + l2c12
b i
ale
y = l1s1 + l2s12

. T
• Algebraic Solution:
. A
x + yH= l + l + 2l l c
o f
2
. 2 2 2

r
1 2 1 2 2

• Solving for c2: P

x2 + y 2 − l12 − l22
c2 =
2l1l2
• For a solution to exist the r.h.s. should be between −1 and 1
p
s2 = ± 1 − c22
θ2 = Atan2(s2, c2)
•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Algebraic Solutions: continued
• The choice of sign leads to multiple solution elbow up or elbow down
• Now, x and y can be used to get θ1
x = k1 c 1 − k2 s1
eb i (1)
y =

T a l
k1 s1 + k2 c 1 (2)

.
k1 = l1 + l2c2
k2
H . A
= l2s2

o f . p
Pr r = k + k
• Change of variable (polar form):
2 2
1 2
γ = Atan2(k2, k1)
k1 = r cos γ
k2 = r sin γ

• Express Equations (1) and (2) in terms of r and γ :

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Algebraic Solution: continued
x
= cos γ cos θ1 − sin γ sin θ1
r
y
= cos γ sin θ1 + sin γ cos θ1
r
x

i
cos(γ + θ1) =

b
r

ale
y
sin(γ + θ1) =
T
r

. A.
• θ1 can be obtained from two-argument tangent:

f . H
o
y x

Pr
γ + θ1 = Atan2( , ) = Atan2(y, x)
r r
θ1 = Atan2(y, x) − Atan2(k2, k1)

• Choice of sign in θ2 =⇒ sign change in k2 =⇒ affecting θ1

• The approach used often in inverse kinematic
• if x = y = 0, the solution is undefined, so θ1 is arbitrary.
• Finally, θ3 can be easily obtained from Inverse Orientation for a given φ:
θ1 + θ2 + θ3 = Atan2(sφ, cφ) = φ
•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Geometric Solution

b i
. T ale
H . A
f .
• In this approach the spatial geometry is broken to several plane-geometry
problems

Pro
• For many manipulator (especially when αi = 0 or ±90) can be done
• Consider the three-link planar manipulator
• Apply low of cosine:
x2 + y 2 = l12 + l22 − 2l1l2 cos(180 + l2)
x2 + y 2 − l12 − l22
c2 =
2l1l2
•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Geometric Solution: continued

eb i
a l
Tx + y
. A. √
• Triangle exists if the distance to the goal ( 2 2) is less than l1 + l2 .

f . H (−180 < θ < 0)

• Equation solved (triangle exists) for
o
2

• To solve for θ P r
• The other solution is found by symmetry: θ20 = −θ2.
we need to find ψ and β :
1,

β = Atan2(y, x)
x2 + y 2 + l12 − l22
cos ψ = √
2l1 x2 + y 2
θ1 = β ± ψ + sign if θ2 < 0, – sign if θ2 > 0

• θ3 can be obtained from orientation (sum of the angles)

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Algebraic Solution: A Polynomial Approach
• Useful to convert the transcendental equation to polynomial equation
• Change of variable using the tangent of half-angle:
θ
u = tan
2u i
2
b
ale1 + u
1 − u2
cos θ = , sin θ =
T
1 + u2 2

. A.
H
• Example: a cos θ + b sin θ = c

o f .
• Substitute for sin & cos from the above and multiply by 1 + u2:

Pr a(1 − u ) + 2bu = c(1 + u )

2

(a + c)u2 − 2bu + (c − a) = 0
2

√
b±b 2 + a2 − c 2
u =
a+ c √ 
b ± b 2 + a2 − c 2
θ = 2Atan
a+c
•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
Pieper’s Solution for Decoupled Manipulator
• Pieper found closed-form solution for 6 DOF manipulators when the three
consecutive axes intersect (included the consecutive three parallel axis)
• Can be also applied to prismatic joints, as well to many industrial robot.

i
• Consider a 6 DOF manipulator with all 6 joint revolute and the last three
b
ale
intersect (such as PUMA)

. T
• The origin of frames {4}, {5}, {6} located at the point of intersection:

H . A  
x

o f . y

Pr
0
P4org = 0T1 1T2 2T3 3P4org =  
z
1

• 3P4org is in fact the position part of 3T4:

a3 f1(θ3)
   
0  −d4sα3  0 1  f2(θ3) 
P4org = 0T1 1T2 2T3  = T1 T2 
d4cα3  f3(θ3) 
1 1
•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Decoupled Manipulator
f1(θ3) a3
   
 f2(θ3)  2  −d4sα3 
 f (θ )  = T3  d cα 
3 3 4 3
1 1

b i
ale
• Substituting for 2T3:
f1 = a2c3 + d4sα3s3 + a2
. T
. A
f2 = a3cα2s3 − d4sα3cα2c3 − d4sα2cα3 − d3sα2
H
f .T
f3 = a3sα2s3 − d4sα3sα2c3 + d4cα2cα3 + d3cα2
o
Pr 
• Similarly, substituting for 1 2 and 0T1 :

c1g1 − s1g2

0  s1g1 + c1g2 
P4org = (3)
 g3 
1
g1 = c2f1 − s2f2 + a1
g2 = s2cα1f1 + c2cα1f2 − sα1f3 − d2sα1
g3 = s2sα1f1 + c2sα1f2 + cα1f3 + d2cα1
•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Decoupled Manipulator
• Consider the squared norm of 0P4org = r = x2 + y 2 + z 2:

r = g12 + g22 + g32

r = f12 + f22 + f32 + a21 + d22 + 2d2f3 + 2a1(c2f1 − s2f2) (4)

• Now, r and z can be written as:

r = (k c + k s )2a + k le
b i
1 2

. T a
2 2 1 3 (5)

. A
z = (k s − k c )sα
1 2 + k
2 2 1 4
k = f
k = f
1

.
1
H
2

3
o
k r=
−f

Pk = ff cα+ f+ +d fcα + a + d + 2d f
2
1
2
2
2
2
3
2
1
2
2 2 3

4 3 1 2 1

• θ1 is eliminated and θ2’s dependence is simple.

– If a1 = 0 =⇒ r = k3 , R.H.S. of k3 is the function of θ3 =⇒ a
polynomial approach can be used for θ3 .
– sα1 = 0 =⇒ z = k4 =⇒ a polynomial approach
– Else, eliminate c2 and s2 from r and z :
•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Decoupled Manipulator
(r − k3)2 (z − k4)2 2 2
+ = k + k

i
1 2
4a21 s2α1

b
ale
• A polynomial approach (4th order) can be used for θ3.

. T
• Then, (5) can be solved for θ2 and (3) for θ1.
θ , θ.A θ
H
• To complete the IK problem,
.
5, and needs to be found.

f
4 6

Pro
• Since they intersect, they just affect the orientation
• So, they can be obtained from orientation part.
• Having found θ1 to θ3, we can find 1R3 and hence 3R6
• For many manipulators, the last three joints can be solved by ZYZ euler
angle
• For any manipulator, a set of proper euler angles can be defined

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 Example: PUMA 560 Inverse Kinematics
• Forward Kinematics obtained from the last lecture:
r11 r12 r13 px
 
r r r p 
0
T6 =  21 22 23 y 
r31 r32 r33 pz

i
0 0 0 1

b
ale
= 0T1(θ1) 1T2(θ2) 2T3(θ3) 3T4(θ4) 4T5(θ5) 5T6(θ6) (6)

• Objective: Solve 0T6 for θ1 to θ6.

[ T (θ )].A
. T
H
0 −1

[ T (θ )] fT. = T (θ ) T (θ ) T (θ ) T (θ ) T (θ )
– Multiply both side by 1 1 :

 ro
0 −1 0 1 2 3 4 5

P
1 1 6 2 2 3 3 4 4 5 5 6 6
c s 0 0 r r r p
 
1 1 11 12 13 x
 −s1 c1 0 0   r21 r22 r23 py  1
∴  = T6 (7)
0 0 1 0   r31 r32 r33 pz 
0 0 0 1 0 0 0 1
– This technique is often used to separate one variable from others.
– Equating (2,4) elements:

− s1px + c1py = d3
•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 PUMA 560 Inverse Kinematics
• Use the polar substitutions:
px = ρ cos φ, py = ρ sin φ
q
ρ = p2x + p2y , φ = Atan2(px, py )

• Hence, we get:
b i
d3
c 1 sφ − s1 c φ =
. T ale
. A
ρ
sin(φ − θ1) =
d3
f . H
P r o
ρ
s
d 2
3
cos(φ − θ ) = ± 1 −
1 2
ρ
s
d3 d23
φ − θ1 = Atan2( , ± 1 − 2 )
ρ ρ
q
θ1 = Atan2(py , px) − Atan2(d3, ± p2x + p2y − d23)

• Two possible solutions for θ1

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 PUMA 560 Inverse Kinematics
• Having found θ1, L.H.S. of (7) is known, equating (1, 4) and (3, 4) ele-
ments:

c1px + s1py = a3c23 − d4s23 + a2c2

−pz = a3s23 + d4c23 + a2s2
b i
. T
• Square both Eq. and add them on: ale
a c − d s = K .A
pH
=.
K f
3 3 4 3

o
2 2 2 2 2 2 2
+p +p −a −a −d −d
r
x y z 2 3 3 4

P=⇒
• θ2 is eliminated
2a 2

can be solved using the approach taken for θ1

p
θ3 = Atan2(a3, d4) − Atan2(K, ± a23 + d24 − K 2)

• Two possible solutions for θ3

• To solve for θ2 , express L.H.S. of (6) in terms of known quantities and θ2

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 PUMA 560 Inverse Kinematics
[ 0T3(θ2)]−1 0T6 = 3T4(θ4) 4T5(θ5) 5T6(θ6)
c1c23 s1c23 −s23 −a2c3 r11 r12 r13 px
  
 −c1s23 −s1s23 −c23 a2s3   r21 r22 r23 py  3
 −s = T6 (8)
c1 0 −d3   r31 r32 r33 px 
i
1

b
0 0 0 1 0 0 0 1
• Equating (1, 4) and (2, 4) elements:
. T ale
H . A
c1c23px + s1c23py − s23pz − a2c3 = a3

o f .
−c1s23px − s1s23py − c23pz + a2s3 = d4

Pcr s
• Can be solved for 23 and 23 :

(−a3 − a2c3)pz + (c1px + s1py )(a2s3 − d4)

s23 =
p2z + (c1px + s1py )2
(a2s3 − d4)pz − (a3 + a2c3)(c1px + s1py )
c23 =
p2z + (c1px + s1py )2
θ23 = Atan2[(−a3 − a2c3)pz + (c1px + s1py )(a2s3 − d4),
(a2s3 − d4)pz − (a3 + a2c3)(c1px + s1py )]
•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 PUMA 560 Inverse Kinematics
• Four possible solution for θ23 (possible combinations of θ1 and θ3)
θ2 = θ23 − θ3

• Orientation Inverse Kinematics

b i
ale
– Having found θ1 the L.H.S. of (8) is known.

.
– Equating (1, 3) and (3, 3) elements:
T
H . A
r13c1c23 + r23s1c23 − r33s23 = −c4s5

o f . −r13s1 + r23c1 = s4s5.

s 6=r0
– As long as
P5 , can solve for θ4 :

θ4 = Atan2(−r13s1 + r23c1, −r13c1c23 − r23s1c23 + r33s23)

– If θ5 = 0, manipulator is in a singular position (joint axes 4 & 6 line up).

– All we can do (all that matters) is the sum or difference of θ4 and θ6
– This is detected when both elements of Atan2 are near zero.
– θ4 is computed arbitrary and θ6 is calculated later.
•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 PUMA 560 Inverse Kinematics
• Orientation Inverse Kinematics:
– Having known θ4 , L.H.S. of (6) can be expressed as a function of θ4 :

i
[40T (θ4)]−160T = 54T (θ5)65T (θ6)
b
ale
– where

. T
A
 
c1 c23 c4 + s1 s4 s1 c23 c4 − c1 s4 −s23 c4 −a2 c3 c4 + d3 s4 − a3 c4

.
 −c1 c23 s4 + s1 c4 −s1 c23 s4 − c1 c4 s23 s4

H
a2 c3 s4 + d3 c4 + a3 s4 
[40T (θ4 )]−1 =

.

−c1 s23 −s1 s23 −c23 a2 s3 − d4

f
 

o
0 0 0 1

(1, 3) r (3, 3)
– Equating
P and elements:

r13(c1c23c4 + s1s4) + r23(s1c23c4 − c1s4) − r33(s23c4) = −s5,

r13(−c1s23) + r23(−s1s23) + r33(−c23) = c5

– We can solve for θ5

θ5 = Atan2(s5, c5)

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit
 PUMA 560 Inverse Kinematics
• Now, express L.H.S. of (6) as a function of θ5:

b i
ale
(50T )−160T = 65T (θ6)

. T
• Equating both (3, 1) and (1, 1) elements:

H . A
.
θ6 = Atan2(s6, c6)

o f
Pr
s6 = −r11(c1c23s4 − s1c4) − r21(s1c23s4 + c1c4) + r31(s23s4)
c6 = −r11[(c1c23c4 + s1s4)c5 − c1s23s5] + r21[(s1c23c4
− c1s4)c5 − s1s23s5] − r31(s23c4c5 + c23s5)

• Four solutions due to the possible combinations of θ1 and θ3.

• All solutions need to be checked for joint violations.

•First •Prev •Next •Last •Go Back •Full Screen •Close •Quit