MTS-800 Advanced

Robotics I

Lecture 4
Inverse Kinematics

Spring 2024 DR AMIR HAMZA Dr. Amir Hamza – Spring

1 24
Introduction
• In the last chapter, we considered the problem of computing the position and orientation of the tool
relative to the user’s workstation when given the joint angles of the manipulator.

• In this chapter, we investigate the more difficult converse problem: Given the desired position and
orientation of the tool relative to the station, how do we compute the set of joint angles which will
achieve this desired result?

• Whereas Chapter 3 focused on the direct kinematics of manipulators, here the focus is the inverse
kinematics of manipulators.

• Solving the problem of finding the required joint angles to place the tool frame, {T}, relative to the
station frame, {S}, is split into two parts.

• First, frame transformations are performed to find the wrist frame, {W}, relative to the base frame,
{B}, and then the inverse kinematics are used to solve for the joint angles.
Spring 2024 DR AMIR HAMZA 2
 Introduction

Forward Kinematics
Compute the position and orientation of the manipulator tip relative to its base as a function of joint
variables 𝑥 𝜃 1
𝑟= 𝑦 𝜃 = 𝜃2
𝒓 = 𝑓(𝜽) 𝜃 𝜃3
Inverse Kinematics
Given the desired position and orientation of the manipulator tip relative to its base, calculate the set
of joint angles which will achieve this configuration.

Highly nonlinear problem having multiple solutions.

Spring 2024 DR AMIR HAMZA 3

Solvability
• The problem of solving the kinematic equations of a manipulator is a nonlinear one. Given the
numerical value of 𝑁0𝑇 , we attempt to find values of θ1, θ2 , ..., θn

• In the case of the PUMA 560 manipulator, the precise statement of our current problem is as
follows:
Given 06𝑇 as sixteen numeric values (four of which are trivial), solve (3.14) for the six joint
angles θ1 through θ6

• For the case of an arm with six degrees of freedom we have 12 equations and six unknowns.

• However, among the 9 equations arising from the rotation-matrix portion of 06𝑇, only 3 are
independent. These, added to the 3 equations from the position-vector portion of 06𝑇, give 6
equations with six unknowns.
Spring 2024 DR AMIR HAMZA 4
Solvability
Challenges
• These equations are nonlinear, transcendental equations, which can be quite difficult to solve
• Existence of Solutions – target must belong to workspace
• Multiple Solutions – which solution to prefer
• Singularity Issues

• The equations of 3.14 (fwd kinematics of PUMA) are those of a robot that had very simple link
parameters—many of the αi were 0 or ±90 degrees. Many link offsets and lengths were zero.

• It is easy to imagine that, for the case of a general mechanism with six degrees of freedom (with all
link parameters nonzero) the kinematic equations would be much more complex than those of
(3.14).

• As with any nonlinear set of equations, we must concern ourselves with the existence of solutions,
with multiple solutions, and with the method of solution

Spring 2024 DR AMIR HAMZA 5

Existence of Solutions
• Target must exist in the manipulator’s workspace.

• Workspace is that volume of space that the end-effector of the manipulator can reach. It
is of two types:

• Dexterous Workspace is that volume of space that the robot end-effector can reach with all
orientations.

• The Reachable Workspace is that volume of space that the robot can reach in at least one
orientation.

• Clearly, the dexterous workspace is a subset of the reachable workspace.

Spring 2024 DR AMIR HAMZA 6

Existence of Solutions
• Consider the workspace of the two-link manipulator in Fig.

Condition Reachable Dexterous

𝐿1 = 𝐿2 Disc Origin
(Radius = 2L1)
𝐿1 ≠ 𝐿2 Ring Null
(IR = 𝐿1 − 𝐿2 & OR = 𝐿1 + 𝐿2

• Inside the reachable workspace there are two possible orientations

of the end-effector. On the boundaries of the workspace there is
only one possible orientation.

Spring 2024 DR AMIR HAMZA 7

Existence of Solutions
Factors Affecting Workspace
• Joint Limits: with joint limits as subset of the full 360o, workspace is reduced (no of possible
orientations) e.g. if 0 ≤ 𝜽𝟏 ≤ 360o and 0 ≤ 𝜽𝟐 ≤ 180o
reachable workspace has the same extent but one orientation is lost

• DOFs: a manipulator has less than six DOFs cannot attain general goal positions and orientations
in 3D-space
(e.g. a planar manipulator cannot reach out of plane)

• Tool Frame Transformation: is performed independently of the manipulator kinematics as tool-

tip is generally considered in determining the reachable workspace.

• If the desired position and orientation of the wrist frame is in the workspace, then at least one
solution exists

Spring 2024 DR AMIR HAMZA 8

Existence of Solutions
• Generally, the tool transformation is performed independently of the manipulator kinematics and
inverse kinematics, so we are often led to consider the workspace of the wrist frame, {W}.

• For a given end-effector, a tool frame, {T}, is defined; given a goal frame, {G}, the corresponding
{W} frame is calculated, and then we ask: Does this desired position and orientation of {W} lie in
the workspace?

• In this way, the workspace that we must concern ourselves with (in a computational sense) is
different from the one imagined by the user, who is concerned with the workspace of the end-
effector (the {T} frame).

Spring 2024 DR AMIR HAMZA 9

Multiple Solutions
• Another challenge is that of multiple solutions as the system has to choose one
• A planar arm with 3 links has a large dexterous workspace (dashed line shows
the other possible configuration)

Criteria to Select the Solutions

• Closest solution (algorithm prefers minimum joint movement to go from A to
B, provided it has the present position as the input)
• Prefer minor joint axis movement
• Obstacle avoidance: might force a “farther” solution to be chosen.
• Minimize torque
• No. of solutions ∝ No. of joints, link parameters and allowable range
• More nonzero DH parameters, more are the number of solutions

Spring 2024 DR AMIR HAMZA 10

Multiple Solutions
• The PUMA 560 can reach certain goals with eight different solutions.
• Figure 4.4 shows four solutions; all place the hand with the same position and orientation. For each
solution pictured, there is another solution in which the last three joints ‘‘flip’’ to an alternate
configuration according to the following formulas:

Spring 2024 DR AMIR HAMZA 11

Method of Solution
• There are no general algorithms that may be employed to solve a set of nonlinear equations.

• The term solvable means if an algorithm can determine all the sets of joint variables associated
with a given position and orientation (i.e. to find all multiple solutions)
Broad Classifications
• Numerical solutions:
❖ No assurance to find multiple solutions
❖ Computationally expensive and hence slower in nature
❖ Not generally preferred

• Closed-form solutions: based on analytical expressions (faster)

❖ Algebraic
❖ Geometric

Spring 2024 DR AMIR HAMZA 12

Solvability
• A major recent result in kinematics is that, according to our definition of solvability, all systems
with revolute and prismatic joints having a total of six degrees of freedom in a single series chain
are solvable.

• However, this general solution is a numerical one.

• Only in special cases can robots with six degrees of freedom be solved analytically.

• These robots for which an analytic (or closed-form) solution exists are characterized either by
having several intersecting joint axes or by having many αi equal to 0 or ±90 degrees.

• Calculating numerical solutions is generally time consuming

• Virtually all industrial manipulators are designed sufficiently simply that a closed-form solution
can be developed.

Spring 2024 DR AMIR HAMZA 13

Sufficient Condition for Existence of Analytical Solution

• A sufficient condition that a manipulator with six revolute joints have a closed form solution is that
three neighboring joint axes intersect at a point.

• Almost every manipulator with six degrees of freedom built today has three axes intersecting. For
example, axes 4, 5, and 6 of the PUMA 560 intersect.

Spring 2024 DR AMIR HAMZA 14

The Notion Of Manipulator Subspace When n < 6
• The set of reachable goal frames for a given manipulator constitutes its reachable workspace.

• For a manipulator with n degrees of freedom (where n < 6), this reachable workspace can be
thought of as a portion of an n-degree-of-freedom subspace.

• In the same manner in which the workspace of a six-degree-of-freedom manipulator is a subset of

space, the workspace of a simpler manipulator is a subset of its subspace.

• For example, the subspace of the two-link robot shown is a

plane, but the workspace is a subset of this plane, namely a circle
of radius l1 + l2 for the case that l1 = l2

Spring 2024 DR AMIR HAMZA 15

 The Notion Of Manipulator Subspace When n < 6
• One way to specify the subspace of an n-degree-of-freedom manipulator is to give an expression
for its wrist or tool frame as a function of n variables that locate it

• If we consider these n variables to be free, then, as they take on all possible values, the subspace is
generated

Example
𝐵𝑇 for the three-link
Give a description of the subspace of 𝑊
manipulator.

Spring 2024 DR AMIR HAMZA 16

 𝐵
The subspace of 𝑊 𝑇 is given by

where x and y give the position of the wrist and φ describes the orientation of the terminal link.

As x, y, and φ are allowed to take on arbitrary values, the subspace is generated.

Any wrist frame that does not have this structure lies outside the subspace (and therefore lies outside
the workspace) of this manipulator.

Link lengths and joint limits restrict the workspace of the manipulator to be a subset of this subspace.

Spring 2024 DR AMIR HAMZA 17

 Algebraic vs Geometric
Algebraic Solution
Consider the three-link planar manipulator introduced in Chapter 3. It
is shown with its link parameters.

Following the method of Chapter 3, we can use the link parameters

easily to find the kinematic equations of this arm:

Spring 2024 DR AMIR HAMZA 19

 Algebraic Solution
𝐵
• The goal point is a specification of the wrist frame relative to the base frame, that is, 𝑊 𝑇

• In the planar manipulator, specification of these goal points can be accomplished most easily by
specifying three numbers: x, y, and φ, where φ is the orientation of link 3 in the plane (relative to
the +𝑋෠ axis)

𝐵
• Instead of 𝑊 𝑇 , we will assume a transformation with the structure

• All attainable goals must lie in the subspace implied by the structure of equation above. By
equating both eqns, we arrive at a set of four nonlinear equations that must be solved for θ1, θ2, and
θ3 :

Spring 2024 DR AMIR HAMZA 20

 Algebraic Solution
• We now begin our algebraic solution of the four equations just presented

(squaring and adding last two equations and using

trigonometric identities of c12 and s12)

In order for a solution to exist, the right-hand side must have a value between −1 and 1. In the
solution algorithm, this constraint would be checked at this time to find out whether a solution exists.

Physically, if this constraint is not satisfied, then the goal point is too far away for the manipulator

Assuming the goal is in the workspace, we write an expression for s2 as

Spring 2024 DR AMIR HAMZA 21

 Algebraic Solution
Finally, we compute θ2, using the two-argument arctangent routine:

The choice of signs in in eqn for s2 corresponds to the multiple solution in which we can choose the
‘‘elbow-up’’ or the ‘‘elbow-down’’ solution.

We determined both the sine and cosine of the desired joint angle and then apply the two-argument
arctangent.

This ensures that we have found all solutions and that the solved angle is in the proper quadrant.

Having found θ2, we can solve for θ1 from initial equations of x and y

𝜃3 can then be found out knowing that 𝜑 = 𝜃1 + 𝜃2 + 𝜃3 where values of cos 𝜑 and sin 𝜑 are known from
the transformation.

Spring 2024 DR AMIR HAMZA 22

 Geometric Solution
Spatial geometry is decomposed into several planar geometries
(easy for manipulators with all or several 𝛼𝑖 = 0, ±90)

In the 3 DOF manipulator shown, we can use law of cosine for

solid triangle (angle included b/w L1 and L2 is (180 − 𝜃2 ) which
is same as (180 + 𝜃2 ))

as cos (180+ θ2) = - cos θ2 so we have

In order for solution to exist 𝑥 2 + 𝑦 2 must be less than or equal to 𝑙1 + 𝑙2

Spring 2024 DR AMIR HAMZA 23

 Geometric Solution
To solve for θ1, we find expressions for angles ψ and β as
indicated in Fig. First, β may be in any quadrant,
depending on the signs of x and y. So we must use
a two-argument arctangent:

(from law of cosines)

θ3 can be found from the following equation

Spring 2024 DR AMIR HAMZA 24

Algebraic Solution by Reduction to Polynomial
Transcendental equations are often difficult to solve because, even when there is only one variable
(say, θ), it generally appears as sinθ and cosθ. Making the following substitutions, however, yields an
expression in terms of a single variable, u:

This is a very important geometric substitution used often in solving kinematic equations.

These substitutions convert transcendental equations into polynomial equations in u.

Appendix A lists these and other trigonometric identities

Spring 2024 DR AMIR HAMZA 25

SCARA Robot
• SCARA Robot is short for Selective Compliance Assembly Robot
Arm or Selective Compliance Articulated Robot Arm

• The SCARA robot is most commonly used for pick-and-place or

assembly operations where high speed and high accuracy is required

Spring 2024 DR AMIR HAMZA 26

 a1 a2

SCARA Robot
d3

d1 d5

Link ai-1 𝜶i-1 di 𝜽i

1 0 0 d1 𝜽1
2 a1 180o 0 𝜽2
3 a2 0 d3 (var) 0
4 0 0 0 𝜽4
5 0 0 d5 0
Spring 2024 DR AMIR HAMZA 27
 Link ai-1 𝜶i-1 di 𝜽i
1 0 0 d1 𝜽1
SCARA Robot 2 a1 180o 0 𝜽2
3 a2 0 d3 (var) 0
Forward Kinematics
4 0 0 0 𝜽4
5 0 0 d5 0

𝑐1 −𝑠1 0 0 𝑐2 −𝑠2 0 𝑎1 1 0 0 𝑎2
0 𝑠1 𝑐1 0 0 1 −𝑠2 −𝑐2 0 0 2 0 1 0 0
1 𝑇 = 2 𝑇 = 3 𝑇 =
0 0 1 𝑑1 0 0 −1 0 0 0 1 𝑑3
0 0 0 1 0 0 0 1 0 0 0 1

𝑐4 −𝑠4 0 0 1 0 0 0
0
3
4 𝑇 =
𝑠4 𝑐4
0 0
0 0 4
5 𝑇 =
0
0
1
0
0 0
1 𝑑5 5𝑇 = 01𝑇 12𝑇 23𝑇 34𝑇 45𝑇
1 0
0 0 0 1 0 0 0 1

Spring 2024 DR AMIR HAMZA 29

SCARA Robot 𝑐1 − 2 − 4 𝑠1 − 2 − 4 0 𝑎1𝑐1 + 𝑎2𝑐1 2
−
0 𝑠1 − 2 − 4 −𝑐1 − 2 − 4 0 𝑎1𝑠1 + 𝑎2𝑠1 2
5𝑇 = 0 0
−
−1 𝑑1 − 𝑑3 − 𝑑5
0 0 0 1

Where
𝑠1 = 𝑠𝑖𝑛𝜃1 𝑐1 = 𝑐𝑜𝑠𝜃1 𝑐1 − 2 = 𝑐𝑜𝑠 𝜃1 − 𝜃2 𝑠1 − 2 = 𝑠𝑖𝑛 𝜃1 − 𝜃2

𝑠1 − 2 − 4 = 𝑠𝑖𝑛 𝜃1 − 𝜃2 − 𝜃4 𝑐1 − 2 − 4 = 𝑐𝑜𝑠 𝜃1 − 𝜃2 − 𝜃4

𝑐1 − 2 − 4 𝑠1 − 2 − 4 0 𝑎1𝑐1 + 𝑎2𝑐1 2 𝑐𝜑 𝑠𝜑 0 𝑥
−
0 𝑠1 − 2 − 4 −𝑐1 − 2 − 4 0 𝑎1𝑠1 + 𝑎2𝑠1 2 𝑠𝜑 −𝑐𝜑 0 𝑦
5𝑇 = 0 0
− =
0 0 −1 𝑧
−1 𝑑1 − 𝑑3 − 𝑑5
0 0 0 1 0 0 0 1

Spring 2024 DR AMIR HAMZA 30

SCARA Robot
Step 2: Nonlinear Equations
𝑐1 − 2 − 4 𝑠1 − 2 − 4 0 𝑎1𝑐1 + 𝑎2𝑐1 2 𝑐𝜑 𝑠𝜑 0 𝑥
−
0 𝑠1 − 2 − 4 −𝑐1 − 2 − 4 0 𝑎1𝑠1 + 𝑎2𝑠1 2 𝑠𝜑 −𝑐𝜑 0 𝑦
5 𝑇 = − =
0 0 −1 𝑑1 − 𝑑3 − 𝑑5 0 0 −1 𝑧
0 0 0 1 0 0 0 1

𝑠𝜑 = 𝑠1 − 2 − 4 = 𝑠𝑖𝑛 𝜃1 − 𝜃2 − 𝜃4
𝑐𝜑 = 𝑐1 − 2 − 4 = 𝑐𝑜𝑠 𝜃1 − 𝜃2 − 𝜃4

𝑥 = 𝑎1𝑐1 + 𝑎2𝑐1 − 2 = 𝑎1𝑐𝑜𝑠 𝜃1+ 𝑎2𝑐𝑜𝑠(𝜃1 − 𝜃2)

𝑦 = 𝑎1𝑠1 + 𝑎2𝑠1 − 2 = 𝑎1𝑠𝑖𝑛 𝜃1+ 𝑎2𝑠𝑖𝑛(𝜃1 − 𝜃2)

𝑧 = 𝑑1 − 𝑑3 − 𝑑5

Spring 2024 DR AMIR HAMZA 31

SCARA Robot
Step 3: Elbow Joint (θ2)
Square and Add the equations
𝑥 2 + 𝑦 2 = (𝑎1𝑐1 + 𝑎2 𝑐1−2 )2 + (𝑎1𝑠1 + 𝑎2 𝑠1−2 )2

= (𝑎12 𝑐12 + 𝑎22 𝑐1−2

2
+ 2𝑎1 𝑎2 𝑐1 𝑐1−2 ) + (𝑎12 𝑠12 + 𝑎22 𝑠1−2
2
+ 2𝑎1 𝑎2 𝑠1 𝑠1−2 )

= 𝑎12 (𝑐12 + 𝑠12 ) + 𝑎22 𝑐1−2

2 2
+ 𝑠1−2 + 2𝑎1 𝑎2 (𝑐1 𝑐1−2 + 𝑠1 𝑠1−2 )
= 𝑎12 (1) + 𝑎22 1 + 2𝑎1 𝑎2 [𝑐 1 − 1 − 2 ] ∴
cos 𝐴 − 𝐵 = 𝑐𝑜𝑠𝐴𝑐𝑜𝑠𝐵 + 𝑠𝑖𝑛𝐴𝑠𝑖𝑛𝐵

∴ 𝑥 2 + 𝑦 2 = 𝑎12 +𝑎22 +2𝑎1 𝑎2 𝑐2

𝑥2 + 𝑦2 − 𝑎12 − 𝑎22 𝑥2 + 𝑦2 − 𝑎12 − 𝑎22

𝑐2 = 𝜃2 = ±arccos[ ]
2𝑎1𝑎2 2𝑎1𝑎2

Spring 2024 DR AMIR HAMZA 32

SCARA Robot
Step 4: Base Joint (θ1)

𝒙 = 𝒂𝟏𝒄𝒐𝒔𝜽𝟏 + 𝒂𝟐𝒄𝒐𝒔(𝜽𝟏 − 𝜽𝟐) = f (θ1,θ2) = f (θ1) as θ2 is already known

= 𝒂𝟏𝒄𝒐𝒔𝜽𝟏 + 𝒂𝟐 𝒄𝒐𝒔𝜽𝟏𝒄𝒐𝒔𝜽𝟐 + 𝒔𝒊𝒏𝜽𝟏𝒔𝒊𝒏𝜽𝟐
= 𝒂𝟏 + 𝒂𝟐𝒄𝒐𝒔𝜽𝟐 𝒄𝒐𝒔𝜽𝟏 + 𝒂𝟐𝒔𝒊𝒏𝜽𝟐 𝒔𝒊𝒏𝜽𝟏
= 𝒂𝟏 + 𝒂𝟐𝒄𝟐 𝒄𝟏 + 𝒂𝟐𝒔𝟐 𝒔𝟏

𝒚 = 𝒂𝟏𝒔𝒊𝒏𝜽𝟏 + 𝒂𝟐𝒔𝒊𝒏(𝜽𝟏 − 𝜽𝟐)

= 𝒂𝟏𝒔𝒊𝒏𝜽𝟏 + 𝒂𝟐 𝒔𝒊𝒏𝜽𝟏𝒄𝒐𝒔𝜽𝟐 − 𝒄𝒐𝒔𝜽𝟏𝒔𝒊𝒏𝜽𝟐
= 𝒂𝟏 + 𝒂𝟐𝒄𝒐𝒔𝜽𝟐 𝒔𝒊𝒏𝜽𝟏 + −𝒂𝟐𝒔𝒊𝒏𝜽𝟐 𝒄𝒐𝒔𝜽𝟏
= 𝒂𝟏 + 𝒂𝟐𝒄𝟐 𝒔𝟏 + −𝒂𝟐𝒔𝟐 𝒄𝟏

Spring 2024 DR AMIR HAMZA 33

SCARA Robot
Arranging above equations
𝑎1 + 𝑎2𝑐2 𝑐1 + 𝑎2𝑠2 𝑠1 = 𝑥
−𝑎2𝑠2 𝑐1 + 𝑎1 + 𝑎2𝑐2 𝑠1 = 𝑦
Method 01: Solve Using Cramer’s Rule

𝐴1 𝑋 + 𝐵1 𝑌 = 𝐶1
𝑎1 + 𝑎2𝑐2 𝑐1 + 𝑎2𝑠2 𝑠1 = 𝑥
𝐴2 𝑋 + 𝐵2 𝑌 = 𝐶2 −𝑎2𝑠2 𝑐1 + 𝑎1 + 𝑎2𝑐2 𝑠1 = 𝑦
𝐶1 𝐵1
𝐶 𝐵2 𝑎1 + 𝑎2𝑐2 𝑥 − 𝑎2𝑠2𝑦
𝑋= 2 𝑐1 =
𝐴1 𝐵1 𝑎2𝑠2 2 + 𝑎1 + 𝑎2𝑐2 2
𝐴2 𝐵2
𝐴1 𝐶1
𝐴 𝐶2 𝑎2𝑠2𝑥 + 𝑎1 + 𝑎2𝑐2 𝑦
𝑌= 2 𝑠1 =
𝐴1 𝐵1 𝑎2𝑠2 2 + 𝑎1 + 𝑎2𝑐2 2
𝐴2 𝐵2

𝜽𝟏 = 𝒂𝒕𝒂𝒏𝟐[𝒂𝟐𝒔𝟐𝒙 + 𝒂𝟏 + 𝒂𝟐𝒄𝟐 𝒚, 𝒂𝟏 + 𝒂𝟐𝒄𝟐 𝒙 − 𝒂𝟐𝒔𝟐𝒚]

Spring 2024 DR AMIR HAMZA 34
 SCARA Robot
Method 2: Matrix Method
𝐴1 𝑋 + 𝐵1 𝑌 = 𝐶1 𝑎1 + 𝑎2𝑐2 𝑐1 +
𝑎2𝑠2 𝑠1 =
𝑥
𝐴2 𝑋 + 𝐵2 𝑌 = 𝐶2 −𝑎2𝑠2 𝑐1 +
𝑎1 + 𝑎2𝑐2 𝑠1 =
𝑦
A X = B

𝑋 𝑐
𝐴1 𝐵1 𝑋 𝐶 = 1 = 𝐴−1 𝐵
= 1 𝑌 𝑠1
𝐴2 𝐵2 𝑌 𝐶2

1 𝑎1 + 𝑎2𝑐2 𝑥 − 𝑎2𝑠2𝑦
𝐵2 −𝐵1 𝑐1 =
𝐴−1 = 𝑎2𝑠2 2 + 𝑎1 + 𝑎2𝑐2 2
𝐴1 𝐵2 − 𝐵1 𝐴2 −𝐴2 𝐴1
𝑎2𝑠2𝑥 + 𝑎1 + 𝑎2𝑐2 𝑦
𝑠1 =
𝑎2𝑠2 2 + 𝑎1 + 𝑎2𝑐2 2
𝐴−1 . 𝐴. 𝑋 = 𝐴−1 . 𝐵

𝑋 = 𝐴−1 . 𝐵 𝜽𝟏 = 𝒂𝒕𝒂𝒏𝟐[𝒂𝟐𝒔𝟐𝒙 + 𝒂𝟏 + 𝒂𝟐𝒄𝟐 𝒚, 𝒂𝟏 + 𝒂𝟐𝒄𝟐 𝒙 − 𝒂𝟐𝒔𝟐𝒚]

Spring 2024 DR AMIR HAMZA 35

SCARA Robot
Vertical Extension Joint d3
𝑧 = 𝑑1 − 𝑑3 − 𝑑4
𝒅𝟑 = 𝒅𝟏 − 𝒅𝟒 − 𝒛

Tool Roll Joint θ4

𝜽𝟒 = 𝜽𝟏 − 𝜽𝟐 − 𝜽𝟏−𝟐−𝟒

Spring 2024 DR AMIR HAMZA 36

Inverse Manipulator Kinematics – Paul Method
GIVEN:
1. All Link Transformations : 𝟎𝟏𝑻, 𝟏𝟐𝑻 , … , 𝒏−𝟏𝒏𝑻
𝑩
2. Desired Tool Position & Orientation: 𝑾 𝑻
FIND:
Joint Variables 𝜽𝒊 and 𝒅𝒊

PROCEDURE:
𝑩 𝑻 = 𝟎 𝑻 × 𝟏 𝑻 × ⋯ × 𝒏−𝟏 𝑻
𝑾 𝟏 𝟐 𝒏

𝟎 −𝟏
Multiplying Both sides 𝟏
𝑻
with 𝟎 −𝟏
𝟎 𝑻
−𝟏 𝑩 𝑻 ×𝟎 𝑻 × 𝟏 𝑻 × ⋯ × 𝒏−𝟏 𝑻
𝟏 𝑾𝑻 = 𝟏 𝟏 𝟐 𝒏

−𝟏 𝑩
Set of 16
⟹ 𝟎 𝑻 = 𝟏𝟐 𝑻 × ⋯ × 𝒏−𝟏
𝟏 𝑾𝑻 𝒏 𝑻 Equations
Constants or Constants or
function of 𝒒𝟏 function of 𝒒𝟐 … 𝒒𝒏
Spring 2024 DR AMIR HAMZA 37
Inverse Manipulator Kinematics – Paul Method

𝟎 −𝟏 𝑩
𝑻 𝑻 = 𝟏 𝑻 × 𝟐 𝑻 × ⋯ × 𝒏−𝟏 𝑻 Gives 𝜽𝟏or 𝒅𝟏
𝟏 𝑾 𝟐 𝟑 𝒏
Using 𝜽𝟏or 𝒅𝟏
𝟎 −𝟏 𝑩
𝑻 𝑻 = 𝟐 𝑻 × 𝟑 𝑻 × ⋯ × 𝒏−𝟏 𝑻 Gives 𝜽𝟐or 𝒅𝟐
𝟐 𝑾 𝟑 𝟒 𝒏

𝟎 −𝟏 𝑩
𝑻 𝑻 = 𝒊 𝑻 × 𝒊+𝟏 𝑻 × ⋯ × 𝒏−𝟏 𝑻 Gives 𝜽𝒊 or 𝒅𝒊
𝒊 𝑾 𝒊+𝟏 𝒊+𝟐 𝒏

𝒏−𝟐 𝑻 −𝟏 𝑩 𝑻 = 𝒏−𝟏 𝑻 Gives 𝜽𝒏−𝟏or 𝒅𝒏−𝟏

𝒏−𝟏 𝑾 𝒏

Resolution of these equations require Intuition !!

Spring 2024 DR AMIR HAMZA 38
 Example – Paul Method

Puma 560

Spring 2024 DR AMIR HAMZA 39

Inverse Manipulator Kinematics – Paul Method
EXAMPLE: PUMA 560

Spring 2024 DR AMIR HAMZA 40

Inverse Manipulator Kinematics – Paul Method
EXAMPLE: PUMA 560

Spring 2024 DR AMIR HAMZA 41

Inverse Manipulator Kinematics – Paul Method
EXAMPLE: PUMA 560

𝒊 𝜶𝒊−𝟏 𝒂𝒊−𝟏 𝒅𝒊 𝜽𝒊
1 0 0 0 𝜽𝟏
2 −90𝑜 0 0 𝜽𝟐
3 0 𝑎2 𝑑3 𝜽𝟑
4 −90𝑜 𝑎3 𝑑4 𝜽4
5 90𝑜 0 0 𝜽5
6 −90𝑜 0 0 𝜽6
DH – PARAMETERS – PUMA 560

Spring 2024 DR AMIR HAMZA 42

 Inverse Manipulator Kinematics – Paul Method
EXAMPLE: PUMA 560 GIVEN:
 r11 r12 r13 Px 
 r21 r22 r23 Py 
WT =
B  
1. All Link Transformations : .𝟏𝟎 𝑻, .𝟐𝟏 𝑻, … , .𝟔𝟓 𝑻  r31 r32 r33 Pz 
2. Desired Tool Position & Orientation : 𝑩
𝑾𝑻
 0
 0 0 1 

 c1 −s1 0 0  c2 −s2 0 0  c3 −s3 0 a 2 

s
s c1 0 0   0 0 1 0  c 0 0
1T =
0  1
2T =
1  3T =
2  3 3 
0 0 1 0  −s2 −c2 0 0  0 0 1 d3 
 0 0 0 1
0 0 0 1  
 0 0 0 1   

 c4 −s4 0 a3   c5 −s5 0 0  c6 −s6 0 0

 0 1 d 
0 0 0 −1 0   0 0 1 0 
4T =
3  4
5T =
4  6T =
5 
 −s4 −c4 0 0  s5 c5 0 0  − s6 −c6 0 0
    0
 0 0 0 1  0 0 0 1  0 0 1 

Spring 2024 DR AMIR HAMZA 43

Inverse Manipulator Kinematics – Paul Method
GIVEN:
1. All Link Transformations : .𝟎𝟏 𝑻, .𝟐𝟏 𝑻, … , .𝟔𝟓 𝑻
2. Desired Tool Position & Orientation : 𝑾 𝑩 𝑻

FIND:
Joint Variables 𝜽𝟏

PROCEDURE:
𝑩 𝑻 = 𝟎 𝑻 ×𝟏 𝑻 × ⋯×𝟓 𝑻
𝑾 𝟏 𝟐 𝟔

𝟎 𝑻 −𝟏
Multiplying Both sides with 𝟏
𝟎 𝑻 −𝟏 𝑩 𝑻 = 𝟎 𝑻 −𝟏 ×𝟎 𝑻 × 𝟏 𝑻 × ⋯ × 𝟓 𝑻
𝟏 𝑾 𝟏 𝟏 𝟐 𝟔

𝟎 𝑻 −𝟏 𝑩 𝑻 Set of 16
⟹ =𝟏𝑻 ×⋯×𝟓𝑻 Equations
𝟏 𝑾 𝟐 𝟔

Constants or 𝟏
function of 𝜽𝟏 𝟔T
Spring 2024 DR AMIR HAMZA 44
Inverse Manipulator Kinematics – Paul Method
INVERSE OF TRANSFORMATION MATRICES

𝑹 𝑷 𝑹𝑻 −𝑹𝑻𝑷
𝑻= 𝑻−𝟏 =
𝟎 𝟎 𝟎 𝟏 𝟎 𝟎 𝟎 𝟏

 c1 −s1 0 0  c1 s1 0 0 
s c1 0 0   −s c1 0 0 
1T =
0  1 0 −1
T =  1
0 0 1 0 1
 0 0 1 0
0 0
 0 1  
 0 0 0 1


Spring 2024 DR AMIR HAMZA 45

Inverse Manipulator Kinematics – Paul Method
EXAMPLE: PUMA 560
𝟏
𝑻 =𝟏𝑻×𝟐𝑻×𝟑𝑻×𝟒𝑻×𝟓𝑻
𝟔 𝟐 𝟑 𝟒 𝟓 𝟔

𝟏
𝟔𝑻 =

Spring 2024 DR AMIR HAMZA 46

Inverse Manipulator Kinematics – Paul Method
EXAMPLE: PUMA 560
𝟎 𝑻 −𝟏 𝑩 𝑻 =𝟏𝑻×𝟐𝑻×𝟑𝑻×𝟒𝑻×𝟓𝑻
𝟏 𝑾 𝟐 𝟑 𝟒 𝟓 𝟔

Constants or Constants or
function of 𝜽𝟏 function of 𝜽𝟐 … 𝜽𝟔

Set of 16
Equations
=

Spring 2024 DR AMIR HAMZA 47

Inverse Manipulator Kinematics – Paul Method
EXAMPLE: PUMA 560

𝑷 𝒙 𝒔 𝟏 − 𝑷 𝒚 𝒄 𝟏 = 𝒅𝟑
Which is a Type 2 :
𝑿 𝒔𝒊𝒏 𝜽𝒊 + 𝒀 𝒄𝒐𝒔 𝜽𝒊 = 𝒁
Where :
𝑿 = 𝑷𝒙 𝒀 = −𝑷𝒚 𝒁 = 𝒅𝟑

Solution :
𝑿𝒁 + 𝒀 𝑿𝟐 + 𝒀𝟐 − 𝒁𝟐
𝐬𝐢𝐧 𝜽𝒊 =
𝑿𝟐 + 𝒀𝟐

𝒀𝒁 − 𝑿 𝑿𝟐 + 𝒀𝟐 − 𝒁𝟐
𝐜𝐨𝐬 𝜽𝒊 =
𝑿𝟐 + 𝒀𝟐

Spring 2024
We haveDRtwo solution for 𝜽𝟏
AMIR HAMZA 48
Inverse Manipulator Kinematics – Paul Method
GIVEN:
1. All Link Transformations : .𝟏𝟎 𝐓, .𝟐𝟏 𝐓, … , .𝐧𝐧−𝟏 𝐓
2. Desired Tool Position & Orientation : 𝐖 𝐁 𝐓

3. Joint Variable 𝛉𝟏
FIND:
Joint Variables 𝛉𝟐

PROCEDURE:
𝐁𝐓 =𝟎 𝐓 ×𝟏𝐓 ×⋯×𝟓𝐓
𝐖 𝟏 𝟐 𝟔

𝟎 𝐓 −𝟏
Multiplying Both sides with 𝟐
𝟎 𝐓 −𝟏 𝐁 𝐓= 𝟎 𝐓 −𝟏 ×𝟎 𝐓 × 𝟏 𝐓 × ⋯ × 𝟓 𝐓
𝟐 𝐖 𝟐 𝟏 𝟐 𝟔

𝟎 𝐓 −𝟏 𝐁 𝐓 Set of 16
⟹ =𝟐𝐓 × ⋯×𝟓𝐓
𝟐 𝐖 𝟑 𝟔 Equations
Constants or 𝟐
function of 𝛉𝟐 𝟔T
Spring 2024 DR AMIR HAMZA 49
Inverse Manipulator Kinematics – Paul Method
INVERSE OF TRANSFORMATION MATRICES

𝑹 𝑷 𝑹𝑻 −𝑹𝑻𝑷
𝑻= 𝑻−𝟏 =
𝟎 𝟎 𝟎 𝟏 𝟎 𝟎 𝟎 𝟏

 c1 c2 −c1 s2 −s1 0  c1 c2 c2 s1 −s2 0

c s −s1 s 2 c1 0  −c s −s1 s2 −c2 0
 2 1  T = 1 2
0 −1 
2 T =1 T  2 T =
0 0 1
 −s2 −c2 0 0 2
 −s1 c1 0 0
   
 0 0 0 1  0 0 0 1

Spring 2024 DR AMIR HAMZA 50

Inverse Manipulator Kinematics – Paul Method
EXAMPLE: PUMA 560
𝟐
𝑻 =𝟐𝑻×𝟑𝑻×𝟒𝑻×𝟓𝑻
𝟔 𝟑 𝟒 𝟓 𝟔

 1 r11 1
r12 1
r13 1
r14 
1 1 1 1 
r22 r23
2
T =  r21 r24 
6
 1 r31 1
r32 1
r33 1
r34 
 
Where :  0 0 0 1 
1
r11 = −c6 (s3 s5 − c3 c4 c5 ) − c3 s4 s6 1
r13 = −c5 s3 − c3 c4 s5
1
r21c6 (c3 s5 + c4 c5 s3 ) − s3 s4 s6 1
r23 = c3 c5 − c4 s3 s5
1
r31 − c4 s6 − c5 c6 s4 1
r33 = s4 s 5
1
r12 = s6 (s3 s5 − c3 c4 c5 ) − c3 c6 s4 1
r14 = a 2 + a 3 c3 − d 4 s3
1
r22 = −s6 (c3 s5 + c4 c5 s3 ) − c6 s3 s4
1
r =d c + a s
24 4 3 3 3
1
r c s s −c c 1
r34 =d 3
32 5 4 6 4 6
Spring 2024 DR AMIR HAMZA 51
Inverse Manipulator Kinematics – Paul Method
JOINT VARIABLE 𝜽𝟐
𝟎 𝑻 −𝟏 𝑩 𝑻 =𝟐𝑻×𝟑𝑻×𝟒𝑻×𝟓𝑻
𝟐 𝑾 𝟑 𝟒 𝟓 𝟔

Constants or Constants or
function of 𝜽𝟐 function of 𝜽𝟑 … 𝜽𝟔

 c1 c2 c2 s1 −s2 0   r11 r12 r13 px   1 r11 1

r12 1
r13 1
r14 
 −c s 
 1 2 −s1 s2 −c2 0  r21 r22 py  1 1
r22 1
r23 1
 r23 =  r21 r24 
 −s1 c1 0 0 r31 r32 r33 pz   1 r31 1
r32 1
r33 1
r34 
     
 0 0 0 1  0 0 0 1  0 0 0 1 

1
r11 = −c6 (s3 s5 − c3 c4 c5 ) − c3 s 4 s 6 1
r13 = −c5 s3 − c3 c4 s5
1
r21c6 (c3 s5 + c4 c5 s3 ) − s3 s 4 s6 1
r23 = c3 c5 − c4 s3 s5
1
r31 − c4 s6 − c5 c6 s4 1
r33 = s4 s 5
1
r12 = s6 (s3 s5 − c3 c4 c5 ) − c3 c6 s 4 1
r14 = a 2 + a 3 c3 − d 4 s3
1
r22 = −s6 (c3 s 5 + c4 c5 s3 ) − c6 s 3 s 4
1
r =d c + a s
24 4 3 3 3
Spring 2024 1
r32 = c5 s4 s6 − c4 c6 DR AMIR HAMZA 1
r34 =d 3 52
Inverse Manipulator Kinematics – Paul Method
JOINT VARIABLE 𝜽𝟐
Equating both the (1,4) elements and (2,4) elements we get

c1 c2 p x + c2 s1 p y − s2 pz = a 2 + a 3 c3 − d 4 s3
−c1 s2 p x − s1 s2 p y − c2 pz =d 4 c3 + a 3 s3
Rearranging
d4 s3 − a 3 c3 = −(c1 px + s1 p y ) c2 + s2 pz + a 2
d 4 c3 + a 3 s3 = −(c1 p x + s1 p y )s2 − c2 pz

Which is a Type 7 :
𝑾𝟏 𝒔𝒊𝒏 𝜽𝒋 − 𝑾𝟐 𝐜𝐨𝐬 𝜽𝒋 = 𝑿 𝐬𝐢𝐧 𝜽𝒊 − 𝒀 𝐜𝐨𝐬 𝜽𝒊 + 𝒁𝟐
𝑾𝟏 𝐜𝐨𝐬 𝜽𝒋 + 𝑾𝟐 𝐬𝐢𝐧 𝜽𝒋 = 𝑿 𝐜𝐨𝐬 𝜽𝒊 + 𝒀 𝐬𝐢𝐧 𝜽𝒊 + 𝒁𝟏

Where :
𝑾 𝟏 = 𝒅𝟒 𝑾𝟐 = 𝒂𝟑 𝑿 = 𝑷𝒛 𝒀 = (𝒄𝟏𝒑𝒙 + 𝒔𝟏𝒑𝒚) 𝒁𝟐 = 𝒂𝟐
Spring 2024 DR AMIR HAMZA 53
Inverse Manipulator Kinematics – Paul Method
JOINT VARIABLE 𝜽𝟐
After Squaring and Adding gives an equation of form

𝑩𝟏 𝐬𝐢𝐧 𝜽𝟐 + 𝑩𝟐 𝐜𝐨𝐬 𝜽𝟐 = 𝑩𝟑
Where
𝑩𝟏 = 𝟐(𝒁𝟏𝒀 + 𝒁𝟐𝑿)
𝑩𝟐 = 𝟐 𝒁𝟏𝑿 + 𝒁𝟐𝒀
𝑩𝟑 = 𝑾𝟐 + 𝑾𝟐 − 𝑿𝟐 − 𝒀𝟐 − 𝒁𝟐 − 𝒁𝟐
𝟏 𝟐 𝟏 𝟐
and
𝑾 𝟏 = 𝒅𝟒 𝑾𝟐 = 𝒂𝟑 𝑿 = 𝑷𝒛 𝒀 = (𝒄𝟏𝒑𝒙 + 𝒔𝟏𝒑𝒚) 𝒁𝟐 = 𝒂𝟐

Which is a Type-2 equation

We have two solution for 𝜽𝟐

Spring 2024 DR AMIR HAMZA 54

Inverse Manipulator Kinematics – Paul Method
JOINT VARIABLE 𝜽𝟑
d 4 s3 − a 3 c3 = −(c1 px + s1 p y ) c2 + s2 pz + a 2
d 4 c3 + a 3 s3 = −(c1 p x + s1 p y )s2 − c2 pz
Substituting values of 𝜃2 We get equation of Type-3

𝑋1 sin 𝜃𝑖 + 𝑌1 cos 𝜃𝑖 = 𝑍1
𝑋2 sin 𝜃𝑖 + 𝑌2 cos 𝜃𝑖 = 𝑍2
where
𝑋1 = 𝑑4 𝑌1 = −𝑎3 Z1 = −(c1 p x + s1 p y ) c2 + s2 pz + a 2
𝑋2 = 𝑎3 𝑌2 = 𝑑4 Z 2 = −(c1 p x + s1 p y )s2 − c2 pz

𝑌2𝑍1 − 𝑌1𝑍2
sin 𝜃3 =
𝑋1𝑌2 − 𝑋2𝑌1

𝑍2𝑋1 − 𝑍1𝑋2
cos 𝜃3 =
Spring 2024
𝑋 𝑌2 − 𝑋2𝑌1
DR AMIR1HAMZA 55
Inverse Manipulator Kinematics – Paul Method
GIVEN:
1. All Link Transformations : .𝟏𝟎 𝑻, .𝟐𝟏 𝑻, … , .𝒏𝒏−𝟏 𝑻
2. Desired Tool Position & Orientation : 𝑾 𝑩 𝑻

3. Joint Variable 𝜽𝟏 , 𝜽𝟐 , 𝜽𝟑
FIND:
Joint Variables 𝜽𝟒

PROCEDURE:
𝑩 𝑻 = 𝟎 𝑻 ×𝟏 𝑻 × ⋯×𝟓 𝑻
𝑾 𝟏 𝟐 𝟔

𝟎 𝑻 −𝟏
Multiplying Both sides with 𝟑
𝟎 𝑻 −𝟏 𝑩 𝑻 = 𝟎 𝑻 −𝟏 ×𝟎 𝑻 × 𝟏 𝑻 × ⋯ × 𝟓 𝑻
𝟑 𝑾 𝟑 𝟏 𝟐 𝟔

𝟎 𝑻 −𝟏 𝑩 𝑻 Set of 16
⟹ =𝟑𝑻 ×⋯×𝟓𝑻
𝟑 𝑾 𝟒 𝟔 Equations
Constants 𝟑T
𝟔
Spring 2024 DR AMIR HAMZA 56
Inverse Manipulator Kinematics – Paul Method
𝟎 𝑻 −𝟏
TRANSFORMATION MATRICES 𝟑
0
T =0 T 1 T 2 T = Constants
3 1 2 3

 c1 c23 s1 c23 −s23 −a2 c3 

 −c s −s1c23 −c23 a2 s3 
0 −1
T =  1 23 
 −s1 −d 3 
3
c1 0
 
 0 0 0 1 

TRANSFORMATION MATRICES 𝟑𝑻
𝟔
𝟑
𝟔𝑻 = 𝟑𝟔 𝑻 × 𝟒𝟓 𝑻 × 𝟓𝟔 𝑻
 c4 c5 c6 − s4 s6 −c6 s4 − c4 c5 s6 −c4 s5 a3 
 c6 s5 −s5 s 6 c5 d 4
3
T =  
−c4 s6 − c5 c6 s4 c5 s4 s6 − c4 c6 0
6
s4 s5
Spring 2024

 0 0
DR AMIR HAMZA 0 1  57
Inverse Manipulator Kinematics – Paul Method
EXAMPLE: PUMA 560
𝟎 𝑻 −𝟏 𝑩 𝑻 =𝟑𝑻×𝟒 𝑻×𝟓𝑻
𝟑 𝑾 𝟒 𝟓 𝟔

Constants Constants or function

of 𝜽𝟒 , 𝜽𝟓 , 𝜽𝟔

 c1 c23 s1 c23 −s23 −a2c3  r11 r12 r13 px   c4 c5 c6 − s4 s6 −c6 s4 − c4 c5 s6 −c4 s5 a3 

 −c s as  r r
−s c −c r py   c6 s5 −s5 s6 c5 d 4
 1 23 1 23 23 2 3   21 22 23 =  
 −s1 c1 0 −d 3  r31 r32 r33 pz  −c4 s6 − c5 c6 s4 c5 s4 s6 − c4 c6 s4 s5 0 
  
 0 0 0 1  0 0 0 1  
 0 0 0 1 
JOINT VARIABLE 𝜽𝟒
Equating both the (1,3) elements and (3,3) elements we get
𝒓𝟏𝟑𝒄𝟏𝒄𝟐𝟑 + 𝒓𝟐𝟑𝒔𝟏𝒄𝟐𝟑 − 𝒓𝟑𝟑𝒔𝟐𝟑 = −𝒄𝟒𝒔𝟓
−𝒓𝟏𝟑𝒔𝟏 + 𝒓𝟐𝟑𝒄𝟏 = 𝒔𝟒𝒔𝟓

𝜽𝟒 = 𝐚𝐭𝐚𝐧𝟐(−𝒓𝟏𝟑𝒔𝟏 + 𝒓𝟐𝟑𝒄𝟏, 𝒓𝟏𝟑𝒄𝟏𝒄𝟐𝟑 + 𝒓𝟐𝟑𝒔𝟏𝒄𝟐𝟑 − 𝒓𝟑𝟑𝒔𝟐𝟑)

Spring 2024 DR AMIR HAMZA 58
Inverse Manipulator Kinematics – Paul Method
GIVEN:
1. All Link Transformations : .𝟏𝟎 𝑻, .𝟐𝟏 𝑻, … , .𝒏𝒏−𝟏 𝑻
2. Desired Tool Position & Orientation : 𝑾 𝑩 𝑻

3. Joint Variable 𝜽𝟏 , 𝜽𝟐 , 𝜽𝟑
FIND:
Joint Variables 𝜽𝟓

PROCEDURE:
𝑩 𝑻 = 𝟎 𝑻 ×𝟏 𝑻 × ⋯×𝟓 𝑻
𝑾 𝟏 𝟐 𝟔

𝟎 𝑻 −𝟏
Multiplying Both sides with 𝟒
𝟎 𝑻 −𝟏 𝑩 𝑻 = 𝟎 𝑻 −𝟏 ×𝟎 𝑻 × 𝟏 𝑻 × ⋯ × 𝟓 𝑻
𝟒 𝑾 𝟒 𝟏 𝟐 𝟔

𝟎 𝑻 −𝟏 𝑩 𝑻 Set of 16
⟹ =𝟒𝑻 ×⋯×𝟓𝑻
𝟒 𝑾 𝟓 𝟔 Equations
Constants 𝟒
𝟔T
Spring 2024 DR AMIR HAMZA 59
Inverse Manipulator Kinematics – Paul Method
𝟎 𝑻 −𝟏
TRANSFORMATION MATRICES 𝟒
0
T = 0 T 1 T 2 T 3 T
4 1 2 3 4
𝟎 −𝟏
𝟒𝑻 =
s1 s4 +c1 c2 c3 c4 −c1 c4 s2 s3 c2 c3 c4 s1 −c1 s4 −c4 s1 s2 s3 −c2 c4 s3 −c3 c4 s2 d3 s4 −a3 c4 −a2 c3 c4 
c s −c c c s +c s s s s s s s −c c s s −c c c s s +c s s d c +a s +a c s 
4 1 1 2 3 4 1 2 3 4 1 2 3 4 2 3 1 4 1 4 2 3 4 3 2 4 3 4 3 4 2 3 4

 −c1 c2 s3 −c1 c3 s2 −c2 s1 s3 −c3 s1 s2 s2 s3 −c2 c3 a2 s3 −d4 

 
 0 0 0 1 

TRANSFORMATION MATRICES 𝟒𝑻
𝟔
𝟒
𝟔𝑻 = 𝟒𝟓 𝑻 × 𝟓𝟔 𝑻
c5 c6 −c5 s6 −s5 0
 s c 0 0 
4
T =  6 6 
6
c6 s5 −s5 s6 c5 0
 0 1 
Spring 2024  DR 0
AMIR 0
HAMZA  60
Inverse Manipulator Kinematics – Paul Method
EXAMPLE: PUMA 560
𝟎 𝑻 −𝟏 𝑩 𝑻 =𝟒𝑻×𝟓𝑻
𝟒 𝑾 𝟓 𝟔

Constants Constants or
function of 𝜽𝟓 , 𝜽𝟔

 1 l11 1
l12 1
l13 1
l14   r11 r12 r13 px  c5 c6 −c5 s6 −s5 0
 1l 1 1 1     s c6 0 0
l22 l23 l24 
 21  r21 r22 r23 py 
=  6 
 1 l31 1
l32 1
l33 1
l34  r31 r32 r33 pz  c6 s5 −s5 s6 c5 0
     1
 0 0 0 1  0 0 0 1  0 0 0

1
l11 = s1 s4 + c1 c2 c3 c4 − c1 c4 s2 s3 1
l21 = c4 s1 − c1 c2 c3 s4 + c1 s2 s3 s4 1
l31 = −c1 c2 s3 − c1 c3 s2
1
l12 = c2 c3 c4 s1 − c1 s4 − c4 s1 s2 s3 1
l22 = s1 s2 s3 s4 − c2 c3 s1 s4 − c1 c4 1
l32 = −c2 s1 s3 − c3 s1 s2
1
l13 = −c2 c4 s3 − c3 c4 s2 1
l23 = c2 s3 s4 + c3 s2 s4 1
l33 = s2 s3 − c2 c3
1
l =d s − a c − a c c 1
l24 = d 3 c4 + a 3 s4 + a 2 c3 s4 1
l34 = a 2 s3 − d 4
14 3 4 3 4 2 3 4
Spring 2024 DR AMIR HAMZA 61
Inverse Manipulator Kinematics – Paul Method

JOINT VARIABLE 𝜽𝟓
Equating both the (1,3) elements and (3,3) elements we get

𝒓𝟏𝟑 𝒄𝟏𝒄𝟐𝟑𝒄𝟒 + 𝒔𝟏𝒔𝟒 + 𝒓𝟐𝟑 𝒔𝟏𝒄𝟐𝟑𝒄𝟒 − 𝒄𝟏𝒔𝟒 − 𝒓𝟑𝟑 𝒔𝟐𝟑𝒄𝟒 = −𝒔𝟓

𝒓𝟏𝟑 −𝒄𝟏𝒔𝟐𝟑 + 𝒓𝟐𝟑 −𝒔𝟏𝒔𝟐𝟑 + 𝒓𝟑𝟑 −𝒄𝟐𝟑 = 𝒄𝟓

𝜽𝟓 = 𝐚𝐭𝐚𝐧𝟐(𝒔𝟓 , 𝒄𝟓 )

Spring 2024 DR AMIR HAMZA 62

Inverse Manipulator Kinematics – Paul Method
• Given:
• 1. All Link Transformations𝟏: .𝟐𝟎 𝑻, .𝟏 𝑻,𝒏… , .𝒏−𝟏 𝑻
• 2. Desired Tool Position & Orientation𝑾𝑩: 𝑻

• 3. Joint Variable 𝜽𝟏 , 𝜽𝟐 , 𝜽𝟑 , 𝜽𝟒 , 𝜽𝟓
• Find:

Joint Variables 𝜽𝟔
𝑩 𝑻 = 𝟎 𝑻 ×𝟏 𝑻 × ⋯×𝟓 𝑻
PROCEDURE: 𝑾 𝟏 𝟐 𝟔

𝟎 𝑻 −𝟏
Multiplying Both sides with 𝟓

𝟎 𝑻 −𝟏 𝑩 𝑻 = 𝟎 𝑻 −𝟏 ×𝟎 𝑻 × 𝟏 𝑻 × ⋯ × 𝟓 𝑻
𝟓 𝑾 𝟓 𝟏 𝟐 𝟔

𝟎 𝑻 −𝟏 𝑩 𝑻 Set of 16
⟹ =𝟓𝑻
𝟓 𝑾 𝟔 Equations

Constants
Spring 2024 DR AMIR HAMZA 63
Inverse Manipulator Kinematics – Paul Method
TRANSFORMATION MATRICES 𝟓𝑻
𝟔
 c6 −s6 0 0
 0 0 1 0 
6T =
5 
 − s6 −c6 0 0
 0 0 1 
 0

JOINT VARIABLE 𝜽𝟔
Equating both the (3,1) elements and (1,3) elements we get

𝒔𝟔 = −𝒓𝟏𝟏 𝒄𝟏𝒄𝟐𝟑𝒔𝟒 − 𝒔𝟏𝒄𝟒 − 𝒓𝟐𝟏 𝒔𝟏𝒄𝟐𝟑𝒔𝟒 + 𝒄𝟏𝒄𝟒 + 𝒓𝟑𝟏 𝒔𝟐𝟑𝒔𝟒

𝒄𝟔 = 𝒓𝟏𝟏 𝒄𝟏𝒄𝟐𝟑𝒄𝟒 + 𝒔𝟏𝒔𝟒 𝒄𝟓 − 𝒄𝟏𝒔𝟐𝟑𝒔𝟓
+ 𝒓𝟐𝟏 𝒔𝟏𝒄𝟐𝟑𝒄𝟒 − 𝒄𝟏𝒔𝟒 𝒄𝟓 − 𝒔𝟏𝒔𝟐𝟑𝒔𝟓 − 𝒓𝟑𝟐(𝒔𝟐𝟑𝒄𝟒𝒄𝟓 + 𝒄𝟐𝟑𝒔𝟓)

𝜽𝟔 = 𝐚𝐭𝐚𝐧𝟐(𝒔𝟔 , 𝒄𝟔 )

Spring 2024 DR AMIR HAMZA 64

Pieper’s Solution When Three Axes Intersect
• As mentioned earlier, although a completely general robot with six degrees of freedom does not
have a closed-form solution, certain important special cases can be solved.
• Pieper studied manipulators with six degrees of freedom in which three consecutive axes intersect
at a point.
• In this section, we outline the method he developed for the case of all six joints revolute, with the
last three axes intersecting.
• His method applies to other configurations, which include prismatic joints
• When the last three axes intersect, the origins of link frames {4}, {5}, and {6}are all located at this
point of intersection. This point is given in base coordinates as

Position vector
comes from 4th
column fwd
kinematics
transform eq with
i=4

Spring 2024 DR AMIR HAMZA 65

Pieper’s Solution When Three Axes Intersect
where

Using (3.6) for 23𝑇 in above eq yields the following expressions for f1:

Using (3.6) for 01𝑇 and 12𝑇 in above eq

where

Spring 2024 DR AMIR HAMZA 66

Pieper’s Solution When Three Axes Intersect
We now write an expression for the squared magnitude of 0P4ORG, which we will denote as
r = x2 + y2 + z2, and which is seen (from eq on last slide) to be

Using expressions of g1, g2 and g3 found earlier:

We now write this equation, along with the Z-component equation from eq giving 0P4ORG as a system
of two equations in the form

where

Spring 2024 DR AMIR HAMZA 67

Pieper’s Solution When Three Axes Intersect
Last Eq is useful because dependence on θ1 has been eliminated and because dependence on θ2 takes
a simple form

• Now let us consider the solution of this eq for θ3 . We distinguish three cases:
1. If a1 = 0, then we have r = k3, where r is known. The right-hand side (k3) is a function of θ3 only.
𝜃
After the substitution (4.35), a quadratic equation in tan 3 may be solved for θ3
2
2. If sα1 = 0, then we have z = k4, where z is known. Again, after substituting via (4.35), a quadratic
equation arises that can be solved for θ3
3. Otherwise, eliminate s2 and c2 from (4.50) to obtain

This equation, after the (4.35) substitution for θ3, results in an equation of degree 4, which can be
solved for θ3
Having solved for θ3, we can solve (4.50) for θ2 and (4.46) for θ1

Spring 2024 DR AMIR HAMZA 68

Pieper’s Solution When Three Axes Intersect
• To complete our solution, we need to solve for θ4 , θ5, and θ6. These axes intersect, so these joint
angles affect the orientation of only the last link. We can compute them from nothing more than the
rotation portion of the specified goal, 06𝑅

• Having obtained θ1, θ2, and θ3, we can compute 04𝑅 |θ4=0, by which notation we mean the
orientation of link frame {4} relative to the base frame when θ4 = 0. The desired orientation of {6}
differs from this orientation only by the action of the last three joints. Because the problem was
specified as given 06𝑅, we can compute

• For many manipulators, these last three angles can be solved for by using exactly the Z–Y–Z Euler
angle solution given in Chapter 2, applied to 04𝑅 |θ4=0

Spring 2024 DR AMIR HAMZA 69