Chapter 1

Robotic Fundamentals
 Robotic fundamentals
• Introduction
• Rigid body motion
• Transformation of coordinates
• Robot kinematics
 Forward kinematics
 Inverse kinematics

10/06/2023 2
 Robot Defined
• Word robot was coined by a Czech novelist Karel
Capek in a 1920 play titled Rassum’s Universal
Robots (RUR)
• Robot in Czech is a word for worker or servant.
• Robot–Any automated machine programmed to
perform specific mechanical functions in the
manner of a man.
• Robotics–the science dealing with design,
construction and operation of robots.

10/06/2023 3
 Definition of robot:

Robot Institute of America (RIA)

 A robot is a reprogrammable, multifunctional manipulator

designed to move material, parts, tools or specialized devices
through variable programmed motions for the performance of a
variety of tasks: 1979

Engineering point of view:

 Robots are complex, versatile devices that contain a mechanical
structure, a sensory system, and an automatic control system.
10/06/2023 4
 Classification of Robots…Locomotion
1. Stationary Robot

Serial Robots
 Accumulated error Parallel Robots
 Less rigidity  Greater rigidity
 Less accurate  Higher speed
 More workspace  Higher accuracy
 Limited workspace 5
10/06/2023
Classification of Robots…Locomotion…
Legged and wheeled Robot
 Classification of Robots…Locomotion…
3. Autonomous Underwater and Unmanned Aerial Vehicle

10/06/2023 7
 Classification of Robots ….Applications

1. Industrial Robots

Coating/Painting

Assembly of an
automobile
10/06/2023 8
Drilling/ Welding/Cutting
 Classification of Robots ….Applications…..
2. Locomotion/Exploration Robots

Underwater exploration

Space Exploration
10/06/2023 9
Robo-Cop
 Classification of Robots ….Applications….
3. Medical Robot
a) Medical robot for head surgery
b) Surgical robot used in spine surgery, redundant
manual guidance.
c) Autoclavable instrument guidance (4 DoF) for
milling, drilling, endoscope guidance and biopsy
applications
Robots assist in high precision
surgeries such as brain and heart
surgery. They are also used to test
quality control in pharmaceuticals.
10/06/2023 10
 Classification of Robots ….Applications….

4. House-hold/Entertainment Robots

Toys
Asimov
Robot vacuum cleaners, Robot pool cleaners, sweepers,
gutter cleaners
10/06/2023 11
 Classification of Robots ….Applications….
5. Military Robots

Tracked Robots
Military robots are autonomous robots or remote-controlled
mobile robots designed for military applications, from
transport to search & rescue and attack. This type of robots
includes bomb disposal robots, different transportation robots,
exploration/reconnaissance drones.
 Introduction
Application example
– Mars exploration (NASA Viking Mission, 1975)
 Uses of Robot
1. Jobs that are dangerous for humans

e.g. Decontaminating Robot

Cleaning the main circulating pump housing in
the nuclear power plant
10/06/2023 14
 Uses of Robot…
2. Repetitive jobs that are boring, stressful, or
labor intensive for humans

Welding Robot
10/06/2023 15
 Uses of Robot…..
3. Menial tasks that human don’t want to do

The SCRUBMATE Robot

10/06/2023 16
 Robotics Discipline
Robot is a multi-disciplinary engineering device
• Mechanical engineering deals with the design of
mechanical components, arms, end-effectors, and also is
responsible for kinematics, dynamics and control
analyses of robots.
• Electrical engineering works on robot actuators,
sensors, power, and control systems.
• System design engineering deals with perception,
sensing, and control methods of robots.
• Programming, or software engineering, is responsible
for logic, intelligence, communication, and networking.
Robot as a system, consists of a manipulator or rover, a wrist,
an end-effector, actuators, sensors, controllers, processors, and
software.
10/06/2023 18
 Knowledgebase for Robotics
Typical knowledgebase for the design and
operation of robotics systems
 Dynamic system modeling and analysis
 Feedback control
 Sensors and signal conditioning
 Actuators (muscles) and power electronics
 Hardware/computer interfacing
 Computer programming

10/06/2023 19
 Advantage and Disadvantage of Robots
Advantage
• Increase productivity, safety, efficiency, quality, and consistency of products.
• Deliver relief from tiresome, monotonous, or hazardous works
• Perform many operations better than people do
• They provide higher accuracy and repeatability
• Used in extreme environments.
• can process multiple stimuli or tasks simultaneously.
Disadvantage
unemployment will increase and may people who can not get work will be-
come poorer
 Lack capability to respond in emergencies.
 have limited capabilities in Degree of freedom, Sensors, Vision system, real
time response.
 they are costly, due to Initial cost of equipment, Installation costs, Need for
Peripherals, Need for training, Need for programming .
10/06/2023 20
 Rigid body motion
• Degree of freedom
• Type of links and joints
• Type of motion
• Description of Position, Orientation, Coordinate
Transformations
• Kinematics

10/06/2023 21
 Degrees of Freedom (DOF)
• The system's DOF is equal to the number of
independent parameters (measurements) which
are needed to uniquely define its position in
space at any instant of time.
• A rigid body in plane motion has three DOF.
• Any rigid body in three-space has six degrees of
freedom

10/06/2023 22
Degree of freedom
Robot arms are described by
their degrees of freedom. This
number typically refers to the
number of single-axis
rotational joints in the arm,
where higher number
indicates an increased
flexibility in positioning a
tool.
 Components and Structure of Robots
Links
• A link is an (assumed) rigid body which have at least two
nodes which are points for attachment to other links.
Some of the common types of links are:
Binary link - one with two nodes.
Ternary link - one with three nodes.
Quaternary link - one with four nodes.
Pentagonal link – one with five nodes.
Hexagonal link – one with six nodes

10/06/2023 24
 Joints
• A joint is a connection between two or more links (at
their nodes), which allows some motion, or potential
motion, between the connected links. Joints (also
called kinematic pairs) can be classified in several
ways:
1. By the type of contact between the elements, line,
point, or surface.
2. By the number of degrees of freedom allowed at the
joint.
3. By the type of physical closure of the joint: either
force or form closed.
4. By the number of links joined (order of the joint). 25
10/06/2023
 Types of Joints

1. Classification by the Type of Contact

We can classify Joints by the type of contact as Lower Pairs
or Higher Pairs. If joints have surface contact, they are
called Lower pair (as with a pin surrounded by a hole). If
joints have point or line contact, they are called Higher pair.
• The main practical advantage of lower pairs over higher pairs
is their better ability to trap lubricant between their
enveloping surfaces.

10/06/2023 26
• The six possible lower pairs are:
1. Revolute (R)
2. Prismatic (P)
3. Screw/Helical (H)
4. Cylindrical (C)
5. Spherical (S) and Revolute Joint – 1 DOF
6. Planar/Flat (F)

Prismatic Joint- 1 DOF

Cylindrical – 2DOF

Spherical Joint- 327DOF

Helical – 1DOF
10/06/2023
More……….Lower pair joints
2. Classification by the Number of Degrees of
Freedom allowed at the joint
We can classify Joints by the number of degrees
of freedom allowed at the joint as One-Freedom
Joints or Full Joints, Two Freedom Joints or Half
Joints and Three Freedom Joints.
• Examples of one freedom joints are : a rotating
pin joint (R) and a translating slider Joint (P).
• Examples of two freedom joints are: link against
plane and a pin in slot.
• Examples of three freedom joints are: a
spherical, or ball-and-socket joints.
10/06/2023 29
 (Form closed
(force closed)
10/06/2023 30
3. Classification by the Type of Physical Closure of
the Joint
We can classify Joints by the type of physical
closure of the joint as Force closed or Form
closed.
• A form-closed joint is kept together or closed by its
geometry. A pin in a hole or a slider in a two-sided
slot are form closed. In contrast, a force-closed
joint, such as a pin in a half-bearing or a slider on
a surface, requires some external force to keep it
together or closed. This force could be supplied by
gravity, a spring, or any external means.
10/06/2023 31
4. Classification by the Order of Joints
We can classify Joints by the order of joints as
1st order, 2nd order and so on.
• Order is defined as the number of links joined
minus one.
• It takes two links to make a single joint; thus
the simplest joint combination of two links has
order one.
• Joint order has significance in the proper
determination of overall degree of freedom for
the assembly.
10/06/2023 32
10/06/2023 33
 Determining Degree of Freedom
Gruebler Condition
• Any link in a plane has 3 DOF. Therefore, a system
of L unconnected links in the same plane will have
3L DOF. When these links are connected by a full
joint this removes 2-DOF, and when these links are
connected to half joint it removes only 1- DOF from
the system (because a half joint has two DOF). In
addition, when any link is grounded or attached to
the reference frame, all three of its DOF will be
removed.
10/06/2023 34
Gruebler’s Equation
• Based on the above reasoning, Gruebler’s Equation is:
M=3L-2J-3G -H

where: M = degree of freedom or mobility

L = number of links
J = number of joints
G = number of grounded links
H = higher pair
• Note that in any real mechanism, even if more than one link of the
kinematic chain is grounded, the net effect will be to create one
larger, higher-order ground link, as there can be only one ground
plane. Thus G is always one, and Gruebler's equation becomes:
M=3(L-1)-2J
10/06/2023 35
10/06/2023 36
 Types of Motion
• A rigid body free to move within a reference frame
will, in the general case, have complex motion,
which is a simultaneous combination of rotation
and translation.
• In three-dimensional space, there may be rotation
about any axis and also simultaneous translation
which can be resolved into components along three
axes. In a plane, or two-dimensional space,
complex motion becomes a combination of
simultaneous rotation about one axis (perpendicular
to the plane) and also translation resolved into
components
10/06/2023 along two axes in the plane. 37
 Description of position, orientation and
frame
• The Position of a point relative to a
coordinate system A ( )

10/06/2023 38
• The Orientation of a body is described by a
coordinate system B attached to the body
relative to A (a known coordinate system)

10/06/2023 39
Each component of c can be written as a dot product of a
pair of unit vectors:

10/06/2023 40
 Description of a frame
• A frame is a set of four vectors giving position and
orientation information
• For example, frame {B} is described by and
where is the vector that locates the origin of
the frame {B}:

10/06/2023 41
 MAPPINGS: Changing descriptions from frame to frame

Mappings involving translated frames

• Expressing a point in terms of frame {A},
when {A} has the same orientation as {B}

10/06/2023 42
 Mappings involving rotated frames
• Expressing a vector in terms of frame
{A} , the origins of the frame {A} and {B}
are coincident

10/06/2023
 Mappings involving general frames
• {A} and {B} has different origin and orientations
• Vector offset between origins:
• {B} is rotated with respect to {A}:

10/06/2023 44
 Mappings involving general frames
• First, describe relative to a frame that
has the same orientation of {A}, but whose
origin coincides with the origin {B}
• Then add for the translation
• Thus:

10/06/2023 45
• “Homogenous transform”

In other words,
1. a "1" is added as the last element of the 4 x 1
vectors;
2. a row "[0 0 0 1]" is added as the last row of
the 4 x 4 matrix.

10/06/2023 46
 Principal Rotation Matrices

Rotation about the z-axis

Rotation about the y-axis

Rotation about the x-axis

10/06/2023 47
Example
 Robot Kinematics

Kinematics –study of motion without regard

to the force generating it

choice of parameterization q
• unambiguous and minimal characterization of robot configuration
n = # degrees of freedom (dof) = # robot joints (rotational or
translational)
10/06/2023 49
 Kinematics Type
• In robots
– Forward kinematics
• Deals with determination of position and orientation
of end effector as a function of joint positions
– Inverse kinematics
• Deals with determination of joint positions for a
given end effector position and orientation

10/06/2023 50
 Classification by Kinematics type
(first 3 DOFs)

10/06/2023 51
 Forward kinematics
 Deals with determination of position and orientation
of end effector as a function of joint positions
 Link parameters
Link Length (ai)
Is measured along a line which is mutually perpendicular to both
axes.
The mutually perpendicular always exists and is unique except
when both axes are parallel.
Link twist ()
Project both axes i-1 and i onto the plane whose normal is the
mutually perpendicular line, and measure the angle between them

10/06/2023 53
 Joint Parameters
A joint axis is established at the connection of two
links. This joint will have two normals connected
to it one for each of the links.
• The relative position of two links is called link
offset dn whish is the distance between the links
(the displacement, along the joint axes between
the links).
• The joint angle qn between the normal is measured
in a plane normal to the joint axis.
10/06/2023 54
10/06/2023 55
 Link and Joint Parameters
zi

xi
Zi-1

Xi-1

10/06/2023 56
 Link and Joint Parameters
4 parameters are associated with each link.
• Link parameters:
ai the length of the link.
ai the twist angle between the joint axes.
• Joint parameters:
qi the angle between the links.
di the distance between the links
……………………………………………………………
…………..
ai = the distance from zi to zi+1 measured along xi.
ai = the angle between zi and zi+1 measured about xi
di10/06/2023
= the distance from xi-1 to xi measured along z i 57

q = the angle between x to x measured about z

 Link Connection Description:
For Revolute Joints: a, , and d are all fixed, then “i”
is the Joint Variable.
For Prismatic Joints: a, , and  are all fixed, then “di”
is the Joint Variable.
These four parameters: (Link-Length ai-1), (Link Twist
i-1(, (Link-Offset di), (Joint-Angle i) are known as
the Denavit-Hartenberg Link Parameters.

10/06/2023 58
 Links Numbering Convention

Link 2
Base of the arm: Link-0 2
1st moving link: Link-1 3 Link 3
. . Link 1
1
. .
. .
0 Link 0
Last moving link Link-n

10/06/2023 A 3-DOF Manipulator Arm 59

 Special cases:
If joint axes zi-1 and zi intersect, parameter ai is
zero.
If the common perpendicular to z i-1 and zi
intersects zi-1 at the origin of frame Oi-1, then di-1
is zero.
If joint axes zi-1 and zi are parallel, angle ai is
zero.

10/06/2023 60
 The Kinematics Model
The robot can now be kinematically modeled by
using the link transforms ie:
T  T1T2T3 Ti Tn
0
n

Where
is the pose of the end-effector relative to base;
Ti is the link transform for the ith joint;
and
n is the number of links.
10/06/2023 61
 The Denavit-Hartenberg (D-H)
Representation
• The D-H representation has been used to derive the
kinematic description of robotic manipulators.
• The method begins with a systematic approach to
assigning and labeling an orthonormal (x,y,z)
coordinate system to each robot joint. It is then
possible to relate one joint to the next and ultimately
to assemble a complete representation of a robot's
geometry.

10/06/2023 62
 Rule for frame assignment
• Zi axis is aligned with axis i, its assignment is
arbitrary
• Xi axis is perpendicular to axis of Z i-1 and Zi and
points away from axis Zi-1
• Origin of ith coordinate frame is located at the
intersection of axis of joint i+1 and common normal
between (i-1) and i
• Yi is completes the RH rule
10/06/2023 63
 General Transformation Between Two
Bodies
In D-H convention, a general transformation
between two bodies is defined as the
product of four basic transformations:

• A rotation about the initial z axis by q,

• A translation along the initial z axis by d,
• A translation along the new x axis by a, and.
• A rotation about the new x axis by a.
10/06/2023 64
A General Transformation in D-H Convention
D-H transformation for adjacent coordinate
frames:
Ai  Rot z ,i Trans z , di Trans x ,ai Rot x ,i
i 1
T i
 Tz , Tz ,d Tx ,aTx , I 44

10/06/2023 65
 DH Convention summary
• Step 1: Locate and label the joint axes z 0, . . . , zn−1.
• Step 2: Establish the base frame. Set the origin
anywhere on the z0-axis. The x0 and y0 axes are
chosen conveniently to form a right-hand frame.
For i = 1, . . . , n − 1, perform Steps 3 to 5.
• Step 3: Locate the origin oi where the common
normal to zi and zi−1 intersects zi. If zi intersects zi−1
locate oi at this intersection. If zi and zi−1 are parallel,
locate oi in any convenient position along z i.
10/06/2023 66
• Step 4: Establish xi along the common normal between
zi−1 and zi through oi, or in the direction normal to the
zi−1 − zi plane if zi−1 and zi intersect.
• Step 5: Establish yi to complete a right-hand frame.
• Step 6: Establish the end-effector frame onxnynzn.
Assuming the n-th joint is revolute, set zn = a along the
direction zn−1. Establish the origin on conveniently
along zn, preferably at the center of the gripper or at the
tip of any tool that the manipulator may be carrying.
Set yn = s in the direction of the gripper closure and set
xn = n as s × a. If the tool is not a simple gripper set xn
and yn conveniently to form a right-hand frame
10/06/2023 67
• Step 7: Create a table of link parameters a i, di,
α i, θ i.
• ai = distance along xi from oi to the intersection
of the xi and zi−1 axes.
• di = distance along zi−1 from oi−1 to the
intersection of the xi and zi−1 axes. di is variable if
joint i is prismatic.
• α i = the angle between zi−1 and zi measured about
xi

10/06/2023 68
• θi = the angle between xi−1 and xi measured
about zi-1. θi is variable if joint i is revolute.
• Step 8: Form the homogeneous transformation
matrices Ti by substituting the above
parameters.
• Step 9: Form = T1 · · ·Tn. This then gives the
position and orientation of the tool frame
expressed in base coordinates.

10/06/2023 69
D-H transformation for adjacent coordinate
frames:

i 1
T i
 Tz , Tz ,d Tx ,aTx , I 44

C i  C  i S i S i S i ai C i 
 S C  i C i  S i C  i ai S i 
T i
i 1  i
 0 S i C i di 
 
 0 0 0 1 

10/06/2023 70
Summery  ai = the distance from zi to zi+1 measured along xi.
 ai = the angle between zi and zi+1 measured about xi
 di = the distance from xi-1 to xi measured along z I
 qi = the angle between xi-1 to xi measured about z i
 Example 1

10/06/2023 72
 Example 1

Where (θ1 + θ2 ) denoted by θ12 and

10/06/2023 73
 Example2
Z3
Z0 Z1
Joint 3
Y0 Z O3 X3
Y1 2

d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Joint i i ai di i
Y2

a0 a1 1 0 a0 0 0

2 -90 a1 0 1

3 0 0 d2 2

10/06/2023 74
 Example2

C i  C  i S i S i S i ai C i 
 S C  i C i  S i C  i ai S i 
T i
i 1  i
 0 S i C i di 
 
 0 0 0 1 

T  (T )(T )(T )
0
3 1
0
2
1
3
2

10/06/2023 75
 Example2
cosθ 0  sinθ 0 0 a0 cos  0 
Joint i i ai di i 
1  sinθ 0 cosθ 0 0 a0 sin  0 
1 0 a0 0 0 T 0
 0 0 1 0 
 
2 -90 a1 0 1  0 0 0 1 

3 0 0 d2 2 cosθ1 0  sin 1 a1 cos 1 

 cos 1 a1 sin 1 
2  sinθ1 0
T 1
 0 1 0 0 
 
 0 0 0 1 

cosθ 2  sinθ 2 0 0
sin  cos  2 0 0 
T 23  2

 0 0 1 d2 
 
 0 0 0 1
10/06/2023 76
 Example3

θ3

a3 θ2

a2
θ1

10/06/2023 77
 Example3
 c1  s1 0 a0  c1  s1 0 0
 s c c c  s 0  s 0 d1   s1 c1 0 0
0
T   1 0 1 0

1
 s1s 0 c1s 0 c 0 c 0 d1   0 0 1 0
   
 0 0 0 1  0 0 0 1
c 2  s 2 0 L1  c3  s3 0 L2 
 s c 2 0 0   s c 0 0 
2T 
1  2 2
T   3 3 
 0 0 1 0
3
 0 0 1 0
   
 0 0 0 1  0 0 0 1 

10/06/2023 78
 Example3
c123  s123 0.0 l1c1  l2 c12 
s c 0 . 0 l s  l s 
W T  3T 
B 0  123 123 1 1 2 12 
.
 0.0 0.0 1.0 0.0 
 
 0 0 0 1 

10/06/2023 79
 Inverse kinematics
• Given
– Length of each link
– Position and orientation of end effector
• Required
– Angle of each joint

10/06/2023 80
 Inverse Transformation
Ci  C i Si S i Si aiCi 
 S C i C i  S iCi ai Si 
Ti i 1   i
 0 S i C i di 
 
 0 0 0 1 
to give
 R P
T i 1i  
0 0 0 1 

where
ai ci 
ci  c i si s i s i   a s 
R   si c i ci 
 s i ci  and P   i i
 0 s i c i   di 

10/06/2023 81
 Inverse Transformation
For example, find the inverse of:
• The inverse 
X
transformation is
3

given by: 
Y1

Y0

Y3

Y2 X 2

 T
 T
P

i 1 1 R R X1 

iT   X 0

 0 0 0 1 
i i-1 i-1 di i

1 0 0 0 1

2 0 L1 0 2

10/06/2023 3 0 L2 0 3
82
 Inverse Transformation of 0
1T 1

cos(1 )  sin(1 ) 0 0
 sin( ) cos( ) 0 0
0
1T   1 1

 0 0 1 0
 0 0 0 1

cos(1 )  sin(1 ) 0  cos(1 ) sin(1 ) 0
R   sin(1 ) cos(1 ) 0  R T   sin(1 ) cos(1 ) 0
   
 0 0 1  0 0 1
0   cos(1 ) sin(1 ) 0 0 0
P  0  R T P   sin(1 ) cos(1 ) 0 0  0
      
0  0 0 1 0 0

10/06/2023 83
 Inverse Transformation of
0
1T 1

cos(1 )  sin(1 ) 0 0
 sin( ) cos( ) 0 0
0
T   1 1
 i 1 1  R T
 R T
P
1
 0 0 1 0 iT  
 0  0 0 0 1 
 0 0 1

 cos(1 ) sin(1 ) 0 Therefore

R T   sin(1 ) cos(1 ) 0
   cos(1 ) sin(1 ) 0 0
 0 0 1  sin( ) cos( ) 0 0
0 1
T   1 1

0  1
 0 0 1 0
 R T P  0   0 0 0 1
  
0
10/06/2023 84
 Inverse Transformation of 1
2T 1
Find the inverse of
cos( 2 )  sin( 2 ) 0 l1 
 sin( ) cos( ) 0 0 
2T 
1  2 2

 0 0 1 0
 
 0 0 0 1

cos( 2 )  sin( 2 ) 0  cos( 2 ) sin( 2 ) 0

R   sin( 2 ) cos( 2 ) 0  R T   sin( 2 ) cos( 2 ) 0
   
 0 0 1  0 0 1

l1   cos( 2 ) sin( 2 ) 0 l1   l1 cos( 2 ) 

P  0   RT P   sin( 2 ) cos( 2 ) 0 0    l1 sin( 2 )
0   0 0 1 0   0 

10/06/2023 85
 Inverse Transformation of T 1
1
2

 cos( 2 ) sin( 2 ) 0 and  l1 cos( 2 )

R T   sin( 2 ) cos( 2 ) 0  RT P   l1 sin( 2 ) 
   0 
 0 0 1

Therefore
 cos( 2 ) sin( 2 ) 0  l1 cos( 2 )
 sin( ) cos( ) 0 l1 sin( 2 ) 
1 1
T   2 2
2
 0 0 1 0 
 
 0 0 0 1 

10/06/2023 86
 Inverse Transformation of 2
3T 1

Find the inverse of

cos(3 )  sin(3 ) 0 l2 
 sin( ) cos( ) 0 0 
3T 
2  3 3

 0 0 1 0
 
 0 0 0 1

cos( 3 )  sin( 3 ) 0  cos( 3 ) sin( 3 ) 0

R   sin( 3 ) cos( 3 ) 0  R T   sin( 3 ) cos( 3 ) 0
   
 0 0 1  0 0 1
l2   cos(3 ) sin(3 ) 0 l2   l2 cos(3 ) 
P   0   RT P   sin(3 ) cos(3 ) 0  0    l2 sin(3 )
 0   0 0 1  0   0 

10/06/2023 87
 Inverse Transformation of 2
3T 1

 l2 cos(3 )
 cos( 3 ) sin( 3 ) 0 and  RT P   l2 sin(3 ) 
R T   sin( 3 ) cos( 3 ) 0  0 
 
 0 0 1

Therefore
 cos(3 ) sin(3 ) 0  l2 cos(3 )
 sin( ) cos( ) 0 l2 sin(3 ) 
2 1
T   3 3
3
 0 0 1 0 
 
 0 0 0 1 

10/06/2023 88
 Inverse Kinematics Problem
• Robot forward kinematics involve finding the gripper
position and orientation given the angles of rotation of the
linkages
• Now consider the following problem for the planar robot:
The arm moves to a new location and the position along with
the orientation of the gripper w.r.t. frame {0} is known. Can
you determine the total rotations of 1, 2, and 3 required for
the arm to reach that new position?
• This10/06/2023 89
problem is called the robot inverse kinematics problem .
 Inverse Kinematics of the Planar Robot

The inverse kinematics problem for the planar robot can be stated
as follow:
Given the global gripper orientation and position, i.e. given

 nx ox ax px 
n oy ay py 
0
3T   y

 nz oz az pz 
0 0 0 1 


Find the joint angles 1, 2, and 3 required

for the arm to reach that new position
10/06/2023 90
 Inverse Kinematics
To solve the inverse kinematics problem, we know that:
 nx ox ax px 
n oy ay py  0 1 2
 y    1T 2T  3T  03T
 nz oz az pz  Or
 
0 0 0 1
cos(1   2  3 )  sin(1   2  3 ) 0 l1 cos(1 )  l2 cos(1   2 )
 sin(     ) cos(     ) 0 l1 sin(1 )  l2 sin(1   2 ) 
3T 
0  1 2 3 1 2 3

 0 0 1 0 
 
 0 0 0 1 

We can equate the elements in the above two matrices

to try to determine the joint angles

10/06/2023 91
 Inverse Kinematics example
 nx ox ax px 
n oy ay p y 
 y 
 nz oz az pz 
 
0 0 0 1
cos(1   2  3 )  sin(1   2  3 ) 0 l1 cos(1 )  l2 cos(1   2 )
 sin(     ) cos(     ) 0 l sin( )  l sin(   ) 
 1 2 3 1 2 3 1 1 2 1 2 

 0 0 1 0 
 
 0 0 0 1 

Let = 1+2+3. Equate elements (1,1) and (2,1):

nx  cos(1   2   3 )  cos( )  sin( ) 
tan( )   
n y  sin(1   2   3 )  sin( )  cos( ) 
Provided that (nx2  n y2 )  1
 ny 
We can find  using =arctan(ny, nx): (1   2   3 )    tan   1

 nx 
10/06/2023 92
10/06/2023 93
 Inverse Kinematics example
Next, equate elements (1,4) and (2,4):
px  l1 cos(1 )  l2 cos(1   2 )

p y  l1 sin(1 )  l2 sin(1   2 )
Square both of these equations to get:
px2  l12 cos 2 (1 )  l22 cos 2 (1   2 )  2l1l2 cos(1 ) cos(1   2 )

p y2  l12 sin 2 (1 )  l22 sin 2 (1   2 )  2l1l2 sin(1 ) sin(1   2 )

Adding the above two equation yields:

px2  p y2  l12  l22  2l1l2 cos( 2 )   
 px2  p y2  l12  l22
cos( 2 )  

2l1l2 
 
sin 2 ( 2 )  1  cos2 ( 2 )
sin( 2 )  1  cos2 ( 2 )
 sin( 2 ) 
 2  tan  1

10/06/2023
 cos( 2 )  94
 Inverse Kinematics example
To solve for 1, we again reuse the following equations:
px  l1 cos(1 )  l2 cos(1   2 )   cos(1 )   sin(1 )
p y  l1 sin(1 )  l2 sin(1   2 )   sin(1 )   cos(1 )

where   l1  l2 cos( 2 )
  l2 sin( 2 )

 and  can be found as 2 was previously determined.

  r cos( ) r  2  2
Let   r sin( )
where 
  tan 1  
 
Substitute r and  into px and py to get:
px px
 cos( ) cos(1 )  sin( ) sin(1 ) cos(  1 ) 
r r
py py
 cos( ) sin(1 )  sin( ) cos(1 ) sin(  1 ) 
r r 95
 Inverse Kinematics example
 sin(  1 )  1  p y 
(  1 )  tan 
1
  tan  
 cos(  1 )   px 
We can then find 1 using the known  :
 py  1  p y  
1  tan      tan    tan 1  
1

 px   px   

Summary of inverse kinematics for the planar robot:

1) Find = 1+2+3 from n 
(     )    tan  
1 2 3
1 y

 nx 
2) Find 2 from   
 px2  p y2  l12  l22 ,
cos( 2 )    sin( 2 )  1  cos 2 ( 2 )
 2l1l2 
3) Find 1 from  py  1  p y  
1  tan      tan    tan 1  
1

 px   px   
4) Find 3 from 3=-(1+2) 96
 Inverse Kinematics exercise 1
The gripper position and orientation for the planar robot is at:

 nx ox ax p x  1 0 0 l2 
n oy ay p y  0 1 0 l1 
 y 
 nz oz az p z  0 0 1 0
   
0 0 0 1  0 0 0 1

Where l1=3, l2=2. Find the joint angles and hence determine
the robot configuration.  n 
(1   2   3 )    tan 1  y   tan 1 0   0
 nx 
  
 px2  p y2  l12  l22
cos( 2 )  
  0
2l1l2 
 
sin( 2 )   1  cos 2 ( 2 )  1
 sin( 2 ) 
 2  tan 1    90
 cos( 2 ) 

99
 Inverse Kinematics exercise 1

Assuming an elbow down configuration for link 2:

 2  90
Remember, positive 2 means clockwise rotation as follow:
Y2
X2
2

10/06/2023 100
 Inverse Kinematics exercise 1

After 2 has been found:

  l1  l2 cos( 2 )  l1  3
  l2 sin( 2 )  2
px  2
py  3

 py  1  p y  1   
1  tan      tan    tan    90
1

 px   px   
 3    (1   2 )  0
Results: 1  90
 2  90
 3  0
10/06/2023 101
 Visualization of Inverse Kinematics exercise 1

To visualize the arm movement, the robot should looks like this
after rotating 1 = 90° (1 is the angle from X0 to X1 measured
along Z1).
X3
Y3

X2
Y2

Y0, X1 1=90°
Y1 X0
10/06/2023 102
 Visualization of Inverse Kinematics exercise 1
To visualize the arm movement, the robot should looks like this
after rotating 1 = 90° (1 is the angle from X0 to X1 measured
along Z1),
and after rotating 2
= -90° (2 is the
angle from X1 to X2
Y2 Y3
measured along Z2)
Note that 3 = 0° X2 X3
2=-90°

Y0, X1
Y1 X0
10/06/2023 103
 Visualization of Inverse Kinematics exercise 1
The orientation of the gripper is as follow:

n  1 0 0 X3 is in the positive X0 direction
T


o  0 1 0 Y3 is in the positive Y0
T


a  0 0 1 direction Z3 is in the positive Z0 direction
T


p  l2 0 Location of gripper in frame {0}
T
l1

Y2 Y3
X2 X3
 nx ox ax p x  1 0 0 l2 
n oy ay p y  0 1 0 l1 
 y 
 nz oz az p z  0 0 1 0
    l2
0 0 0 1  0 0 0 1 l1
Y0,X1
10/06/2023 Y1 X0 104
 Inverse Kinematics exercise 2
The gripper position and orientation for the planar robot is at:
 nx ox ax p x  0  1 0 0
n oy ay p y  1 0 0 5
 y  
 nz oz az p z  0 0 1 0
   
0 0 0 1  0 0 0 1

Where l1 =3 and l2= 2. Find the joint angles and hence determine
the robot configuration.
n  1
(1   2   3 )    tan 1  y   tan 1    90
 nx   0

  px2  p y2  l12  l22 

cos( 2 )    1

 2 l l
1 2 
sin( 2 )   1  cos 2 ( 2 )  0
 sin( 2 ) 
 2  tan 1    0
10/06/2023  cos( 2 )  105
 Inverse Kinematics exercise 2

After 2 has been found:

  l1  l2 cos( 2 )  l1  l2  5
  l2 sin( 2 )  0
px  0
py  5

 py  p  
1  tan 1      tan 1  y   tan 1    90
 px   px   
 3    (1   2 )  0
Results: 1  90
 2  0
 3  0
10/06/2023 106
 Visualization of Inverse Kinematics exercise 2

To visualize the arm movement, the robot should finally looks

like this after rotating 1 = 90° (1 is the angle from X0 to X1
measured along Z1), 2 = 0° (2 is the angle from X1 to X2
measured along Z2) and 3 = 0°.
X3
Y3

X2
1=90°
Y2

Y0, X1
10/06/2023 Y1 X0 107
 Inverse Kinematics – the general approach

For a more complex robot with n>3 links, it is generally not

possible to solve for all the joint variables by just equating the
elements in
 nx ox ax px 
n oy ay py  0
 y   nT
 nz oz az pz 
 
0 0 0 1

To solve for the other joint angles, it may be necessary to find

 nx ox ax px 
 py 
0 1  n y oy ay
 1T 3T   1T  1T 2T  3T   2T  3T    1T  n
0 1 0 0 1 0 1 2 1 2
oz az

pz 
z
 
0 0 0 1
The elements in the above equation are compared to solve for
the other joint angles. It may be necessary to repeat the process
with premultiplication of the LHS and RHS with more inverse
transformation
10/06/2023 matrices. 108
 Inverse Kinematics – Summary

• The approach involves the following:

1. Solve for joint variables by comparing elements in
 nx ox ax px 
n oy ay py  0
 y   nT
 nz oz az pz 
 
0 0 0 1
2. Repeat step 1 with

 nx ox ax px   nx ox ax px 
n oy ay p y  n oy ay p y 
 T   T 
1
n
0
1
1  y
 nz oz az pz 
,  T    T  T 
2
n
1
2
1 0
1
1  y
 nz oz az pz 
   
0 0 0 1 0 0 0 1

etc. until all the joint variables are obtained

10/06/2023 109