Inverse Manipulator Kinematics (1/4)

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Direct Versus Inverse Kinematics

Direct (Forward) Kinematics

Given: Joint angles and links geometry

Compute: Position and orientation of the end
effector relative to the base frame

f (θ ) = BTT = N0T
Inverse Kinematics

Given: Position and orientation of the end

effector relative to the base frame
Compute: All possible sets of joint angles and
links geometry which could be
used to attain the given position and
orientation of the end effetor

θ = f −1 ( BTT ) = f −1 ( N0T )

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Solvability - PUMA 560

0
Given : PUMA 560 - 6 DOF, 6T

Solve: θ1 Lθ 6

 r11 r12 r13 px 

r r
0 0 1 2 3 4 5  21 22 r23 py 

6T = T T T T T
1 2 3 4 5 6T =
 r31 r32 r33 pz 
 
0 0 0 1

Total Number of Equations: 12

Independent Equations: 3 - Rotation Matrix

3 - Position Vector
Type of Equations: Non-linear

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Solvability

• Existence of Solutions

• Multiple Solutions

• Method of solutions

– Close form solution

• Algebraic solution
• Geometric solution

– Numerical solutions

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Solvability - Existence of Solution

0
• For a solution to exist, N T must be in the workspace of the manipulator

• Workspace - Definitions

– Dexterous Workspace (DW): The subset of space in which the robot end
effector can reach all orientation.
orientation

– Reachable Workspace (RW): The subset of space in which the robot end
effector can reach in at least 1 orientation

• The Dexterous Workspace is a subset of the Reachable Workspace

DW ⊂ RW

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
Solvability - Existence of Solution - Workspace - 2R
Example 1 - L1 = L2

Reachable
Workspace

Dexterous
Workspace

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
Solvability - Existence of Solution - Workspace - 2R
Example 2 - L1 ≠ L2

Reachable
Workspace

NO Dexterous
Workspace

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
Solvability - Existence of Solution - Workspace - 3R
Example 3 - L1 = L2

Reachable
Workspace
&
Dexterous
Workspace

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Solvability - Multiple Solutions

• Multiple solutions are a common

problem that can occur when solving
inverse kinematics because the
system has to be able to chose one

• The number of solutions depends on

the number of joints in the
manipulator but is also a function of
the links parameters ai αi d i θ i

• Example: The PUMA 560 can reach

certain goals with 8 different
(solutions) arm configurations
θ 4′ = θ 4 + 180o
θ 5′ = −θ 5
θ 6′ = θ 6 + 180o

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Solvability - Multiple Solutions

• Problem: The fact that a manipulator

has multiple multiple solutions may
cause problems because the system
has to be able to choose one

• Solution: Decision criteria

– The closest (geometrically) -
minimizing the amount that each
joint is required to move
• Note 1: input argument -
present position of the
manipulator
• Note 2: Joint Weight -
Moving small joints (wrist)
instead of moving large
joints (Shoulder & Elbow)
– Obstacles exist in the workspace
- avoiding collision
Instructor: Jacob Rosen Ph.D.
Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Solvability - Multiple Solutions - Number of Solutions

• Task Definition - Position the end

effector in a specific point in the
plane (2D)

• No. of DOF = No. of DOF of the task

Number of solution: 2
(elbow up/down)

• No. of DOF > No. of DOF of the task

Number of solution: ∞
Self Motion - The robot can be
moved without moving the the end
effector from the goal

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Solvability - Methods of Solutions

• Solution (Inverse Kinematics)- A “solution” is the set of joint variables

associated with an end effector’s desired position and orientation.

• No general algorithms that lead to the solution of inverse kinematic equations.

• Solution Strategies

– Closed form Solutions - An analytic expression includes all solution sets.

• Algebraic Solution - Trigonometric (Nonlinear) equations
• Geometric Solution - Reduces the larger problem to a series of plane
geometry problems.

– Numerical Solutions - Iterative solutions will not be considered in this

course.

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Solvability

Robot - 6 DOF
Single Series Chain
Revolute & Prismatic Joints

Analytic Solution Numeric Solution Non

Real-Time
Real-Time

Close Form Solution

Sufficient Condition
Industrial Three adjacent axes
Robots (rotary or prismatic)
must intersect

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Mathematical Equations

• Law of Sinus / Cosines - For a general triangle

c B
a
A C
b
sin A sin B sin C
= =
a b c

a 2 = b 2 + c 2 − 2bc cos A

• Sum of Angles
sin (θ1 + θ 2 ) = s12 = c1s2 + s1c2
cos(θ1 + θ 2 ) = c12 = c1c2 − s1s2

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 1/12

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 2/12

c1 − s1 0 0
 cθ i − sθ i 0 ai −1   s1 c1
 0  0 0
i −1
 sθ cα c θ i c α i −1 − sα i −1 − sα d 1T =
iT =
 i i −1 i −1 i 
0 0 1 0
 sθ i sαi −1 cθ i sαi −1 cαi −1 cαi −1d   
  0 0 0 1
 0 0 0 1 
c 2 − s 2 0 L1
s2
1  c 2 0 0 
2T =
0 0 1 0
 
0 0 0 1
 c1 − s3 0 L 2
 s3
2  c3 0 0 
3T =
0 0 1 0
 
0 0 0 1

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 3/12

c1c 2c3 − c1s 2 s 3 − s1s 2c 3 − s1c 2 s 3 − c1c 2 s 3 − c1s 2c 3 + s1s 2 s 3 − s1c 2c 3 0 c1( L 2 c 2 + L1) − s1s 2 L 2 
B 0 0 1 2
 s1cc 3 − s1s 2 s 3 + c1s 2c 3 + c1c 2 s 3 − s1c 2 s 3 − s1s 2c 3 − c1s 2 s 3 + c1c 2c 3 0 s1( L 2 c 2 + L1) + c1s 2 L 2
W T = 3T = T T
1 2 3T =  
0 0 1 0
 
 0 0 0 1 

• Using trigonometric identities to simplify BT , the solution to the forward

W
kinematics is:

c123 − s123 0 L1c1 + L2c12 

s
B 0  123 c123 0 L1s1 + L2 s12 

W T = 3T =
 0 0 1 0 
 
 0 0 0 1 

• were c123 = cos(θ1 + θ 2 + θ 3 ) s123 = sin(θ1 + θ 2 + θ 3 )

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 4/12

• Given:
– Direct Kinematics: The homogenous transformation from the base to the
wrist WBT
– Goal Point Definition: For a planar manipulator, specifying the goal can be
accomplished by specifying three parameters: The position of the wrist in
space ( x, y ) and the orientation of link 3 in the plane relative to the X̂ axis
(φ )

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 5/12

• Problem:
What are the joint angles ( θ1 ,θ 2 ,θ 3 ) as a function of the wrist position and
orientation ( x, y , φ )

• Solution:
• The goal in terms of position and orientation of the wrist expressed in terms of
the homogeneous transformation is defined as follows

cφ − sφ 0 x c123 − s123 0 L1c1 + L2c12 

s s
B cφ 0 y B 0  123 c123 0 L1s1 + L2 s12 

T
W Goal = φ  W T = 3T =
0 0 1 0  0 0 1 0 
   
0 0 0 1  0 0 0 1 

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 6/12

B
T
W Goal = 03T
• A set of four nonlinear equations which must be solved for θ1 , θ 2 , θ 3
cφ = c123
sφ = s123
x = l1c1 + l2c12
y = l1s1 + l2 s12

• Solving for θ 2
• If we square x and y add them while making use of c12 = c1c2 − s1s2 ;s12 = c1s2 + s1c2
we obtain

x 2 + y 2 = l12 + l22 + 2l1l2c2

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 7/12

• Solving for c2 we obtain

x 2 + y 2 − l12 − l22
c2 =
2l1l2
• Note: In order for a solution to exist, the right hand side must have a value
between -1 and 1. Physically if this constraints is not satisfied, then the goal
point is too far away for the manipulator to reach.

• Assuming the goal is in the workspace, and making use of

we write an expression for s2 as c22 + s22 = 1

2
s2 = ± 1 − c2
• Note: The chose of the sign corresponds to the multiple solutions in which we
can choose the “elbow-up” or the “elbow-down” solution

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 8/12

• Finally, we compute θ 2 using the two argument arctangent function

2
x 2 + y 2 − l12 − l22
θ 2 = Atan2(s2 , c 2 ) = A tan 2( ± 1 − c , 2 )
2l1l2

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 8/12

• Solving for θ1
• For solving θ1 we rewrite the the original nonlinear equations using a change of
variables as follows

x = l1c1 + l2c12
y = l1s1 + l2 s12 k2

x = k1c1 + k2 s1
y = k1s1 + k2c1 l2 c2

• where

k1 = l1 + l2c2 k1
k2 = l2 s2

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 9/12

• Changing the way in which we write the constants k1 and k 2

r = + k12 + k22
γ = A tan 2(k2 , k1 )
k2
• Then
k1 = r cos γ
r
k2 = r sin γ l2 c2

γ
k1

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 10/12

• Base on the previous two transformations the equations can be rewritten as

x = r cos γ cos θ1 − r sin γ sin θ1

y = r cos γ sin θ1 + r sin γ cos θ1
• or
x
= cos γ cos θ1 − sin γ sin θ1
r
y
= cos γ sin θ1 + sin γ cos θ1
r

• or x
= cos(γ + θ1 )
r
y
= sin(γ + θ1 )
r

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 11/12

• Using the two argument arctangent we finally get a solution for θ1

y x
γ + θ1 = A tan 2( , ) = A tan 2( y , x )
r r
θ1 = A tan 2( y , x ) − A tan 2( k2 , k1 )
k1 = l1 + l2 c2
k2 = l2 s2
• Note:

(1) When a choice of a sign is made in the solution of θ 2 above, it will cause a
sign change in k 2 thus affecting θ1

(2) If x = y = 0 then the solution becomes undefined - in this case θ1 is

arbitrary

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington
 Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 12/12

• Solving for θ3

• Base on the original equations

cφ = c123
sφ = s123

• We can solve for the sum of θ1 , θ 2 , θ 3

θ1 + θ 2 + θ 3 = A tan 2( sφ , cφ ) = φ
θ 3 = φ − θ1 + θ 2
• Note: It is typical with manipulators that have two or more links moving in a
plane that in the course of a solution, expressions for sum of joint angles arise

Instructor: Jacob Rosen Ph.D.

Models of Robot Manipulation - EE 543 - Department of Electrical Engineering - University of Washington