MTS-417 Introduction to

Robotics

Lecture 4
Inverse Kinematics

1
Introduction
Forward kinematics
• 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.
2
 Video lecture:
https://youtu.be/Nh35v3SZhSw?si=l5_usybsB_xIvtH6

Introduction

Represents mapping of Joint space to Cartesian space.

Forward Kinematics
Compute the position and orientation of the manipulator tip relative to its base as a function of joint
variables Position and orientation of manipulator in cartesian space 𝑥 𝜃 1
𝑟= 𝑦 𝜃 = 𝜃2
𝒓 = 𝑓(𝜽) 𝜃 𝜃3
Inverse Kinematics Vector in joint space containing all joint variables
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.

3
Whether the solution is solvable or not.

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.
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 (singular point??)

• 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

5
Existence of Solutions Explanation video: Time stamp 6:30
https://www.youtube.com/watch?v=Nh35v3SZhSw

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

• Dextrous 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 dextrous workspace is a subset of the reachable workspace.

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.

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

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

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

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:

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

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.

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.

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

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 :

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

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.

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

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

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

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

26
SCARA Robot
a1 a2
• Draw line diagram of the robot

d3
d1

d5

28
SCARA Robot
a1 a2
• Draw infinite lines passing
through the joint axes

d3
d1 𝜃3
𝜃2
d5
𝜃4
𝜃1

29
 𝑍2 𝑍3 , 𝑍4
SCARA Robot 𝑍1
a1 a2
• Name the location of joint
axes along infinite lines

d3
d1 𝜃3
𝜃2
d5
𝜃4
𝜃1

30
SCARA Robot
a1 a2
• Mark frames origins and 𝒁1
assign 𝑍 directions
𝒁2
• 𝑍2 (and onwards) directions d3
can be taken upwards or d1 𝜃3
downwards. We take it
downwards, as it is an 𝜃2 𝒁𝟑 , 𝒁𝟒
assembly robot.
d5
𝜃4
• 𝑍2 and 𝑍3 are parallel, 𝜃1
infinite perpendicular exists
between them. Hence, we
chose this 𝑍2 origin 𝒁𝟓

• As 𝑍3 is prismatic joint,
hence we chose the origin as
shown

31
SCARA Robot
a1 a2
𝒁1
• Assign 𝑋 axes directions 𝑿1 𝑿2
• 𝑍1 and 𝑍2 are parallel,
𝒁2
hence we draw 𝑋1 along
link length. Same is the d1 𝜃3 𝑿3 , 𝑿𝟒
case with 𝑋2
𝜃2 𝒁𝟑 , 𝒁𝟒
• 𝑍3 and 𝑍4 are intersecting,
d5
hence, 𝑋4 should be their 𝜃4
perpendicular. Same is the 𝜃1
case with 𝑋4 𝒁0 𝑿5
𝑿0 𝒁𝟓

• 𝑋5 direction is chosen to
be parallel with 𝑋4

32
 SCARA Robot
a1 a2
𝒁1
𝑎𝑖−1 = 𝑇ℎ𝑒 𝑑𝑖𝑠𝑡𝑎𝑛𝑐𝑒 𝑓𝑟𝑜𝑚 𝑍መ𝑖−1 𝑡𝑜 𝑍መ𝑖 𝑚𝑒𝑎𝑠𝑢𝑟𝑒𝑑 𝑎𝑙𝑜𝑛𝑔 𝑋෠𝑖−1 ; 𝑿1 𝑿2
𝛼𝑖−1 = 𝑇ℎ𝑒 𝑎𝑛𝑔𝑙𝑒 𝑓𝑟𝑜𝑚 𝑍መ𝑖−1 𝑡𝑜 𝑍መ𝑖 𝑚𝑒𝑎𝑠𝑢𝑟𝑒𝑑 𝑎𝑙𝑜𝑛𝑔 𝑋෠𝑖−1 ; 𝒁2
𝑑𝑖 = 𝑇ℎ𝑒 𝑑𝑖𝑠𝑡𝑎𝑛𝑐𝑒 𝑓𝑟𝑜𝑚 𝑋෠𝑖−1 𝑡𝑜 𝑋𝑖 𝑚𝑒𝑎𝑠𝑢𝑟𝑒𝑑 𝑎𝑙𝑜𝑛𝑔 𝑍መ𝑖 ; d1 𝜃3 𝑿3 , 𝑿𝟒
𝜃𝑖 = 𝑇ℎ𝑒 𝑎𝑛𝑔𝑙𝑒 𝑓𝑟𝑜𝑚 𝑋෠𝑖−1 𝑡𝑜 𝑋𝑖 𝑚𝑒𝑎𝑠𝑢𝑟𝑒𝑑 𝑎𝑙𝑜𝑛𝑔 𝑍መ𝑖 ; 𝜃2 𝒁𝟑 , 𝒁𝟒
d5
𝜃4
Link ai-1 𝜶i-1 di 𝜽i 𝜃1
𝒁0 𝑿5
1 0 0 d1 𝜽1
𝑿0 𝒁𝟓
2 a1 180o 0 𝜽2
3 a2 0 d3 (var) 0
4 0 0 0 𝜽4
5 0 0 d5 0
33
 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

35
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 𝑥 x,y, z give the location of end

− effector whereas Phi gives the
0 𝑠1 − 2 − 4 −𝑐1 − 2 − 4 0 𝑎1𝑠1 + 𝑎2𝑠1 2 𝑠𝜑 −𝑐𝜑 0 𝑦
5𝑇 = =
− orientation of the end effector
0 0 −1 𝑑1 − 𝑑3 − 𝑑5 0 0 −1 𝑧
0 0 0 1 0 0 0 1

36
See this video for clarification (contains a quick method to solve equations): https://www.youtube.com/watch?v=1-FJhmey7vk
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

37
 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
Trigonometric Identity = 1
= (𝑎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

38
SCARA Robot
Step 4: Base Joint (θ1)

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

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

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

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

39
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

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

40
 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 . 𝐵 𝜽𝟏 = 𝒂𝒕𝒂𝒏𝟐[𝒂𝟐𝒔𝟐𝒙 + 𝒂𝟏 + 𝒂𝟐𝒄𝟐 𝒚, 𝒂𝟏 + 𝒂𝟐𝒄𝟐 𝒙 − 𝒂𝟐𝒔𝟐𝒚]

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

Tool Roll Joint θ4

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

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

PROCEDURE: 1. Set the desired position matrix equal to the product of all transformation matrices from 0 - n.
𝑩 𝑻 = 𝟎 𝑻 × 𝟏 𝑻 × ⋯ × 𝒏−𝟏 𝑻
𝑾 𝟏 𝟐 𝒏
Gives identity matrix.
𝟎 −𝟏
Multiplying Both sides 𝟏
𝑻
with 𝟎 −𝟏
𝟎 𝑻
−𝟏 𝑩 𝑻 ×𝟎 𝑻 × 𝟏 𝑻 × ⋯ × 𝒏−𝟏 𝑻
𝟏 𝑾𝑻 = 𝟏 𝟏 𝟐 𝒏

−𝟏 𝑩
Set of 16
⟹ 𝟎 𝑻 = 𝟏𝟐 𝑻 × ⋯ × 𝒏−𝟏
𝟏 𝑾𝑻 𝒏 𝑻 Equations
Constants or Constants or
function of 𝒒𝟏 function of 𝒒𝟐 … 𝒒𝒏
43
Inverse Manipulator Kinematics – Paul Method

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

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

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

𝒏−𝟏 𝑾 𝒏

Resolution of these equations require Intuition !!

44
Example – Paul Method

Puma 560

45
Inverse Manipulator Kinematics – Paul Method
EXAMPLE: PUMA 560

46
Inverse Manipulator Kinematics – Paul Method
EXAMPLE: PUMA 560

47
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

48
 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 

49
Inverse Manipulator Kinematics – Paul Method
• Given
• 1. All link transformations 01𝑇, 12𝑇, … , 56𝑇
𝐵
• 2. Desired Tool Position and Orientation 𝑊𝑇

• Find
• Joint Variables 𝜃1
Procedure
𝐵
𝑊𝑇 = 01𝑇 × 12𝑇 × ⋯ ×, 56𝑇
0 −1
Multiplying both sides with 1𝑇
𝟎𝑻 −𝟏 𝑩 −𝟏
𝑻= 𝟎𝑻 ×𝟎 𝑻 × 𝟏 𝑻 × ⋯ × 𝟓 𝑻
𝟏 𝑾 𝟏 𝟏 𝟐 𝟔

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

Constants or 𝟏
function of 𝜽𝟏 𝟔T
51
Inverse Manipulator Kinematics – Paul Method
INVERSE OF TRANSFORMATION MATRICES

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

𝑐1 −𝑠1 0 0 𝑐1 𝑠1 0 0
0 𝑠1 𝑐1 0 0 0 −1 −𝑠1 𝑐1 0 0
1𝑇 = 1𝑇 =
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1

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

𝟏
𝟔𝑻 =

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

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

Set of 16
Equations
=

54
Inverse Manipulator Kinematics – Paul Method
EXAMPLE: PUMA 560

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

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

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

We have two solution for 𝜽𝟏 55

 Now for theta_2
Inverse Manipulator Kinematics – Paul Method
• Given
1. All link transformations 01𝑇, 12𝑇, … , 56𝑇
𝐵
2. Desired Tool Position and Orientation 𝑊 𝑇
3. Joint variable 𝜃1

• Find
• Joint Variables 𝜃1

𝐵
𝑊𝑇 = 01𝑇 × 12𝑇 × ⋯ ×, 56𝑇
0 −1
Multiplying both sides with 2𝑇
𝟎 𝑻 −𝟏 𝑩 𝑻 = 𝟎 𝑻 −𝟏 ×𝟎 𝑻 × 𝟏 𝑻 × ⋯ × 𝟓 𝑻
2 𝑾 𝟏 𝟏 𝟐 𝟔

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

Constants or 𝟏
function of 𝜽2 𝟔T
56
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

57
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
58
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 s4 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
1
r32 = c5 s4 s6 − c4 c6 1
r34 =d 3 59
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 :
𝑾 𝟏 = 𝒅𝟒 𝑾𝟐 = 𝒂𝟑 𝑿 = 𝑷𝒛 𝒀 = (𝒄𝟏𝒑𝒙 + 𝒔𝟏𝒑𝒚) 𝒁𝟐 = 𝒂𝟐
60
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 𝜽𝟐

61
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 =
𝑋1𝑌2 − 𝑋2𝑌1 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 𝟑T
𝟔
63
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

 0 0 0 1  64
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   1 
 0 0 0
JOINT VARIABLE 𝜽𝟒
Equating both the (1,3) elements and (3,3) elements we get
𝒓𝟏𝟑𝒄𝟏𝒄𝟐𝟑 + 𝒓𝟐𝟑𝒔𝟏𝒄𝟐𝟑 − 𝒓𝟑𝟑𝒔𝟐𝟑 = −𝒄𝟒𝒔𝟓
−𝒓𝟏𝟑𝒔𝟏 + 𝒓𝟐𝟑𝒄𝟏 = 𝒔𝟒𝒔𝟓

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

65
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
66
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 c6 0 0
4
T =  6
6
c6 s5 −s5 s6 c5 0
 0 
 0 0 1 67
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
68
Inverse Manipulator Kinematics – Paul Method

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

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

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

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

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

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

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

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

71
Solution of Transcendental Equations
TYPE 2
𝑿 𝒔𝒊𝒏 𝜽𝒊 + 𝒀 𝒄𝒐𝒔 𝜽𝒊 = 𝒁
Where : 𝑿 and 𝒀 are constants
𝒀 𝒄𝒐𝒔 𝜽𝒊 = 𝒁 − 𝑿 𝒔𝒊𝒏 𝜽𝒊

Taking Square 𝒀𝟐 𝒄𝒐𝒔𝟐 𝜽𝒊 = 𝒁𝟐 − 𝟐𝒁𝒀 𝐬𝐢𝐧 𝜽𝒊 + 𝑿𝟐 𝒔𝒊𝒏𝟐 𝜽𝒊

𝒀𝟐 (𝟏 − 𝐬𝐢𝐧𝟐 𝜽𝒊 ) = 𝒁𝟐 − 𝟐𝒁𝒀 𝐬𝐢𝐧 𝜽𝒊 + 𝑿𝟐 𝒔𝒊𝒏𝟐 𝜽𝒊

Rearranging 𝑿𝟐 + 𝒀𝟐 𝒔𝒊𝒏𝟐 𝜽𝒊 − 𝟐𝒁𝒀 𝒔𝒊𝒏 𝜽𝒊 + 𝒀𝟐 + 𝒁𝟐

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

𝒀𝒁 ± 𝑿 𝑿𝟐 + 𝒀𝟐 − 𝒁𝟐
𝐜𝐨𝐬 𝜽𝒊 =
𝑿𝟐 + 𝒀𝟐
Solution of Transcendental Equations
TYPE 3
𝑿𝟏 𝒔𝒊𝒏 𝜽𝒊 + 𝒀𝟏 𝒄𝒐𝒔 𝜽𝒊 = 𝒁𝟏
𝑿𝟐 𝒔𝒊𝒏 𝜽𝒊 + 𝒀𝟐 𝒄𝒐𝒔 𝜽𝒊 = 𝒁𝟐

Eliminate Either 𝐜𝐨𝐬 𝜽𝒊 or 𝐬𝐢𝐧 𝜽𝒊

Multiplying with 𝐘𝟐 𝑿𝟏 𝒀𝟐 𝒔𝒊𝒏 𝜽𝒊 + 𝒀𝟏 𝒀𝟐 𝒄𝒐𝒔 𝜽𝒊 = 𝒀𝟐 𝒁𝟏

Multiplying with 𝒀𝟏 𝑿𝟐 𝒀𝟏 𝒔𝒊𝒏 𝜽𝒊 + 𝒀𝟏 𝒀𝟐 𝒄𝒐𝒔 𝜽𝒊
= 𝒀𝟏𝒁 𝟐

𝒀𝟐𝒁 𝟏 − 𝒀 𝟏 𝒁 𝟐
𝐬𝐢𝐧 𝜽𝒊 =
𝑿 𝟏𝒀𝟐 − 𝑿 𝟐 𝒀 𝟏
Similarly
𝒁 𝟐𝑿 𝟏 − 𝒁 𝟏𝑿 𝟐
𝐜𝐨𝐬 𝜽𝒊 =
𝑿 𝟏𝒀𝟐 − 𝑿 𝟐𝒀𝟏
Solution of Transcendental Equations
TYPE 4
𝑿𝟏 𝐝𝐣 𝒔𝒊𝒏 𝜽𝒊 = 𝒀𝟏
𝑿𝟐 𝐝𝐣 𝐜𝐨𝐬 𝜽𝒊 = 𝒀𝟐
Squaring both equations and adding
𝑿𝟐 𝒅𝟐 𝐬𝐢𝐧𝟐 𝜽𝒊 + 𝑿𝟐 𝒅𝟐 𝒄𝒐𝒔𝟐 𝜽𝒊 = 𝒀𝟐 + 𝒀𝟐
𝟏 𝒋 𝟐 𝒋 𝟏 𝟐

𝑿𝟐 𝒅𝟐 (𝐬𝐢𝐧𝟐 𝜽𝒊 + 𝒄𝒐𝒔𝟐 𝜽𝒊) = 𝒀𝟐 + 𝒀𝟐

𝟏 𝒋 𝟏 𝟐

𝑿𝟐 𝒅𝟐 (𝐬𝐢𝐧𝟐 𝜽𝒊 + 𝒄𝒐𝒔𝟐 𝜽𝒊) = 𝒀𝟐 + 𝒀𝟐

𝟏 𝒋 𝟏 𝟐

𝒀𝟐 + 𝒀𝟐
𝟏 𝟐
𝒅𝒋 =
𝑿𝟏
After substituting 𝒅𝒊we obtain 𝜽𝒋 by solving Type 3 equation
Solution of Transcendental Equations
TYPE 5
𝑿𝟏 𝒔𝒊𝒏 𝜽𝒊 = 𝒀𝟏 + 𝒁𝟏 𝐝𝐣

𝑿𝟐 𝐜𝐨𝐬 𝜽𝒊 = 𝒀𝟐 + 𝒁𝟐 𝒅𝒋

1. Eliminate 𝐜𝐨𝐬 𝜽𝒊
a. Normalize
b. Squaring and Add
2. Substituting 𝒅𝒋 to obtain type 3 equations
Solution of Transcendental Equations
TYPE 6
𝑾 𝒔𝒊𝒏 𝜽𝒋 = 𝑿 𝐜𝐨𝐬 𝜽𝒊 + 𝒀 𝐬𝐢𝐧 𝜽𝒊 + 𝒁𝟏
𝑾 𝐜𝐨𝐬 𝜽𝒋 = 𝑿 𝐬𝐢𝐧 𝜽𝒊 − 𝒀 𝐜𝐨𝐬 𝜽𝒊 + 𝒁𝟐

After Squaring and Adding

𝑩𝟏 𝐬𝐢𝐧 𝜽𝒊 + 𝑩𝟐 𝐜𝐨𝐬 𝜽𝒊 = 𝑩𝟑

Where
𝑩𝟏 = 𝟐(𝒁𝟏𝒀 + 𝒁𝟐𝑿)

𝑩𝟐 = 𝟐 𝒁𝟏𝑿 + 𝒁𝟐𝒀
𝑩𝟑 = 𝑾𝟐 − 𝑿𝟐 − 𝒀𝟐 − 𝒁𝟐 − 𝒁𝟐
𝟏 𝟐

After substituting 𝜽𝒊 we obtain 𝜽𝒋 by solving type 3 equation

Solution of Transcendental Equations
TYPE 7
𝑾𝟏 𝐜𝐨𝐬 𝜽𝒋 + 𝑾𝟐 𝐬𝐢𝐧 𝜽𝒋 = 𝑿 𝐜𝐨𝐬 𝜽𝒊 + 𝒀 𝐬𝐢𝐧 𝜽𝒊 + 𝒁𝟏
𝑾𝟏 𝒔𝒊𝒏 𝜽𝒋 + 𝑾𝟐 𝐜𝐨𝐬 𝜽𝒋 = 𝑿 𝐬𝐢𝐧 𝜽𝒊 − 𝒀 𝐜𝐨𝐬 𝜽𝒊 + 𝒁𝟐

After Squaring and Adding gives type 2 equation of form

𝑩𝟏 𝐬𝐢𝐧 𝜽𝒊 + 𝑩𝟐 𝐜𝐨𝐬 𝜽𝒊 = 𝑩𝟑

Where

𝑩𝟏 = 𝟐(𝒁𝟏𝒀 + 𝒁𝟐𝑿)

𝑩𝟐 = 𝟐 𝒁𝟏𝑿 + 𝒁𝟐𝒀

𝑩𝟑 = 𝑾𝟐 + 𝑾𝟐 − 𝑿𝟐 − 𝒀𝟐 − 𝒁𝟐 − 𝒁𝟐
𝟏 𝟐 𝟏 𝟐

After substituting 𝜽𝒊 we obtain 𝜽𝒋 by solving type 3 equation

Solution of Transcendental Equations
TYPE 8
𝑿 𝐜𝐨𝐬 𝜽𝒊 + 𝒀 𝐜𝐨𝐬 𝜽𝒊 + 𝜽𝒋 = 𝒁𝟏
𝑿 𝐬𝐢𝐧 𝜽𝒊 + 𝒀 𝐬𝐢𝐧 𝜽𝒊 + 𝜽𝒋 = 𝒁𝟐

After Squaring and Adding gives type 2 equation of form

𝒁𝟐𝟏 + 𝒁𝟐𝟐 − 𝑿𝟐 − 𝒀𝟐
𝐜𝐨𝐬 𝜽𝒋 =
𝟐𝑿𝒀

After Substitution of 𝜽𝒋 System reduces to two equations in 𝜽𝒊

𝑩𝟏 𝒁 𝟐 − 𝑩𝟐 𝒁 𝟏
𝐬𝐢𝐧 𝜽𝒊 = 𝑩𝟐 + 𝑩𝟐
𝟏 𝟐

𝑩𝟏 𝒁 𝟏 + 𝑩𝟐 𝒁 𝟐
𝐜𝐨𝐬 𝜽𝒊 = 𝑩𝟐 + 𝑩𝟐
𝟏 𝟐
where
𝑩𝟏 = 𝑿 + 𝒀 𝐜𝐨𝐬 𝜽𝒋 𝑩𝟐 = 𝒀 𝒔𝒊𝒏 𝜽𝒋
 PUMA 560
• This figure shows a detail of the forearm of the robot PUMA 560
• Note that the frame {0} (not shown) is coincident with frame {1} when θ1 is zero. Note also that,
for this robot, as for many industrial robots, the joint axes of joints 4, 5, and 6 all intersect at a
common point, and this point of intersection coincides with the origin of frames {4}, {5}, and {6}.
• Furthermore, the joint axes 4, 5, and 6 are mutually orthogonal. This wrist mechanism is illustrated
schematically in Fig.
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 revolute joints, 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

80
Pieper’s Solution When Three Axes Intersect
• For i=4
𝑐3 −𝑠4 0 𝑎3
𝑠4 𝑐𝛼3 𝑐4 𝑐𝛼3 −𝑠𝛼3 −𝑠𝛼3 𝑑4
𝑠4 𝑠𝛼3 𝑐4 𝑠𝛼3 𝑐𝛼3 𝑐𝛼3 𝑑4
0 0 0 1

Using the fourth column, we can write

𝑎3
−𝑠𝛼3 𝑑4
0𝑃
4𝑜𝑟𝑔 = 01𝑇 12𝑇 23𝑇3𝑃4𝑜𝑟𝑔 0 1 2
= 1𝑇 2𝑇 3𝑇
𝑐𝛼3 𝑑4
1

Where
Pieper’s Solution When Three Axes Intersect
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

82
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

83
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

84
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

85