Introduction to Robotics

Chapter 6. Inverse Kinematics

Dr. Tran Minh Thien

HCMUTE, Faculty of Mechanical Engineering

Department of Mechatronics
Introduction to Robotics 2 C6. Inverse Kinematics

• What are the joint variables for a given configuration of a robot?

⇒ This is the problem to be answered by inverse kinematic analysis.
• Determination of the joint variables reduces to solving a set of
nonlinear coupled.
• The main difficulty of inverse kinematic is the multiple solutions.

Multiple solution for inverse kinematic problem of a planar 2R manipulator

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 3 C6. Inverse Kinematics

6.1. Decoupling Technique

• Computer-controlled robots are usually actuated in the joint variable

space; however, objects to be manipulated are usually expressed in
the global Cartesian coordinate frame.

• To control the configuration of the end-effector to reach an object,

the inverse kinematics problem must be solved.
⇒ The required values of joint variables are to reach a desired point
in a desired orientation.

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 4 C6. Inverse Kinematics

6.1. Decoupling Technique

• Determination of joint variables in terms of the end-effector

position and orientation is called inverse kinematics.
• Mathematically, inverse kinematics is searching for the elements of
joint variable vector q,

• A transformation 0Tn is given as a function of the joint variables q1,

q2, q3, …, qn.

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 5 C6. Inverse Kinematics

6.1. Decoupling Technique

Solution of first type of trigonometric equation
The first type of trigonometric equation for the unknown angle θ is a
linear combination of cos θ and sin θ.

This equation can be solved by introducing two new variables r and

𝜙 such that:

Substituting the new variables

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 6 C6. Inverse Kinematics

6.1. Decoupling Technique

Solution of first type of trigonometric equation

The unknown angle θ of the trigonometric equation is:

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 7 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 1: Inverse kinematics for 2R planar manipulator
Figure illustrates a 2R planar manipulator with two R∥R links. The
forward kinematics of the manipulator as:

The global position of the tip point of the manipulator is at:

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 8 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 1: Inverse kinematics for 2R planar manipulator

C1: To find θ2, we use

where,

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 9 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 1: Inverse kinematics for 2R planar manipulator
C2: Let us employ the half angle formula,

Find θ2 using an atan2 function,

where,

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 10 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 1: Inverse kinematics for 2R planar manipulator
• The first joint variable θ1 of an elbow up/down configuration can
geometrically be found from:

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 11 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 2: An articulated manipulator.
Consider an articulated manipulator as is shown in Figure. The links
of the manipulator are R⊢R(90), R∥R(0), R⊢R(90)

The forward kinematics of the manipulator

is:

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 12 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 2: An articulated manipulator.
The tip point P is at:

The first angle can be found from:

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 13 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 2: An articulated manipulator.
⇒ That is:

We may combine the first and second elements of 0dP to find:

Rewrite the third component as:

A combining Equations provides:

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 14 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 2: An articulated manipulator.

That is a trigonometric equation of the form:

⇒ We solve this equation for θ2

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 15 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 2: An articulated manipulator.
The third element of 0dP determines θ3:

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 16 C6. Inverse Kinematics

6.1. Decoupling Technique

The result of forward kinematics of such a six DOF multibody is a
4 × 4 transformation matrix.

It is possible to decouple the inverse kinematics problem into two

subproblems, known as inverse position and inverse orientation
kinematics.

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 17 C6. Inverse Kinematics

6.1. Decoupling Technique

Following the decoupling principle, the overall transformation
matrix of a robot can be decomposed to a translation and a rotation.

• The translation matrix 0D6 indicates the position of the end-

effector in the base frame B0 and involves only the three joint
variables of the manipulator. We will solve 0d6 for the variables that
control the wrist position.
• The rotation matrix 0R6 indicates the orientation of the end-effector
in B0 and involves only the three joint variables of the wrist.

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 18 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 3: Inverse kinematics of an articulated robot.
The decoupling method will be reviewed in this example for a 6
DOF. The forward kinematics of the articulated robot, illustrated in
Figure.

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 19 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 3: Inverse kinematics of an articulated robot.
Transformation matrix of the end-
effector was found

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 20 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 3: Inverse kinematics of an articulated robot.
where,

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 21 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 3: Inverse kinematics of an articulated robot.

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 22 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 3: Inverse kinematics of an articulated robot.
The wrist position vector d = [X Y Z]T , which is [t14 t24 t34]T of 0T7
for d7 = 0, and (X, Y, Z) are coordinates of the position of the wrist
point.

It can be seen that

Combining the first two elements of d gives

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 23 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 3: Inverse kinematics of an articulated robot.
Then, the third element of d may be utilized to find

which can be rearranged to the following form

with two solutions:

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 24 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 3: Inverse kinematics of an articulated robot.
Summing the squares of the elements of d gives

Find the orientation of the end-effector by solving 3T6 or 3R6 for θ4,
θ5 , θ6

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 25 C6. Inverse Kinematics

6.1. Decoupling Technique

Ex 3: Inverse kinematics of an articulated robot.

The angles θ4, θ5, θ6 can be found by examining elements of 3R6.

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 26 C6. Inverse Kinematics

6.2. Inverse Transformation Technique

• Assume we have the 4×4 transformation matrix 0T6 from forward
kinematics expressed by numbers. The matrix 0T6 includes the global
position and the orientation of the end-effector of a 6 DOF robot in
the base frame B0.
• Assume the individual transformation matrices 0T1(q1), 1T2(q2),
2T (q ), 3T (q ), 4T (q ), and 5T (q ) are known as functions of joint
3 3 4 4 5 5 6 6
variables analytically.

• According to forward kinematics we have:

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 27 C6. Inverse Kinematics

6.2. Inverse Transformation Technique

• The inverse kinematics problem as follows

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 28 C6. Inverse Kinematics

6.2. Inverse Transformation Technique

Ex 4: Articulated manipulator and numerical case.
Here is the use of inverse transformation technique to solve its
inverse kinematics. Consider the articulated manipulator shown in
Figure.

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 29 C6. Inverse Kinematics

6.2. Inverse Transformation Technique

Ex 4: Articulated manipulator and numerical case.
The forward kinematics of the manipulator is:

Therefore, we attach a coordinate frame B4 at P that is at a constant

distance l3 from B3

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 30 C6. Inverse Kinematics

6.2. Inverse Transformation Technique

Ex 4: Articulated manipulator and numerical case.
The overall forward kinematics of the manipulator is:

Using the following dimensions: l1= 1m, l2= 1.05m, l3= 0.89m

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 31 C6. Inverse Kinematics

6.2. Inverse Transformation Technique

Ex 4: Articulated manipulator and numerical case.
Let us multiply both sides of 0T4 by 0T1−1 to have:

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 32 C6. Inverse Kinematics

6.2. Inverse Transformation Technique

Ex 4: Articulated manipulator and numerical case.
Let us multiply both sides of 0T4 by 0T1−1 to have:

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 33 C6. Inverse Kinematics

6.2. Inverse Transformation Technique

Ex 4: Articulated manipulator and numerical case.
Equating the element r24 of both sides of provides an equation to
determine θ1.

Substituting θ1 = 0.83298 rad in provides a matrix 1T4

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 34 C6. Inverse Kinematics

6.2. Inverse Transformation Technique

Ex 4: Articulated manipulator and numerical case.
We multiply both sides of 1T2−1 to have:

where,

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 35 C6. Inverse Kinematics

6.2. Inverse Transformation Technique

Ex 4: Articulated manipulator and numerical case.

Squaring the elements r14 and r24 of the left-hand sides, to

determine θ2.

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 36 C6. Inverse Kinematics

6.2. Inverse Transformation Technique

Ex 4: Articulated manipulator and numerical case.

Having θ2, we can calculate θ3 from the last column

If θ2= 0.7555 rad,

If θ2= ‒0.0.488 rad,

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 37 C6. Inverse Kinematics

6.3. Iterative Technique

The inverse kinematics problem of robots can be interpreted as
searching for the unknowns qk of a set of nonlinear algebraic equations

where n is the number of degree of freedom (DOF) of the robot.

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 38 C6. Inverse Kinematics

6.3. Iterative Technique

The most common method is known as the Newton-Raphson
method. The iteration technique can be set in an algorithm.
Inverse kinematics iteration technique.
1. Set the initial counter i = 0.
2. Find or guess an initial estimate q(0).
3. Calculate the residue δT(q(i)) = J(q(i)) δq(i).
If every element of T(q(i)) or its norm , ,T(q(i)), , is less than a
tolerance, , ,T(q(i)), , < then terminate the iteration. The q(i) is
the desired solution.
4. Calculate q(i+1) = q(i) + J-1(q(i)) δT(q(i)).
5. Set i = i + 1 and return to step 3.

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 39 C6. Inverse Kinematics

6.3. Iterative Technique

The most common method is known as the Newton-Raphson
method. The iteration technique can be set in an algorithm.
Inverse kinematics iteration technique.

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 40 C6. Inverse Kinematics

6.3. Iterative Technique

The tolerance ϵ can equivalently be set up on variables

Or, the condition Jacobian J:

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 41 C6. Inverse Kinematics

6.3. Iterative Technique

Ex 5: Inverse kinematics for a 2R planar
manipulator.
The position of tip point of a 2R planar manipulator is calculated

Define

The Jacobian
of the equations

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 42 C6. Inverse Kinematics

6.3. Iterative Technique

Ex 4: Inverse kinematics for a 2R planar
manipulator.
The inverse of the Jacobian is

The iterative formula is set up as:

Assume,

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 43 C6. Inverse Kinematics

6.3. Iterative Technique

Ex 4: Inverse kinematics for a 2R planar
manipulator.
Start from a guess value q(0)

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 44 C6. Inverse Kinematics

6.3. Iterative Technique

Ex 4: Inverse kinematics for a 2R planar
manipulator.
Therefore,

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 45 C6. Inverse Kinematics

6.3. Iterative Technique

Ex 4: Inverse kinematics for a 2R planar
manipulator.
Iteration 1.

Iteration 2.

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 46 C6. Inverse Kinematics

6.2. Inverse Transformation Technique

Ex 4: Inverse kinematics for a 2R planar
manipulator.
Iteration 3.

Iteration 4.

The result of the fourth iteration q(4) is close enough to the exact
value q = [ π/2 −π/2]T .

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien
Introduction to Robotics 47 C6. Inverse Kinematics

C6. End!

HCMUTE, Faculty of Mechanical Engineering, Dept. of Mechatronics © Dr. Tran Minh Thien