Robotic

Contents
Contents i
Notation 1
1 Introduction 5
1.1 Robot manipulator history and brands . . . . . . . . . . . . . . . . 5
1.2 Robot programming background . . . . . . . . . . . . . . . . . . . 6
1.3 Outline of the subsequent chapters . . . . . . . . . . . . . . . . . . 7
2 Kinematics of Robot Manipulators 11
2.1 Links and Joints . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.2 Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.3 Joint Topologies . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.4 Describing a serial robot . . . . . . . . . . . . . . . . . . . . . . . . 14
2.4.1 Coordinate frames . . . . . . . . . . . . . . . . . . . . . . . 15
2.5 Describing a complete robot scene . . . . . . . . . . . . . . . . . . 17
2.6 State vector of joint variables. Degrees of freedom . . . . . . . . . 17
2.6.1 Dependent Joints . . . . . . . . . . . . . . . . . . . . . . . . 19
2.7 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.8 Programming Exercises . . . . . . . . . . . . . . . . . . . . . . . . 21
3 Rotation matrices and homogeneous transformations 23
3.1 Points, vectors and their coordinates in a frame . . . . . . . . . . . 23
3.2 Denition of the rotation matrix . . . . . . . . . . . . . . . . . . . 24
3.3 Rotation matrices around coordinate axes . . . . . . . . . . . . . . 25
3.4 Rool-pitch-yaw (RPY) Euler angles . . . . . . . . . . . . . . . . . . 26
3.5 Homogeneous coordinates . . . . . . . . . . . . . . . . . . . . . . . 27
3.6 Computing the state of a robot system . . . . . . . . . . . . . . . . 29
3.7 Example:Computing the state of a serial device . . . . . . . . . . . 30
3.8 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
i
ii CONTENTS
4 Manipulator Jacobians and related Numerical Inverse Kine-
matics 37
4.1 Multidimensional rst order Taylor expansions. Jacobians . . . . . 38
4.2 Positional and angular displacements . . . . . . . . . . . . . . . . . 39
4.2.1 Example: J(q) for a 3-axis robot . . . . . . . . . . . . . . . 41
4.3 Numerical Inverse Kinematics for innitesimal displacements . . . 42
4.4 Numerical Inverse Kinematics for small but nite displacements . . 43
4.5 The equivalent angle axis representation for a rotation matrix . . . 44
4.6 Numerical Inverse Kinematics for large displacements . . . . . . . 46
4.7 Solution methods for J(q)q = u . . . . . . . . . . . . . . . . . 47
4.8 Singularities and Joint limits . . . . . . . . . . . . . . . . . . . . . 48
4.9 Changing reference and target frame for the Jacobian . . . . . . . 48
4.10 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.11.1 Script Format . . . . . . . . . . . . . . . . . . . . . . . . . . 51
5 Tool Trajectory Descriptions 53
5.1 Linear Interpolation methods . . . . . . . . . . . . . . . . . . . . . 54
5.2 Inverse kinematics for interpolated tool curve . . . . . . . . . . . . 56
5.2.1 Limits on joint positions, velocities and accelerations . . . . 56
5.2.2 Handling velocity and acceleration limits by time scaling . . 57
5.3 Rotation representation by quaternions . . . . . . . . . . . . . . . 58
5.4 Cubic splines . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
5.5 Augmenting trajectory way point information for Cubic Splines . . 61
5.6 Applying interpolation . . . . . . . . . . . . . . . . . . . . . . . . . 62
5.7 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
Notation
In order to have an easy access to the notation, we provide here a list:
Link and joint indices
n denotes the total number of joints in the robot.
(i) denotes the index of the parent moving link/joint of link/joint i. That
is, the neighbour link/joint closer to the base. If the parent link to link i is
the base, we set (i) 0.
Coordinate frames
In general, a robotic platform consists of a number of serial robots and/or serial
robotic subunits. A frame dened as F
[k],
belongs to the kth serial subunit
in the scene. Some of the notation below is devoted to contexts where only an
isolated serial robot or serial robotic subunit is considered. In these cases, the
[k] part of the superscript is omitted. A coordinate frame, say F
[k],
, is dened
by the location and orientation of a coordinate system relative to some link. It
is explicitly given by to which part of the kth link in the robot it belongs. We
will also introduce frames that belong to the xed part or robot environment.
We denote these by F
[W],
. More specically, we for the kth subunit have that
F
[k],i
is a unique and always existing coordinate frame xed with respect
to link i and located at the ith joint axis with the z-axis pointing in the
positive direction of the joint axis.
F
[k],i,j
is the jth additional coordinate frame xed with respect to link i
that for some reason has been dened. Often, there are no such additional
frames.
A superscript used in a frame F
[k],
indicates an arbitrary frame of the
kth serial robot/subunit.
1
2 NOTATION
o
[k],
, x
[k],
, y
[k],
, z
[k],
denote the origin and coordinate axes of frame
F
[k],
.
(F
[k],
) denotes the parent frame of a frame F
[k],
as dened by a refer-
ence tree with root at the world frame.
Here follows some special coordinate frames:
F
[k],base
denotes the frame of the base. or at a central grasping point on a
nger.
F
[k],tool
denotes a mandatory frame at the tool mounted on the outermost
link of a serial robot. The frame is typically located at the location where
the tool is mounted to the outermost link.
F
[k],TCP
denotes a mandatory frame at the tool typically located to repre-
sent the active part of the tool.
F
[k],tool,j
denotes optional additional frame associated with the tool.
Coordinates and transformations
Here we present notations for coordinates of vectors and matrices. Notice the
clear distinction to frames as we now always state a reference frame.
[v]
A
denote the homogeneous coordinates of a vector v measured in coor-
dinate frame A
[v]
A
NH
denote the non-homogeneous coordinates of a vector v measured in
frame A
R
j
i
denotes the rotation matrix given by the coordinate axes of frame j
relative to frame i.
p
j
i
denotes the position of the origin of frame j measured in frame i.
x
j
i
, y
j
i
and z
j
i
denotes the position of respectively the x-axis, y-axis and
z-axis of frame j measured in frame i.
T
j
i
denotes the homogeneous transformation matrix that gives frame j in
the coordinates of frame i.
A frame index [k], used as sub or superscript in a homogeneous transfor-
mation matrix indicates reference to a frame F
[k],
.
3
Special Rotation and Transformation Matrices
Here, we list the notation concerning a set of special rotation and homogeneous
transformation matrices that are used
R
c
() denotes the rotation matrix for a rotation with an angle around
coordinate axis c, where c is x, y or z.
R
RPY
(
x
,
y
,
z
) R
z
(
z
)R
y
(
y
)R
x
(
x
) is the Roll-Pitch-Yaw Euler-angles
rotation matrix.
R
EAA
(v, ) is the equivalent angle axis rotation. It corresponds to a rota-
tion with an angle around the unit vector v.
W
rot
(R) returns v, so that R
EAA
(v, ) = R.
Q (Q
1
, Q
2
, Q
3
, Q
4
) denotes a unit quaternion.
R
Q
(Q) is the rotation matrix corresponding to a unit quaternion Q.
T
c
()
r
denotes the homogeneous transformation matrix for a rotation an
angle around coordinate axis c, where c is x, y or z.
T
c
(s)
t
denotes the homogeneous transformation matrix for a translation s
along coordinate axis c, where c is x, y or z.
Denavit-Hartenberg notation
The denitions below are only used in Chapter ?? and Chapter ??. They all
relate to the Denavit-Hartenberg representation. We consider an isolated n-axis
serial robot and thus omit the [k] indicating which serial chain we are considering.
G
i
for i = 0, 1, . . . , n is the Denavit-Hartenberg joint coordinate frame for
joint i + 1. It is also xed with respect to link i. In particular, G
0
is xed
with respect to the base link and G
n
is xed with respect to the tool link.
O
i
, X
i
, Y
i
, Z
i
denote the origin and coordinate axes of frame G
i
. a robot
with tree topology.
A frame index gi used as sub or superscript in a transformation matrix
indicates reference to a Denavit-Hartenberg frame G
i

DH
(, d, , a) denotes the homogeneous transformation matrix computed
by the Denavit-Hartenberg parameters.

DH
tot
(q)
DH
tot
(q
1
, . . . , q
n
) denotes the forward kinematics computed
by the Denavit-Hartenberg parameters of a given n-axis serial robot where
the dependencies on the constant Denavit-Hartenberg parameters are left
out.
4 NOTATION

DH
j
i
(q)
DH
j
i
(q
i+1
, . . . , q
j
) denotes the forward kinematics between
frame G
i
and frame G
j
computed by the Denavit-Hartenberg parameters of
a given n-axis serial robot where the dependencies on the constant Denavit-
Hartenberg parameters are left out.
Chapter 1
Introduction
In this course, we will give an introduction to a very important part of the robotics
area, namely how to program robot manipulators. A robot manipulator can be
dened in many ways. Often it is used in the context of an industrial robot, which
is dened by the ISO standard as an automatically controlled, reprogrammable,
multipurpose manipulator programmable in three or more axes. This denition
is quite adequate also in this course although we will not restrict the use to only
industrial production. Examples of industrial operations of robot manipulators
are welding, painting, milling, assembly and in particular pick and place opera-
tions. In this course, we will not directly address such processes, but rather give
generic examples that illustrate the relevant objectives.
1.1 Robot manipulator history and brands
There is worldwide a variety of robot manufacturers. The rst company to pro-
duce a robot was Unimation founded in 1956. The robots were hydraulically
actuated with joint control (similar to excavation machines). Until around 1970,
further development was mainly in laboratory environments. In the 1970s, the
commercial development of industrial robots took o with electrically actuated
robots. First, Unimation launched the PUMA robots. Rapidly, robots from the
European brands ABB robotics and Kuka robotics, from the US company Adept
technologies and from the Japanese company Fanuc were launched. Further man-
ufacturers have since then entered the market. Below is a non-complete list of
the most important manufacturers with links
Kuka robotics http://www.kuka.com/
ABB robotics http://www.abb.com/product/us/9AAC910011.aspx
5
6 CHAPTER 1. INTRODUCTION
Fanuc robotics http://www.fanucrobotics.eu/
Adept technologies http://www.adept.com/
Yaskawas (Motoman) robotics http://www.motoman.com/
Staubli robots http://www.staubli.com/en/robotics/
Kawasaki robotics http://www.kawasakirobotics.com/
1.2 Robot programming background
In a nutshell, robot programming is about generating collision free robot paths
such that a specied task or a sequence of specied tasks are executed. The robot
is typically embedded in a work cell containing machines, conveyor belts, part
feeders, fences, etc.
The transfer of programs (real time or batch jobs) is carried out by connecting
the robot controller to a computer or network of computers. Unfortunately,
there is no standard interface to robot controllers. Rather, each robot brand
provides their own programming interface which diers both conceptually and in
the format. In this course, we will not go into details of each of these interfaces,
but rather discuss the most important issues with these in a generic way.
Conceptually, the programming of robots can be carried out manually in the
real environments or manually, semiautomatic or fully automatic in a virtual
environment.
Manual programming in the real environment is traditionally carried out using
a teach pendant, which is a hand held device used both as a controller and
programmer. The robot trajectories are carefully traced by moving the robot.
This is a time consuming procedure and is thus only feasible if the program is
going to be repeated thousands of times. A similar method suitable for robots
that can be switched to a control allowing them to be pulled around by the user
is that the user simply pulls the robot along the desired path. Conceptually, the
method is similar to using a teach pendant, but the procedure is typically much
faster. We shall occationally use the term teach in programming for these two
similar programming methods.
Programming using a virtual environment requires a 3D-CAD model (or sim-
ilar) of the robot workcell. The programming can still be carried out manually
now just in a virtual environment. The advantage is then that the real robot
can meanwhile be used for other purposes. More importantly, the programming
can be partly or fully automated. The big additional advantage is that human
working hours for preparing robot programs goes down to a minimum. This
is particularly important for few-of-a-kind production where each robot path is
1.3. OUTLINE OF THE SUBSEQUENT CHAPTERS 7
executed only once or a few times (say less than a thousand or so). We shall oc-
cationally call this oine programming and automatic oine programming
respectively.
Historically, robots have been used in applications with well structured and
extremely predictable environments like that of a production line in the auto-
motive industry. Recently, new trends have arisen within industrial automation.
One trend is to move from mass production to production of customized products
which demands exibility of the production line. Another related trend is the
need to handle a greater variety of products achieved by increasing the intelli-
gence in robot cells. This increased intelligence is essential when working with
less structured environments and autonomous applications, which is also the type
of environments and applications that is faced by the service robotics commu-
nity. Both trends highlight the increased need for exible robot cells and thus for
automatic robot programming. This can be both automatic oine programming
as mentioned above, but increasingly also automatic online programming where
the robots react adaptively, precise and reliable to sensorial input received on the
y.
It is the main purpose here to present a state-of-the-art programming envi-
ronment suitable for any robot type and outlining a set of methods needed for
automatic oine and online programming.
1.3 Outline of the subsequent chapters
Figure 1.1: WorkCell with Kuka KR16 .
In this section, we shall give an overall outline of the content of this book
through the example of a exible robot cell shown in Figure 1.1. All objects
(robot location, table, pallet etc.) have a given position and orientation. These
positions and orientations are given in a scene frame, which in robotics often is
referred to as a world frame F
World
.
The task of the robot is to move objects between the pallet and the table. In
order to pick and place an object, we must be able to move the robots tool to a
specied position and orientation. A typical task where the robot start and end
in the Home conguration could in pseudo code look like:
Move F
1a
//Move to frame F
1a
where the tool is adjacent to the bottle
Move Linear to frame F
1b
//Move the tool in a straight line to the bottle
Close Gripper //Pick up the bottle
Move F
1a
//Move back to frame F
1a
Move F
2a
//Move to frame F
2a
adjacent to the place position at the table
2b
//Move the tool in a straight line to the place po-
sition
Open Gripper //Release the bottle
2a
//Move back to frame F
2a
Move Home //Move back to the Home conguration
Notice, that at the frames F
1a
and F
2a
, the movements is changing from a
point-to-point movement along an arbitrary path (such as between F
1a
and F
2a
)
and completely specied trajectories (such as between F
1a
and F
1b
and back).
We thus need to be able to move the robot tool to specied frames and along
specied trajectories. However, the robot is driven by motors at the joints which
can be set to specied values. Thus, we require the availability of a mathemat-
ical relation between tool frames and joint values. Although these relations are
often oered in a more or less open way by the robot controller, we will still
need to address this issue for several reasons: (1) Not all controllers oer this
functionality, (2) it is desirable that we can adjust the controller relations if we
discover that they are inaccurate and (3) we need the relation to simulate the
robot and evaluate whether it can reach a given location and whether the path
to the location is collision free.
The technologies needed to solve this problem in a virtual environment is han-
dled in Chapters 25. In Chapter 2, we give an introduction to robot kinematics
including various denitions and terminology. In Chapter 3, we introduce math-
ematical transformations between coordinate frames such that we can compute
the tool frame as function of joint values (direct kinematics). Inverse kinemat-
ics is a procedure for nding joint angles satisfying a specied tool position and
1.3. OUTLINE OF THE SUBSEQUENT CHAPTERS 9
orientation. The computation of an inverse kinematics solution can be gener-
alized similar to the direct kinematics using a numerical approach ?? based on
the Jacobian matrix. Finally, we need to use the knowledge described above to
specify the path (possibly fully or partly as function of time). We need to be
able to specify unconstrained paths between two congurations (such as between
F
1a
and F
2a
) and constrained paths between two congurations (such as from
F
1a
to F
1b
and F
2a
to F
2b
). Finally, we (outside the example) discuss how to
compute long constrained paths such as for welding or milling processes and how
to compute paths determined online through input from sensors. The subjects of
path specications are treated in detail in Chapter 5.
In a real robotic environment, additional issues must be addressed. The most
important issues are: kinematic calibration, robot dynamics and robot controller
interface. In order to study kinematic calibration, we in Chapter ?? present a set
of independent parameters describing the robot kinematics. The parameters are a
modication of the socalled Denavit-Hartenberg coordinates that are widely used
in other robot textbooks. In Chapter ??, we then present a generic calibration
methodology that can be applied in most practical cases.
Another issue to be concerned with when dealing with real robots is the
robot dynamics. In Chapter ??, we give an overview of how all relations between
robot movements and external forces on the one side and the forces/torques at
the motors can be computed. This can be used for planning paths where the
forces/torques at the motors are within specied limits. Moreover, it is used
internally in the (black box) native controller of industrial robots. In order to
understand the behaviour of these native controllers and possibly develop your
own, robot dynamics is crucial to understand.
In order to solve the task, we need to dene how we communicate with the
robot. Actually this communication goes through the abovementioned native
controller, where there unfortunately is no widely used standard as each robot
manufacturer seem to have their own solution. Most generally speaking, we
can communicate to the controller by either providing sets of coordinated values
of the joint angles or by providing coordinated sets of end eector positions and
orientations (frames). On a more detailed levels there are other issues such as how
to communicate a desired speed and how to choose (interpolate) the movement
between two consequtive sets of joint angles or end eector frames. In general,
it is desirable to be able to compute a complete robot trajectory as function of
time from any input we give to the controller and vice versa for any given desired
complete robot trajectory as function of time to be able to compute the best
tting input to the controller. These tasks are not trivial at all and are actually
impossible to solve for quite a range of robot types. We shall discuss this issue
and how it can be managed in Chapter ??.
In the third part of the book, we study work-space analysis and planning of
collision free robot motions. In Chapter ??, we study the subset of robots for
which all inverse kinematics solutions (joint values as functions of tool frames) can
be found in closed form. Notice that numerical inverse kinematics just nds a
solution. Historically, robot designs have been constrained by this property, but
nowadays this is no longer important at all as numerical inverse kinematics meth-
ods have become computationally feasible. However, the associated workspace
analysis serves as the introduction to analyzing which parts of the workspace that
is collision free. Based on this, we introduce the problem of planning collision
free robot motions for a prespecied task (See Chapter ??).
Chapter 2
Kinematics of Robot
Manipulators
The term kinematics is used to describe the subset of mechanics, which deals
purely with motions and does not consider masses and forces acting on the system.
In this book we will focus on the kind of kinematics used to describe robot
manipulators, simple grippers, dexterous hands and the environments in which
they act.
2.1 Links and Joints
A robot link is dened as a rigid subunit of the robot. A robot joint is dened as
a connection between two links that permits the links to move relative to each
other.
Figure 2.1 shows dierent types of joints. The most typical joint is the revolute
joint (a) where rotation around an axis xed relative to the two associated links is
permitted. Linear actuators as used in for example gantry systems are modeled
as prismatic joints (b) where a translation along an axis xed relative to the
two associated links is permitted. Both types of movements have one degree of
freedom (DOF) and these joints are therefore referred to as 1 DOF joints.
Besides these two most fundamental joints there exists a number of more
complicated joints which can often be modeled as a combination of revolute and
prismatic joints. Examples of these are the cylindrical joint (c) which has 2 DOF
and can be divided into two 1 DOF joints and the spherical joint (d) which has
3 DOF and may be divided into three 1 DOF revolute joints.
11
12 CHAPTER 2. KINEMATICS OF ROBOT MANIPULATORS
Axis
(a) 1 DOF Revolute joint.
Axis
(b) 1 DOF Prismatic joint.
Axis
(c) 2 DOF Cylindrical joint (d) 3 DOF Spherical joint
Figure 2.1: Joint types.
2.2 Robots
The denition of a robot in a broad sense covers a wide range including industrial
manipulators, autonomous mobile devices, toy robots, articial pets and in the
most broad denition also non-physical entities such as software agents. Within
this book we shall limit ourselves to the specic subset of robots, where each
robot must satisfy the following conditions:
The robot consists of a set of links and joints connected into a single struc-
ture
Any two links are connected by at most one joint.
Some, but not necessarily all, joints are motorized
Each motorized joint can move independent of the other motorized joints.
If all motorized joints are xed at a given position, the robot must be kine-
matically sti (we shall refer to this criterion as kinematically determinable).
It will be assumed that a part of the robot is xed with respect to the platform
on which the robot is mounted (often the static environment), which we will refer
2.3. JOINT TOPOLOGIES 13
to as the base link of the robot. Thus, the position and orientation of all other
links (moving links) of the robot relative to the environment is by denition
uniquely given by the positions of the motorized joints.
An example of a robot is the Kuka KR16 robot shown in Figure 2.2 containing
6 joints, the base link and 6 moving links. The dierent links are shown in
dierent colours on the left picture. The blue link is the base.
Figure 2.2: Kuka KR16 robot.
2.3 Joint Topologies
The structure into which links and joints are assembled is called the joint topology.
When the joints are connected in a serial chain such as the Kuka KR16 robot,
we refer to it as a serial topology or serial robot. Most robots considered here
will have a serial topology. However, we will also discuss robots where joints are
connected in a tree structure such as the 3 ngered Schunk Dextrous Hand (SDH)
shown in Figure 2.3a. We shall refer to these robots as having a tree topology.
More complicated topologies with (possibly multiple) closed loops also exist
(see e.g. 2.3b). Such robots are referred to as closed loop topology robots. How-
ever, we shall here only discuss the special case of a 4-linked planar closed loop
robot that is relevant to describe an important type of a socalled dependent joint.
The joint topology of a robot can be visualized using schematics in which
revolute and prismatic joints are visualized as illustrated in Figure 2.4. The
(a) Tree joint topology (b) Parallel joint topology
Figure 2.3: 3 ngered Schunk Dextrous Hand (SDH) robot and parallel DTVGT
robot
revolute joint has two dierent schematic forms, one (2.4a) in which the rotation
is around an axis in the plane and one (2.4b) in which the rotation is around
an axis normal to the plane. The prismatic joint (2.4c) is visualized as a piston
and can only show translations along axes in the plane of the paper. Figure 2.4d
shows the symbol for the base link grounded to the oor and Figure 2.4e denes
the tool.
The topology of the Kuka KR16 is shown in Figure 2.5. The three outmost
joints forms a spherical wrist, which corresponds to a spherical joint. Notice, that
when the rst and the last joint axes of the spherical wrist are parallel, we get
into an unfortunate situation referred to as a Gimbal Lock. We shall later discuss
why this situation is unfortunate.
2.4 Describing a serial robot
In order to derive a mathematical formulation of the kinematics of a serial robot,
we will assume that we have split the possible cylindrical and spherical joints into
1 DOF joints as discussed in the previous sections. In such a splitting, we may
obtain physically void links, but this will as we shall see cause no problems in the
kinematic description. Assume that after this splitting, we have a serial robot
consisting of a set of n moving links connected by n prismatic or revolute joints
in one long chain. At one end of the chain is the base link that can be attached to
a point xed on the ground or to another moving link. At the other end is a link
to which a tool can be xed. To describe such a serial robot, we introduce a set of
mandatory coordinate frames that completely species kinematic conguration
of the chain and a set of additional optional coordinate frames that allow for
almost any use case that can be thought of.
2.4. DESCRIBING A SERIAL ROBOT 15
(a) (b) (c)
(d) (e)
Figure 2.4: Schematics of revolute and prismatic joints, a) Revolute joint rotating
around vertical axis. b) Revolute joint rotating around axis normal to the paper.
c) Prismatic joint translating in the vertical direction. d) Ground symbol for
marking the base link. e) Tool symbol.
Figure 2.5: Schematics of Kuka KR16
2.4.1 Coordinate frames
For each moving link, say i, in the chain we choose a unique mandatory coordinate
frame F
i
xed with respect to this link and located at the joint that drives link
i. More specically, the origin o
i
of this frame must thus be on joint axis i and the
z-axis z
i
must be chosen along the positive direction of the joint axis. Optionally,
an additional set of frames xed with respect to link i may be arbitrarily chosen.
The jth of these is denoted as F
i,j
.
Similarly, for the base link, we must choose a frame F
base
and have the option
of choosing additional frames F
base,j
.
Each of the coordinate systems F
(where = i or = i, j) has a unique

reference link coordinate system (F
). The reference link coordinate system to

a coordinate system F
i,j
may be chosen as any other link coordinate system xed
with respect to link i. The references must be established so that the frame F
i
is
reachable from any F
i,j
. We shall refer to a frame F
a
being reachable from
another frame F
b
as that there exists an m so that F
a
= (F
1
) = (F
2
) =
= (F
m
) = (F
b
) (see Figure 2.6). A frame that is reachable from all other
frames in a set is called the root frame of that set. Thus F
i
is the root frame
of the set of all frames associated with link i. Similarly, F
base
must be the root
of all frames associated with the base link.
Figure 2.6: Frame parent reference structure.
The reference link coordinate system to the mandatory coordinate system
F
i
i 2 may be chosen as any link coordinate system xed with respect to link
i 1. The reference link coordinate system to the coordinate system F
1
must be
chosen as one of the coordinate systems xed with respect to the base link. The
set of all frames are thus connected to each other through the references with the
frame F
base
as the overall root frame of the chain(see Figure 2.7).
Figure 2.7: Example of device frame structure for base to F
3
for Kuka KR16 .
At the nth link, a tool link may be rigidly connected. A tool link may have a
frame F
tool
that has a reference to a frame xed with respect to link n (F
n
or one
2.5. DESCRIBING A COMPLETE ROBOT SCENE 17
of the optional F
n,j
s). In addition, or alternatively, the tool may have a frame
F
TCP
that is also located at some suitable pose for the active part of the tool.
For an isolated robot, the frame F
base
has no reference frame. However, when
the robot is placed in a scene, a reference frame will be given. If the robot is
mounted on the ground, we may choose (F
base
) = F
World
, but the robot could
also be mounted on some link of another moving device where we then must
choose (F
base
) as some frame xed on the link to which the robot is attached.
2.5 Describing a complete robot scene
Consider a scene consisting of one or several serial robots and/or robots with a
tree topology. To describe such a scene, we must rst split the robots in the scene
into a set of disjoint serial robots. It should be noticed that this splitting is not
unique and our description should be open to any splitting that the user would
like as long as it results in a disjoint set of serial robots covering all moving links
and joints.
Having performed the splitting into a set of serial chains, we now consider an
arbitrary, say kth, serial chain. On each of the frames associated with a serial
chain, we now use a parameter [k] so that e.g. F
[k],i,j
is the frame F
i,j
for the kth
chain, F
[k],base
is the base frame of the kth chain and F
[k],tool
, F
[k],TCP
are the
frame F
tool
, F
TCP
respectively for the kth chain. We uniquely place the serial
chain in the scene by providing a reference frame (F
[k],base
).
It should be noticed that we will throughout the book omit the extra param-
eter [k] if it from the context is clear that we are only considering a single serial
robot chain.
Example of a robot scene
As an example, consider the scene in Figure 2.8 consisting of two 6-axis Universal
Robots UR-6-85-5-A robots. One of the robots has a parallel gripper attached
and the other robot has a Schunk SDH attached. Notice that two of the ngers in
the Schunk SDH can rotate at the base in a direction opposite to each other. This
forms another type of a dependent joint. In Figure 2.9, we show all the coordinate
frames at the joints. In Exercise 2.4, you will be asked to ll in remaining frames,
dene frame nomenclature and a reference tree.
2.6 State vector of joint variables. Degrees of freedom
Each frame F
[k],i
is located at a prismatic or revolute joint. We associate with
that frame a joint variable which we call q
i
k
. The state vector or set of all joint
Figure 2.8: A two robot scene
Figure 2.9: Schematics of the two robot scene
variables q in a scene with M serial chains is dened as the tuple
q = (q
1
1
, q
2
1
, . . . , q
n
M
M
)
where n
k
is the number of joints in the kth serial chain.
Again, if we only consider a single serial chain, we omit the k and write the
state vector as
q = (q
1
, q
2
, . . . , q
n
)
where n is the number of joints in the serial chain.
The state vector q thus uniquely determines the position and orientation of
all parts of all robots. In the next chapter, we will derive analytical expressions
2.7. EXERCISES 19
that allow us to compute the relation between any two frames as function of the
state vector. Here, we can already notice, that if we move a joint q
i
k
and keep all
the other joints xed, only frames from which F
[k],i
is reachable will move.
For a given robot, the total number of elements in the state vector that can
be associated with joints from that robot is called the degrees of freedom or DOF
of the robot. There are thus typically as many degrees of freedom in a robot as
there are joints. However, there are exceptions as will be described now.
2.6.1 Dependent Joints
A joint is said to be dependent if its value depend purely on one or more other
joints in the robot. Dependent joints provide an easy and convenient method for
modelling robots with parallelograms (see gure 2.10), which is commonly found
in robots for palletizing. The dependency is often implemented as a mechanical
linkage between joint 2 and 3, but can also be purely in software. Notice, that the
parallelogram actually is a physical implementation of the 4-linked closed loop
robot discussed in Exercise 2.1. A dependent joint in the robot thus by denition
is not motorized and despite of this, the robot is kinematically determinable.
Dependent joints are also commonly found in dextrous hands, where multiple
joints are controlled by a single motor. If we for instance wish to model a human
nger we will nd that the two outermost joints of each nger are connected,
such that when bending the innermost joint the outermost automatically follows.
If a joint q
i
k
is dependent on other joints, we remove it from the state vector
q so that all elements in the state vector are still independent. Instead, we
associate the function
i
k
(q) with frame F
[k],i
that denes the dependency. For
each dependent joint, the number of degrees of freedom is thus reduced by one.
We use the convention of choosing the dependent joints so that a dependent joint
is not reachable from any joint that it depends on. This convention has the
advantage, that it then still holds that if we move a joint q
i
k
from the state vector
and keep all the other joints from the state vector xed, only frames from which
F
[k],i
is reachable will move.
2.7 Exercises
Exercise 2.1.
i) Show that for a robot with a serial or tree structure, all joints must be
motorized in order for the robot to be kinematically determinable.
ii) Consider a 4-linked planar closed loop robot. Make a sketch of the robot.
What is the minimum set of joints required to be motorized in order for the
robot to be kinematically determinable ?
Figure 2.10: Palletizing robot Kuka KR-100 PA
Figure 2.11: Robot scene from Exercise 2.1
Exercise 2.2. Use the schematic notation to sketch how the spherical and cylin-
drical joints shown in Figure 2.1 can be modeled by 1 DOF revolute and prismatic
joints.
Exercise 2.3. When modeling a spherical joint by three 1 DOF joints we may
get into a Gimbal Lock. Discuss how the axes should align and the consequences.
Exercise 2.4. Draw the necessary remaining frames in Figure 2.9 in order to
establish a reference tree from a world frame to a tool frame at the gripper and at
2.8. PROGRAMMING EXERCISES 21
each of the three ngers in the Schunk hand. Write down the resulting reference
tree.
2.8 Programming Exercises
In the programming exercises you will need RobWork which is an open source
robotics framework and RobWorkStudio which is the associated user interface.
Precompiled versions of RobWorkStudio is available in the course material.
Programming Exercise 2.1.
Run RobworkStudio and open the le Kr16WorkCell\Scene.xml. A scene
as illustrated in Figure 1.1 should then appear. Initially familiarize yourself
with the 3D view and discover how to rotate, translate, zoom and move the
center of rotation.
Now open the Jog plugin and use the sliders to move the joints of the robot.
Also switch to the InvKin tab and try to move the robot using the Cartesian
input available.
Open the TreeView plugin and expand the tree. Familiarize yourself with the
three dierent views available in the TreeView. Visualize the WORLD, TCP
and Bottle frames by right clicking on the nodes and selecting Show/Re-
move Frame.
While discovering new features write down a small tutorial for future reference
and which you can share with your class mates.
You should now be familiar with the basic tools of RobWorkStudio. Then
try to move the Tool frame of the robot such that is aligns with the Bottle
frame. Do so only by jogging directly with the joints.
See how many dierent joint congurations you can nd which enables to
align the Tool and Bottle frames. Write down the congurations found.
Bonus Question: How many ways can you grip the bottle if you are allowed
to exploit the object symmetry.
Open the le SchunkSDH.xml found in the SchunkSDH folder.
Create a schematic drawing showing the kinematic structure of the hand.
Figure 2.12: Robot scene from Exercise 2.1
Chapter 3
Rotation matrices and
homogeneous transformations
In the previous chapter, we introduced link coordinate frames F
i
. The purpose of
this chapter is to derive a mathematical formulation of the relation between any
two coordinate frames and particular to address the usage for relating frames from
adjacent links in a device. In order to do this, we rst discuss how the rotational
relation can be derived and we then generalize this formulation to combined
translations and rotations by introducing socalled homogeneous coordinates.
3.1 Points, vectors and their coordinates in a frame
We shall in this section discuss various representations of points and vectors. The
points and vectors are residing in three dimensional space. More specically, we
refer to a point p as some position in three dimensional space and dene a vector
v as the displacement between two points p
1
and p
2
(thus v = p
2
p
1
).
Consider now any coordinate frame, say F
A
. With [p]
A
NH
we refer to the
coordinates of a point p in frame F
A
. The subscript NH indicates that it is the
classical (Non-Homogeneous) coordinates of p. The homogeneous coordinates of
p has an extra coordinate and will be dened in the subsection 3.5.
If the frame F
A
has an origin o
A
in three dimensional space and coordinate
axes (vectors) x
A
, y
A
, z
A
, we may compute the coordinates as
[p]
A
NH
= ([p o
A
] x
A
, [p o
A
] y
A
, [p o
A
] z
A
)
T
(3.1)
The coordinates of a vector v may be computed as
[v]
A
NH
(v x
A
, v y
A
, v z
A
)
T
(3.2)
23
24
CHAPTER 3. ROTATION MATRICES AND HOMOGENEOUS
TRANSFORMATIONS
(notice that this denition is independent of the origin o
A
because a vector is a
subtraction of two points). We shall also use the expansion of a vector in the
basis vectors of a coordinate frame:
v = (v x
A
)x
A
+ (v y
A
)y
A
+ (v z
A
)z
A
(3.3)
It is of the outmost importance to understand the above discussion of points and
vectors.
3.2 Denition of the rotation matrix
Consider two coordinate frames A and B with a common origin at the origin
(o
A
= o
B
= 0) but rotated relative to each other (see Figure 3.1). Consider now
an arbitrary vector p. We then immediately may obtain ps coordinates in the
A frame using Eq.3.1 as
[p]
A
NH
(p x
A
, p y
A
, p z
A
)
T
P
Z
A
Z
B
X
A
Y
A
Y
B
X
B
Figure 3.1: Rotated coordinate frames.
Similarly, we obtain ps coordinates in the B frame as
[p]
B
NH
(p x
B
, p y
B
, p z
B
)
T
We now wish to establish a relation between [p]
A
NH
and [p]
B
NH
. We rst expand
the coordinate axes of the A frame in the basis vectors of the B frame using
Eq.3.3:
x
A
= (x
A
x
B
)x
B
+ (x
A
y
B
)y
B
+ (x
A
z
B
)z
B
y
A
= (y
A
x
B
)x
B
+ (y
A
y
B
)y
B
+ (y
A
z
B
)z
B
z
A
= (z
A
x
B
)x
B
+ (z
A
y
B
)y
B
+ (z
A
z
B
)z
B
By insertion, we thus get
[p]
A
NH
=
_
_
p ((x
A
x
B
)x
B
+ (x
A
y
B
)y
B
+ (x
A
z
B
)z
B
)
p ((y
A
x
B
)x
B
+ (y
A
y
B
)y
B
+ (y
A
z
B
)z
B
)
p ((z
A
x
B
)x
B
+ (z
A
y
B
)y
B
+ (z
A
z
B
)z
B
)
_
_
3.3. ROTATION MATRICES AROUND COORDINATE AXES 25
Which may be rearranged to a matrix vector product
[p]
A
NH
=
_
_
x
A
x
B
x
A
y
B
x
A
z
B
y
A
x
B
y
A
y
B
y
A
z
B
z
A
x
B
z
A
y
B
z
A
z
B
_
_
_
_
p x
B
p y
B
p z
B
_
_
which we may abbreviate as
[p]
A
NH
=
_
_
x
A
x
B
x
A
y
B
x
A
z
B
y
A
x
B
y
A
y
B
y
A
z
B
z
A
x
B
z
A
y
B
z
A
z
B
_
_[p]
B
NH
and even shorter as
[p]
A
NH
= R
B
A
[p]
B
NH
where
R
B
A

_
_
x
A
x
B
x
A
y
B
x
A
z
B
y
A
x
B
y
A
y
B
y
A
z
B
z
A
x
B
z
A
y
B
z
A
z
B
_
_ (3.4)
is the rotation matrix relating frame A to frame B. By interchanging A and B,
we immediately see from the denition of R
B
A
that
R
A
B
= (R
B
A
)
T
Thus, we obtain the inverse of a rotation matrix by its transpose.
In Exercise 3.2, we show that R
C
A
= R
B
A
R
C
B
, so that we construct rotation
compositions by matrix multiplications. we shall use this property in the next
section.
3.3 Rotation matrices around coordinate axes
Often, we will address rotation around coordinate axes. Therefore, these deserve
special attention. Assume thus that frame B is identical to frame A except that it
has been rotated an angle
z
around z
A
. Notice that the angle around a rotation
axis is always dened as being positive in the clockwise direction seen in the
direction of the axis as illustrated in the Figure 3.2. We then obtain by direct
computation using Eq. 3.4
R
B
A
=
_
_
cos
z
sin
z
0
sin
z
cos
z
0
0 0 1
_
_ R
z
(
z
) (3.5)
26
TRANSFORMATIONS
Similarly, if the rotation is an angle
x
around x
A
, we obtain
R
B
A
=
_
_
1 0 0
0 cos
x
sin
x
0 sin
x
cos
x
_
_ R
x
(
x
) (3.6)
and nally, if the rotation is an angle
y
around y
A
, we obtain
R
B
A
=
_
_
cos
y
0 sin
y
0 1 0
sin
y
0 cos
y
_
_ R
y
(
y
) (3.7)
X
A
X
B
Y
A
Y
B
Z
A
,Z
B
Figure 3.2: [Rotation around the z-axis.]
3.4 Rool-pitch-yaw (RPY) Euler angles
Assume now that we rotate an angle
z
around z
A
(roll) to obtain frame B, then
an angle
y
around y
B
(pitch) to obtain frame C and nally an angle
x
around
x
C
(yaw) to obtain frame D. We then obtain
[p]
A
NH
= R
z
(
z
)[p]
B
NH
= R
z
(
z
)(R
y
(
y
)[p]
C
NH
) = R
z
(
z
)(R
y
(
y
)(R
x
(
x
)[p]
D
NH
))
and by using the associative law of matrix-vector multiplication
[p]
A
NH
= (R
z
(
z
)R
y
(
y
)R
x
(
x
))[p]
D
NH
(3.8)
The matrix R
RPY
(
x
,
y
,
z
) R
z
(
z
)R
y
(
y
)R
x
(
x
) can be computed directly.
However, we rst introduce the abbreviations
C
i
= cos
i
S
i
= sin
i
3.5. HOMOGENEOUS COORDINATES 27
where i is either an integer or one of the letters x,y or z. Using this notation,
we now get
R
RPY
(
x
,
y
,
z
) =
_
_
C
z
S
z
0
S
z
C
z
0
0 0 1
_
_
_
_
C
y
0 S
y
0 1 0
S
y
0 C
y
_
_
_
_
1 0 0
0 C
x
S
x
0 S
x
C
x
_
_
=
_
_
C
y
C
z
S
x
S
y
C
z
C
x
S
z
C
x
S
y
C
z
+ S
x
S
z
C
y
S
z
S
x
S
y
S
z
+ C
x
C
z
C
x
S
y
S
z
S
x
C
z
S
y
S
x
C
y
C
x
C
y
_
_ (3.9)
It can be shown that we can express any rotation matrix as R
RPY
(
x
,
y
,
z
)
for some angles
x
,
y
,
z
. However, whereas a given choice of
x
,
y
,
z
yields a
unique rotation matrix R
RPY
(
x
,
y
,
z
), the vice versa does not hold. To show
this, consider a rotation matrix
R =
_
_
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
_
_
To nd the corresponding roll, pitch and yaw angles, we start by observing that
the pitch angle can be found as
y
(R) = sin
1
(r
31
) (3.10)
If [r
31
[ , = 1, we obtain two solutions to this equation. For each of these solutions,
we may uniquely nd
x
and
z
as
x
(R) = atan2(C
y
r
32
, C
y
r
33
) (3.11)
z
(R) = atan2(C
y
r
21
, C
y
r
11
) (3.12)
where atan2(u, v) is dened as the unique angle so that u = r sin , v = r cos
for some positive r (notice that we require (u, v) ,= (0, 0)).
3.5 Homogeneous coordinates
Consider now two frames A and B that are both rotated and translated with
respect to each other. Let the origin of A be o
A
and the origin of B be o
B
(see
Figure 3.3).
Consider now an arbitrary point in space p. The coordinates of p in the A
frame is then
[p]
A
NH
((p o
A
) x
A
, (p o
A
) y
A
, (p o
A
) z
A
)
T
Similarly, we obtain ps coordinates in the B frame as
[p]
B
NH
((p o
B
) x
B
, (p o
B
) y
B
, (p o
B
) z
B
)
T
28
TRANSFORMATIONS
X
B
Z
B
Y
B
Y
A
X
A
Z
A
O
B
O
A
P
B
P
A
P
Figure 3.3: Translated and rotated coordinate frames.
It would be desirable if we also in this case could nd some matrix T such that
[p]
A
NH
= T[p]
B
NH
. Then we would again be able to apply matrix algebra for com-
position by multiplication and nding the inverse map by computing the inverse
matrix. To achieve this, we need to augment our matrix-vector formulation. We
dene the socalled homogeneous coordinates of a point p computed in frame A:
[p]
A
_
(p o
A
) x
A
(p o
A
) y
A
(p o
A
) z
A
1
_
_
(3.13)
We now dene the associated homogeneous transformation matrix between frames
A and B as
T
B
A

_
_
R
B
A
[ [o
B
]
A
[
0 0 0 [ 1
_
_ (3.14)
Direct computation yields
T
B
A
[p]
B
=
_
_
R
B
A
[ [o
B
]
A
[
0 0 0 [ 1
_
_
_
_
[p o
B
]
B
NH
1
_
_
=
_
_
R
B
A

0 0 0
_
_
_
_
[p o
B
]
B
NH
1
_
_ +
_
_
[o
B
]
A
1
_
_
=
_
_
[p o
B
]
A
NH
1
_
_ +
_
_
[o
B
o
A
]
A
NH
1
_
_
= [p]
A
3.6. COMPUTING THE STATE OF A ROBOT SYSTEM 29
We thus have
[p]
A
= T
B
A
[p]
B
(3.15)
We can therefore similar to pure rotations construct compositions by matrix
multiplication:
T
C
A
= T
B
A
T
C
B
The inverse transformation
_
T
B
A
_
1
T
A
B
is easily found as
T
A
B

_
_
R
A
B
[ [o
A
]
B
[
0 0 0 [ 1
_
_
=
_
_
_
R
B
A
_
T
[
_
R
B
A
_
T
[o
B
]
A
[
0 0 0 [ 1
_
_
(3.16)
3.6 Computing the state of a robot system
We now turn back to considering a robot state as described in the previous
section. Assume that we with each reference between a frame F
B
and the frame
F
A
= (F
B
) associate the homogeneous transformation Tref
B
A
when the state
vector q = 0. Assume now that we update the state vector q. We then only need
to update the transformations between the frame (F
[k],i
) and frame F
[k],i
for all
the mandatory frames F
[k],i
located at the joints. We immediately get that we
can update the transformation matrix by the relation
T
[k],i
([k],i)
:= Tref
[k],i
([k],i)
T
z
(q
i
k
)
(3.17)
where q
i
k
is the new value of the joint variable and T
z
(q
i
k
)
is the transformation
due to the joint. This transformation can be computed for a revolute joint as
T
z
(q
i
k
)
=
_
_
R
z
(q
i
k
) [ 0
[
0 0 0 [ 1
_
_ T
z
(q
i
k
)
r
(3.18)
and for a prismatic joint as
T
z
(q
i
k
)
=
_
_
I [ q
i
k
K
[
0 0 0 [ 1
_
_ T
z
(q
i
k
)
t
(3.19)
where K = (0, 0, 1)
T
.
30
TRANSFORMATIONS
Clearly, by using suitable traversing algorithms through the frame structure
given by the parental references, all transformations
T
[k],i
([k],i)
k = 1, . . . , M, ; i = 1, . . . , n
k
can be updated.
Assume now that we wish to compute the updated T
[k],
World
. We may com-
pute this matrix by using the reference list to move from F
[k],
to F
World
as
follows
1: j = [k],
2: i = ([k], )
3: T
[k],
World
:= T
j
i
4: while i ,= World do
5: j = i
6: i = (i)
7: T
[k],
World
= T
j
i
T
[k],
World
If we need to nd the transformation from the world frame to many or all
other frames, this algorithm is clearly inecient. We then need an ecient traver-
sal method through the frames and use the possibility to store already computed
transformations. Moreover, transformations between specic frame pairs are typ-
ically used over and over again, in which case it would be suitable to store infor-
mation concerning the chain between these frames. In the next section, we will
discuss this issue for the common case of a single serial robot.
3.7 Example:Computing the state of a serial device
Consider a serial device described with frames F
base
, F
1
, . . . , F
n
, F
TCP
with
reference chain (F
1
) = F
base
, (F
i
) = F
i1
i = 2, . . . , n, (F
TCP
) = F
n
. We
may then compute the transformation T
i
base
as
T
i
base
= T
1
base
T
2
1
. . . T
i
i1
(3.20)
and
T
TCP
base
= T
1
base
T
2
1
. . . T
n
n1
T
TCP
n
(3.21)
State vector independent transformations (in this case only T
TCP
n
) can be com-
puted directly as described above. Transformations that depends on joint vari-
ables can be computed as illustrated with T
i
i1
:
T
i
i1
= Tref
i
i1
T
z
(q
i
)
where q
i
is the state vector component from joint i, Tref
i
i1
is the transformation
if q
i
= 0, and r, t indicates if the joint is prismatic or revolute.
3.7. EXAMPLE:COMPUTING THE STATE OF A SERIAL DEVICE 31
Consider the example of a 2-axis robot with associated frames F
base
, F
1
, F
2
,
F
TCP
where joint 1 is revolute and joint 2 is prismatic. From a le describing the
robot (e.g. a RobWork device le), the Tref s are given by the position (x, y, z)
and the RPY angles (R, P, Y ) given in degrees.
For the transformation Tref
1
base
, the values are (x, y, z) = (1.2, 2.3, 3.5)
p
1
and (R, P, Y ) = (90, 0, 0). We then compute Tref
1
base
as
Tref
1
base
=
_
_
R
RPY
(0, 0,

2
) [ p
1
[
0 0 0 [ 1
_
_ =
_
_
0 1 0 [ 1.2
1 0 0 [ 2.3
0 0 1 [ 3.5
[
0 0 0 [ 1
_
_
If similarly, the values for Tref
2
1
are (x, y, z) = (0, 0, 4) p
2
and (R, P, Y ) =
(0, 0, 90), we get
Tref
2
1
=
_
_
R
RPY
(
2
, 0, 0) [ p
2
[
0 0 0 [ 1
_
_ =
_
_
1 0 0 [ 0
0 0 1 [ 0
0 1 0 [ 4
[
0 0 0 [ 1
_
_
Finally, if the values for Tref
TCP
2
are (x, y, z) = (0, 0, 2) p
TCP
and (R, P, Y ) =
(0, 0, 0), we get
Tref
TCP
2
=
_
_
R
RPY
(0, 0, 0) [ p
TCP
[
0 0 0 [ 1
_
_ =
_
_
1 0 0 [ 0
0 1 0 [ 0
0 0 1 [ 2
[
0 0 0 [ 1
_
_
Assume now that the state vector is given as q = (
3
, 2.8). We then get
T
TCP
base
= T
1
base
T
2
1
T
TCP
2
=
_
Tref
1
base
T
z
(
3
)
r
_ _
Tref
2
1
T
z
(2.8)
t
_
Tref
TCP
2
32
TRANSFORMATIONS
Insertion yields
T
TCP
base
=
_
_
0 1 0 [ 1.2
1 0 0 [ 2.3
0 0 1 [ 3.5
[
0 0 0 [ 1
_
_
_
_
1
2

3
2
0 [ 0
3
2
1
2
0 [ 0
0 0 1 [ 0
[
0 0 0 [ 1
_
_
1 0 0 [ 0
0 0 1 [ 0
0 1 0 [ 4
[
0 0 0 [ 1
_
_
_
_
1 0 0 [ 0
0 1 0 [ 0
0 0 1 [ 2.8
[
0 0 0 [ 1
_
_
1 0 0 [ 0
0 1 0 [ 0
0 0 1 [ 2
[
0 0 0 [ 1
_
_
Matrix multiplication yields
T
TCP
base

_
3
2
0
1
2
[ 1.2
1
2
0
3
2
[ 1.857
0 1 0 [ 7.5
[
0 0 0 [ 1
_
_
3.8 Exercises
Exercise 3.1. Assume x
A
= (1, 0, 0), y
A
= (0,
3
2
,
1
2
), z
A
= (0,
1
2
,
3
2
) and
x
B
= (0,
3
2
,
1
2
), y
B
= (0,
1
2
,
3
2
), z
B
= (1, 0, 0). Compute R
B
A
.
Exercise 3.2. Show that R
C
A
= R
B
A
R
C
B
.
Exercise 3.3. Consider the general rotation matrix in Eq.3.4.
Show that the columns of the matrix are orthogonal and have lengths one.
Show that the inverse rotation matrix is the transpose
Show that the rows of the matrix are also orthogonal and have lengths one.
Show that the determinant is one
Show that if 5 elements of which at most one is zero are identied and of which
3 are in the same row or in the same column, then the remaining elements are
uniquely given (this means that identifying such 5 elements in e.g. inverse RPY
is sucient.
Exercise 3.4. Show that if r
31
= 1, we have an innite set of solutions to the
RPY Euler angles
y
=
x
+
z
= atan2(r
12
, r
22
)
and if r
31
= 1, we have an innite set of solutions
y
=

2
z
= atan2(r
12
, r
22
)
Exercise 3.5. Assume x
A
= (1, 0, 0), y
A
= (0,
3
2
,
1
2
), z
A
= (0,
1
2
,
3
2
) and
x
B
= (0,
3
2
,
1
2
), y
B
= (0,
1
2
,
3
2
), z
B
= (1, 0, 0). The origins are o
A
=
(2, 1, 0), o
B
= (4, 4, 1).
i) Compute T
B
A
and
_
T
B
A
_
1
.
Assume now that we have an additional frame C with x
C
= (0, 1, 0), y
C
=
(0, 0, 1), z
C
= (1, 0, 0) and o
C
= (5, 0, 0).
ii) Compute T
C
B
.
iii) Compute T
C
A
by matrix multiplication.
Exercise 3.6. Assume for a serial robot that joint i is revolute and that Tref
i
i1
is given as
Tref
i
i1
=
_
_
1 0 0 [ 1
0

3
2
1
2
[ 1
0
1
2
3
2
[ 0
[
0 0 0 [ 1
_
_
i) Compute T
i
i1
as function of the joint angle q
i
.
Throughout the book there will be a number of exercises which will build up a
kinematics toolbox. You are asked to program these in whatever environment
you prefer (Mathematica, MatLab, Java, C/C++ etc.). When reaching the end
of the course you should have a nice toolbox from which you can benet in the
nal project and in the future.
Programming Exercise 3.1. Write code which calculates the 3 3 rotation
matrix given:
A specication of which axis to rotate around.
34
TRANSFORMATIONS
A specication of whether to use xed frame or Euler rotation.
The 3 parameters corresponding to the rotation around the individual axes.
Programming Exercise 3.2. Write code for transforming a 3 3 rotation
matrix into RPY. Use equation (3.10) to (3.11) and remember to pay attention
to the special cases where [r
31
[ = 1.
Programming Exercise 3.3. In this exercise you shall construct your own
device le for the Universal-Robots UR-6-85-5-A (see Figure 3.4). In the course
material you can nd the necessary data sheets and geometry. Devices are written
as XML-les. A denition of the format can be found at
http://www.robwork.dk/apidoc/nightly/rw/page_xml_workcell_format.html
Rather than starting from scratch it may be easier to use an existing device le
and modify it.
Joints frames F
i
, i = 1, . . . , n are directly specied in the device le. Links
frames in the device le is optional, but can be useful in positioning the geometry
relative to the joint frames. The main dierence between the notation used in
the book and the device les is that the latter allows arbitrary (but unique) names
and not only indices.
All link geometry associated to this exercise is located relative to the base of
the robot. When inserting geometry, you will need to create a link frame, which
has a transformation corresponding to that from the base and to the frame on
which it are attached. You can load the geometry into RobWorkStudio to get an
idea of how it is positioned and oriented.
Use RobWorkStudio to load and verify your device. It may be a good idea to
load it after adding each joint to make sure everything looks right.
Figure 3.4: Universal-Robots UR-6-85-5-A
Programming Exercise 3.4. Write a program that for a serial robot takes
the state vector q and all the transformations Tref
i
i1
as input (the rotation
part may be given by the RPY angles). The program should then compute the
transformation T
i
base
using Eq. 3.20 for an arbitrary i and T
TCP
base
using 3.21.
Generalize the program to cover the case where there are arbitrary many ad-
ditional link frames F
i,j
along the chain.
Chapter 4
Manipulator Jacobians and
related Numerical Inverse
Kinematics
In the previous chapter, we have derived how to compute a state of the robotic
system as function of the state vector q. This includes the positions of the
endeectors. In this chapter, we will consider the inverse problem, namely how
to compute the state vector (or the relevant part of it) from a set of endeector
positions. More precisely, we will present numerical inverse kinematics methods.
We will for simplicity assume that we have a single serial robot, but the method
can easily be extended to multiple robot and/or multiple tools. We will as before
assume that frame F
i
has origin on joint axis i and that z
i
is oriented in the
positive direction of joint axis i. Input to the numerical inverse kinematics method
is an initial state q, a target frame T
tool
base
= T
desired
and the method discussed
in the previous chapter for computing all T
i
base
(q) for i = 1 . . . , n as function of
the state q.
The presented methods will rely on being able to compute the Jacobian for
a set of non-linear functions. We will review Jacobians in the rst section. We
will then derive the Jacobian for a robot (often referred to as the Manipulator
Jacobian) and use that in various numerical methods. We will present methods
for small (or even dierential) displacements between T
tool
base
(q) and T
desired
.,
which can be used to for example track a specied trajectory of the tool and
large displacements that can used for just nding an inverse kinematics solution
to T
desired
.
At the end of the chapter, we discuss straight forward extensions to consid-
ering inverse kinematics for the transformation between any two frames and to
37
38
CHAPTER 4. MANIPULATOR JACOBIANS AND RELATED NUMERICAL
INVERSE KINEMATICS
solving several dependent inverse kinematics problems such as to each endeector
of a robot with a tree topology.
4.1 Multidimensional rst order Taylor expansions. Jaco-
bians
Consider a set of functions
f
1
(x
1
, . . . , x
n
)
f
2
(x
1
, . . . , x
n
)
...
...
...
f
n
(x
1
, . . . , x
n
)
Consider now displacements (dx
1
, . . . , dx
n
). We apply Taylors formula:
f
i
(x
1
+ dx
1
, . . . , x
n
+ dx
n
) = f
i
(x
1
, . . . , x
n
)
+
n
i=1
f
i
(x
1
, . . . , x
n
)
x
j
dx
j
i = 1, . . . , n
By dening the Jacobian J
ij
(x
1
, . . . , x
n
)
f
i
(x
1
,...,x
n
)
x
j
, we can abbreviate this
expression as
f
i
(x
1
+ dx
1
, . . . , x
n
+ dx
n
) = f
i
(x
1
, . . . , x
n
)
+
n
i=1
J
ij
(x
1
, . . . , x
n
)dx
j
i = 1, . . . , n
or in matrix vector notation
f (x +dx) = f (x) +J(x)dx
It is important to notice that the linear property of the impact of dierential dis-
placements on any function means that we can split the dierential displacements
into any subset we like and reorder any otherwise non-commuting sequence in any
way we like. Here, we shall in particular use this property for rotations. Consider
as an example the matrix RPY(
x
,
y
,
z
) R
z
(
z
)R
y
(
y
)R
x
(
x
). In general,
the matrices in the product do not commute (thus e.g. R
z
(
z
)R
y
(
y
)R
x
(
x
) ,=
R
x
(
x
)R
y
(
y
)R
z
(
z
). However, if the angles are innitely small, we may replace
cos(d) 1 and sin(d) d. We then obtain
RPY (d
x
, d
y
, d
z
) = (I +dR
z
(d
z
))(I +dR
y
(d
y
))(I +dR
x
(d
x
))
= I +dR
x
(d
x
) +dR
y
(d
y
) +dR
z
(d
z
)
4.2. POSITIONAL AND ANGULAR DISPLACEMENTS 39
where e.g.
dR
z
(d
z
) =
_
_
0 d
z
0
d
z
0 0
0 0 0
_
_
We then get
dR
x
(d
x
) + dR
y
(d
y
) + dR
z
(d
z
) =
_
_
0 d
z
d
y
d
z
0 d
x
d
y
d
x
0
_
_
Thus, if the RPY coordinates are innitely small, the rotation matrices commute.
A similar argument can be used for any sequence of dierential rotations. Con-
sider now that a dierential matrix dR is given. Then, there exists a unique set of
small RPY angles d
x
, d
y
, d
z
so that dR
x
(d
x
) + dR
y
(d
y
) + dR
z
(d
z
) = dR.
We shall return to this in the next section. We shall often refer to this as a vector
of rotation (d
x
, d
y
, d
z
)
4.2 Positional and angular displacements
In the following, we assume for simplicity that the reference frame is the base
frame. Thus, when we refer to e.g. an origin o
i
, we refer to the coordinates of
the origin of frame i computed in the base frame. Assume that we have placed
frames F
i
i = 1, . . . , n so that o
i
is located on joint axis i and z
i
along the
positive direction of joint i. Assume that the robot is at a joint conguration
q = (q
1
, . . . , q
n
)
T
. The transformations T
tool
base
(q) and T
i
base
(q
1
, . . . , q
i
) for any i
are then given. Although we thus realize that T
i
base
only depends on the rst i
joint angles, we for the sake of brevity write it as T
i
base
(q). Assume now that
we move joint i by a signed innitesimal amount dq
i
. We then obtain a new
transformation T
tool
base
(q +dq
i
) where dq
i
= (0, . . . , 0, dq
i
, 0, . . . , 0)
T
. If joint i is
rotational, we immediately obtain
p
tool
base
(q +dq
i
) = p
tool
base
(q) + dq
i
_
z
i
base
(q)
[p
tool
base
(q) p
i
base
(q)]
where in general p
j
i
(q) is the positional part of T
j
i
(q) and z
j
i
(q) is the z-axis of
T
j
i
(q) (third column in the rotation matrix.
If joint i is prismatic, we get
p
tool
base
(q +dq
i
) = p
tool
base
(q) + dq
i
z
i
base
(q)
Consider now the angular displacement between two rotations R
tool
base
(q) and
R
tool
base
(q + dq). As mentioned in the previous section, there exists a unique in-
nitesimal vector dw
tool
base
(q, dq) given in base coordinates so that if we initially
40
INVERSE KINEMATICS
are at R
tool
base
(q) and then rotate around dw
tool
base
(q, dq) with a rotation of size the
length of the vector, we reach R
tool
base
(q + dq).
Consider now joint i. Clearly, if joint i is prismatic, we obtain no angular dis-
placement due to dq
i
. If the joint is revolute, we may write the angular displace-
ment as the unique innitesimal rotation vector dw
tool
base
(q, [dq]
i
) = dq
i
z
i
base
(q).
Thus, the angular displacement is represented as a vector in the direction around
which the rotation takes place and with length equal to the size of the rotation.
In order to proceed, we dene a joint type function
i
=
_
1 if joint i is revolute
0 if joint i is prismatic
(4.1)
We may then write the positional displacement as
p
tool
base
(q +dq
i
) = p
tool
base
(q) +
i
dq
i
_
z
i
base
(q)
[p
tool
base
(q) p
i
base
(q)]
+(1
i
)dq
i
z
i
base
(q)
and the angular displacement of the tool frame as the vector
i
dq
i
z
i
base
(q)
We now consider a general displacement dq = (dq
1
, . . . , dq
n
). We now use the
result of the previous section, that the total displacement may then be found as a
sum of the individual displacements. That is, we obtain a positional displacement
dp
tool
base
(q, dq) = p
tool
base
(q +dq) p
tool
base
(q)
=
n
i=1
dq
i
_
i
_
z
i
base
(q)
[p
tool
base
(q) p
i
base
(q)]
+(1
i
)z
i
base
(q)
_
A(q)dq (4.2)
where A(q) is a 3 n matrix with jith element
[A(q)]
ji
=
_
i
_
z
i
base
(q)
[p
tool
base
(q) p
i
base
(q)] + (1
i
)z
i
base
(q)
_
j
(4.3)
The total angular displacement of the tool frame is given by
dw
tool
base
(q, dq) =
n
i=1
dq
i
_
i
z
i
base
(q)
_
B(q)dq (4.4)
where B(q) is a 3 n matrix with jith element
[B(q)]
ji
=
_
i
z
i
base
(q)
_
j
(4.5)
4.2. POSITIONAL AND ANGULAR DISPLACEMENTS 41
We thus have the relations
A(q)dq = p
tool
base
(q +dq) p
tool
base
(q)
B(q)dq = dw(R
tool
base
(q), R
tool
base
(q +dq)) (4.6)
where dw(R
tool
base
(q), R
tool
base
(q +dq)) denotes the unique small rotation vector for
rotating from R
tool
base
(q) to R
tool
base
(q +dq). We now formally dene the Jacobian
J(q)
_
_
A(q)
B(q)
_
_ (4.7)
where A(q), B(q) are given by Eq.4.3 and Eq.4.5 respectively. The 6 n matrix
J(q) is called the Manipulator Jacobian (or sometimes just the Jacobian) of the
robot. In some textbooks such as

, an analytical formula for the Jacobian based
on the Denavit-Hartenberg formulation is presented. It should be pointed out
that these formulae are not necessary and give only very little additional insight.
4.2.1 Example: J(q) for a 3-axis robot
Consider the three axis robot from the previous section also illustrated in Figure
4.1. The frames F
base
, F
1
, F
2
, F
3
, F
tool
are shown on the gure. All joints are
revolute. We have L = 3, a
2
= a
3
= 2 and q = (q
1
, q
2
, q
3
) = (0,
6
,

6
) where
q
i
is chosen as the angle between x
i1
and x
i
around z
i
. A numerical forward
kinematics routine yields
p
1
base
(q) = p
2
base
(q) = (0, 0, 3) ; p
3
base
(q) = (1, 0, 3 +
3) ; p
tool
base
(q) = (3, 0, 3 +
3)
z
1
base
(q) = (0, 0, 1) ; z
2
base
(q) = (0, 1, 0) ; z
3
base
(q) = (0, 1, 0)
Thus, we now obtain
[A(q)]
1
= z
1
base
(q) (p
tool
base
(q) p
1
base
(q)) = (0, 0, 1) (3, 0,
3) = (0, 3, 0)
[A(q)]
2
= z
2
base
(q) (p
tool
base
(q) p
2
base
(q)) = (0, 1, 0) (3, 0,
3) = (
3, 0, 3)
[A(q)]
3
= z
3
base
(q) (p
tool
base
(q) p
3
base
(q)) = (0, 1, 0) (2, 0, 0) = (0, 0, 2)
[B(q)]
1
= z
1
base
(q) = (0, 0, 1)
[B(q)]
2
= z
2
base
(q) = (0, 1, 0)
[B(q)]
3
= z
3
base
(q) = (0, 1, 0)
R.J.Schilling, Fundamentals of Robotics: Analysis and Control

42
INVERSE KINEMATICS
where e.g. [A(q)]
1
denotes the rst column of A. We now obtain
J(q) =
_
_
0
3 0
3 0 0
0 3 2
0 0 0
0 1 1
1 0 0
_
_
Figure 4.1: 3-axis robot with coordinate frames
4.3 Numerical Inverse Kinematics for innitesimal dis-
placements
Assume that we are at a joint conguration q with associated forward kinematics
T
tool
base
(q). We now wish to nd dq so that we obtain a desired innitesimal change
to [T
tool
base
]
desired
. That is, we wish to nd dq so that T
tool
base
(q+dq) = [T
tool
base
]
desired
.
First, we compute the desired positional change
[dp
tool
base
]
desired
= [p
tool
base
]
desired
p
tool
base
(q) (4.8)
Consider now an arbitrary angular displacement vector dw
tool
base
. In order to obtain
a corresponding rotation matrix, we use again the result from the section 4.1 to
split our innitesimal small rotation into rotations around the coordinate axes.
We thus get a total rotation R
RPY
([dw
tool
base
]
x
, [dw
tool
base
]
y
, [dw
tool
base
]
z
). Inserting
into Eq.3.9 and applying the rst order approximations cos([dw
tool
base
]
c
) = 1 ;
sin([dw
tool
base
]
c
) = [dw
tool
base
]
c
for c = x, y, z yields
R
RPY
([dw
tool
base
]
x
, [dw
tool
base
]
y
, [dw
tool
base
]
z
) =
_
_
1 dw
tool
basez
dw
tool
basey
dw
tool
basez
1 dw
tool
basex
dw
tool
basey
dw
tool
basex
1
_
_
(4.9)
4.4. NUMERICAL INVERSE KINEMATICS FOR SMALL BUT FINITE
DISPLACEMENTS 43
Consider now the other way around where the rotation matrix is given as R. By
considering Eq. 4.9, we then get
[dw
tool
base
] =
1
2
_
_
R
32
R
23
R
13
R
31
R
21
R
12
_
_
For a desired tool rotation matrix [R
tool
base
]
desired
, we then obtain a corresponding
matrix from which we can compute the vector of rotation [dw
tool
base
]
desired
through
the relation
[R
tool
base
]
desired
= [R
tool
base
]
desired
_
[R
tool
base
(q)]
T
R
tool
base
(q)
_
=
_
[R
tool
base
]
desired
[R
tool
base
(q)]
T
_
R
tool
base
(q)
We thus get
[dw
tool
base
]
desired
=
1
2
_
_
[[R
tool
base
]
desired
R
tool
base
(q)
T
]
32
[[R
tool
base
]
desired
R
tool
base
(q)
T
]
23
[[R
tool
base
]
desired
R
tool
base
(q)
T
]
13
[[R
tool
base
]
desired
R
tool
base
(q)
T
]
31
[[R
tool
base
]
desired
R
tool
base
(q)
T
]
21
[[R
tool
base
]
desired
R
tool
base
(q)
T
]
12
_
_
(4.10)
Using Eq. 4.2 and Eq.4.4, we then have 6 linear relations in n unknowns
A(q)dq = [dp
tool
base
]
desired
B(q)dq = [dw
tool
base
]
desired
that should be solved for the desired dq. Again, we may formally write these as
J(q)dq = [du]
desired
(4.11)
where J(q) is the Manipulator Jacobian dened in the previous section and
du
_
_
[dp
tool
base
]
desired
[dw
tool
base
]
desired
_
_ W(T
tool
base
(q), [T
tool
base
]
desired
) (4.12)
where the two components of W(T
tool
base
(q), [T
tool
base
]
desired
) is given by Eqs. 4.8 and
4.10.
4.4 Numerical Inverse Kinematics for small but nite dis-
placements
Assume still that we are at a joint conguration q with associated forward kine-
matics T
tool
base
(q). We now wish to nd a nite displacement q so that we obtain
a desired nite but small change to [T
tool
base
]
desired
. That is, we wish to nd q
44
INVERSE KINEMATICS
so that T
tool
base
(q +q) = [T
tool
base
]
desired
. By a procedure completely equivalent to
innitesimal dierences, we nd to rst order in q
J(q)q = [u]
desired
(4.13)
where J(q) is identical to the innitesimal case and we get
u W(T
tool
base
(q), [T
tool
base
]
desired
)
The only dierence to the innitesimal case is that we have neglected quadratic
and higher order terms in q which are now all nite. We will thus generally
hit the target transformation [T
tool
base
]
desired
with an error O(q
2
). In order to
control the error, the iteration presented in Algorithm 1 be applied.
Algorithm 1 Jacobian based Inverse Kinematics for small displacements
Require: q, T
tool
base
(q), [T
tool
base
]
desired
1: Compute u = W(T
tool
base
(q), [T
tool
base
]
desired
)
2: while |u| > do
3: Compute J(q) (see Eq. 4.7)
4: Solve J(q)q = u
5: q := q +q
6: Compute T
tool
base
(q)
7: Compute u = W(T
tool
base
(q), [T
tool
base
]
desired
)
4.5 The equivalent angle axis representation for a rotation
matrix
The shortest rotational path between two coordinate frames is to rotate around
the so called Equivalent Angle Axis (EAA), which is a unique axis around which
a rotation can take place. A rotation an angle around an arbitrary unit vector
v = (v
1
, v
2
, v
3
)
T
yields a rotation matrix
R
EAA
(v, ) =
_
_
v
2
1
(1 C
) + C
v
1
v
2
(1 C
) v
3
S
v
1
v
3
(1 C
) + v
2
S
v
1
v
2
(1 C
) + v
3
S
v
2
2
(1 C
) + C
v
2
v
3
(1 C
) v
1
S
v
1
v
3
(1 C
) v
2
S
v
2
v
3
(1 C
) + v
1
S
v
2
3
(1 C
) + C
_
(4.14)
where v
i
= [v]
i
. We shall often use the alternative notation
R
eaa
(v) R
EAA
(v, ) (4.15)
It should be noticed that R
EAA
(v, ) ,= R
RPY
(v
1
, v
2
, v
3
) because whereas
innitely small rotations commute, nite rotations do not. It is left as an exercise
to prove Eq. 4.14.
4.5. THE EQUIVALENT ANGLE AXIS REPRESENTATION FOR A ROTATION
MATRIX 45
If we have a given rotation matrix Rwith jkth element R
jk
, we can (assuming
0 uniquely compute the equivalent angle axis by the formulae
= cos
1
_
R
11
+ R
22
+ R
33
1
2
_
v =
1
2 sin
_
_
R
32
R
23
R
13
R
31
R
21
R
12
_
_
=
1
|(R
32
R
23
, R
13
R
31
, R
21
R
12
)|
_
_
R
32
R
23
R
13
R
31
R
21
R
12
_
_
(4.16)
which we will formally write as v = W
rot
(R) where we thus have
W
rot
(R) =
cos
1
_
R
11
+R
22
+R
33
1
2
_
|(R
32
R
23
, R
13
R
31
, R
21
R
12
)|
_
_
R
32
R
23
R
13
R
31
R
21
R
12
_
_ (4.17)
However, due to singularities, this formula should be avoided if 0 or if .
If 0, we may estimate sin . Using Eq.4.16, we then get
W
rot
(R) =
1
2
_
_
R
32
R
23
R
13
R
31
R
21
R
12
_
_ (4.18)
If , it get slightly more involved. We may rst again use the linearization
sin together with Eq.4.16 to obtain
2( ) = |(R
32
R
23
, R
13
R
31
, R
21
R
12
)|
or
=
|(R
32
R
23
, R
13
R
31
, R
21
R
12
)|
2
To obtain a singularity free equation for v, we use the diagonal terms in the
rotation matrix with cos 1 to obtain
v =
(
1
1 + R
11
,
2
1 + R
22
,
3
1 + R
33
)
T
3 + R
11
+ R
22
+ R
33
where
1
= sign(R
32
R
23
),
2
= sign(R
13
R
31
),
3
= sign(R
21
R
12
). We
then get
W
rot
(R) =
2 |(R
32
R
23
, R
13
R
31
, R
21
R
12
)|
2
3 + R
11
+ R
22
+ R
33
_
1 + R
11
1 + R
22
1 + R
33
_
_ (4.19)
46
INVERSE KINEMATICS
It can be studied where to shift between the various formulae for W
rot
(R). A
rule of thumb for double precision machines is to rst compute
x =
1
2
|(R
32
R
23
, R
13
R
31
, R
21
R
12
)|
Then use the appropriate Eq.4.18 or Eq.4.19 if x 10
6
or x 10
6
respectively. Otherwise use the general formula in Eq. 4.17.
On the other hand, if we have computed an equivalent angle axis y, we may
compute the rotation matrix R
eaa
(y) by rst computing = |y| and then choose
R
eaa
(y) =
_
R
EAA
(
y
, ) if ,= 0
I if = 0
(4.20)
where I is the 3 3 identitymatrix.
4.6 Numerical Inverse Kinematics for large displacements
Assume still that we are at a joint conguration q with associated forward kine-
matics T
tool
base
(q). We now wish to nd a displacement to a new point q
target
so
that T
tool
base
(q
target
) [T
tool
base
]
desired
. Here, we adopt the idea of dividing the dis-
placement into a set of M small displacements. That is, we wish to establish a
set of trajectories
[T
tool
base
]
desired,i
i = 0, . . . , M
so that [T
tool
base
]
desired,0
= T
tool
base
(q), [T
tool
base
]
desired,M
= [T
tool
base
]
desired
and so that we
can move from [T
tool
base
]
desired,i
to [T
tool
base
]
desired,i+1
using a joint displacement that
is small enough to use the method from the previous section. It is easy to nd
the positional part of these frames. We choose equidistant interpolation of the
straight line between the two positions.
[p
tool
base
]
desired,i
= p
tool
base
(q) +
i
M
_
[p
tool
base
]
desired
p
tool
base
(q)
_
p
tool
base
(q) +
i
M
W
pos
(T
tool
base
(q), [T
tool
base
]
desired
) (4.21)
The rotational part is a little more subtle to handle. We rst use the results from
the previous section to observe that
[R
tool
base
]
desired
= R
tool
base
(q)
_
R
tool
base
(q)
T
[R
tool
base
]
desired
_
= R
tool
base
(q)R
eaa
(W
rot
(
_
R
tool
base
(q)
T
[R
tool
base
]
desired
_
))
We may thus compute the desired equivalent angle axis as
v
desired
= W
rot
(
_
R
tool
base
(q)
T
[R
tool
base
]
desired
_
) (4.22)
4.7. SOLUTION METHODS FOR J(Q)Q = U 47
and then we can choose
[R
tool
base
]
desired,i
= R
tool
base
(q)R
eaa
(
i
M
v
desired
) (4.23)
We have now dened [R
tool
base
]
desired,i
and [p
tool
base
]
desired,i
and thus [T
tool
base
]
desired,i
except that we need to choose the number of tesselations M. This value depends
on the size of the positional and angular displacements. We suggest to use
M = Trunc
_
1
_
|p
tool
basedesired
p
tool
base
(q)|
|WS|
+
|v
desired
|
__
+ 1 (4.24)
where |WS| denotes some (possibly rather crude) estimate of the size of the
positional part of the workspace that the robot can reach and should be chosen
tessellation size relative to the whole workspace. We suggest to be chosen
between 0.01 and 0.05 which will work eciently in most cases.
Algorithm 2 Jacobian based Inverse Kinematics for large displacements
Require: q
start
, T
tool
base
(q
start
), [T
tool
base
]
desired
1: Compute v
desired
by Eq.(4.22)
2: Compute M = Trunc
_
1
_
p
tool
basedesired
p
tool
base
(q)
WS
+
v
desired
__
+ 1
3: q = q
start
4: for i = 1 to M do
5: Compute [T
tool
base
]
desired,i
by Eqs. (4.21) and (4.23) from T
tool
base
(q
start
) and
[T
tool
base
]
desired
6: Compute u = W(T
tool
base
(q), [T
tool
base
]
desired,i
) (see Eq. 4.12)
7: while |u| > do
9: Solve J(q)q = u
10: q := q +q
11: Compute T
tool
base
(q)
12: Compute u = W(T
tool
base
(q), [T
tool
base
]
desired,i
)
4.7 Solution methods for J(q)q = u
In general, the formula J(q)q = u will be 6 linear equations in n unknowns.
If n < 6, a reduction of the workspace dimensions and thus in the number of
equations should be addressed. However, we will here only consider n 6. The
simplest way is to solve this set of linear equations without further considerations.
If n = 6, we can use a standard solver for a set of linear equations. If n > 6, we
have extra degrees of freedom. The most standard way of handling this situation
is to apply the Generalized Inverse also known as the Moore-Penrose inverse:
Solve (J(q)[J(q)]
T
)y = u
48
INVERSE KINEMATICS
Choose q = [J(q)]
T
y
Formally, this can also be written as q = [J(q)]
T
(J(q)[J(q)]
T
)
1
u. The
solution minimizes |u|
2
subject to the constraint J(q)q = u.
4.8 Singularities and Joint limits
From the previous chapter, we know that we may encounter a singularity. At a
singularity, the matrix J(q) loses rank and we can no longer be sure that a solution
exists to J(q)q = u. Furthermore, there are in general positional, velocity
and possibly also acceleration limits on the joint variables. It can be shown
that we at any stage in the inverse Jacobian method can combine these limits to
q
min
q+q q
max
. We may then robustly handle limits and singularities by
rephrasing the set of linear equations J(q)q = u to an optimization problem:
min|J(q)q u|
subject to (4.25)
q
min
q +q q
max
This is a Quadratic Programming Problem. Such a problem can be solved
eciently by various algorithms. A method that works well and is relatively easy
to understand is the Interior Point Method described in Nocedal and Wright:
Numerical Optimization, pp. 480-485.
In cases where singularities and joint limits causes no problems, we just obtain
the solution to J(q)q = u, but otherwise we obtain a sensible solution as
close to the desired displacement as possible.
4.9 Changing reference and target frame for the Jacobian
Clearly, we can directly generalize the formulation above to replace F
base
and
F
tool
with any two frames, say F
A
and F
B
. However, we also have the possibility
of changing frames after we have computed the Jacobian and the right hand side.
Assume thus rst that we have computed a Jacobian [J
B
]
A
and a right hand side
[u
B
]
A
. Assume that we wish to change the tool frame B to another frame C
and that we know the transformation T
C
B
. We rst observe the relations
p
C
A
= p
B
A
+ w
B
A
(p
C
A
p
B
A
)
w
C
A
= w
B
A
We may formally write this relation as
[J
C
]
A
= S
e
B,C
[J
B
]
A
[u
C
]
A
= S
e
B,C
[u
B
]
A
(4.26)
4.10. EXERCISES 49
where
S
e
B,C
=
_
_
I [ (p
C
A
p
B
A
)

0 [ I
_
_ (4.27)
where for a vector p = (p
1
, p
2
, p
3
, 1), we have dened a 3 3 matrix
(p) =
_
_
0 p
3
p
2
p
3
0 p
1
p
2
p
1
0
_
_
so that [p]u = u p for any vectors p and u. Similarly, if we wish to change
the base frame A to another frame C, we can achieve this through the formulae
[J
B
]
C
= S
b
A,C
[J
B
]
A
[u
B
]
C
= S
b
A,C
[u
B
]
A
(4.28)
where
S
b
A,C
=
_
_
R
A
C
[ 0

0 [ R
A
C
_
_ (4.29)
4.10 Exercises
Prove equation 4.14.
Programming Exercise 4.1. Extend your ToolBox with a method to compute
J(q). You should have done Programming Exercise 3.4 prior to this one. Check
your Jacobian on the example in Section 4.2.1. The relevant parameters for the
constant transformations Tref
i
i1
are
x y z R P Y
Base1 0 0 L 0 0 0
1 2 0 0 0 0 0

2
2 3 0 a
2
0 0 0 0
3 TCP a
3
0 0 0 0 0
Programming Exercise 4.2. Write a program that implements Algorithm 1.
You may check your program on the Universal Robot in the following way.
Choose a random conguration q.
Compute T
tool
base
(q).
50
INVERSE KINEMATICS
Choose a small positional change p of the order e.g. 10
4
.
Choose [T
tool
base
]
desired
by adding the positional change p to the position part
of T
tool
base
(q).
Apply your method.
Check that the results gives the desired p and leaves the orientation un-
changed (to an accuracy of approximately 10
8
).
Programming Exercise 4.3. Extend your ToolBox with the following meth-
ods to
Compute (v, ) given a rotation R.
Compute R
EAA
given (v, ).
Verify your program by converting back of forth using the two methods. Remember
to pay attention to the cases where 0 and .
Programming Exercise 4.4. In this programming exercise you should imple-
ment the numerical inverse kinematics method from Algorithm 2. As input our
program should take a set of Denavit Hartenberg parameters, the T
G0
base
, T
TCP
Gn
transformations and the desired T
TCP
base
.
Programming Exercise 4.5. The purpose of this programming exercise is to
cross check the analytical and numerical inverse kinematics method.
We use a robot with a rather simple kinematics, so that you may also do a
cross check by using sketches. Consider the 4-axis Scara robot from Exercise ??.
Choose now a
1
= a
2
= 1, d
4
= 0.5. We now choose F
base
= G
0
and F
tool
= G
4
.
1. Compute T
tool
base
T
g4
g0
for q
1
= q
2
= q
4
= 0, q
3
= 1.
2. Move T
tool
base
so that the position becomes p
desired
= (1, 1, 0) without chang-
ing the orientation at any stage. Use numerical inverse kinematics with a
tessellation of M = 10. Store the results for all intermediate congurations.
3. Compute the corresponding analytical inverse kinematics solutions for all
intermediate congurations and compare the results.
Programming Exercise 4.6. In this exercise we will consider the Kuka KR16
, which has 6 degrees of freedom. We will try to generate a number of paths and
visualize these in RobWorkStudio.
1. Select a singularity free start conguration and compute the corresponding
T
TCP
base
.
2. Use RobWorkStudio to determine a feasible target transformation T
target
.
3. Use numerical inverse kinematics with a tessellation of M = 100 and store
the points to a script le in the format specied below.
4. Load the generated script into RobWorkStudio and execute the path.
4.11.1 Script Format
To execute the motions you will have to output the path found into a script,
which can then be loaded and executed in RobWorkStudio. An example of a
script-le can be found on Black Board. The rst approximately 25 lines of the
script is a header you do not have to worry about. The rest of the le is a shown
below:
-- load device, gripper, object and state
arm = wc:findDevice("KukaKr16") -- robot arm to use
gripper = wc:findFrame("PG70.TCP") -- the end effector frame which is the gripper
object = wc:findFrame("Bottle") -- the object which is to be grasped
table = wc:findFrame("Table") -- the object which is to be grasped
-- samples are in the form addQ(path, time, configuration)
tpath = rw.TimedStatePath:new()
addQ( tpath, 1, {0,0.1,0,0,0,0,0} )
addQ( tpath, 2, {0,0.3,0,0,0,0,0} )
.....
addQ( tpath, 7, {0,0.7,0,0,0,0,0} )
-- grasp the object with the gripper
attachObject( tpath, object, gripper)
-- move robot arm to table
addQ( tpath, 8, {0,0.7,0,0,0,0,0} )
addQ( tpath, 9, {0,0.6,0,0,0,0,0} )
....
addQ( tpath, 14, {0,0.1,0,0,0,0,0} )
-- release object onto table
attachObject( tpath, object, table)
-- move robot away from table
addQ( tpath, 15, {0,0.1,0,0,0,0,0} )
addQ( tpath, 16, {0,0.1,0,0,0,0,0} )
...
addQ( tpath, 19, {0,0.3,0,0,0,0,0} )
-- save the state path in the trajectory plugin
rwstudio:setTimedStatePath( tpath )
The symbol wc represents the workcell loaded into RobWorkStudio. On this we
initially nd the device and frames, which is done using their string identiers. We
52
INVERSE KINEMATICS
then construct a TimedStatePath into which the congurations are stored. What
your program should output is just the lines addQ( tpath, <time>, <Q> ),
where <time> is an increasing time stamp and <Q> is conguration.
The command attachObject changes the parent frame of dynamically attach-
able frame (DAF). The line attachObject( tpath, object, gripper) there-
fore attaches the bottle to the gripper and the second places it on the table.
Once the script is loaded and executed it sends a TimedStatePath to the
playback plugin, which can then be used to play the path.
Chapter 5
Tool Trajectory Descriptions
In programming a robotic application, the starting point is the generation of a
trajectory of the tool so that the robot carries out the desired task(s). Based
on the process, the trajectory may be completely specied or just be required
to move through certain points. In this case the remaining parts of the trajec-
tory determined which is done using interpolation. Here, we shall consider the
following examples of interpolation methods that cover most needs:
A linear trajectory between two given frames
Linear trajectories with parabolic blends along a sequence of given frames
Cubic interpolation along a sequence of given frames
Controller specic interpolation along a sequence of given frames
The presented interpolation methods should be viewed as a toolbox that may
be used in a given application. We thus seek to make a clear division between
interpolation methods and applications.
In this chapter, a given frame is a desired position and orientation possibly
also with a desired passage time or passage speed. In addition, the frame may
be equipped with a desired accuracy in terms of a maximal 6-dimensional dis-
placement vector. We shall assume that the speed and accuracy at the rst and
last frame in the sequence is always zero. For internal frames in the sequence, we
shall assume that if the passage time and speed is void, a maximal speed should
be seeked, and if the desired accuracy is void, the trajectory needs to go through
the specied point. Interpolation may take place in tool space as well as joint
space and we shall therefore discuss both.
In the following, we let F
i
be the ith tool frame in the sequence, p
i
and R
i
denotes the positional and rotational part of F
i
and q
i
denotes the chosen inverse
53
54 CHAPTER 5. TOOL TRAJECTORY DESCRIPTIONS
kinematics solution for F
i
. Throughout this chapter, t will denote time and t
i
is
the passage time at frame F
i
.
5.1 Linear Interpolation methods
Until now, we have only considered one type of interpolation for position, namely
the straight line interpolation and one type for rotation, namely using the equiv-
alent angle axis interpolation. These establishes the most simple interpolation
type:
Linear interpolation between two given frames
Consider two m-dimensional vectors X
i1
and X
i
. A linear interpolation between
these two vectors is given as
X(s(t)) = X
i1
+ (X
i
X
i1
)s(t) 0 s 1
where the parameter s(t) denotes a non-decreasing continuous time prole sat-
isfying that s(t
i1
) = 0 and s(t
i
) = 1. The study of time proles for linear
interpolations is a whole subject in itself. Here, we shall only consider three
examples:
s(t) =
t t
i1
t
i
t
i1
(constant velocity)
s(t) =
_
t t
i1
t
i
t
i1
_
2
(linear ramp-up of velocity)
s(t) = 1
_
t
i
t
t
i
t
i1
_
2
(linear ramp-down of velocity)
Consider two given frames F
i1
and F
i
. The constant velocity linear inter-
polation in tool space consists of the path
p(t) = p
i1
+
t t
i1
t
i
t
i1
(p
i
p
i1
)
R(t) = R
i1
[R
eaa
(
t t
i1
t
i
t
i1
W
rot
([R
i1
]
T
R
i
))] (5.1)
where W
rot
(R) = v is the equivalent angle axis as discussed in Section 4.5.
Similarly, the matrix R
eaa
(x) was dened in Eq. ??.
We can also interpolate linearly in joint space
q(t) = q
i1
+
t t
i1
t
i
t
i1
(q
i
q
i1
) (5.2)
Notice that the resulting robot motions by using interpolated curves in tool and
joint space respectively may be quite dierent. It is therefore important to care-
fully consider which type of interpolation is appropriate.
5.1. LINEAR INTERPOLATION METHODS 55
Parabolic blends
Consider two vector valued linear functions
x
1
(t) = X+v
1
(t T) t T
x
2
(t) = X+v
2
(t T) t T
(5.3)
that intersects at X at t = T where t is time. For an arbitrarily chosen blend
interval T t T +, we may then dene a parabola that blends from x
1
(t)
to x
2
(t). The parabola is dened as
P(t T, X, v
1
, v
2
, ) =
v
2
v
1
4
(t T + )
2
+v
1
(t T) +X (5.4)
Notice that the acceleration is constant and given by
v
2
v
1
2
. We may apply
this technique in the following situation. Consider three adjacent frames in the
sequence F
i1
, F
i
, F
i+1
. Assume that we have interpolated between F
i
and F
i+1
with the linear interpolation
p
i,i+1
(t) = p
i
+
t t
i
t
i+1
t
i
(p
i+1
p
i
)
R
i,i+1
(t) = R
i
[R
eaa
(
t t
i
t
i+1
t
i
W
rot
([R
i
]
T
R
i+1
))]
Similarly, assume that we have interpolated between F
i1
and F
i
with the linear
interpolation which we here write on the following form suitable for the parabolic
blend
p
i1,i
(t) = p
i
+
t t
i
t
i1
t
i
(p
i1
p
i
)
R
i1,i
(t) = R
i
[R
eaa
(
t t
i
t
i1
t
i
W
rot
([R
i
]
T
R
i1
))]
(notice that in the second equation, we use W
rot
([R
i1
]
T
R
i
) = W
rot
([R
i
]
T
R
i1
)).
The position part is now directly in the form in Eq. 5.3 with T = t
i
, X = p
i
,
v
1
=
p
i1
p
i
t
i1
t
i
, and v
2
=
p
i+1
p
i
t
i+1
t
i
. We may then dene a blend between the two
interpolations as
p
i,
i
(t) = P(t t
i
, p
i
,
p
i1
p
i
t
i1
t
i
,
p
i+1
p
i
t
i+1
t
i
,
i
) (5.5)
For the rotation part, we blend the interpolated equivalent angle axis. Then we
get T = t
i
, X = 0, v
1
=
W
rot
([R
i
]
T
R
i1
)
t
i1
t
i
, and v
2
=
W
rot
([R
i
]
T
R
i+1
)
t
i+1
t
i
. We then
get
R
i,
i
(t) = R
i
[R
eaa
(P(t t
i
, 0,
W
rot
([R
i
]
T
R
i1
)
t
i1
t
i
,
W
rot
([R
i
]
T
R
i+1
)
t
i+1
t
i
,
i
))]
(5.6)
The parabolic blend in joint space can be chosen as
q
i,
i
(t) = P(t t
i
, q
i
,
q
i1
q
i
t
i1
t
i
,
q
i+1
q
i
t
i+1
t
i
,
i
) (5.7)
5.2 Inverse kinematics for interpolated tool curve
For an interpolated tool curve T
tool
base
(t), a corresponding joint curve can be found
by the following algorithm:
Algorithm 3 Jacobian based Inverse Kinematics for interpolated tool curves
Require: T
tool
base
(t) T
1
t T
2
, q
0
so T
tool
base
(q
0
) = T
tool
base
(T
1
)
1: Compute a tesselation t
0
, t
1
, t
2
, . . . , t
M
with t
0
= T
1
and t
M
= T
2
so that
_
p
tool
base
(t
i
)p
tool
base
(t
i1
)
WS
+
W
rot
([R
i1
]
T
R
i
)
_
i = 1, . . . , M
2: q = q
0
3: for i = 1 to M do
4: Compute u = W(T
tool
base
(q), T
tool
base
(t
i
)) (see Eq. 4.12)
5: while |u| > do
7: Solve J(q)q = u
8: q := q +q
9: Compute T
tool
base
(q)
10: Compute u = W(T
tool
base
(q), T
tool
base
(t
i
))
11: q
i
:= q
It should be noticed that |WS| denotes the an estimate of the positional size
of the workspace and is a scaled step size which should be chosen based on the
required accuracy. Choosing = 0.01 is mostly sucient. Similarly, can be set
to e.g. = 10
6
. The tesselation can then easily be found step by step using
a root nding technique in one variable or simply by just deciding to split the
time interval into M equidistant subintervals and check that each satises the
inequality.
5.2.1 Limits on joint positions, velocities and accelerations
As mentioned in Section 4.8, we can avoid violating joint limits by relaxing the
resulting path of the end eector by solving the problem given in Eq. 4.25 in
Algorithm 3 instead of J(q)q = u. If it is not acceptable to relax the end
eector path, we may encounter a situation where at some step, Algorithm 3 is
unable to return a solution. It is a hard robot motion planning task to actively
avoid this situation. This problem is beyond the scope here, and we will therefore
assume that this problem does not occur.
Velocities and accelerations can be handled in a similar way. Consider rst
velocities. If we consider the step between t
i
and t
i+1
in Algorithm 3, we get
5.2. INVERSE KINEMATICS FOR INTERPOLATED TOOL CURVE 57
the constraint that the velocity as estimated by nite dierence
q
i+1
q
i
t
i+1
t
i
must be
within the velocity limits. This can be written as
v
min
t
i
(q q
i
) +q v
max
t
i
where t
i
= t
i+1
t
i
and v
min
and v
max
are the minimal (max negative) and
maximal velocity. Similarly, we can estimate the acceleration at t
i
by nite
dierences as
a =
2
_
t
i1
(q
i+1
q
i
) t
i
(q
i
q
i1
)
t
i1
t
i
[t
i1
+ t
i
]
Observe thus that also limits on velocities and accelerations can be stated as a
prescribed upper and lower bound on the displacement q. We may thus handle
all joint limit constraints by Eq.4.25. This procedure is strongly recommended for
redundant robot (more than 6 joints). For non-redundant robots there is nothing
better to do than hope to nd an inverse kinematics solution which satises the
joint limits. Velocity and acceleration limits can then be handled by time scaling,
which modies the time prole, but not the path.
5.2.2 Handling velocity and acceleration limits by time scaling
If the time prole of the generated path is not a hard constraint, we may handle
velocity and acceleration constraints by adjusting the time prole.
Consider thus a continuous dierentiable trajectory x(t) t
min
t t
max
with limits on the time derivative v
min
x(t) v
max
where v
min
< 0 and
v
max
> 0 and t is supposed to be time. If the velocity constraints are violated,
we may adjust for this by dening a continuous time prole t() where is the
modied time and a corresponding curve x
() = x(t()). By observing that

x
() = x(t)t
(), we may dene t() by the dierential equation

t
() =
_
_
v
min
x(t)
x(t) < v
min
v
max
x(t)
x(t) > v
max
1 otherwise
(5.8)
Then x
() will satisfy the velocity constraints if we choose as time instead

of t. Notice that as x(t) is continuous dierentiable, t() will be continuous
dierentiable as well.
It should be noticed that the formula for t
() is implicit as the right hand

side contains t and we thus assume to know t(). To nd t() and thus the
modied trajectory x
(t), consider the discretized trajectory with M + 1 steps

x
k
= x(t
min
+ kh) where h = [t
max
t
min
]/M is a small t-step. We now device
an iterative method to nd the modied time
k
corresponding to the time t
k
=
t
0
+ kh and thereby a discretization of t(). We start by choosing
0
= t
0
.
Assume now that we have found
k1
. We may then estimate x(t
0
+ kh) =
[x
k+1
x
k1
]/(2h) x
k
. We then compute t
(
k
) according to Eq. 5.8. Finally,
observing that
(t
k
) = 1/t
(
k
) for corresponding t
k
,
k
, we compute
k
=
k1
+
h
2
[
1
t
(
k
)
+
1
t
(
k1
)
].
If x(t) is vector valued, we nd a t
i
(
k
) for each coordinate x
i
and simply
choose t
(
k
) = min
i
t
i
(
k
).
It should be observed that the nite dierence formula for x(t
0
+ kh) does
not work for the endpoints (k = 0 and k = M). In many situations, the velocity
limits have by other means been satised, and we can then just neglect these
and choose t
(
0
) = t
(
M
) = 1. If we do not have this prior knowledge we
have to estimate them by the rst order formulas x(t
0
) = [x
1
x
0
]/h x
0
and
x(t
M
) = [x
M
x
M1
]/h x
M
.
5.3 Rotation representation by quaternions
In order to interpolate rotations nonlinearly, we need a coordinate description of
rotation matrices that we can use. RPY angles have proven to be inadequate
because of their singularities. It can actually be shown that no other set of
singularity free independent coordinates can be chosen. To avoid singularities.
We shall use a set with four parameters having only a single easily controllable
singularity. We choose the common unit quaternion representation that is closely
related to the equivalent angle axis formulation. The quaternions, which we will
write as Q = (Q
1
, Q
2
, Q
3
, Q
4
) for a rotation matrix Rare related to the equivalent
angle axis representation in the following way
Q
1
= u
1
sin

2
(5.9)
Q
2
= u
2
sin

2
(5.10)
Q
3
= u
3
sin

2
(5.11)
Q
4
= cos

2
(5.12)
where is the angle of the equivalent angle axis rotation derived from R and
(u
1
, u
2
, u
3
) denotes the unit vector around which the rotation takes place. Clearly,
there is a singularity at = where any u
1
, u
2
, u
3
forms an equivalent angle axis.
But there are no other singularities. We shall throughout assume that we can
avoid this singularity by choosing an appropriate reference frame for the tool
description or if that is not possible to divide the task into appropriate subtasks.
That is, we can assume that Q
4
a > 0.
5.3. ROTATION REPRESENTATION BY QUATERNIONS 59
Notice that the quaternion parameters are not independent as they must
satisfy the constraint
Q
2
1
+ Q
2
2
+ Q
2
3
+ Q
2
4
= 1 (5.13)
For a given set of quaternion parameters Q, the rotation matrix is given by
R
Q
(Q) =
_
_
1 2Q
2
2
2Q
2
3
2(Q
1
Q
2
Q
3
Q
4
) 2(Q
1
Q
3
+ Q
2
Q
4
)
2(Q
1
Q
2
+ Q
3
Q
4
) 1 2Q
2
1
2Q
2
3
2(Q
2
Q
3
Q
1
Q
4
)
2(Q
1
Q
3
Q
2
Q
4
) 2(Q
2
Q
3
+ Q
1
Q
4
) 1 2Q
2
1
2Q
2
2
_
_
(5.14)
The quaternion parameters are given as
Q
4
=
1
2
1 + r
11
+ r
22
+ r
33
(5.15)
Q
1
=
r
32
r
23
4Q
4
(5.16)
Q
2
=
r
13
r
31
4Q
4
(5.17)
Q
3
=
r
21
r
12
4Q
4
(5.18)
Occationally (for example with the cubic splines below), velocity relations are
needed as well. We obtain relations for these by straightforward time dierenti-
ation of Equation 5.15. We get
Q
4
=
1
4
r
11
+ r
22
+ r
33
1 + r
11
+ r
22
+ r
33
=
r
11
+ r
22
+ r
33
8Q
4
Q
1
=
r
32
r
23
4Q
4
(r
32
r
23
)

Q
4
4Q
2
4
Q
2
=
r
13
r
31
4Q
4
(r
13
r
31
)

Q
4
4Q
2
4
Q
3
=
r
21
r
12
4Q
4
(r
21
r
12
)

Q
4
4Q
2
4
(5.19)
Consider now two quaternions, say Q
i1
and Q
i
. We may then interpolate
between the corresponding rotation frames by
R(t) = R
Q
(
sin[
t
i
t
t
i
t
i1
]
sin
Q
i1
+
sin[
tt
i1
t
i
t
i1
]
sin
Q
i
) (5.20)
R
Q
(S(Q
i1
, Q
i
, t
i1
, t
i
, t)) (5.21)
where = cos
1
(Q
i1
Q
i
). This is called Slerp interpolation (Spherical Linear
intERPolation). It is straightforward but somewhat involved to show that this
interpolation corresponds to the unique equivalent angle axis rotation given by
Eq. 5.1.
Notice that the interpolated quaternions S(Q
i1
, Q
i
, t
i1
, t
i
, t) will satisfy the
quaternion constraint.
Notice furthermore that for small s, we may approximate sin x with x.
We then get
R(t) = R
Q
(
t
i
t
t
i
t
i1
Q
i1
+
t t
i1
t
i
t
i1
Q
i
)
i.e. the standard linear interpolation between Q
i1
and Q
i
. Notice however, that
these quaternions are no longer unit quaternions. It is therefore very important to
remember to normalize the quaternions interpolated in this way before computing
the resulting rotation matrices by invoking R
Q
.
5.4 Cubic splines
Consider now a dense set of frames F
0
, . . . , F
N
with corresponding passage times
t
0
, . . . , t
N
. Dense here means that we may assume that the displacements be-
tween adjacent frames are small. We can thus use linear interpolation between
quaternions rather than spherical linear interpolation. For cubic spline interpo-
lation in tool space, we thus use the corresponding 7-tuple X
i
(p
i
, Q
i
) for
i = 0, 1, . . . , N. For a cubic interpolation in joint space, we use the n-tuple
X
i
q
i
. We may then consider cubic interpolation in tool and joint space by
the same formulation. Between X
i1
and X
i
we obtain a curve
X
i1,i
(t) = a
i
t
3
+ b
i
t
2
+ c
i
t + d
i
where the parameters a
i
, b
i
, c
i
, d
i
are to be determined. As we have N of the
curves, we have 4N parameters. The curves must satisfy the following constraints
X
i1,i
(t
i1
) = a
i
t
3
i1
+ b
i
t
2
i1
+ c
i
t
i1
+ d
i
= X
i1
i = 1, . . . , N
X
i1,i
(t
i
) = a
i
t
3
i
+ b
i
t
2
i
+ c
i
t
i
+ d
i
= X
i
i = 1, . . . , N
which establishes 2N linear equations in the unknown parameters. The remaining
2N equations may be given in several ways. If there are prescribed velocities

X
i
at all passage points, we obtain
X
i1,i
(t
i1
) = 3a
i
t
2
i1
+ 2b
i
t
i1
+ c
i
=

X
i1
i = 1, . . . , N
X
i1,i
(t
i
) = 3a
i
t
2
i
+ 2b
i
t
i
+ c
i
=

X
i
i = 1, . . . , N
which establishes a set of 4 linear equations in 4 unknowns for each subpath. The
solution can be written as the function X
i1,i
(t) = C(t, t
i1
, t
i
, X
i1
, X
i
,

X
i1
,

X
i
)
5.5. AUGMENTING TRAJECTORY WAY POINT INFORMATION FOR CUBIC
SPLINES 61
where
C(t, t
s
, t
f
, P
s
, P
f
, V
s
, V
f
) = [2(P
f
P
s
) + (t
f
t
s
)(V
s
+ V
f
)]
_
t t
s
t
f
t
s
_
3
+ [3(P
f
P
s
) (t
f
t
s
)(2V
s
+ V
f
)]
_
t t
s
t
f
t
s
_
2
+ V
s
(t t
s
) + P
s
(5.22)
Sometimes, the velocities are only given at the endpoints X
0
and X
N
. Then the
following set of equations is usually applied:
X
0,1
(t
0
) = 3a
1
t
2
0
+ 2b
1
t
0
+ c
1
=

X
0
X
N1,N
(t
N
) = 3a
N
t
2
N
+ 2b
N
t
N
+ c
N
=

X
N
3a
i
t
2
i
+ 2b
i
t
i
+ c
i
= 3a
i+1
t
2
i
+ 2b
i+1
t
i
+ c
i+1
i = 1, . . . , N
6a
i
t
i
+ 2b
i
= 6a
i+1
t
i
+ 2b
i+1
i = 1, . . . , N
In this case, we thus require that the rst and second order derivatives to be
continuous. We then have a total of 4N coupled linear equations, which may be
solved using standard numerical techniques. Notice that if the frame velocities

F
i
are available, the positional velocities can be taken directly from the positional
part of

F
i
whereas the quaternion velocities must be computed using Eq. 5.19.
5.5 Augmenting trajectory way point information for Cu-
bic Splines
As mentioned, the input to Cubic Splines is a set of way points X
i
,

X
i
, t
i
. How-
ever, sometimes the user only species X
i
, t
i
or X
i
,

X
i
or even only X
i
. Often, the
remaining information can be estimated from a required process speed, but we
will now outline options for choosing these when a process speed is not available.
The situation where only X
i
, t
i
is specied can be handled by requiring the second
order derivatives to be continuous at the way points as outlined in the previous
section. Consider now the case where X
i
, V
i
is given. We may then select the
t
i
s as those yielding the fastest possible execution path satisfying velocity and
acceleration limits. For an arbitrary Cubic spline, we get the acceleration
d
2
dt
2
C(t, t
s
, t
f
, P
s
, P
f
, V
s
, V
f
) = 6 [2(P
f
P
s
) + (t
f
t
s
)(V
s
+ V
f
)]
_
t t
s
t
f
t
s
_
+ 2 [3(P
f
P
s
) (t
f
t
s
)(2V
s
+ V
f
)]
A(t, t
s
, t
f
, P
s
, P
f
, V
s
, V
f
)
which is a linear function and thus has extrema at the endpoints
6(P
f
P
s
) (t
f
t
s
)(4V
s
+ 2V
f
) A
s
(t
s
, t
f
, P
s
, P
f
, V
s
, V
f
)
6 [2(P
f
P
s
) + (t
f
t
s
)(V
s
+ V
f
)] +
2 [3(P
f
P
s
) (t
f
t
s
)(2V
s
+ V
f
)] =
6(P
f
P
s
) + (t
f
t
s
)(2V
s
+ 4V
f
) A
f
(t
s
, t
f
, P
s
, P
f
, V
s
, V
f
)
(5.23)
We thus get the bounds
a
min
A
s
(t
i
, t
i+1
, X
i
, X
i+1
,

X
i
,

X
i+1
) a
max
a
min
A
f
(t
i
, t
i+1
, X
i
, X
i+1
,

X
i
,

X
i+1
) a
max
for each segment. Notice, that all bounds are linear in the passage times t
i
. Fi-
nally, we have the constraint t
i
t
i+1
. We may then optimize the total execution
time t
N
with respect to these constraints for all the segments. This yields a lin-
ear programming problem in the unknown passage times. Finally, the velocity
constraints are handled by time scaling as outlined in Section 5.2.2 in a postpro-
cessing step. As the velocity constraints are satised at the passage points, the
scaling function will then be continuous dierentiable.
5.6 Applying interpolation
Preparing for interpolation in a given application scenario typically requires the
following steps
Find a sequence of frames and associated types of interpolation between
them, which species the task.
For each frame specify (only if required by the process) a passage time
(either absolute or relative to the last frame) or passage speed. If cubic
polynomial interpolation is used, both a time stamp and a speed can be
given as input.
For each frame specify (only if using a parabolic blend at the frame) a
desired maximal deviation from the frame.
An example of a possible specication is:
F
0
, [LTu], F
1
, [CST], (F
2
,

F
2
), [CST], (F
3
, t
3
,

F
3
), [CST], . . . ,
[CST], (F
27
, t
27
,

F
27
), [CST], F
28
, [LTd], F
29
. (5.24)
where LTd denotes constant velocity linear interpolation in tool space, LTu
and LTd denotes linear ramp up and ramp down interpolation in tool space
5.7. EXERCISES 63
and CST denotes cubic spline interpolation in tool space. The times t
i
are
the times relative to the not yet specied absolute time of the previous frame.
A typical example where this could be relevant is an air movement starting
with zero velocity at F
0
to a frame F
1
near the starting point of a painting task,
which starts at F
2
and ends at F
27
followed by an air movement to leave the
workpiece by moving to F
28
followed by an air movement to get to F
29
to end
the task.
To nd the resulting interpolation, we rst address the linear ramp up interpo-
lation from F
0
to F
1
. This yields to an augmentation of F
1
with a frame velocity
F
1
and a time stamp t
1
. Then the passage times t
2
, . . . , t
27
are also known and
the path to F
28
can be computed by cubic spline interpolation. Finally, the last
linear ramp down interpolation is computed.
5.7 Exercises
Exercise 5.1. Consider the frames
F
0
=
_
_
1 0 0 [ 15
0 1 0 [ 8
0 0 1 [ 3
[
0 0 0 [ 1
_
_
F
1
=
_
_
0 1 0 [ 10
1 0 0 [ 4
0 0 1 [ 2
[
0 0 0 [ 1
_
_
F
2
=
_
_
0 0 1 [ 6
1 0 0 [ 0
0 1 0 [ 2
[
0 0 0 [ 1
_
_
with t
0
= 0, t
1
= 1 and t
2
= 4.
i) Verify that W
rot
([R
0
]
T
R
1
) = (0, 0,
2
) and W
rot
([R
1
]
T
R
2
) = (
2
, 0, 0).
ii) Compute the linear segments p
i1,i
(t) and R
i1,i
(t) for i = 1, 2.
iii) Compute the parabolic blend p
i,
i
(t) and R
i,
i
(t) for i = 1. Use
1
= 0.1.
Exercise 5.2. Specify (similar to Eq. 5.24) how to interpolate so that the tool
moves along a line segment between F
s
and F
e
with constant orientation and
constant linear speed. The movement must start and end with zero velocity.
Exercise 5.3. Specify (similar to Eq. 5.24) how to interpolate between a pick
frame F
pick
and a place frame F
place
passing through frames F
pick,lift
and F
place,lift
.
Exercise 5.4. Consider two points X
i1
and X
i
. Assume that the maximal speed
is |

X
max
| and the maximal acceleration is |
X
max
|. For each of the following
cases, nd the time prole s(t) that satises the velocity and acceleration con-
straints and leads to the fastest execution of the the trajectory between X
i1
and
X
i
:

X
i1
= 0

X
i
= 0

X
i1
= 0 and

X
i
= 0

X
i1
= 0 and

X
i
= v
i

X
i1
= v
i1
and

X
i
= v
i
Implement a method which takes in M N-dimensional points with associated
velocities and calculates the resulting cubic spline.
Run your program on a set of dimension N = 2 and plot the trajectories
given by the cubic spline.
Try to vary the velocities in the input and study how this eects the paths.
Mandatory exercise
In the rst part of this exercise we will consider the Kuka KR 120. We will
consider a milling application where the workpiece shown in Figure5.1b is to be
manufactured by a milling robot. The exercise consists of sequence of questions
and you are supposed to hand in a brief report. It must be clearly appearing from
the report which questions you are answering. Furthermore, whenever the word
state appears in your report, you must present the result that I am asking for
in the report.
(a) The robotic scene. (b) The workpiece.
Figure 5.1: The robotic scene and the workpiece to be manufactured by milling
Files related to the mandatory exercise can be found on Blackboard, in the
folder Mandatory Exercise 2. The device le is named Rob01MillingSceneKR120.wc.xml
and can be found in the zip-le KUKA KR120 scene.zip. In the rst exercise,
you should just check your kinematics to remove possible initial errors:
i) Compute and state the position and orientation of the milling tool frame
T
tool
base
(q) for q q
A
= (0,
2
,

2
, 0, 0.1745, 0) and for q q
B
= (0,
4
,

8
, 0, 1, 0)
and nally for q q
C
= (0.65, 1, 1, 0.1, 0.1745, 0.1). State also the
(x, y, z, R, P, Y ) values obtained in RobWorkStudio and state if your re-
sults are equivalent to those in RobWorkStudio.
The path of the milling tool is computed by a CAD-CAM system and is given
in the le transforms.dat. One transform is given per line, and the transform
gives position and rotation of the tool frame relative to the world frame. Only
12 values are given (the last row is always 0 0 0 1). The values are given in
row-major order. The path consists of N = 6907 frames, which we will call
T
(1)
world,desired
, . . . , T
(N)
world,desired
. Positions are given in meters. The transforma-
tions T
(1)
world,desired
, . . . , T
(N)
world,desired
are as indicated given in the world coordi-
nate system. The rst step is therefore to compute corresponding transformation
T
(1)
desired
, . . . , T
(N)
desired
with the robot base frame as reference:
ii) State a formula for computing T
(i)
desired
from T
(i)
world,desired
and T
base
world
. Use
the formula to compute T
(i)
desired
for i = 1, . . . , N.
Unfortunately, the CAD-CAM system provides neither passage times nor pas-
sage velocities, so we have to derive these (this is a typical situation in applica-
tions). The desired positional milling velocity is v
milling
= 0.05m/s (the milling
velocity should be viewed as a guideline rather than a strict constraint). For
T
(1)
desired
, . . . , T
(N)
desired
we as usual denote the corresponding positions as p
(1)
desired
, . . . , p
(N)
desired
and corresponding orientations as R
(1)
desired
, . . . , R
(N)
desired
.
iii) Choose t
1
= 0 and compute passage times t
i
, i = 2, . . . , N so that
|p
(i)
desired
p
(i1)
desired
|/[t
i
t
i1
] = v
milling
. State t
2
and t
10
.
iv) For i = 1, . . . , N1 compute linear interpolations T
(i,i+1)
linear
(t) t
i
t t
i+1
between T
(i)
desired
and T
(i+1)
desired
. State T
(1,2)
linear
(
t
2
2
) and T
(9,10)
linear
(
t
9
3
+
2t
10
3
).
The joint conguration q
(1)
= (0.158, 1.18, 1.526, 1.48, 0.522, 1.256) cor-
responds to T
tool
base
(q
(1)
) = T
(1)
desired
.
v) Use Algorithm 3 together with the linear interpolations T
(i,i+1)
linear
(t) to com-
pute q
(2)
, . . . , q
(N)
so that T
tool
base
(q
(i)
) = T
(i)
desired
. State the joint congura-
tions q
(2)
and q
(10)
.
We now derive appropriate passage velocities.
vi) Compute the positional velocities v
i,i+1
= [p
(i+1)
desired
p
(i)
desired
]/[t
i+1
t
i
] for
i = 1, . . . , N 1. State v
1,2
and v
9,10
.
vii) Compute the angular velocities w
i,i+1
=
1
t
i+1
t
i
W
rot
([R
(i)
desired
]
T
R
(i+1)
desired
)
for i = 1, . . . , N 1. State w
1,2
and w
9,10
.
viii) Compute positional passage velocities as v
1
= v
1,2
, v
N
= v
N1,N
and
v
i
= [v
i,i+1
+v
i1,i
]/2 for i = 2, . . . , N 1. State v
2
and v
10
.
ix) Compute angular passage velocities as w
1
= w
1,2
, w
N
= w
N1,N
and
w
i
= [w
i,i+1
+w
i1,i
]/2 for i = 2, . . . , N 1. State w
2
and w
10
.
We now wish to convert the passage velocities to joint velocities. We thus
dene the usual 6-dimensional velocity screw u
i
= (v
i
, w
i
).
x) For i = 1, . . . , N solve J(q
(i)
) q
(i)
= u
i
. State the joint velocities q
(2)
and
q
(10)
.
We have now derived joint positions q
(i)
, joint velocities q
(i)
and passage
times t
i
for each T
(i)
desired
.
xi) For i = 1, . . . , N 1 compute the cubic splines q
i,i+1
(t) that interpolates
between q
(i)
and q
(i+1)
. State q
1,2
(
t
2
2
) and q
9,10
(
t
9
3
+
2t
10
3
).
We now have a complete joint trajectory q(t) t
1
, . . . , t
N
dened as q(t)
q
i,i+1
(t) for the interval t
i
t t
i+1
.
xii) With M = 20000 steps and a corresponding stepsize H = [t
N
t
1
]/M,
output the joint congurations Q
k
q(kH) k = 1, . . . , M to a le out-
put.dat. Put one joint conguration per line, and seperate the values by a
space. State in the report Q
5
and Q
200
.
xiii) Execute the generated trajectory by using the OcTreeSim plugin in Rob-
WorkStudio, and discuss if the generated workpiece corresponds to the
desired one. Load the le by pressing the Load Joint le button, and
press play to execute the trajectory. Zip-les can be found for Windows
32 bit and 64 bit (RobWorkStudio Milling 32.rar and RobWorkStu-
dio Milling 64.rar). Please run the bat-le runRobWork.bat to auto-
matically run RobWorkStudio with the scene and plugin loaded. For other
versions of Windows and for Linux users, the source for the plugin can
be found on blackboard. Note that to compile the plugin, RobWorkStudio
must be compiled rst. Further instructions will be available on blackboard.
Assume now that we want to execute the path as fast as possible within the
given velocity limits (this is of course unrealistic for milling, but is realistic for
other processes).
xiv) Modify the velocity scaling method to generate the fastest possible path
within the given joint velocities. State a brief outline of how you have
solved the problem. State the resulting total execution time and state a
plot of the computed time prole t().
To nalize the application, we also need to derive an approach and exit tra-
jectory to and from the workpiece. Thus assume that the robot must start in q
A
given in question [i)] with velocity zero and end in q
B
also given in question [i)]
and also with velocity zero.
xv) Augment the milling trajectory q(t) with an approach trajectory q
approach
(t)
for t
start
t t
1
and an exit trajectory q
exit
(t) for t
M
t t
end
so that the
path obtained by concatenating q
approach
(t), q(t) and q
exit
(t) becomes con-
tinuous dierentiable and so that q
approach
(t
start
) = q
A
, q
exit
(t
end
) = q
B
and the joint velocities at t
start
and t
end
are zero. State a brief outline of
how you have solved the problem. State a plot of the resulting path for the
rst (closest to the base) joint as function of time.
xvi) Output the concatenated path with h = [t
end
t
start
]/M in the same way as
in question xii) to a le output2.dat and execute the path in RobWork-
Studio using the OcTreeSim plugin. Discuss if the generated workpiece
corresponds to the desired one and the whole path seems correct.
Important: Deadline for handing in the mandatory exercise is January 4th at
10.00am. The deadline is STRICT and there will be no extensions. All documents
should be sent by email to Thomas Nicky Thulesen and me. Please state the full
name and date of birth for each participant
The report must contain a pdf le with the answers to the specic questions
above indicated with the word State. In addition, all programs that you have
developed must be attached. In addition, the generated les output.dat and
output2.dat must be sent.
Important: You should work in the groups from the rst mandatory exercise unless
other arrangements have been agreed with me. Each group hand in one report. It
is strictly forbidden for the groups to help each other or receive any help from
other people. .

Robotic

Enviado por

Dados do documento

Direitos autorais

Formatos disponíveis

Compartilhar este documento

Compartilhar ou incorporar documento

Opções de compartilhamento

Você considera este documento útil?

Este conteúdo é inapropriado?

Direitos autorais:

Formatos disponíveis

Robotic

Enviado por

Direitos autorais:

Formatos disponíveis

Contents

(where = i or = i, j) has a unique

). The reference link coordinate system to

R.J.Schilling, Fundamentals of Robotics: Analysis and Control

() = x(t()). By observing that

(), we may dene t() by the dierential equation

() will satisfy the velocity constraints if we choose as time instead

() is implicit as the right hand

(t), consider the discretized trajectory with M + 1 steps

Você também pode gostar