Inverse Manipulator Kinematics

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 )

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

Solvability
• Existence of Solutions
• Multiple Solutions
• Method of solutions
– Close form solution

• Algebraic solution
• Geometric solution
– Numerical solutions

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

Solvability - Existence of Solution - Workspace - 2R
Example 1 - L1 = L2
Reachable
Workspace
Dexterous
Workspace

Example 2 - L1 ≠ L2
Reachable
Workspace
NO Dexterous
Workspace

Example 3 - L1 = L2
Reachable
Workspace
&
Dexterous
Workspace

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

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
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

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.

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

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

Inverse Kinematics - Planar RRR (3R) -
Algebraic Solution - 1/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

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 )

• 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
(φ )

• 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 

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

• 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

• 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

• 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

• 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

• 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

• 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

• 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


Inverse Manipulator Kinematics

Uploaded by

Document Information

Original Description:

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Inverse Manipulator Kinematics

Uploaded by

Copyright:

Available Formats

Inverse Manipulator Kinematics (1/4)

Instructor: Jacob Rosen Ph.D.

Direct (Forward) Kinematics

Given: Joint angles and links geometry

Given: Position and orientation of the end

Instructor: Jacob Rosen Ph.D.

 r11 r12 r13 px 

Total Number of Equations: 12

Independent Equations: 3 - Rotation Matrix

Instructor: Jacob Rosen Ph.D.

– Close form solution

Instructor: Jacob Rosen Ph.D.

• The Dexterous Workspace is a subset of the Reachable Workspace

Instructor: Jacob Rosen Ph.D.

Instructor: Jacob Rosen Ph.D.

Instructor: Jacob Rosen Ph.D.

Instructor: Jacob Rosen Ph.D.

• Multiple solutions are a common

• The number of solutions depends on

• Example: The PUMA 560 can reach

Instructor: Jacob Rosen Ph.D.

• Problem: The fact that a manipulator

• Solution: Decision criteria

• Task Definition - Position the end

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

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

Instructor: Jacob Rosen Ph.D.

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

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

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

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

Instructor: Jacob Rosen Ph.D.

Analytic Solution Numeric Solution Non

Close Form Solution

Instructor: Jacob Rosen Ph.D.

• Law of Sinus / Cosines - For a general triangle

Instructor: Jacob Rosen Ph.D.

Instructor: Jacob Rosen Ph.D.

Instructor: Jacob Rosen Ph.D.

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

c123 − s123 0 L1c1 + L2c12 

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

Instructor: Jacob Rosen Ph.D.

Instructor: Jacob Rosen Ph.D.

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

Instructor: Jacob Rosen Ph.D.

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

Instructor: Jacob Rosen Ph.D.

• Solving for c2 we obtain

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

Instructor: Jacob Rosen Ph.D.

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

Instructor: Jacob Rosen Ph.D.

Instructor: Jacob Rosen Ph.D.

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

Instructor: Jacob Rosen Ph.D.

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

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

Instructor: Jacob Rosen Ph.D.

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

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

Instructor: Jacob Rosen Ph.D.