Theory of Robotics

Theory of Robotics and Mechatronics

Course number 151‐0601‐00

Instructors
Sascha Stoeter Petr Korba
sstoeter@ethz.ch korbap@ethz.ch
Teaching Assistant
Hong Ayoung
Cedric Fischer
Michael Mattmann
ahong@ethz.ch
fischece@student.ethz.ch michaema@student.ethz.ch
+41 44 632 86 76
CLA H 13

Prof. Brad Nelson

Institute of Robotics and Intelligent Systems

Suggested Literature

G. Strang, “Linear algebra and its applications”, Academic Press, New York, USA, 1976
J. J. Craig, “Introduction to Robotics: Mechanics and Control”, 3rd ed., Prentice Hall, 2003.
R. M. Murray, Z. Li, and S. S. Sastry, “A Mathematical Introduction to Robotic Manipulation”, CRC Press,
1994.
M. Spong and M. Vidyasagar, "Robot Dynamics and Control", Wiley, 1989. P. McKerrow, "Introduction to
Robotics", Addison‐Wesley, 1999.
T. Yoshikawa, "Foundations of Robotics: Analysis and Control", MIT Press, 1990.
J.‐P. Merlet, "Parallel Robots", Springer, 2001.
R. Siegwart and I. R. Nourbakhsh, “Introduction to Autonomous Mobile Robots”, MIT Press, 2004.
H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki, and S. Thrun, “Principles of
Robot Motion: Theory, Algorithms, and Implementations”, MIT Press, 2005.
J. Borenstein, H. Everett, and L. Feng, Navigating Mobile Robots: Systems and Techniques, A. K. Peters,
1996.
G. Dukek and M. Jenkin, "Computational Principles of Mobile Robotics", Cambridge University Press,
2000.
D. Forsyth and J. Ponce, "Computer Vision — A Modern Approach", Prentice Hall, 2003.

Theory of Robotics &
Mechatronics
Introduction

ETH Zurich
Robots—The People’s View
• Humanoids, Evil Robots, BattleBots
Theory of Robotics & Mechatronics — 151-0601-00 2
1
Pierre Jaquet-Droz (1721–1790)
Swiss watchmaker and

builder of automatons
Exercise
• Part 1: Get to know each other

– Hello, my name is …
– Where are you from?
– What couldn’t be guessed about you?
– …
• Part 2: What is a robot?
2
What is a Robot?
• Actuation
• Sensing
• Autonomy
• Intelligence?
Artificial Intelligence
• Connection between sensors and actuators

– Action selection
• Interface between humans and robots
– Speech recognition
– Gesture recognition
– …
• Example: Deep Blue
– “It’s just a program …”
3
What is Robotics?
IRIS is particularly
focused on vision and
Perception force perception
Robot-Human
Interaction
Manipulation Cognition
The Scale of Robotics
1015
Nearest
1012 Stars
Edge of the
109
solar system
Earth
106 to Mars
103
1 km
m
10-3
mm
10-6
m
10-9
nm
Å CNT
4
Exploratory Robotics
Deep Space Exploration
Voyager I and II
5
Exploring the Solar System
Rover II
Rover I
Beagle 2
Mars Rovers
Underwater Exploration
Underwater Archeology
6
Robotic Surgery
Robotics for Exploring Life at a

Cellular Level
7
Microrobotics for
Handling Biological Cells Research
• In vitro fertilization (IVF)

– Intracytoplasmic sperm injection (ICSI)
• Transgenic organisms (TO)
– Embryo pronuclei DNA injection
• Cell injury studies
– Membrane properties
50 m
Typical failure

Sun and Nelson, IJRR, Oct-Nov, 2002
A Microrobotic
Cell Manipulation System Research
3 DOF Microrobot
Force Feedback
Vision Feedback
8
Robotic Manipulation of Rigid Objects
at Micro- and Nano-Scales Research
• Gravity
becomes negligible as part dimensions
fall below ~100μm
• Surface Tension
caused by adherence of H2O molecules
to part surfaces + +
++
• Electrostatic Forces - -
--
due to differences in electric potential
between objects
• Van der Waals Forces
atomic level interactions, highly
dependent on surface smoothness
Atomic Force Measurements

Research
9
1015
Nearest
1012 Stars
Edge of the
109
solar system
Earth
106 to Mars
103
1 km
m
10-3
mm
10-6
10-9
m What is the future of
nm
Robotics and
Å CNT Robotics Research?
A History of Robotics Research
RW
Knowledge Representation/Frames
Minsky & McCarthy
and the AI Lab at MIT
1959 EM Agents
Behaviors/Subsumption
Brooks
=
Expectation/Verification Frameworks
Dickmanns/Jain
Mobile Robotics
Space Robotics
Microassembly
Biomedical Robotics/
Computer Aided Surgery
Manufacturing
Underwater Robotics
10
Parallels to Modern Western
Philosophy
RW
Knowledge Representation/Frames
Minsky & McCarthy
and the AI Lab at MIT
1959 EM Agents
Behaviors/Subsumption
Brooks
=
Expectation/Verification Frameworks
Dickmanns/Jain
Descartes Locke
Berkeley Kant
Discourse de la Method Hume
Meditations Critique of Pure Reason
Experience is the a priori vs. a posteriori
source of all ideas knowledge
What is the Future of Robotics?
• Robots for environments for which humans are ill-suited

(small, far away, dangerous)
– Microrobotics, Nanorobotics
– Military
– Space
– Medical
• Entertainment
– Companions (Aibo)
– Humanoid (Honda, Sony)
– Virtual Reality
• Robotics as a testbed for investigating intelligence
11
Robotics in Industry:
Microassembly Research
MEMS
Wafer-level Microassembly
of Hybrid Microsystems
Medical Robotics:
Surgery Now and Then
+ 500 years
Robot-assisted, minimally
invasive surgery
Medieval dentist Tele-Operation
12
Medical Robotics:
Robotic Surgery
• Minimally Invasive Procedures
– Direct control of tools vs. teleoperation
Coronary Artery Bypass Grafting
Teleoperated Minimally Invasive

Procedures
• Intuitive Surgical, Inc.
– daVinci Surgical System
13
Teleoperated Surgery
Surgeon’s Interface
Military Robotics
• National Academy of Sciences 2002 report on “Technology

Development for Army Unmanned Ground Vehicles”
– Searcher
– Donkey
– Wingman
– Hunter-Killer
14
Military Robotics:
A “Searcher” Robotic System
1997–2002
DARPA’s Grand Challenge
• U.S. Congress mandates robotic forces

– Air Force: ⅓ UAV until 2010
– Army: ⅓ autonomous vehicles until 2015
• 2004: no winner
– Best team reached 12km (of 240km!) in
several hours
• 2005: 5 teams succeeded

Stanford’s Stanley
– Goal: 212km off-road in less than 10h
– Stanley fastest with under 7h
– $ 2,000,000
15
DARPA’s Urban Grand Challenge
• The third competition of the DARPA Grand Challenge, known as the "Urban
Challenge", took place on November 3, 2007 at the site of the now-closed George Air
Force Base (currently used as Southern California Logistics Airport), in Victorville,
California. The course involved a 96 km (60-mile) urban area course, to be
completed in less than 6 hours. Rules included obeying all traffic regulations while
negotiating with other traffic and obstacles and merging into traffic.
• The $2 million winner was Tartan Racing, a collaborative effort by Carnegie Mellon
University and General Motors Corporation, with their vehicle "Boss", a Chevy Tahoe.
The second place finisher earning the $1 million prize was the Stanford Racing Team
with their entry "Junior", a 2006 Volkswagen Passat. Coming in third place was team
Victor Tango from Virginia Tech winning the $500,000 prize with their 2005 Ford
Escape hybrid, "Odin". MIT placed 4th, with Cornell University and University of
Pennsylvania/Lehigh University also completing the course.
From wikipedia
Humanoid Robotics
Honda Asimo, P3, and P2
Sony SDR-3X
16
Humanoid Robotics
Sony SDR-3X
The “Killer App” ??

Nanorobotics:
Manipulation of Carbon Nanotubes
17
The PC Industry as a Metaphor for
Robotics
• The Robotics Industry is at the same state now as the PC
industry was in the late 1970’s (Brooks)
– Mainly expensive and used in industry
– PC kits — Robot kits
– PC’s for the home — Robots for the home
– The failure of conventional wisdom?
• The demise of the mainframe industry
Future Robotics Research
• Distributed robotic systems
• Fusing multiple “orthogonal” sensing modalities

– e.g., manipulating deformable objects
• Radically new theories of intelligence
• Robots may not be what we think they are

– New types of Robot-Human Interaction
• The future of robotics is being determined by those that have

both a vision and the energy to pursue their vision
18
Social Impact of Robots
• Sometimes robots are the only way

(e.g., hazardous environments)
• Mostly competes with manual labor
• Social tax on robots just like workers?
• Impact on human-human relationships?
Exercise
• Why don’t we use monkeys (humans, dolphins, …)?
19
What we will learn
• Introduction
Overview of Robotics
• Mechanical Design of Robots

Types of robots, sensors, actuators, gearboxes,
robot end-effectors, resolution, accuracy,
precision.
What we will learn
• The Mathematical Basics of Robotics Using a Classical

Approach
– Describing the position and orientation of objects in 3D space
– Coordinate frames, position, orientation and velocity vectors
in 3D, coordinate transformations.
– Applies directly to Computer Graphics
• The Mathematical Basics of Robotics Using Screw Theory

– A mathematical description of rigid body motion
– Eliminates deficiencies of the classical approach
20
What we will learn
• Forward Kinematics
Given a robot’s joint angles (positions) what is the
end-effector pose (position and orientation)?
Forward kinematic transform

PX
PX
X PY
PY PZ
f ( 1, 2 , N )
X
Y Y
PZ Z
Z
What we will learn
• Rigid Body Velocity

How do we describe the full 6-DOF velocity of a robot
manipulator as it moves through space?
• The Jacobian Forward velocity transform matrix

Given a robot’s joint speeds (The Jacobian matrix)
what is the end-effector PX
translational and angular velocity 1
(position and orientation)? PY

2
PZ
6 N 3
X
Z N
21
What we will learn
• Inverse Kinematics
Given where I want to be what should the joint angles be;
what should be the speed of actuators to attain a certain
velocity at the tip?
Does not always have a solution, sometimes has multiple

solutions.
What we will learn
• Redundant and Parallel Robots

– Redundant robots that have more joints
than are needed to perform the task
– Parallel robots are closed-kinematic
chains that provide great strength and
rigidity
• Numerical Methods
– How do we use efficient computer
algorithms to plan the motions of
complex robots
The SML Parallel Haptic Device (Uchiyama,Tsumaki)
22
What we will learn
• Dynamics
Given a robot’s joint angles, velocities, accelerations and
inertial properties, what are the loads on the actuators?
1 f1 ( 1,2, , N , 1,2, , N , 1,2, , N )
2 f2 ( 1,2, , N , 1,2, , N , 1,2, , N )
N fN ( 1,2, , N , 1,2, , N , 1,2, , N )
What we will learn
• Trajectory Generation and

Position Control
– How to mathematically define a path in
space
– How to control the robot to follow that
path
23
What we will learn
• Force Control and Haptics

– Contact tasks, force sensing and
control
– Haptic interfaces, which provide force
feedback to humans interacting with
virtual remote environments to give
humans a sense of touch
What we will learn
• Introduction to Computer Vision

– Overview of computer vision and robotic
applications of vision
– Elements of a vision system, lighting,
sensors, optics
– Geometry of imaging, projections,
distortions, depth of field
– Digitization, brightness, color space,
color depth, image formats
– Camera calibration
24
What we will learn
• Computer Vision Algorithms

– Binary images, thresholding, histograms
– Area/moment statistics, morphological operations
– Segmentation, blob analysis, labeling
– Spatial operations and transformations:
Pixel neighborhoods, convolution.
– Mean, Gaussian, Laplacian, gradient filters
– Edge detection, Canny, Hough transform
• Vision for Robotics

– Visual servoing, image based, position based
– Tracking, snakes
– 3D vision: multi-camera geometry, stereo and
model-based vision
– Range imaging and robotic applications
What we will learn
• Introduction to Mobile Robotics

– Overview of mobile robotics, applications
– Sensors and estimation
– Distributed robotics
• Introduction to Micro- and Nano-robotics

– Overview of MEMS, scaling effects, micromanipulation
– Microscope optics, depth from defocus, focus measures
– Examples from current research
25
Robotics is …
• fun
• an interdisciplinary area
• unsolved
ICRA Video Proceedings

Winner 2006
26
Mechatronics
Mechanical Design of Robots

ETH Zurich
Robots—The Industrial View
• “A robot is a reprogrammable, multifunctional machine

designed to manipulate materials, parts, tools, or specialized
devices, through variable programmed motions for the
performance of a variety of tasks.”
Definition of the Robotics Industries Association

( www.robotics.org )
1
Common Robotic Tasks
• Manufacturing:
Hard Automation vs. Flexible Automation
Large Volume vs. Small Volume
• Hazardous Environments
Toxic, Radioactive, Underwater, Space
• Medical
Surgery, Rehabilitation
Robotics Industry
• Extracts from U.N. World Robotics 2004 (from www.ifr.org)

– Worldwide investment in industrial robots up 19% in 2003
– In first half of 2004, orders for robots were up… to the highest
level ever recorded
– Worldwide growth in the period 2004–2007 forecast at an
average annual rate of about 7%
– Over 600,000 household robots in use—several millions in the
next few years
– Germany is the world’s second largest user
and producer of industrial robots
– Buoyant robot sales in North America
2
Robotics Industry
Degrees of Freedom (DOF)
• The number of independent movements an object can make

with respect to a coordinate system
X PX
PX
X X PY
Z
Y PY
Y
Y
Z
PZ
3
Degrees of Mobility (DOM)
• The number of independently controlled joints on a robot

• DOM DOF
f ( x, y )
2 DOM, 2 DOF
2 DOM, 1 DOF: a singularity configuration
Robot Types: Gantry Robots
• A.k.a. Cartesian Robots

• Large workspace and heavy lifting
4
Robot Types: Cylindrical Robots
Robot Types: Spherical Robots
• A.k.a. Polar Robots
5
Robot Types: SCARA Robots
• Selective Compliance Assembly Robot Arm

– Stiff along z-axis, compliant along x, y axes.
– Good for z-axis assembly.
– Many products are designed for z-axis assembly
www.adept.com
Robot Types: Articulated Arms
• Mostly 6 DOF
• Redundant manipulators
– DOM>DOF (normally DOM > 6)
www.snakerobots.com
Robotics Research Corp. 7 DOM Arm
6
Robot Types: Parallel Robots
• Actuators do not stack on each other

• Rigid, lightweight and fast
• 3 DOF delta mechanism invented by
Dr. Clavel from EPFL
Industrial Applications of Robots
• Machine Tending
Transfer of parts and components to and from
machine tools and other machinery
ABB IRB7600 robot

(500 kg load)
7
• Palletizing
• Pick and place
• Parallel robots enable very
fast pick and place
• Assembly
• Macro and Micro Scale
• Flexible Feeding
8
• Welding
• Non-contact
• Painting
• Non-contact
ABB IRB540 Painting Robot
9
• Machining
Robot Programming
• Robots are not electric-men

• Teach Pendant
Low Level
• Simulation and off-line programming
• Path Planning, Assembly Planning
High Level
• Artificial Intelligence
(mostly research)
10
Regrasping
(Robot Programming Example)
Components of a Robot
• Joints
• Sensors
• Transmissions and gearboxes
• Actuators
• Wrists
• End-effectors
11
Robot Joints
• Prismatic and Revolute Joints

• Rigidity, backlash, friction
Position and Orientation
• Prismatic joints change only the position of the end-point

• Revolute joints always change the orientation and almost
always the position as well
end point coordinate frame
end point
base coordinate frame
12
Robot Joints
• Universal (Cardan) and Spherical Joints
Transmissions and Gearboxes
• Typical servo motors provide high speed at low torque

Power = Torque * Speed
• Industrial robots are designed to be stiff but transmissions
and gearboxes reduce stiffness
Spur gears Spur gearbox
13
Planetary gear drive animation

From www.howstuffworks.com
Planetary gearbox
Harmonic Drives
• Harmonic drives provide high ratios at low weight and

compact size, but are inherently flexible (torsional stiffness)
• Flexspline has 2 teeth less than circular spline

Nflexspline = Ncircularspline - 2
• Gear ratios Nflexspline/2. 50:1 to 320:1 are commercially available
14
• Tendon Drives
Sensors: Encoders
• Linear and Angular Encoders

• Digital Output
• Can also be used to sense speed
15
Sensors: Resolvers
• Resolvers are very robust, durable

• Can sense position and velocity
from www.maxonmotor.com
Sensors: Tachometers
• Tachometers are small DC generators

• Every brushed DC motor is a tachometer
• Volts/rpm constant (~1%), sign shows direction
• Not useful for position
Encoder/ Gearbox Motor

Resolver
Tachometer
Controller
16
Sensors: Force
• Mostly at the wrist, sometimes at the joints and grippers
Robotics Research Corp. 7 DOF Arm
Robot Actuators
• Hydraulic
• Pneumatic
• Electric
• Piezo
17
Hydraulic Actuators
• The Good…
- Large forces and large power/weight ratio
(due to remote power source)
- Oil is (almost) incompressible,
joints can be locked
- Damage-safe in stall
- Can be fast and controlled precisely
- Safe in flammable environments
- Smooth operation at low speeds
• The Bad…
- Expensive
- Hydraulics are dirty and hard to maintain
- Need to circulate the oil (return line)
- Remote power source occupies space
- Cannot be back driven against valves
Hydraulic Actuators
MTS structural testing system

18
Pneumatic Actuators
• The Good…
- Inexpensive
- Damage-safe in stall
- Can be fast
- Safe in flammable environments
- No return line needed, air is ubiquitous
• The Bad…
- Air is compressible, bad for accuracy and control
- Air exhausts are noisy
Air muscle robot, www.shadow.org.uk
Electric Actuators
• Most common, many types and sizes

– DC servo, stepper, direct drive, etc.
• The Good…
- Fast, accurate, inexpensive
- High stiffness and dynamic range
- New magnet materials (NdFeB,SmCo)
enable high torque in compact size.
• The Bad…
- Inherently high speed, low
torque. Gearboxes are
needed. Direct drive motors
are becoming more common.
- Electrical arcing can be a
problem
- Brakes are needed to lock
joints
19
Brushed-DC Motors
• Permanent magnets or coils for stator field

• Carbon or metal brushes for commutation create friction and
sparks, and require maintenance
Brushless-DC Motors
• Permanent magnet in the rotor

• Contactless sensors (typically Hall-effect type) sense the
position of rotor, stator coils are electronically commutated
20
Stepper Motors
• Driven through a train of pulses, drive electronics are more

complex
• Inherently position controlled if steps are not missed.
Piezo-Electric Actuators
• Piezoelectric materials expand under

electric fields
• Common in Microrobotics and optical
systems
• The Good…
- Nanometer (10-9 m) resolution!
- High stiffness, force and dynamic range
- Capacitive device, low power consumption at stall
- Can be made very small
- Doubles as sensor
• The Bad…
- Small travel (typical extension 0.1%), stick slip
actuators extend coarse travel range
- Expensive
- High voltage electronics (>100V) needed to drive
21
Robotic End Effectors: Grippers
Parallel jaw grip
Robotic End Effectors: Hands
Dr. Peter Allen

Columbia Univ.
MIT-Utah Hand
Barret Hand
Robonaut hand
22
Robotic End Effectors: welding torches,
paint nozzles, vacuum cups
Welding torch
Vacuum Cups
Robotic Sheep Shearing (Australia)
• http://www.mech.uwa.edu.au/jpt/shearmagic/Default.html
23
Tool Changers
Compliance Devices
• Remote Center Compliance (RCC)

devices
24
Robot Wrists
• The spherical wrist eases the

inverse kinematics problem
• Wrist partitioned robots use the
first 3 DOF to control the position
and the last 3 DOF at the wrist for
the orientation
Wrist Singularities
http://www.anthrobot.com/
25
Avoiding Singularities
• NASA 4-axis wrist
Precision, Accuracy
26
Resolution
• Resolution of an actuator is the smallest commandable

distance of travel (or angle of rotation)
• For a sensor it is the smallest measurable interval
First Position
Targeted Position
Adjacent Position
Accuracy
Command Resolution
Spatial Resolution
• Actual spatial resolution of a manipulator may change

depending on configuration, loads, etc.
First Position
Targeted Position Adjacent Position
Accuracy
Mechanical Errors
Spatial Resolution
27
Resolution, Precision, Accuracy
Spatial Resolution
Accuracy
+
+ +
Arbitrary Target Position
Precision
(Repeatability)
Angular Resolution
• The spatial resolution due to angular joints changes by

configuration
28
A Last Word on Robot Design
• Industrial robots are made to be rigid and accurate. This is

good for positioning tasks.
• Not the ideal way for contact tasks.
MIT’s COG
29
Mechatronics
Spatial Descriptions 1
(Classical Formulation)

ETH Zurich
Motivation
• What are we going to learn here?
– We want to deal with rigid bodies (not only with points)

– We want to deal even with multi-bodies
connected together by prismatic
or revolute joints, i.e. robots.
• Before we can deal (model, simulate, control) with complex

kinematic links, we have to define their position,
orientation, velocity and acceleration
…and we start with position in this lecture.
1
Location of Objects
Simple 2D Case
• A point has a unique position in 2D space described with
respect to (wrt) a coordinate frame. Two components are
necessary to describe the position, a point has two
degrees-of-freedom in 2D space.
Can we attach a reference point
X0 to an object to represent it?
P0
Y0 Y0
What about orientation?
X0
Location of Objects
• Solution: Attach a coordinate frame to the object

• Rigid Object Every point on the object has fixed
coordinates wrt its coordinate frame.
• Position + Orientation = Pose
Y1
Y0
X1
X0
2
Degrees of Freedom
• Particles/points vs. Rigid body (most intuitive def. : “distances between

all points remain constant … in the body frame”)
• Particles
d dimensional space, d translational components of DoF
• Rigid body
additional d(d-1)/2 rotational components of DoF
Examples: in 1-, 2- and 3- dimensions,

we get 1, 3, 6 DoF for a rigid body
Names used in Robotics
• Common arrangements in robotics

describing kinematics
1. Tilting forward and backward (pitching)

2. Turning left and right (yawing)
3. Tilting side to side (rolling)
3
System of rigid bodies
(multi-bodies)
• System of several bodies has a combined DoF which is the
sum of the DoF of all bodies
• Clearly, a multi-body has more DoF than a single rigid body
• In this course, you will see & understand that the term
degrees of freedom
is equivalent to the
number of independent parameters
required to specify a particular spatial pose

(position + orientation).
Some Examples
• Human arm: How many DoF ?

Considered to have 7 DoF: Can you count all of them? How ?
• shoulder gives pitch, yaw and roll (3 in one joint !)
elbow allows for pitch (1)
wrist allows for pitch, yaw and roll (3)
only 3 of these movements necessary to move the hand to any point in

space, but people would lack the ability to grasp things from different
angles or directions…
• Holonomicity in robotics
Relation between controllable & total number of DoF
• Robot in 3D is holonomic if it has 6 DoF, non-holonomic if less than 6, and

redundant if more than 6
Examples: - Human arm is redundant

- Car is non-holomonic with
controlled yawing left/right and surging accelerating/breaking
i.e. 2DoF (sliding aside is not possible) vs. 3 DoF for 2D space
4
Location of Rigid Objects
Reference frame
• Position of something is always measured with respect to
“something else”
Our something else will be a right handed orthogonal

reference coordinate frame (directions of rotation) :
Y1
Y0
X1
X0
Concepts for Description of Position of a Rigid Body (n

point masses rigidly connected) 1/2
Alternatives: A1 — A3
A1. If we assume that n is small (say n=5), we could keep track

of each point separately by assigning it a position vector.
What do you think about it ?
Yes, we could but observe that any motion of a rigid body will
not change the relative position between its point masses.
Such motion is referred to as rigid body motion.
A2. Hence, it would be sufficient to account for the position of

only 3 (non-collinear) points…
… and if necessary, we reconstruct the position of the others.
5
Concepts for Description of Position of a Rigid
Body (n point masses rigidly connected) 2/2
We can observe that rigid body motion can be represented

as a combination of pure translation and a rotation …
A3. Hence, the position of the whole body could be represented

as the linear position of one of the particles of the body and
the angular position (orientation) of the body
The 3rd approach is mathematically very convenient.

We will adopt it through out this course!
Body Frame
• In order to be able to account for both 1.

position & orientation (rotation) of
a rigid body, we associate a
coordinate frame with it:
Body(-fixed) frame
2.
– There is no relative motion between the rigid body
and the body-fixed frame
– The body frame can rotate (together

with the rigid body it is attached to)
= 3 additional rotational DoFs
Example 1:
Aircraft - only coordinates of
the origin & rotation of the body frame
are then of interest …
Example 2:
Rigid body consisting of 2 points
6
Position
• Simple !
Linear position of frame B in frame A is the position of the

origin of B in A
(see the vector Ar1 in the

Example below)
Orientation (Rotation)
• Parameterizing the orientation/rotation of frame B relatively to

frame A is in general more difficult problem
• There are many alternatives for its description

(all of them have their pros & cons)
• So far, there is no method that is found superior to the others

in general
• We will have a look into few useful concepts here …
7
Coordinate Frame Algebra
(Principle in 2D)
• CF1 is found by rotating CF0 by radians ccw (counter
clockwise).
• Consider the same point defined wrt (with respect to)
2 different coordinate frames.
c cos( )
Y0 s sin( )
Y1
X1
X0 c X 1 s Y1
cos( ) sin( ) Y0 s X 1 c Y1
sin( ) X0 • Relations between two coordinate frames
cos( )
• Try to derive it on your own now !
What happens if defined clockwise?
Rotation Matrices
(Forming)
• Rotation matrix R10 defines the orientation of CF1 with
respect to (i.e., starting with) CF0.
• Therefore, it defines the orientation of the object it is attached
to wrt CF0.
X0 c s X1
Y0 s c Y1 P0 R10 P1
R10
8
Rotation Matrices
(Meaning)
c s
Y0 R10 X 10 Y10
Y1 s c
• The columns of the R10 are the X and Y
X1 axes of CF1 described wrt CF0
cos( ) sin( )
sin( ) X0 1 0
X 11 Y11
cos( ) 0 1
c s
Very important for intuitive
X 10 Y10
s c
understanding!
Rotation Matrices
(Properties)
P0 R10 P1 • A rotation matrix is orthogonal (even ortho-
normal since each column and therefore each
row are unit vectors, mutually orthogonal)
P0 P1 • Rotations do not change the vector length
P1 ( R10 ) 1 P 0 • A rotation matrix is always invertible
R01 ( R10 ) 1
( R10 )T • The inverse is simply the transpose
( R10 )T R10 R10 ( R10 )T I
1
det R 0 1 • The determinant of a proper rotation is 1
(improper rotations/reflections incl. -1)
9
Rotation and Translation
Given: P1 what is P 0 ?
Y1 P0 R10 P1 T10
P0 P2
1 Given: P 2 what is P1 ?
Y0
P X1
Y2 P1 R21 P 2 T21
Given: P 2 what is P 0 ?
T21 P0 R10 ( R21 P 2 T21 ) T10
T10
X0
Given: P 0 what is P 2 ?
P2 ( R21 )T [( R10 )T ( P 0 T10 ) T21 ]
X2
A 6DOF robot would have 7 such frames! Ugly!
Homogenous Transformation
Matrices
P0 R10 P1 T10 Can we write this as P 0 H10 P1 ?
X0 c s T10x X1 R10 T10 1

P0 P
Y0 s c T10y Y1 0 1
1 0 0 1 1
H10
X0 c X 1 s Y 1 T10x Hint:
Points become column vectors
whose last component is 1.
Y0 s X 1 c Y 1 T10y
1 1
10
Matrices
P0 H10 P1 H10 H 21 P 2
Y1
P0 P2 H 20 H10 H 21 P0 H 20 P 2
Y0
P1 X1
Y2
P1 ( H10 ) 1 P 0 H 01 P 0
R10 T10
T21 H10
T10 0 1
X0
1 ( R10 )T ( R10 )T T10

H 0
X2 0 1
(Extension from 2D to 3D)
X axis of CF1 Y axis of CF1 Position vector from origin of CF0 to

described wrt described wrt origin of CF1, described wrt CF0
CF0 CF0
Y1
0
X 10 Y10 T10
H 1 Y0 X1
0 0 1
• Easy extension to 3D
0 0 0 0 T10
X 1 Y1 Z 1 T 1 X0
H10
0 0 0 1
11
Rotations in 3D
(Principal / Basic Rotations)
Y0 • Our former 2D rotation can be considered a 3D
Y1 rotation about the Z axis!
c s 0
X1
cos( ) sin( )
RZ , s c 0 X 10 Y10 Z10
sin( ) X0 0 0 1
cos( )
1 0 0 c 0 s
Z0,Z1
RX , 0 c s RY , 0 1 0
Advice:
Derive & remember these 3
0 s c s 0 c
matrices of principal rotations
for the rest of this course
(Example in 3D as used in robotics)
• For the rest of this course, remember all these matrix forms
and their dimensions:
X0 X1 R10 T10
H10
Y0 Y1 0 1x 3 1
4x4
0
H10 1 0 T
Z Z 1
(R )
1 ( R10 )T T10
H 0
1 1 0 1x 3 1
4x4
12
Other Transformations
(Euclidean, Affine, Projective)
• Euclidean transformation
– Most commonly used
(e.g. in robotics – prismatic & revolute joints)
– Preserve length and angle measure between frames
(shape of a geometric object will not change)
– Operations: Translation & rotation (incl. reflection, det(R) = -1 )
– Form:
r11 r12 r13 Tx r11 r12 r13 Tx

r21 r22 r23 Ty r21 r22 r23 Ty
H 10
r31 r32 r33 Tz r31 r32 r33 Tz
0 0 0 1 0 0 0 1
• Affine transformation
– Generalization of Euclidean transformations
(commonly used e.g. in computer graphics)
– Length and angle not preserved but parallel lines/planes and
intersecting lines/planes are translated into parallel and
intersecting lines/planes correctly
(shape of a transformed object can be changed)
– Additional operations: Scaling, shear:
– Form:
a11 a12 a13 a14
a21 a22 a23 a24
A10
a31 a32 a33 a34
0 0 0 1
13
• Projective transformation
– Generalization of affine transformations
– Length and angle not preserved & parallel and intersecting
lines/planes are not preserved either
(shape of a transformed object can be changed)
– Additional operations:
converts parallel lines/planes into intersecting and vice versa
– Form:
p11 p12 p13 p14
p21 p22 p23 p24
P10
p31 p32 p33 p34
p41 p42 p43 p44
(observe simply that the 4th row is not [0 0 0 1] anymore)
Composition of Transformations
(Problem description)
• We have introduced several transformations
• In many cases, one may need several transformations to
complete the desired operation
• The overall effect can be summarized into a single
transformation (matrix operation) represented by
the product of all involved matrices:
– Example:
1. Scale in the x-direction using a scale factor 5 (i.e. make it 5x larger)
2. Followed by a rotation about z-axis 30 degrees
3. Followed by a shear transformation in x- and y- direction with
shearing factor 2 and 3 respectively
4. Followed by a transformation moving the point in
the direction [2 1 2]
14
Composition of Transformations
(Solution: Matrix multiplication)
• Let the scaling, rotation, shearing & translation matrices be
A, B, C and D, respectively.
• The following matrix H = DCBA describes the net effect:
Composition of Rotations in 3D
(2 basic rules)
• Description: Starting from CF0, CF1 is found by rotating along
X axis by and then rotating along the current Z axis by
P0 R10 P1 R10 RX , RZ ,
• The order is important! Postmultiply for rotations about the

current frame
RX , RZ , RZ , RX ,
• Description: Starting from CF0, CF1 is found by rotating along

X axis by and then rotating along the original / fix Z axis
by 0 0 1 0
P R1 P R1 RZ , RX ,
Premultiply for rotations about the original frame
15
Composition of Rotations in 3D
(Example)
Find the composed rotation matrix R defined by the following sequence

of principal rotations carried out in this order:
1. Rotation by about the current x-axis

2. Rotation by about the current z-axis
3. Rotation by about the fixed z-axis
4. Rotation by about the current y-axis
5. Rotation by about the fixed x-axis
Hint: a) Start with step 1. – i.e. rotation R(x, ) – and then pre- or post-multiply
for all subsequent rotations …
b) Follow this rule: “Fixed goes First…” (pre-multiply if about fixed axis)
Alternative Representation of R
(Euler angles)
The 9 elements of R are not independent quantities. Note that
• Columns of R are of unit magnitude, and
• Columns of R are mutually orthogonal
It leads to 6 independent equations/constraints, which implies there are only

3 independent variables (consistent with our intuitive understanding that a rigid
body possesses at most 3 rotational degrees of freedom)
• Minimal representation of orientation can be obtained by using a set of 3 angles

{alpha, beta, gamma}. We can always find such {alpha, beta, gamma} by choosing an
appropriate sequence of principal rotation matrices Rx, Ry, Rz
• We refer to {alpha, beta, gamma} in combination with composition sequence

as a set of Euler angles
16
Alternative Representation of Rotations
(Euler angles)
• Euler’s rotation theorem:

“Any rotation can be represented by not more
than 3 basic rotations about coordinate axes”
• The ZYZ sequence is commonly used in

aeronautics and mechanics:
On Appropriate Sequences
of Rotation Matrices
• An appropriate sequence of rotations means a sequence where no 2 successive
rotations are made around parallel axes (it would be only 2 rotations, not required 3)
Example: we do not consider Rx Rz Rz

as an appropriate sequence
• There are 3^3=27 possible combinations,

however, only 12 are considered to be valid:
If considered in fixed frames,

we call this sequence “RPY”
(Roll-Pitch-Yaw)
If considered in current frames,

we called this sequence “Euler angles”
• Above 12 sequences can be performed using both current frame as well as the
fixed/original frame (24 possibilities in total)
17
Euler Angles Representation of
Orientation/Rotation
• Euler Angles (and its variations called e.g. yaw, pitch & roll) are 3
principal rotations commonly used to define the orientation of one
frame wrt another.
• In robotics, usually for the last 3 DoF , i.e. for the wrist (orientation)
Z’ Z Euler 1. Rotate about Z by Φ

2. Rotate about Y’ by θ
Φ
3. Rotate about Z’ by ψ
ψ
(interpret as: Z Y Z; all around current axes
Y’’ and do this: R = Rz * Ry’ * Rz’ )
θ Y’
Y
and other 23 variations, e.g.

X
X’ X’’X’’’ • X,Y, Z or RPY (roll, pitch, yaw)
(interpret as: X Y Z; all around fixed axes
and do this : R = Rz * Ry * Rx)
Another representation of R
(Axis-Angle)
• 3 parameters are enough to represent orientations in
3D space
• A 3x3 rotation matrix has 9 elements, but they are not
independent.
• A vector k and an angle is enough to define a rotation
matrix Rk ,
• k has 3 elements but its length is 1 so it gives only 2
independent parameters. With θ there are only 3 parameters.
k k x2 k y2 k z2 1
• A single vector θk can be used for direction and magnitude
18
Rotation About an Arbitrary Axis
• “A new CF is found by rotating CF0 about

a vector k by θ rad.”
Rk , ?
• First rotate k such that the Z axis aligns
with the k vector, then rotate about Z by θ,
then rotate k back to its original position
Rk , RZ , RY , RZ , RY , RZ ,
Demo
• Case 1 – 6
Rotation of different points P1 around various
1
axes k (always passing the common origin) by
angle theta
0.8
– Case 1
Rotation theta = 30°
0.6
– Case 2,3
z
Rotation of the same point around the same 0.4

axis as above where theta = 180° and 360°
0.2
– Case 4
Rotation as above, but around a different axis
0
0
– Case 5
Rotation of another point around
0.5 0
yet another axis 0.2
0.4
0.6
1 0.8
1
– Case 6 P0 = [0.6 0 0]’ y x
Singularity problem with
the employed appropriate sequence
P1 = [0 0.6 0]’
19
Velocity Vectors
Z0 Z1
1
Y1 0
R10 T10
H 1
X1 0 1x 3 1
P 1 v1
0
P0 H10 P1
P
X0 v0 H10 v1 0
H10 1
0
v0 R10 v1 R10 1
Y0
• Velocity vectors show direction and magnitude, not position
• Use only/directly the rotation part of the transformation matrix on velocity
vectors (no homogenous transformation necessary)
• More on velocity kinematics in the lecture “Rigid Body Velocity”
20
Mechatronics
Spatial Descriptions 2
(Screw Theory)

ETH Zurich
Screw Theory (ST) in Robotics

(Introduction to its application)
• There are alternatives for spatial descriptions in robotics

– Recall drawbacks observed in classical approaches such as
• Euler angles - one singularity in any sequence
• Angle-axis - axis not always defined (if = 0 k for any k = 0,1,… )
• Screw Theory is mathematically elegant and powerful

• Relatively new to robotics, many robotics researchers are not familiar with it
• A lot of different words are used to described it:
(a) Screws (b) Twists (c) Product of matrix exponentials (POE)
• The entire method is based on thorough understanding of
rigid-body motion rather than location
Observe the following:

- Time was not included in previously introduced (homogeneous) transformations.
- Since the screw theory will introduce the notion of time through a differential
equation, we move “from location to motion” now.
1
Classical Formulation
vs. Screw Theory
• Classical formulation is focused on expressing/transforming points &
vectors in one (arbitrary) robot-link frame with respect to another
(different) robot-link frame, based on the current configuration
• This method (ST) has its advantages, but essentially treats each
robot link directly as equally important (in base/world/spatial
coordinates)
• We are typically most interested only in the last robot-link frame,
usually called the tool frame
• Screw theory is very good for direct description of the configuration
and motion of the tool frame with respect to the base coordinates
(this will become more evident when we discuss rigid-body velocity)
• The other robot links are just elements which position the tool frame,
and we have less information about them (although we could always
calculate this information if necessary, see later the demo).
• Perhaps, this can be seen as the only disadvantage?
Rigid Body Motion
• Let’s forget about robots and consider just a rigid body in a space…
• Embed a frame within the body to describe its pose
• Move the body from its initial location at time = 0 to time = t
• The initial frame A is an inertial/base reference frame, from which
we will describe the pose of the body throughout time
• The points p and q on the rigid body are always the same distance
apart: ||p(t) - q(t)|| = ||p(0) – q(0)|| (rigid body not deformable)
• Given any two points p and q in the body, we z p y
can find a vector from p to q given by v = q - p
• And a function g maps a point B
to its new location at time t g q x

z
• And also maps embedded t=t
vectors: g*(v) = g(q) – g(p)
y
A
x
t=0
2
Rigid Body Transformation
• A mapping g is a rigid body transformation if it satisfies the

following properties:
– Length is preserved: ||g(p) - g(q)|| = ||p - q|| for all points p and q
– The cross product is preserved: g*(v x w) = g*(v) x g*(w) for all
vectors v and w
z p y
B
g x
q
z
y
A
x
Linear Form of Cross Product

How to get it?
• The cross (vector) product between two vectors is defined as
a2b3 a3b2 0 a3 a2
a b a3b1 a1b3 a3 0 a1 b (a )^ b
a1b2 a2b1 a2 a1 0
• (a)^ is a skew-symmetric matrix built from the elements of a
• A skew-symmetric matrix S is defined by S = -ST
• We will also use the equivalent notation aˆ ( a )^
3
Exponential Coordinates for Rotation
• If we rotate a body at constant unit velocity about axis where

|| || = 1, the velocity of the point q is written as
q (t ) q (t ) ˆ q (t )
• This time-invariant linear differential equation can be integrated
q (t ) e ˆt q (0)
• This makes use of the matrix exponential
ˆt ( ˆt)2 ( ˆ t )3
e I ˆt
2! 3!
• Considering units of time, we compute
the net rotation as
R( , ) e ˆ
Exponential Coordinates for Rotation

Overview
• The matrix exponential – i.e. expm((omega)^) - of a skew-symmetric matrix
is a rotation matrix
• The axis of rotation is the vector encoded in the skew-symmetric matrix,
normalized to unit length, i.e. our vector omega where || || = 1 is normalized
• The magnitude of rotation (angle in radians) is the length of the vector
encoded in the skew-symmetric matrix, i.e. our theta we added to exponent
• All rotation matrices R( , ) can be represented by a matrix exponential
of a skew-symmetric matrix
• There is a simple computation for the matrix exponential, known as
Rodrigues’ formula (where || || = 1 ):
( ˆ )2 ( ˆ )3
eˆ I ˆ I ˆ sin ˆ 2 (1 cos )
2! 3!
4
Rigid Motions
SE(3), SO(3) and O(3) groups
• We want to completely describe the pose of the rigid body w.r.t. an
inertial reference frame, i.e. we have to deal with both rot. & translat.
• Given by the orientation of the frame Rab and the position pab
• Let qa and qb be the coordinates of a point q relative to frames A
and B, where qa = Rabqb + pab
• We can also think of the configuration as an abstract item on its own,
which contains two pieces of information:
gab = (pab,Rab) i.e. translation & rotation
• We call this abstract object a member
of the 3-D special Euclidean group:
SE(3); rotation Rab is a member
of SO(3) - special orthogonal group
(for rotations in a right-handed coord.
frame det(R)=1; O(3) group has det(R)= 1)
• We also think of gab to be rigid transformation of a point: qa = gab(qb)
Homogeneous Representation
• Rigid transformations are not linear but affine, i.e. qa = Rabqb + pab
• Can we construct a ‘more’ linear version of these motions?
• Yes! We append systematically 1 to the coordinates of a point
and 0 to a the coordinates of a vector (both to brcome 4x1)
• We will work with vectors & points – distinguish and treat
them slightly differently:
q1 v1
q2 v2
q v
q3 v3
1 0
• Why? The above syntax helps us to reflect physical realities:

– Sums and differences of vectors are vectors
– The sum of a vector and a point is a point
– The difference between two points is a vector
– The sum of two points is meaningless
HINT: Have a look into the 4th “extended” coordinate being maintained 1 or 0 !
5
Homogeneous Representation
(From skew-matrix exponential to full twist…)
• Now we can express a rigid transformation qa = Rabqb + pab
as a linear multiplication of a 4x4 matrix with a 4x1 vector
qa Rab pab qb
qa g ab qb
1 0 1 1
• We already found that a 3x3 rotation matrix is the matrix exponential

of a 3x3 skew-symmetric matrix
• Note:
– There is a 4x4 equivalent of a skew-symmetric matrix (which we
had before to describe only rotations), called a twist.
– The matrix exponential of the twist is a 4x4 homogeneous
transformation matrix (which as we already know describes both
rotation and translation)
The Twist – Special case 1

only rotation (generic rotation = incl. offset from origin)
• Consider a one-link robot (or just a point p), rotating about axis
(with || ||=1 , i.e. with a unit velocity) where is passing a point q)
p (t ) ( p (t ) q )
p ˆ q p ˆ p p ˆp
0 0 0 1 1 )
where ˆ v
ˆ , v q
0 0
• Integrating gives (recall Rodrigues’ formula and see ‘screw motions’ later)
ˆ e ˆt ( I e ˆt )( v)
p (t ) e t p (0) where e
ˆt
0 1
6
The Twist – Special Case 2
only translation
• Consider a one-link robot, sliding on a prismatic joint with unit

velocity
p (t ) v
p 0 v p ˆ p ˆp
p
0 0 0 1 1
• Integrating gives
ˆ ˆ I vt
p (t ) e t p (0) where et
0 1
The Twist and Screw Motion

Structure, Names, Notations
• A twist is a 4x4 matrix of the structure
3x3 skew-symmetric matrix 3x1 vector
ˆ v
ˆ
0 0
1x3 matrix of zeros
• We can also extract the 6x1 twist coordinates

ˆ v v
ˆ
0 0
• The twist is a characteristic of screw motions
• Matrix exponential maps twist into screw motion
7
Screw Motions
• Theorem (Michel Chasles): Every rigid body motion can be

realized by a rotation about an axis combined with a translation
parallel to that axis (“screw”)
• A screw S consists of an axis l, a pitch h, and a magnitude M
• A screw motion represents rotation by an amount = M about
axis l followed by translation by an amount h parallel to axis l
• If h = then the screw motion consists of a pure translation along
the axis ‘ ’ by a distance ‘M’
• If h = 0 then the screw motion
consists of a pure rotation
about the axis ‘l’ by
a distance ‘M’
Screw Motions
• Description of final location of the point p:

gp q e ˆ ( p q) h
or, in homogeneous coordinates:
p eˆ ( I e ˆ )q h p
g
1 0 1 1
• This has the same form as the exponential of this twist:

ˆ eˆ ( I e ˆ )( v) T
v
e v q h , || || 1, 0
0 1
• In case of pure rotation (h=0), is the angle, and g is simply:
ˆ eˆ ( I e ˆ )( v)
e
0 1
8
Calculating a Screw from a Twist
(When we say “screw”, we mean how to get [h, l, M])
• Pitch: The pitch of a twist is the ratio of translational motion to

rotational motion. If = 0, we say has infinite pitch
T
v
h
|| ||2
• Axis: The axis l is a directed line through a point
v
: , if 0
l || ||2
v: , if 0
• Magnitude: The magnitude of a screw is the net rotation if the motion

contains a rotational component, or the net translation otherwise
|| ||, if 0
M
|| v ||, if 0
Calculating a Twist from a Screw

(When we say “twist”, we mean how to get )
• h = : Let l = { v : ||v|| = 1, real}, = M and define
ˆ 0 v
0 0
the rigid body motion expm( ^ ) corresponds to pure translation
along the screw axis by an amount
• h finite: Let l = {q + : || || = 1, real}, = M and define
ˆ q h
ˆ
0 0
the appropriate screw motion is generated again by expm( ^ )
9
Example: Rotation about any axis
(Step 1 of 2 – Get “twist” )
• Consider motion corresponding to a zero-pitch screw (i.e. pure
rotation, h=0) about an axis = (0,0,1) passing through
point q = (0,l1,0). The corresponding twist is
l1
0
q 0
0
0
1
• The exponential of the twist is given by
cos sin 0 l1 sin
ˆ eˆ ( I e ˆ )( v) sin cos 0 l1 (1 cos )
e
0 1 0 0 1 0
0 0 0 1

(Step 2 of 2: Get initial condition)
• The rigid transformation which maps points in B coordinates to A
coordinates --- and hence describes the configuration of the rigid
body --- is given by
0
ˆ I l1
g ab ( ) e g ab (0) where g ab (0)
0
0 1
• Taking the exponential and performing
the matrix multiplication yields the
configuration, which can be verified by
inspection
cos sin 0 0
sin cos 0 l1
g ab ( )
0 0 1 0
0 0 0 1
10
Solution in Matlab - Simple code 1/2
syms l1 th real % define symbolic vars as "real“ (otherwise all considered complex)
% 1. Define axis of rotation as a free vector and select a point it goes through:
q = [0 ; l1 ; 0 ]
w = [0 ; 0 ; 1]
% 2. Define v & xi according to your slides:
v = cross( - w , q)
xi = [v ; w]
% 3. Get 4x4 twist matrix from the above 6x1 vector xi called “Twist Coordinates”
xi_ = twist(xi) % i.e. = [skew(w), v ; zeros(1,4)]
% 4. Calculate the matrix exponential:
exp_xi_th = expm(xi_ * th)
% 5. Get initial condition (by inspection from the picture):
g0 = [eye(3) , q; zeros(1,3) , 1]
% 6. Resulting rigid body transformation denoted as “g_ab” is given by:
g_ab = simplify(exp_xi_th * g0)

Solution in Matlab – Useful functions 2/2
11
Matlab demo (demo_twist.m)
• Case 1 – Case 7
twist of the same point around Twist demo

different axis passing different
points by angle theta 4
3.5
• Case 1,2 3
Rotations 2.5
• Case 3 2
z
Translation 1.5
• Case 4,5 1
Twist (both, trans & rot) by different 0.5
angles theta 0 0
• Case 6,7,8 3
2
1
0
2
1
3
Twists around the same axis -1
y
passing different points x
12
Mechatronics
Forward Kinematics

ETH Zurich
Robot Forward Kinematics
• “ Given the joint angles, what is the position and orientation of

the end-effector? ”
1
What have we learned?
• Body frame (BF) approach

• 3rd approach to rigid body pose description
• BF rotation/orientation + translation of the BF origin in the world frame
• 3 + 3 DoF i.e. 6 independent parameters
• Translation/position is rather simple to describe
• Description of rotation/orientation more complex:
– Euler angles
– 2x12 appropriate sequences (current vs. fixed frames), e.g. RPY
– Advantage: Minimum representation (directly by means of only 3 parameters)
– Drawback: Every sequence has 1x singularity
– Angle-axis representation
– Advantage: Every sequence of rotations can be described as 1 rotation
about an axis going through the origin and by an angle
– 4 parameters (axis – 3x vector + 1x angle = 4; if axis normalized 2x parameters)
– Drawback: Axis not always defined (if = k where k any integer)
• Screw theory (suitable for description of both translations & rotations)

– Very elegant and powerful approach
– No problems with singularities or undefined axes
– More advantages coming up in this lecture (e.g. POE – Product Of Exponentials)
Robot Forward Kinematics

What is the advantage of revolute joints?
• Dealing with them mathematically is more complicated than in case
of prismatic joints (rotations vs. translations)
– They result in large amount of coupling in
kinematics & dynamics of connected links
(it leads to accumulation of errors and making control problems more difficult)
• Their advantage? Higher compactness & dexterity
1. Revolute joint can be made much smaller than a link with linear joint
(i.e. manipulators made of revolute joints occupy a smaller space)
2. Robots with revolute joints are better able to maneuver around
obstacles (i.e. there is a wider range of possible applications)
2
Kinematic Chains
• Given the joint angles, what is the • Each link (including the base) has
transformation matrix that defines a coordinate frame attached to it.
the end-effector coordinate frame • Each link is related to the one
wrt the world (base) coordinate before it through a joint variable
frame? (i.e., an angle for revolute joints, or
the travel distance for prismatic
joints).
P3 H10 f ( 1)
H 21 f ( 2)
P0 H 32 f ( 3)
H 30 H10 H 21 H 32 f ( 1, 2 , 3 )
P0 H 30 P 3
Kinematic Chains
H10 f ( 1)
RotZ 1 Trans 0, 0, d1 RotY 90 RotZ 90
Y1
Z
Y Y X1
X Z Z1
Z0 Z X
d1
d1 • Not a unique sequence
Y We could arrive at the
Y0 final frame also through
1
X a different order of
X0
rotations and translations
3
Kinematic Chains
H10 f ( 1)
RotZ 1 Trans 0, 0, d1 RotY 90 RotZ 90
c1 s1 0 0 1 0 0 0 0 0 1 0 0 1 0 0
s1 c1 0 0 0 1 0 0 0 1 0 0 1 0 0 0
0 0 1 0 0 0 1 d1 1 0 0 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1
s1 0 c1 0
c1 0 s1 0 • Position vector
0 1 0 d1 • Rotational part
0 0 0 1
Denavit-Hartenberg (D-H) Convention

Quick Overview 1/4
• We can attach a coordinate frame to a link arbitrarily. The frame (i.e. its
origin) doesn’t even have to be on the link! This can make finding the
transformation matrix rather difficult.
• The Denavit-Hartenberg (D-H) Convention regulates where we attach the
coordinate frames and how we define the transformation matrices on an
industrial robot.
• In general, we would need 6 independent parameters to define the
transformation between two “neighboring” coordinate frames
– Arbitrary homogeneous transformation is characterized by
6 independent numbers:
• 3 for the position vector in column 4 - H(1:3,4)
• 3 for the rotational part - H(1:3,1:3)
• The D-H convention reduces the problem to 4 parameters by a clever
choice of the origin and orientation for the coordinate frames
– we are not cheating - we introduce 2 rules/constraints and in turn we allow the
frame origin to be anywhere (not unconditionally on the link/joint which we are
describing)
• J. Denavit and R. S. Hartenberg.

A kinematic notation for lower-pair mechanisms based on matrices.
Trans. ASME J. Appl. Mech., 23:215–221, 1955.
4
D-H Convention
One Sequence 2/4
• The transformation between two adjacent frames on the link
is defined in terms of 4 parameters and a fixed
composition/sequence of transformations:
Aii 1
Rot Z i Trans Z d i Trans X a i Rot X i
D-H Convention
Four Parameters 3/4
• Our 4 parameters i (angle), di (offset ), ai (length), i (twist ) are
generally given the names:
– joint angle
– link offset
– link length
– link twist
• Names derived from

the specific aspects of
geometric relationship
between the 2 linked
coordinate frames
5
D-H Convention
Two Constraints 4/4
• Depending on the type of joint, 1 parameter either i or di is

the joint variable and the other 3 parameters are constant.
• Why not 6 parameters? To ensure that only 4 parameters are
sufficient, the location and orientation of the coordinate
frames are chosen by the following 2 rules:
1. The axis Xi is perpendicular to Zi-1

2. The axis Xi intersects the axis Zi-1
D-H Convention – How do we proceed?

Step 1: Establish the Z axes
• For the links Z0 - Zn-1 the z axis is
the “actuated” axis about which
the joint variable is defined.
Link 2
2
• Notice that the joint variable 1 is
Link 0
Link 1
defined about the Z0 axis. Each
(base) link rotates about (or moves along)
1
Z1 the previous link’s coordinate
frame.
Z0
6
Step 2: Establish the Base frame
• Locate the origin anywhere on
the Z0 axis
Link 2 the X0 and Y0 axes are then

chosen conveniently to establish
Y0 2 a right-hand orthonormal
Link 1 coordinate frame see next slides
1
X0 Z1
Z0

Step 3A: Place middle coordinate frames
• For i=1,…,n-1, perform this:
– Locate the origin Oi where the

Y0
common normal to Zi and Zi-1
intersects Zi.
2
X0 Z1 ( If Zi intersects Zi-1 locate Oi at the
this intersection. If Zi and Zi-1 are
parallel locate Oi at joint I ).
1
Z0
Note: the origin Oi doesn’t have
to be on the link.
It can be anywhere (as long as

the corresponding link ‘i’ is rigidly
attached to the frame ‘i’
Origin must be on the
actuated z-axis!
7
Step 3B: Place middle coordinate frames
• Establish Xi along the common
normal between Zi and Zi-1 through
Oi or in the direction normal to the
Y1 Zi-Zi-1 plane if Zi and Zi-1 intersect.
Y0 • Place Yi to form a right hand

X1
coordinate frame.
2
X0 Z1
1
Z0

Step 4: Place final coordinate frame
• The final frame (i.e., the end-
X2
effector or tool frame) is
Y2
not actuated like the previous ones.
– Establish the Zn axis along Zn-1.

Y1 Z2
– The origin is typically placed at the
Y0 tip of the end-effector or some point
X1 of importance (e.g., the tip of a
nozzle)
2
X0 Z1
1
Z0
8
D-H Convention
Final Coordinate Frame - Names
• Typically, for a robot with a gripper the
final frame is placed between the
gripper fingers.
• And, the (x,y,z) axes are labeled (n,s,a).
– a (z-axis) is for approach direction

since the gripper typically approaches an
object from this direction.
– s (y-axis) is called the sliding direction

since the gripper slides to open and
close in this direction.
– n (x-axis) is called the normal direction

since it is the normal to (s x a)
• In most contemporary industrial robots, the

last Zn-1 and Zn axis coincide (and both
related joints are revolute)
– an important observation that will simplify the
computation of the inverse kinematics problem later
D-H Convention
Overview
(link length)
(link offset)
(link twist)
(joint angle)
9
D-H Convention
Table of Link Parameters
Y2 X2
Link: i di ai i
*
1 1 0 a1 0
Y1 Z2 *
2 2 0 a2 0
Y0 * joint variable
X1
a2
X0 Z1
ci si 0 ai ci
2
i 1 si ci 0 ai si
1 A i Rot Z i TransZ d i Trans X ai Rot X i
Z0 0 0 1 0
a1 0 0 0 1
c1c2 s1s2 c1s2 s1c2 0 c1c2 a2 s1s2 a2 c1a1
s1c2 c1s2 c1c2 s1s2 0 s1c2 a2 c1s2 a2 s1a1
H 20 A10 A21
0 0 1 0
0 0 0 1
Three-Link Cylindrical Robot
Link: i di ai i
*
1 1 d1 0 0
2 0 d*2 0 -90
3 0 d*3 0 0
* joint variable
10
Spherical Wrist
Link: i di ai i
d6
*
4 4 0 0 -90
*
5 5 0 0 +90
*
6 6 d6 0 0
* joint variable
The Stanford Manipulator
Link: i di ai i
*
1 1 d1 0 -90
*
2 2 d2 0 +90
3 0 d*3 0 0
*
4 4 0 0 -90
*
5 5 0 0 +90
*
6 6 0 0 0
Could be d6 to place the
coordinate frame between x0
the gripper
11
PUMA 560
Practise more
Joint θi i ai (mm) di (mm)
1 90 -90 0 0
2 0 0 431.8 149.09
3 90 90 -20.32 0
4 0 -90 0 433.07
5 0 90 0 0
6 0 0 0 56.25
PUMA 560
Resulting Description
6 n s a p
T06 Aii 1 = end-effector pose with respect to base frame
1 0 0 0 1
c1 0 s1 0 c4 0 s4 0
s1 0 c1 0 s4 0 c4 0
A01 A43
c1 c23 c4c5c6 s4 s6 s23 s5c6 s1 s4 c5 c6 c4 s6 0 1 0 0 0 1 0 d4
0 0 0 1 0 0 0 1
n s1 c23 c4 c5c6 s4 s6 s23 s5c6 c1 s4c5c6 c4 s6
c2 s2 0 a2c2 c5 0 s5 0
s23 c4 c5c6 s4 s6 c23 s5c6
s2 c2 0 a2 s2 s5 0 c5 0
c1 c23 c4c5 s6 s4 c6 s23 s5 s6 s1 s4c5 s6 c4 c6 A12 A4
5
0 0 1 d2 0 1 0 0
s s1 c23 c4 c5 s6 s4 c6 s23 s5 s6 c1 s4c5 s6 c4c6 0 0 0 1 0 0 0 1
s23 c4c5 s6 s4c6 c23 s5 s6 c3 0 s3 a3c3 c6 s6 0 0
c1 c23c4 s5 s23c5 s1s4 s5 3 s3 0 c3 a3 s3 5 s6 c6 0 0
A2 A6
a s1 c23c4 s5 s23c5 c1s4 s5 0 1 0 0 0 1 0 d6
s23c4 s5 c23c5 0 0 0 1 0 0 0 1
c1 d6 c23c4 s5 s23c5 s23 d 4 a3c23 a2 c2 s1 d6 s4 s5 d2

p s1 d 6 c23c4 s5 s23c5 s23d 4 a3c23 a2 c2 c1 d6 s4 s5 d 2
d 6 c23c5 s23c4 s5 c23d 4 a3 s23 a2 s2
12
Joint Space and Task Space
• The forward kinematic transform defines a mapping between

a given set of joint parameters and the end-effector position
and orientation.
1
q H n0 f (q)
n
• The independent parameters theta (actuated z-axes)

define the joint space (configuration space)
• The position and orientation of the end-effector is defined in
the task space (Cartesian space)
Manipulator Workspace
• A link can move only within the joint limits. The locus of all
points in the task space that are attainable within the joint
limits is called the manipulator workspace.
• The reachable workspace is the region that the origin of the

end-effector frame can reach with at least one orientation
• The dexterous workspace is the region the end-effector

origin can describe while attaining different orientations.
13
Example of Joint & Task Spaces
Joint space
Y1
Y0
X1
2
X0 Z1
1
Z0
Task space
More Examples
• Many possible ways how to construct kinematic chains using
prismatic (P) & revolute (R) joints
• Only few combinations are used in practice
• Most typical arrangements:
(considering the first 3 joints, i.e. robot arm / excl. wrist)
1. Articulated robot (RRR)
some literature calls it “the elbow manipulator”
2. Spherical robot (RRP)

3. SCARA robot (RRP)
4. Cylindrical robot (RPP)
5. Cartesian robot (PPP)
• Why important? The shape of the workspace dictates type of

applications for which the robot can be used
14
More Examples Articulated Robot - 1
• Work space of the
Articulated robot (RRR)
(axes of rotation z & x & x)
• e.g. ABB IRB1400 robot

(or the elbow manipulator)
• RRR configuration provides

largest freedom of
movements in
a compact space
More Examples Spherical Robot - 2
• Work space of the spherical robot (RRP)
(RRP with axes of rotation z & x)
• e.g. Stanford Arm
• The joint coordinates are
the spherical coordinates
of the end-effector relative
to the coordinate frame
of the shoulder joint
15
More Examples SCARA Robot - 3
• Tailored for table assembly operations
• Work space of the SCARA robot (RRP)
(axes of rotation z & z )
• Selective
Compliant
Articulated
Robot for
Assembly
More Examples Cylindrical Robot - 4
• Work space of the
Cylindrical robot (RPP)
• The joint variables are

the cylindrical coordinates
of the end-effector
with respect to the base
16
More Examples Cartesian Robot - 5
• Work space of the Cartesian robot (PPP)
• Simplest kinematic
description among
all considered robots
• The joint variables are
the Cartesian coordinates
of the end-effector
with respect to the base
The Product of Exponentials

Forward Kinematic with the Screw Theory
• Classical formulation of forward kinematics for open-chain robots is written
in terms of relative transformations for n adjacent joints
H n0 H10 H 21 ...H nn 1
f ( 1 , 2 , ... , n )
• Recall that the rigid motion associated with moving a rigid body along one
twist is given by ˆ
g ( ) e g (0)
• A description of the forwards kinematics with the screw theory makes
use of the fact that the motion generated by n individual joints
corresponds to n such twists written directly in spatial coordinates.
• Using substitution, we can get the Product Of Exponentials (POE)
ˆ ˆ ˆ
g st ( 1 , 2 , ... , n ) e 1 1e 2 2
... e n n
g st (0)
• Note that the resulting POE g st ( 1 , 2 , ... , n ) H n0 but not its
single elements e i i
H ii 1
. Why? (missing initial condition!)
17
POE Composition (1. “backwards”)
Consider a 2-DOF robot as example:
1. Fix the 1st joint and only move
the 2nd joint. This corresponds to
a simple rigid-body motion of the
tool frame about a twist associated
with the second joint:
ˆ
g st ( 2 ) e 2 2
g st (0)
2. Now, hold the 2nd joint fixed, and move the 1st joint.
This corresponds to a simple rigid-body motion of the tool frame

(the two links are a rigid body) about a twist associated with the first
joint ˆ ˆ ˆ
g st ( 1 , 2 ) e 1 1 g st ( 2 ) e 1 1e 2 2
g st (0)
(once composed, it doesn’t matter in what order we then move the joints later)

POE Composition (2. “forwards”)
The same idea as before:
Fixing 1 and moving 2 allowed us to consider the twist about a
constant axis
Does the POE representation depend on the order we move the
joints? (no - “Backwards & forwards” POE compositions give the same)
- Corresponds to moving 1 while 2 fixed

- However, this motion moved also the joint 2
- Thus, a similarity transformation describes

the motion about the new axis
- However, the composition gives
the same result as before
- In both cases, use pre-multiplication as
composition (resolve in the same fix frame)
- This can be generalized also for n>2 DoF
18
How do we proceed? Revolute & Prismatic joints
• To generalize to any open-chain (serial) manipulator:
– define the reference configuration as the case where =0, and gst(0)
as the transformation between the tool and spatial frame when =0
– For each joint, construct a twist i which corresponds to the screw
motion of the ith joint with all other joints fixed at j=0
• Recall: (where either i or vi is a unit vector)
1. For a revolute joint, the twist i has the form
i qi
i
i
where i is a unit vector in the direction of the rotation axis

and qi is any point on the axis
vi
i
2. For a prismatic joint 0
where vi is a unit vector pointing in the direction of translation

Summary
• The i ’s must be numbered sequentially starting from the robot base
• gst(0) gives the configuration of the tool frame independently of the
order in which the rotations and translations are actually performed
• This is an alternative formulation (e.g. to the D-H convention based
on the classical spatial description) of the forward kinematics known
as the product of exponentials:
ˆ ˆ ˆ
g st ( ) e 1 1e 2 2
e n n
g st (0)
• Advantage: it allows to compute the forward kinematics of the tool

frame by inspection - directly in spatial coordinates
• Drawback: it comes at the cost of reduced information about the

relative transformations from link to link
…of little value - in fact, we usually do not need them at all
19
Example 1:
SCARA Forward Kinematics (1 of 3)
• The SCARA robot has four joints
(3 revolute, 1 prismatic)
• Let =0 correspond to the fully extended
configuration, and attach the base and
tool frames as shown
• This gives the initial configuration
0
I l1 l2
g st (0)
l0
0 1
• translation between frames S to T
• can be further simplified
• To construct the twists for the if S=T then [0;0;0]
revolute joints, note that 0
1 2 3 0
1
Example 1:
• We can choose following axis points
(or any other point located at the z-axis, why?)
0 0 0
q1 0 q2 l1 q3 l1 l2
0 0 0
i qi
• This yields the twists: i
i
0 l1 l1 l2
0 0 0
0 0 0
1 2 3
0 0 0 0
0 0 0 0
1 1 1 v4 1
4
0 0
• The prismatic joint points in the z
direction and has the associated twist 0
0
20
Example 1:
• The forward kinematics map has the form
ˆ ˆ ˆ ˆ R( ) p( )
g st ( ) e 1 1 e 2 2 e 3 3 e 4 4 g st (0)
0 1
• The individual matrix exponentials are given by:
cos 1 sin 1 0 0 cos 2 sin 2 0 l1 sin 2
ˆ sin 1 cos 1 0 0 ˆ sin 2 cos 2 0 l1 (1 cos 2 )
e11 e 2 2
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
cos 3 sin 3 0 (l1 l2 ) sin 3 1 0 0 0

ˆ sin 3 cos 3 0 (l1 l2 )(1 cos 3 ) ˆ 0 1 0 0
e 3 3
e 4 4
0 0 1 0 0 0 1 4
0 0 0 1 0 0 0 1
Example 2: Elbow Manipulator

with spherical wrist (1 of 2)
• The mechanism consists of two
intersecting axes at the shoulder, an
elbow (RRR), and a spherical wrist
(RRR with three intersecting axes)
q
• 6xR kinematics
i i
i
i
• The reference configuration ( =0)

is fully extended, as figure shows
• The initial configuration is given by It can be any value here vs. it must be be l0 - Why?
0
I l1 l2 0 0 0 1 0 0
g st (0)
l0 0 0 0 0 0 l0
0 1 1 l0 0 0 l0 0
1 2
0 0 1 1
• The first 2 joints 0 0
0 0
have twists:
1 1 0 0
21
with spherical wrist (2 of 2)
• Note that we have used q1=(0,0,l0) for the first twist but we could just
have used the origin or any other point on the axis of the twist too
• The other 4 twists are 0 l1 l2 0 l0
calculated in a similar l0 0 l0 0
matter: l1 0 l1 l2 0
3 4 5 6
1 0 1 0
0 0 0 1
0 1 0 0
• The full forward
kinematics are
ˆ ˆ R( ) p( )
g st ( ) e 1 1 e 6 6
g st (0)
0 1

Forward Kinematics - Solution
22
Choice of the Base Frame and
the Reference Configuration – 2 Hints
• The product-of-exponentials formulations allows any choice of
base frame, and any choice of reference configuration
• So, they can be chosen in such a way that it further

simplifies our kinematic problem
1. Choosing the base frame to coincide with the

tool frame when =0 is physically very meaningful and
• It simplifies the forward kinematics since then gst(0) = I
2. The reference configuration (i.e. when =0; in practice, this is

based on our calibration) should be chosen such that the joint
twists are simple, i.e. they should lie along the principle axes
Example 3: SCARA Forward Kinematics

with Alternate Base Frame
• Consider the base frame coincident with the tool frame at =0
0 0 l1 l2 0 0 l2
0 l1 l2 0 0 l2 0
1 0 0 1 0 0 gst(0) = I
1 2
0 0 0 0
0 0 0 0
1 1 1 1
• Similar calculations give

0 0
0 0
0 1
3 4
0 0
0 0
1 0
23
Matlab Demo 3 (Demo_fwrd_kin.m)
• Case 1 Forward kinematics demo

SCARA robot
- as on slides earlier, i.e.
3 rev. joints + 1x prismatic 5
(gripper) 4
= 4 DoF
3
2
• Case 2
z
1
the same SCARA robot as 0
above
-1
- different positions of the 5
the base frame -2

-5 0
0
5 -5
y
x
SCARA robot – in home position
24
Mechatronics
Rigid Body Velocity

ETH Zurich
Main points covered in this lecture

(Concepts to remember)
• Linear & angular velocity
• Derivative of a rotation matrix R
• Skew symmetric matrices and groups O(3), so(3) / SO(3), se(3) / SE(3)
• What is spatial & body velocity

– in SO(3) - rotational velocity:
– in SE(3) - rigid body velocity: [v, ] = ’
• Approach based on twists - Velocity of screw motion

– Adjoint transformation
– How to get spatial and body velocities from g(R,p) and the twist
– Transformation between frames
1
Rigid Body Position & Orientation
(Review & prerequisites - 1)
• Important things to remember from previous lectures:
• There are alternative ways to represent mathematically

position of a rigid body:
1. Assign position vectors to all points

of the rigid body. Aren’t they too many ?
2. Use position of three co-linear points on the body

(and find a way how to reconstruct all the others out of them)
3. Attach a fix body frame to our rigid body and use

linear position (translation) of one point (frame origin)
in combination with the angular position (rotation/orientation)
• We chose to use 3rd approach:

• Mathematically most convenient
• In 3D, this results in 3 translational DoF and 3 rotational DoF,

i.e. 6 independent parameters
Rigid Body Position & Orientation

(Review & prerequisites - 2)
• Linear position (translation) of any point in 3D can
be represented by a vector r R3 (3 independent parameters)
• Angular position (orientation/rotation) in 3D requires

another 3 independent parameters and can be represented by:
– Rotation matrix (3 x 3 = 9 parameters but 6 constraints)

What constraints?
• All vectors orthogonal to each other (3) and scaled to unit length (3)
– Euler angles (3 parameters and composition sequence, i.e.
this is directly the minimal realization/description)
– Axis & angle (3 +1 = 4 parameters with 1 constraint – unit scaling of axis ‘k’)
• Screw theory – twists describe both translation & rotation

How many parameters can we expect?
• 6 parameters
• 1x axis ( ), 1x point (q), 1x angle ( ) & pitch (h) = 3+3+1+1 = 8 > 6, however,
• axis (only 2 since scaled) + point (only 2 since on the axis) + angle (1) + pitch (1) = 6
2
Translational (Linear) Velocity
• Points in space can only have a translational

velocity.
• A coordinate frame represents a rigid group of
Z1 points (i.e., a rigid body).
• When the body translates all points move
together. The velocity of the origin of the
coordinate frame represents this common
v1 translational velocity. We assign a translational
(linear) velocity vector v to the frame.
X1
1
v1P v1
Y1 P vP
• Well-known and simple !
Rotational (Angular) Velocity
• When the body rotates, all its points have

Z1 different velocities. However, we still can
1
assign to all of them (to the body frame)
only one quantity: rotational/angular velocity
vector .
– The direction of the angular velocity vector
represents the instantaneous axis of rotation of
X1
the body. Its magnitude is the rate of rotation
(rad/s).
Y1 P1 • The translational velocity of any point on the

vP rotating body is given by:
v1P P1 1
P1
• In general, a body can both rotate and translate:
v1P v1 1
P1
3
Transforming Velocity Vectors
Z1
Z0 Y1 0
R10 T10
X1 H 1
0 1x 3 1
P 1 v
P0 P0 H10 P1 R10 P1 T10

X0
v0 H10 v1 0
H10 1
v0 R10 v1 0
R10 1
Y0
• Velocity vectors have direction and magnitude, not position:
– Use only the rotation part of the transformation matrix on
velocity vectors
Complete Velocity of a Rigid Body
• Finding the velocity of a point q(t) moving through space is

easy: vq(t) = dq(t)/dt
• But we would like to describe the velocity of the entire rigid
body, given by the pose g(t)
• This velocity is 6-DOF (3 translation and 3 rotation), and it is
not as simple as taking directly the time derivative of g(t)
• The classical formulation typically chooses three Euler angles
to represent rotational velocity
• This leads to mathematical singularities (and non-unique
solutions)
• Using screw theory, it is possible to solve this problem without
singularities in an unambiguous way
4
Rotational Velocity
Few Important Facts
• First consider pure rotational motion
• Let Rab(t) be a curve (a rotation matrix) representing a trajectory
of an object in frame B, with the origin at the origin of frame A,
and rotating relative to the fixed frame A
• We call A the spatial (fixed, inertial, world) coordinate frame
• We call B the body coordinate frame
• Given a point q attached to the rigid body, the point
can be described in either of these 2 frames
through the relationship Za
qa (t ) Rab (t )qb Zb
q
• Note that the description of the point q with Yb
respect to the body frame does not vary
in time …hence:
Ya
Xa
the velocity of q in B is zero
but B is allowed to rotate Xb
(incl. its static point q) with respect to A
Rotational Velocity
The same equations again
• So, the velocity can be computed with respect to the spatial frame A:
qa (t ) Rab (t )qb d
vqa (t ) qa (t ) Rab (t )qb
dt
• This requires 9 numbers to describe the velocity of a point
• Let’s use the special structure of rotation matrices to develop a more
compact description
vqa (t ) Rab (t ) Rab1 (t ) Rab (t )qb
• It can be shown that, given any rotation matrix R(t)
1
R(t ) R 1 (t )
and R (t ) R (t )
are skew-symmetric and therefore
• This only requires 3 numbers to describe rotational velocity,

which is what we would expect for rotation about an axis
5
Rotational Velocity – New Terms
(Assigning names to equations)
• We can represent the velocity of a rotating body by a 3-vector
• We define the instantaneous spatial angular velocity, denoted ab
s, as
ˆ abs Rab (t ) Rab1 (t )

This corresponds to the instantaneous angular velocity of the object
as seen from the spatial coordinate frame
• We define the instantaneous body angular velocity, denoted abb, as
b
ˆ ab Rab1 (t ) Rab (t )
• The body angular velocity describes the angular velocity written in the
instantaneous body frame
• We also have the relationships (from the above two equations)
b
b
ˆ ab Rab1 ˆ abs Rab and ab Rab1 ab
s
Rotational Velocity
Why these names?
• Here’s where we get the names spatial and body velocity:
• We can express the velocity of a point in terms of the instantaneous
angular velocities
vqa (t ) ˆ abs Rab (t ) qb s

ab (t ) qa (t )
T b
vqb (t ) Rab (t )vqa (t ) ab (t ) qb
• Only the first expression corresponds with our physical understanding

of velocity of a point…
…nevertheless both above
equations are mathematically
equally true
6
Example: Rotational Motion of a
1-DOF Manipulator
• Let t) be the angle of rotation about some
reference configuration. The trajectory is
cos (t ) sin (t ) 0
R(t ) sin (t ) cos (t ) 0
0 0 1
• The spatial velocity is
sin cos 0 cos sin 0 0 0 0
s T s
ˆ RR cos sin 0 sin cos 0 0 0 0
0 0 0 0 0 1 0 0 0
• The body velocity is

0 0 0
b T b
ˆ R R 0 0 0
0 0 0
Rigid Body Velocity

The approach based on Twists
• Let gab(t) be the trajectory of a rigid body: the rigid motion of the
frame B relative to the inertial frame A (recall 2nd lecture)
Rab (t ) pab (t )
g ab (t )
0 1
• Where Rab(t) is the rotation of A wrt B and pab(t) is the translation of
A wrt B, both changing with time
• As with rotation, dgab(t)/dt is not really useful
• Similar to the case of rotational velocity, we will have two types of
rigid body velocity, that make use of two different twists: the body
velocity and the spatial velocity
7
Body Velocity
• The body velocity is specified with a twist (we apply exactly the same
approach to the twist as we did with pure rotations before )
(6x1)
T T (4x4) b T
R Rab R pab vab Rab pab
Vâbb g ab1 g ab ab ab
Vabb b T
0 0 ab Rab Rab
• The velocity of a point q in the body frame is given by
vqb Vâbb (t )qb b

ab
b
qb vab
• The action of the twist is to take a point in body coordinates and
return the velocity of that point (with respect to the spatial frame)
written in body coordinates, thus:
– vabb is the velocity of the origin of the body frame (relative to the spatial
frame) written in the body coordinates
– and abb is the angular velocity of the body frame, written in the body
coordinates
Spatial Velocity
• The spatial velocity is specified with a twist

(3x3) (3x1) (4x4) (6x1)
T T s T
Rab R Rab R pab pab v Rab R pab pab
Vâbs g ab g ab1 ab ab
Vabs ab
s
ab
T
0 0 ab Rab R ab
• The velocity of a point in the spatial frame is given by
vq a Vâbs (t )qa s
ab qa s
vab
• The action of this twist is to take a point of the body, but expressed in
spatial coordinates, and return the velocity of that point written in spatial
coordinates
– vabs (not very intuitive because of its more complex form) is not the
velocity of the origin of the body frame but the velocity of a (possibly
imaginary) point attached to the body frame and passing through the
origin of the spatial frame, written in spatial coordinates…
– abs is the instantaneous angular velocity of the body as viewed in the
spatial frame (intuitive)
8
The Adjoint
• The 6x6 matrix which transforms twists from one coordinate frame
to another is referred to as the adjoint transformation associated with
g, written Adg ( 4 x 4) (6 x 6)
R p R pˆ R
g , Ad g
0 1 0 R
• The adjoint is invertible. The inverse of the adjoint is the adjoint of the
inverse: T T T T T
R ( R p )^ R R R pˆ
Ad g 1 T
Ad g 1
0 R 0 RT
Remark: Twist transformations
If ˆ is a twist (matrix 4x4) with twist coordinates (vector 6x1) then for
1
any g , ˆ ' g ˆ g is the transformed twist (4x4) with new twist coordinates
‘ Adg ( thus, Adg corresponds to matrix similarity transformation).
Coordinate-free Description
How to get the twist from g(R,p)?
• We can define rigid body velocity without explicit reference

to any coordinate frame. We define the spatial velocity as
vs RR T p p
Vˆ s gg 1
Vs s T
( RR )
and the body velocity as
vb RT p
Vˆ b g 1g Vb b
( RT R)
• They are related by the adjoint:
Vs Ad gV b
9
Example: 1-DOF Manipulator
• The configuration of B relative to A is

cos (t ) sin (t ) 0 l2 sin (t )
sin (t ) cos (t ) 0 l1 l2 cos (t )
g (t )
0 0 1 l0
0 0 0 1
• The spatial velocity of the rigid body is
l1 0
s vs vs RRT p p
V s s
v s
0 s
0
( RR T )
0
• The body velocity is
l2 0
b vb vb RT p vb 0 b
0
V b b
( RT R) 0
• Note that vs is velocity of a point attached to the rigid body as it traveles through the origin of frame A
• The body velocity is the velocity of the origin B as seen in frame B
Velocity of a Screw Motion
• Consider a general case where

ˆ
g ab ( ) e g ab (0)
represents configuration of coordinate frame B relative to frame A.
• Using the fact that for a constant twist
d ˆ ˆ eˆ
e
dt
the spatial velocity for this rigid body motion is
Vâbs g ab ( ) g ab1 ( ) ˆ e ˆ g (0) g 1 (0)e ˆ

ˆ
ab ab
• Thus, the spatial velocity corresponding to this motion is precisely

the velocity generated by the screw
10
Coordinate Transformations
• Consider the motion of three coordinate frames, A, B, and C.

The following relation exists between their spatial velocities:
Vacs Vabs Ad g ab Vbcs

• The following relation exists between their body velocities:
Vacb Ad g 1Vabb Vbcb
bc
• If A and B are two inertial frames which are fixed relative to each
other, then:
Vacs Ad g ab Vbcs Vacb Vbcb
Example: 2-DOF Manipulator
• We wish to find the velocity of frame C relative to frame A. Since

each joint motion is a screw, we can directly write:
0 0 l1 2 0
vab s
vbc
V s
ab vab 0 ab 0 V
bc vbc 0 bc 0
bc
ab
0 0 2
AB v-fixed – can only rotate! 1
• We also calculate 0 • The velocity consists of two

Ad g ab Rab 0 Rab components, and they add
l0 together linearly through
0 Rab adjoint as 0 l cos 1 1
0 l1 sin 1
s s s 0 0
V ac Vab Ad g ab V
bc 1 2
0 0
0 0
1 1
11
Appendix
(Rigid Body Velocity)
Rigid Body Orientation (Rotations – R)
There is no representation found to be superior to all the others.

All of them have pros & cons - for example:
– Any sequence of Euler angles have1x singularity

– In Axis-angle representation, axis not defined for = k
If one parameterization is available we can obtain all others ! (useful idea)
1. The length of a vector r is preserved under rotation:
2. Proper rotation preserves also orientation of space:

det(R) = 1
(i.e. handedness or cross-product)
– Otherwise, how could we do the composition of rotations?
– Recall, we selected frame orientation according to the “right-hand” rule
Therefore, rotations are rigid body transformations!

12
Skew Symmetric Matrices S = skew( ) so(3)
Generate Rotations R( ) = e(S ) SO(3)
• Our key tool will become a skew symmetric matrix. Why?
• We are going to talk about relative velocities between frames and it will involve
rotations and derivatives of rotations.
• Note: it is possible to simplify many calculations by introducing the notion of a
skew-symmetric matrix which encodes the axis of rotation, and its exponential
generates a rotation. :
or S = - ST
then equations above are equivalent to 9 equations:
From the equation above we see:
Groups SE(3), O(3), SO(3)

Recall H(R,p)
• O(3) – Orthogonal Group
– All matrixes fulfilling:
RT R = I or RT = R-1
– If R O(3) then det(R) = 1
• Why? Since det(R)=det(RT) then from above equation see that det(R)2=1
– Proper & improper rotations

– 2 eig. vectors (values) complex and the remaining real 1 is the axis of rotation
• Recall: axis defined only for k where k any integer (otherwise, 3 real eigenvectors)
• SO(3) – Special Orthogonal Group

– Subgroup of O(3) with det(R) = 1 (i.e. proper rotations only)
– Preserve not only length but also orientation (important e.g. for composition of rotations)
– Examples:
- Exponential of a skew-symmetric matrix, i.e. if S so(3) then eS = R SO(3).
- Basic & composed rotations etc.
13
How to get derivative of rotation R
• RR 1 R 1 R I
For any invertible matrix R, we can write
R 1 R T and we can write
Since our R matrix is a rotation then
T T
RR R R I
Now, we differentiate the above equation and we get RR T RR T 0
which can also be rewritten as
RR T ( RR T ) T 0
Recall the definition of a skew symmetric matrix now: S ST
Thus, we can write S ST 0 (which is exactly what we had above).
Therefore, we can write S RR T or R SR and since S encodes
the axis of rotation , we can also write R SR S ( )R R .
• Thus, derivative of rotation matrix R corresponds simply to R itself

pre-multiplied by its axis of rotation encoded in the skew-symmetric S( ) .
Derivative of a Rotation Matrix

(Example)
• Consider the principal rotation matrix Rz , i.e. assume that a rigid body is
rotating with constant angular velocity =[0 0 z]T = z[0 0 1]T
1. Direct differentiation leads to:

(this is how one can derive it analytically)
Remark: For a computer implementation (without

a symbolic toolbox), we prefer solution shown
below it is correct, generally valid
and can be obtained through a simple
matrix multiplication
2. Using previously derived equations (axis of rotation encoded as a skew

symmetric matrix S post-multiplied by the rotation matrix R itself), we get:
Corresponding Matlab code:

1. R_dot = diff(R,t) % symbolic toolbox required
vs.
2. R_dot = skew( ) * R % linear algebra pure
14
Differential Translations and Rotations
• Translational velocity vector represents the differential

amount of motion along the x,y,z axes during an infinitely
small time… 1 0 0 P x
0 1 0 Py
v Px Py Pz t H tt t
0 0 1 Pz
0 0 0 1
• Rotational velocity vector represents the differential amount
of rotation about the x, y, z axes (or an axis of rotation )
during an infinitely small time.
x y z t
H tt t Rot (' x ', x ) Rot (' y ', y ) Rot (' z ', z )
Differential Rotations
(Note: order is not important)
1 0 0 0 1 0 0
y
0 Rot (' x ', x)

0 1 x 0
Rot (' y ', )
0 1 0 0
0 x 1 0 y
0 1 0
y
cos( ) 1 0 0 0 1 0 0 0 1
sin( ) 1 z 0 0
z 1 0 0
1* 0 Rot (' z ', z )
2 0 0 1 0
0 0 0 1
Here is a MATLAB symbolic toolbox
program to test this
syms dx dy dz
%differential rotation matrices
1 z y 0 Rdx=[1 0 0 0 ; 0 1 -dx 0 ; 0 dx 1 0 ; 0 0 0 1]
Rdy=[1 0 dy 0 ; 0 1 0 0 ; -dy 0 1 0 ; 0 0 0 1]
Rdz=[1 -dz 0 0 ; dz 1 0 0 ; 0 0 1 0 ; 0 0 0 1]
z 1 x 0 %different combinations
H tt t
Rdx*Rdy*Rdz
y x 1 0 Rdx*Rdz*Rdy
Rdy*Rdx*Rdz
Rdy*Rdz*Rdx
0 0 0 1 Rdz*Rdx*Rdy
Rdz*Rdy*Rdx
15
Mechatronics
Jacobian

ETH Zurich
Lecture outline
1. Introduction
2. How to get the Jacobian?

a. (SD 1) Classical approach based on homogeneous transformations
Forward kinematics formulation using DH convention
b. (SD 2) Screw Theory
Using twists and product of exponentials
3. What is the Jacobian good for in robotics ?
1
The Jacobian Matrix in Robotics
What is it good for?
• Mathematically, the forward kinematics defines a function between the
cartesian space (all positions and orientations of the end-effector) and the
joint space (set of all joint angles) – without regard to forces which cause it
• The Jacobian of above function determines the velocity relationships.
• The Jacobian is one of the most important quantities in the analysis and
synthesis of robots in motion.
– It arises in many aspects of robotics, for example:
• in the transformation of forces and torques from
the end-effector to the manipulator joints
• in the determination of singular configurations
(conf. in which J loses rank)
• in the derivation of the dynamic equations of motion
• in the planning and execution of smooth trajectories

What do we mean? Systematically – Dimension 6 x N
• In robotics we are often interested

Y2 in the velocity of the end-effector
X2 (tool) frame, expressed in the
base (spatial) frame.
Y1 Z2
1
v0
Y0
X1 0
J 6 xN
N
X0
2 • It is an instantaneous relationship
Z1
1 • Jacobian matrix varies over time as a function of
Z0 the current configuration
(vector ) of the robot:
J 6 xN f ( 1, , N )
2
Example 1: 2-DoF manipulator in 2D
“Direct” approach to the Jacobian…
a1 a2
a1 a2
(2 x 2)
Y
a1 a2 a2
X
a1 a2 a2
a2 Z
Y1
Y0
X1
a1
X0
Observe: The dimension - not the expected matrix 6 x 2 2
For example: Where is J ? -- the part of J related to Z1
the rotational velocity is missing completely here) 1
Z0
Note: In this lecture, we want to investigate how to
1. Derive the complete Jacobian matrix (6 x N) systematically
2. Use what we learned – (a) Homegenous transformations and (b) Twists

Structure of J in 3D space
• We can divide the Jacobian into 2 parts 3XN as the
translational and rotational Jacobians.
1
0 0
v0 Jv
Px Px
...
1 N
0
J
Py0 Py0 N
...
1 N
• Each column of the Jacobian describes the
Pz0 Pz0 contribution of one joint to the final end-effector
... 1
v0 1 N velocity at a given configuration.
0 0 0
x x
N • We can “build” the Jacobian column-by-column
1 N
0 0 considering each joint.
y y
1 N • We must be careful that each contribution must be

0
z
0
z
expressed in a common coordinate frame so that
1 N
we can add them all together
J6 xN • Usually, we use base frame as the common frame
3
Lecture outline
1. Introduction

The Jacobian and the Z axis for DH
• We can easily find the Z axis of any coordinate frame expressed in

the base frame from the forward kinematics (recall the A matrices).
• For the first joint, the Z axis is Z0 which is already expressed in the
base frame Z0 0 0 1
T
• For the other joints find the forward kinematic transform up to that
joint. Y X 2 0 0
R1 T1
2
H10 A10
Y1 Z2
0 1x 3 1
Y0
X1
X 10 Y10 Z10 T10
0 0 0 1
X0
Z1 2
H 20 A10 A21
Z0 1 H i0 A10 A21 Aii 1
4
The Jacobian using DH Convention
Prismatic Joints – Simple!
• A prismatic joint translates the coordinate frames after itself along its axis of
motion. There is no angular velocity induced at the end-effector frame.
Therefore, the last three rows of the Jacobian column for a prismatic joint
are zero.
Xi0 Yi0 Zi0 Ti0

v0 J v ,3 1 Hi0 AA
1 2 Ai
di 0 0 0 1
d
0
03 1
• By the D-H convention, the axis of motion for joint (coordinate frame) i is
the axis Zi-1. The magnitude of the velocity is equal to the speed of the joint.
– Therefore, the first three rows of the Jacobian column for a prismatic joint
represent the unit vector along the direction of motion, Zi-1.
– However, this vector must be expressed
in the base coordinate frame. v0 Z i0 1
0
di
03 1
The Jacobian for a Revolute Joint

Part 1: Translational Velocity
• Each revolute joint induces a translational velocity
(unless the axis of rotation is passing through the origin of the tool frame)
v0 0
TN0 ( i 1)
Z3
Y2 X2 0
T Y3
i Z i0 1 TN0 ( i 1) Z i0 1 TN0 ( i 1) i
3
3 2
T30 0
Jv X3
Y1 Z2
0
T3 1
Y0
X1
X0 2
Z1
Z0 1
5
Part 1: Translational Velocity
• Again, we find the vector T between the origin of the joint’s frame
and the origin of the end-effector frame from the forward kinematic
transform and known structure of H
X i0 Yi 0 Z i0 Ti 0
v0 Zi0 1 TN0 ( i 1) i H i0 A1 A2 Ai
0 0 0 1
Jv
X N0 YN0 Z N0 TN 0
H N0 A1 A2 Ai AN
0 0 0 1
v0 Z i0 1 (TN0 Ti 01 ) i
Jv

Part 2: Rotational Velocity – Simple!
• Each revolute joint also induces a

rotational velocity about its axis of rotation:
0
Z i0 1 i
We only need to write the axis in the spatial coordinates

– we use the z-axis of the correct HT again:
X i0 Yi 0 Z i0 Tii00
H i0 A1 A2 Ai
0 0 0 1
6
The Jacobian using D-H Convention
Summary
• Putting both parts the angular (J ) and the linear (Jv) Jacobians
together yields the complete Jacobian J , a matrix :
(6 x N)
J J1 ... J i ... J N
where N is the number of joints (columns of Ji)
and all single columns Ji are given by:
(6 x 1) (6 x 1)
v0 Z i0 1 (TN0 Ti 01 ) v0 Z i0 1
J0 i i J0i di
Z i0 1 or 03 1
(for a revolute joint) (for a prismatic joint)

Example 2: Two-Link Planar Manipulator 1/3
Forward Kinematics:
z10 T10
Y2 X2
T20
Z2
a2
Y0 Y1
T2 0 a1 X1
T10
X0
T0 = [0;0;0] Z1 2
Z0 1 z0 = [0;0;1] (always when using D-H convention)
7
• Both (2 joints) are revolute, therefore the Jacobian matrix
will be (6x2) of the form:
z0 T20 T0 z10 T20 T10

J
z0 z10
0 0
• The 1st joint is (always) a rotation about z0 0 at (spatial) origin T0 0
1 0
0 a1c1
• The 2nd joint is a rotation about z10 0 at T10 a1 s1
(both in terms of spatial coordinates 1 0
taken from A1) a1c1 a2 c12
0
• The end-effector position is in spatial coordinates at T 2 a1 s1 a2 s12
(taken from A1 A2 where c12 = cos( 1+ 2) etc.) 0

• Performing all required calculations gives:
a1s1 a2 s12 a2 s12
a1c1 a2c12 a2c12
z0 T20 T0 z10 T20 T10 0 0
J
z0 z10 0 0
0 0
1 1
• The first 2 rows give exactly the same 2 x 2 Jacobian as derived
“directly” (recall Example1) it is the linear velocity of the
origin of frame 2 (T20) written relative to the (base) frame 0.
• The 3rd row is the linear velocity in the direction of the z-axis,
always zero in this case (of course, planar manipulator in x, y)
• The last 3 rows give the angular velocity of the tool frame, which is
simply a rotation about the z-axis at the rate d 1/dt + d 2/dt.
8
Lecture outline
1. Introduction

The Jacobian with a Twist
• Traditionally, one describes the Jacobian for a manipulator by

differentiating the forward kinematics map
• This works if the forward kinematics is a mapping g : n p
• In this case, the Jacobian is a linear map g/ : n p
• However, if we represent the forward kinematics more completely

as g : n SE(3) – i.e. both rotations & translations together – the
Jacobian is not so easy to obtain (since g is a matrix-valued
function; recall the terms of Ai)
• One could always choose coordinates for SE(3), but the description would
only hold locally (it would have representational singularities)
• Choosing local coordinates destroys the natural geometric structure of rigid
body motions
• The product-of-exponentials formulation leads to a very natural

description of the Jacobian, without any mathematical singularities
9
The Spatial Manipulator Jacobian
• The spatial velocity of the end-effector (t means “task frame” and

s means “spatial frame” here) is given by the twist
Vˆsts g st ( ) g st1 ( )
• Applying the chain-rule
n n
g st g st
Vˆsts i g st1 ( ) g st1 ( ) i
i 1 i i 1 i
• The end-effector velocity is linearly related to the velocity of the

individual joints
• In twist coordinates
g st g st
Vsts J sts ( ) J sts ( ) g st1 g st1
1 n
• We call Jsts( ) the spatial manipulator Jacobian

• Recalling the product-of-exponentials formula

ˆ ˆ
g st ( ) e 1 1 e n n
g st (0)
• After “some manipulation”, we compute
g st
g st1 Ad e ˆ1 1 e
ˆ
i 1i 1 i
i
• The spatial manipulator Jacobian becomes

J sts ( ) 1 2 n
i Ad e ˆ1 1 e
ˆ
i 1i 1 i
• The ith column of the spatial Jacobian is the ith joint twist transformed
to the current manipulator configuration
• This means we can calculate the Jacobian by inspection
10
“After some manupulation …”
The Body Manipulator Jacobian
• It’s also possible to define a body manipulator Jacobian

Vstb J stb ( )
• Calculated similar to before

J stb ( ) 1 n 1 n
1
i Ad ˆ ˆ i
e i i e n n g st ( 0 )
• The columns correspond to the joint twists written with respect to the
tool frame at the current configuration
• The spatial and body manipulator Jacobians are related by the adjoint
J sts ( ) Ad g st ( ) J stb ( )
11
Example: Jacobian for a SCARA Robot
• Consider a SCARA robot at an arbitrary configuration . For the

SCARA, the directions of the twists (all vectors ‘omega’) are fixed
and only the points q’i through which the axes of the twists pass are
functions of . By inspection:
0 l1 sin 1 l1 sin 1 l2 sin( 1 2 )
q1 0 q2 l1 cos 1 q3 l1 cos 1 l2 cos( 1 2 )
0 0 0
are points on the axes.
• Calculating the twists gives
0 l1 cos 1 l1 cos 1 l2 cos( 1 2 ) 0
0 l1 sin 1 l1 sin 1 l2 sin( 1 2) 0
0 0 0 1
J sts
0 0 0 0
0 0 0 0
1 1 1 0
Example 3:
Jacobian for a SCARA Robot (solution)
Part 1/3: Solve the forward kinematics problem
12
Example 3: Jacobian for a SCARA Robot (solution)
Part 2/3: Approach 1- Transform all twists from the reference configuration to
the current robot configuration…
- you have to do this in order to make the impact of all joints “visible”
- don’t say “we transform all twists to the base frame”
because all has been done directly in the base frame…
Example 3:
Jacobian for a SCARA Robot (solution)
Part 3/3: Approach 2- based on adjoints
13
Example: Jacobian for the Stanford
Arm (1 of 3)
• The Stanford arm is a 6-DOF robot with two revolute joints at the
base, a prismatic joint, and a spherical wrist
• We will compute the spatial manipulator Jacobian
• The first 2 joints pass through the point q1 = q2 = (0,0,l0) and point in
the direction 1 = (0,0,1) and ’2 = (-cos 1,-sin 1,0). This gives the joint
In Matlab:
twists syms th1 real
w1=[ 0 0 1]'
0 l0 sin 1 w2=[-1 0 0]'
0 l0 cos 1 w2_=expm(skew(w1)*th1)*w2
1 q1 0 2 q1 0
1 2
1 0 2 cos 1
0 sin 1
1 0

Arm (2 of 3)
• The 3rd joint is prismatic and hence we care only about its direction.
Taking into account the change in orientation due to the first two
joints, we have
sin 1 cos 2
0 cos 1 cos 2
e zˆ 1 e xˆ 2
1 sin 2 v3
3
0 0 0
0 0
0
• We next compute the twists corresponding to the wrist. The wrist is

located at the point
0 0 (l1 3 ) sin 1 cos 2
xˆ
qw 0 e zˆ 1 e 2
l1 3 (l1 3 ) cos 1 cos 2
l0 0 l0 (l1 3 ) sin 2
14
Arm (3 of 3)
• The direction of the wrist axes depend on 1 and 2 as well as the
preceding wrist axes. These are given by
0 s1s2
xˆ
4 e zˆ 1 e 2
0 c1s2
1 c2
1 c1c4 s1c2 s4
xˆ
5 e zˆ 1 e 2
e zˆ 4
0 s1c4 c1c2 s4
0 s2 s4
0 c5 ( s1c2 c4 c1s4 ) s1s2 s5
zˆ xˆ zˆ xˆ
6 e e 1 2
e e 4 5
1 c5 (c1c2 c4 s1s4 ) c1s2 s5
0 s2 c4 c5 c2 s5
• We can now write the complete Jacobian:

0 2 q1 v3 4 qw 5 qw 6 qw
J sts
1 2 0 4 5 6
Lecture outline
1. Introduction

15
Static Force-Torque Relationship
What is the Jacobian good for?
• Robots do not always move through space sometimes they also have to
apply a static force/torque (wrench) to another object
• In this case, the reaction forces are balanced by the joint forces/torques.
• Note that these two (force of end-effector applied to another object and
the joint torques) are related through the Jacobian matrix.
Fx
Fy
FR F FR F Fz
FR
x
Y0 y
X0
Z1 2 Remark:
Force is related to a translational movement
Z0 1
Torque is related to a rotational movement
Static Force-Torque Relationship

Why?
• We find this relationship using the virtual work principle.

• An infinitesimal (differential) motion x of the tip is caused by an
infinitesimal (differential) motion of the joints θ. The work done is
equal to the vector product of the force/torque and linear/angular
displacement vectors.
Wtip FRT x FRT J Wtip Wjoints

J T FR
Wjoints T
FRT J T
( FRT J )T ( T T
)
(recall the dot product of two column vectors A and B)

AN 1 , BN 1 : A B AT B ( AB)T BT AT
16
Inverse Velocity Kinematics
(Jacobian Inverse)
• When controlling the path of a robot we are usually interested in the
inverse Jacobian relationship:
“At what speed should the joints rotate for the tool frame to move at a
certain direction and velocity? ”
1 v0
θ J 0
• To invert the Jacobian J , it must be square and full rank.

– However, A redundant 3D manipulator, for example, has more than six
joints and, in general, there isn’t a unique solution.
– Even with a square Jacobian (holonomic robot – 6 DoF) the matrix
may not always be invertible. Such configurations of a robot where the
Jacobian is not invertible are called kinematic singularities
(or “mechanical singularities,” or just “singularities”).
Kinematic Singularities
• Remember the Jacobian represents the relationship between a differential

motion of the joints and the corresponding motion of the end-effector.
– It is a mapping from the joint space to the task space.
x J θ
• At a singularity condition, there is a direction in the task space that cannot be
obtained by (is not mapped to) any vector in the joint space
1. When the robot is at the boundary of its workspace it cannot move in a direction
away from the boundary. (These are called workspace boundary singularities)
2. Even inside the workspace singularities occur, for example, when two links line up.
In this case they do not cause independent motion and a degree of freedom is lost.
Cannot move this way
17
Kinematic Singularities
• At a singular configuration, attaining motion along the singular

direction requires infinite joint speeds. (sort of a “division by zero”)
• This condition is not instantaneous but continuously approached as

the robot gets close to a singular configuration.
• We would like to have a measure of a robot’s closeness to

singularities. This is useful, for example, for path planning where we
want to avoid singularities.
Manipulability
Robot Manipulability
Y
X
a2 Z
Y1 a1 a2 a2
Y0 a1 a2 a2
X1
a1
What tip velocities can be attained by the combination of
X0 joint speeds such that:
2 2
Z1
2
θ 1 2 1
1
Z0
2 Ytip Manipulability ellipsoid.
1 the circle becomes through the Jacobian
J X tip
1 1
1 X
J
2 Y Tip
18
Singular Value Decomposition
• Any m x n matrix J can be decomposed into

T
Jm n Um m V
m n n n U , V are orthonormal
• contains the r singular values of J along its diagonal such that

1 2 r 0 Singular values are non negative numbers !
• For the typical cases in robotics where n m (i.e., the degrees of

mobility of the robot is equal to or larger than the dimension of the
task space) the matrix looks like this
1 0 0 0 0 0 • r is the rank of J
0 2 0 0 0 0 • (n-r) is the nullspace
0 0 0 0 0
m n
0 0 0 0 0
n m r
r
0 0 0 0 0 0
• First r columns of U span the column space of J while first r columns

of V span the row space of J
• We can rewrite the matrix keeping only the first r columns of U and V
that get multiplied by non-zero singular values. This is called
“economy size” SVD.
T
Jm n Um r V
r r n r u1 1v1T u2 vT
2 2 ur v T
r r
• Notice how each one of these terms

U u1 u2 ur “works” on an “incoming” vector
V v1 v2 vr X J mxn nx1
T T T
u 1 1 1 v u2 v
2 2 ur v
r r
"Project onto v1 (inner product), then scale by 1 then multiply u1 by this value..."
19
X Jm n n 1 u1 1v1T u2 T
v
2 2 ur v T
r r
1 2 r 0
• Since the columns of U and V are orthogonal and unit length, the largest
magnitude output X is obtained by an input along v1. The output is then
along u1 with magnitude 1. u1 is the “best” direction to move the robot along
(i.e., the direction with the largest manipulability of the robot). Direction of ur,
in contrast, requires the most effort from the joint motors.
• The singular values and the corresponding columns of U can be thought of
as the axes of a hyper-ellipsoid in m dimensional task space
(i.e., 6 for most articulated robots).
– The ratio of smallest to largest singular value (i.e., the condition number of the
matrix J) is a measure of the “skewness” of this ellipsoid (or if you want how much
it differs from a perfect circle)
– The volume of the ellipsoid is proportional to the product of all singular values
Robot Manipulability
• Manipulability is defined by where are the

singular values of the manipulator Jacobian (manipulatibility
characterizes the range of possible end-effector velocities for a given configuration theta)
Manipulability measure W
W ~ Volume of Manipulability
Ellipsoid
Video frames
• These measures are used for path/task planning and the optimal
design of robot geometry.
20
Lecture “the Jacobian”
Appendix

Matrix Pseudo-inverse (Moore-Penrose)
• How to use SVD for a robust matrix pseudo-inverse ? (Demo_SVD2.m)
[U,S,V'] = svd(M)
% where
% M - any matrix whatsoever, Mx = b / multiply by MT from left
% S - diagonal matrix with sorted positive singular values MTMx = MTb
% U,V - orthogonal matrices (even orthonormal since factors taken into S)
x = (MTM)-1MTb
%% Goal: Robust calculation of the inverse (the part: inv(M‘*M) using SVD factors of M)
%
% % Recall that
% 1) V'V=I since V orthonormal by definition of the SVD factorization,
Result/Summary:
% inv(V)=V' and note that V' is much simpler to do than inv(V) !! 1. Inv(S2) is very simple to calculate with SVD
% 2) U’U=I same as above since S2 is diagonal (we only have to square
% 3) S2=S’*S is a square diagonal matrix with squared elements of S
and invert r-diagonal elements of S2,
% therefore:
% inv(M’*M) = inv((USV’)’USV’) =
i.e. 1/(sii*sii)
% = inv(VS’U’USV’) =
% = V*inv(S2)*V’ where S2=S’*S 2. Hence, using SVD factorization, a matrix
%
inversion (in general, a complex problem,
% the Moore-Penrose pseudo-inverse is then given by
% inv(M’M)M’=V*inv(S’*S)V’*VS’U’= V*inv(S)’*U’
numerically not always very well conditioned)
becomes a weighted product of an orthonormal
3. inv(S)’ = take reciprocal of all non-zero
matrix V with its transposition V‘
diagonal elements, leave zeros in place
and transpose the whole matrix
21
SVD-based Computation of
the Null Space of J
Jx=0
• Recall:
- Any m x n matrix J can be decomposed into
T
Jm n Um m V
m n n n U , V are orthonormal
1 0 0 0 0 0
where the diagonal matrix (same dimension as J) 0 0 0 0 0
2
contains r singular values of J such that
m n 0 0 0 0 0
n m r 1 2 r 0 0 0 0 r 0 0
0 0 0 0 0 0
- matrix V= [ ] (the “bigger” one) • This part of “makes sure” that

contains column-wise the null space of J Jx = (U V)x = 0
given by vectors/columns for i > r • The orthonormal basis of this subspace

(corresponds to singular values equal zero) (called null space) of J is directly
encoded in “the bigger” orthonormal
matrix, i.e. in V
The Analytical Jacobian
• The J discussed so far is often called the Geometric Jacobian

• Some literature defines also the Analytical Jacobian (denoted as Ja)
which is based on a minimal representation
for the orientation of the end-effector
• If X is the end-effector pose, then = [ , , ] should denote the minimal

representation for the orientation and d the position as usual
• The minimal representation for the orientation is given by Euler angles,
e.g. using S ( ) RR T we get as
• Combining gives the relationship for both Jacobians:
22
Decoupling of Singularities
(Simplification - union of arm J11 and wrist singularities J22 obtained independently)
• Manipulators with a spherical wrist (last 3 joints are revolute with

coinciding axes of rotation) allow to decouple (simplification) the
determination of singular problems for the wrist and for the arm
• In practice, the arm typically consists of 3 joints, hence
arm + wrist = 6 DOF robot where J is a square 6x6 matrix which is
singular if and only if det J = 0
• The above Jacobian can be partitioned for Position
(arm) and orientation (wrist) as follows:
• Since the last 3 joints are revolute
and the axes intersect at a common point: o = o3 = o4 = o5 = o6
then JO becomes
and we can write with
Linear Mapping
23
Perpendicular Subspaces
Linear mapping:
Inverse Mapping
24
Mechatronics
Inverse Kinematics

ETH Zurich
Inverse Kinematics
• “Given where I want to be what should the joint angles be?

What should be the speed of actuators to attain a certain
velocity at the tip?”
Forward Kinematics
g st ( ) H N0 ( )
Robot sensors
Position + Orientation (Pose)
(joint space) of the robot in Cartesian space
Inverse Kinematics
1
Introduction
• Varying angles (joint variables) yields an indefinite number

of robot configurations.
• Recall the solution to the forward kinematics problem,(for a

given set of angles ) the pose of the robot is gst( ) or HN0( ).
• The solution to the more difficult inverse kinematics problem is

to find the joint angles
g st ( ) gd H N0 ( )
for a given/desired configuration gd of the robot in terms of

the pose of its end-effector.
This is a complex problem! Why?
Example: Stanford arm + spherical wrist

(Solving the Forward Transform)
r11 r12 r13 Tx

Link: i di ai i
*
r21 r22 r23 Ty
1 d1 0 -90
1
H N0 A1 A2 AN r31 r32 r33 Tz
*
2 2 d2 0 +90
RN0
3 0 d*3 0 0 0 1
*
4 4 0 0 -90
*
5 5 0 0 +90
6 *
6 d06 0 0
Ai RotZ ( i )TransZ (di )Trans X (ai ) Rot X ( i )
12 equations
(not independent)
6 unknowns in 2
trigonometric functions
x0
2
Challenges / Overview
Forward kinematic equations are non-linear (coupled, algebraic), thus:
1. There might be no analytic solution for the

inverse kinematics problem, or
2. the solution does not have to be unique (if it exists at all)

(multiple solutions are very common)
3. Traditionally, inverse kinematics solutions are separated

into 2 classes:
a. Closed-form solutions (analytical solution exists)
fast, efficient preferred way of getting joint angles for desired configuration in real-time
b. Numerical solutions (via iterative non-lin. programming techniques)
Remark: Some kinematic chains (e.g. those with a spherical wrist) allow to find a closed-form
solution – applies for most industrial robots (usually based on decomposition into many sub-problems)

(Inverse kinematics)
• Suppose that the desired position and orientation of the final frame are given
by:
H N0 ( 1 , 2 , d3 , 4 , 5, 6 )
• To find the corresponding

joint variables:
1, 2, d3, 4, 5 , 6
we must solve the following

set of 12 nonlinear equations:
3
(Numerical solution)
• One solution to this set of equations is given by:

(for d1 = 0, d2 = 0.154 and d6 = 0.263)
+ any other solution satisfying 4+ 6= can be easily obtained

(see the slide with robot and D-H table for more details)
• Even though you have not seen how this solution was derived,
it is not difficult to verify that it satisfies the forward kinematics
equations for the Stanford arm!
Overview - Inverse kinematics

What have we seen so far
• The inverse kinematics equations in this example were too difficult to solve directly
in close form – it is the case for most robots.
• Note, there are few efficient techniques which allow us to exploit several particular
kinematic structures (see next slides)
• In general, we can say:

1. Forward kinematics (FK) problem has always a unique solution that can be
obtained simply by evaluating the forward equations
2. Inverse kinematics (IK) problem may or may not have a solution
3. Even if a solution exists, it may or may not be unique
4. Because the FK equations are in general complicated non-linear functions
(of joint variables), the solution(s) may be difficult to obtain even when it exists
– Solving the inverse kinematics problem, we are interested in finding

a closed form (explicit solution) rather than a numerical one
Why? Real-time, computational time, convergence problems etc.
– Finding a (closed form) solution means to get this explicit relationship:
4
Known Problems & Solutions
1. The same variable encoded in several transcendental functions

(not possible to express/replace them by a finite number of algebraic
operations to solve our problem - note that all trigonometric functions
belong to this class)
2. cos(+ ) = cos(– )
3. sqrt(x) = ( )
1. Atan(y/x) – leads to a loss of information Atan2(y,x)

2. Sometimes you can find an algebraic solution and sometimes there is
3. Geometric solution – decomposition of the spatial 3D geometry into
several plane (2D geometry) problems
Solving the Forward Transform

(Try to find an algebraic solution)
• There is in general no “easy way” to go
back (perform the inverse transform).
H N0 A1 A2 A3 AN
A1 1 H N0 A2 A3 AN • Useful trick: Rewrite the forward
transform matrices in a different order
A2 1 A1 1 H N0 A3 AN and look for simple/suitable terms which
you can solve (algebraic approach).
AN1 1 A3 1 A2 1 A1 1 H N0 AN • Each of these give 12 (R 3x3 + T 3)

equations. We usually search for
expressions that can be combined (add,
R10 T10 divide, trigonometric identities, etc.) to
H10
013 1 contain only one of the joint variables.
4 4
0 T T
0 1 R1 R10 T10
H 1
013 1
4 4
5
Decoupled Wrist Kinematics –
Spherical wrist
• Spherical wrist: 3 revolute joints,
the axes of the final 3 joints pass
through the same point (i.e., the
wrist center). This simplifies the
inverse kinematics problem
significantly by separating the
T30 inverse orientation and inverse
position problems.
T60
• Many industrial manipulators have

spherical “wrists” (all joints in the d6
kinematic chain after the 3rd one)
• Those before are called “arm”
Decoupling with spherical wrist
• Manipulators with a spherical wrist (last 3 joints are revolute with

coinciding axes of rotation) allow to decouple (simplification) the
determination of position (arm) and orientation (wrist )
• In practice, the arm often consists of another 3 joints, hence
arm + wrist = 6 DOF robot, thus J is a square 6x6 matrix
• The above Jacobian can be partitioned for Position
(arm) and orientation (wrist) as follows:
• Since the last 3 joints are revolute (recall D-H convention)
and the axes intersect at a common point: o = o3 = o4 = o5 = o6
then JO becomes
and we can write with
6
Decoupled Wrist Kinematics
If R60 and T60 are the desired orientation and position of the tool frame:
1. Find the first three joint angles that place the wrist center at:
T30 T60 d 6 R60 a , ( a is the Z 6
axis of the tool frame also k nown as the approach vector )
2. Using the joint angles found in Step 1 determine:

R 30 f ( 1, 2 , 3 )
3. Find the wrist joint angles that create the orientation matrix:
R 36 (R 30 ) 1 R60
d6
T30
T60
Transcendental Functions
7
Atan2
The function atan2(y,x) is defined as the atan(y/x)

with a range of (-π,π] & few additional rules:
• If (x = y = 0) then the result is not defined
• If (x > 0 and y = 0) then atan2 = 0
• If (x < 0 and y = 0) then atan2 = π
• If (y < 0) then −π < atan2 < 0,
• If (y > 0) then 0 < atan2 < π
Example: Inverse kinematics

Articulated robot - Inverse position (First 3 joints = Arm)
• The position of the wrist center is given by a set of transcendental

equations involving the first 3 joint angles. The problem is less
difficult than the general case, and a geometric approach is
possible.
• However, there may be more than 1 solution!
– The existence of some expressions in the equations
(such as the cosine or the square root) indicate
e.g. these cases:
cos cos
2 2
a a
8
Example: Articulated Robot - 1
Geometric approach (for theta1)
T
Pc xc yc zc
wrist center infinitely many solutions
for xc yc 0
1 atan 2( y c , x c ) result in different

atan 2( y c , x c ) set of 2 and 3
1
A singularity
configuration

(Avoid singularities through appropriate mechanical design)
• The shoulder offset avoids the singularity Right-arm
Left-arm
1
1
atan 2 y c , x c
atan 2 y c , x c
atan 2 d , r 2 d2
atan 2 d , r 2 d2
atan 2 d , x c 2 yc 2 d2
atan 2 d, xc 2 yc 2 d2
9
Geometric approach (for theta2 and theta3)
Problems with cos(.) & sqrt(.)
r2 s 2 a 22 a 32
cos 3
2 a 2 a3
xc 2 yc 2 d12 z c2 a 22 a 32
2 a 2 a3
3 atan 2 1 cos( 3 ) 2 , cos( 3 )
2 atan 2( s , r ) atan 2( a 3 s3 , a 2 a 3 c3 )
atan 2( z c , x c 2 yc 2 d 12 ) atan 2( a 3 s3 , a 2 a 3 c3 )
Example: Inverse Orientation - 1

Algebraic approach (Spherical wrist attached e.g. to the previous articulated robot)
• Recall the composition of rotations for a spherical wrist (3 revolute joints with
axes of rotation intersecting in one point) and use Euler angles as follows:
Rotate about Z by , then rotate by current Y by , then rotate by current Z by
R, , RZ , RY , RZ , R63
ccc ss ccs sc cs
R , , scc cs scs cc ss
sc ss c
4 Y3
5
6 X3
,
Z3
10
Algebraic approach (Concentrate on the simplest elements)
We get either 2 solutions or infinitely many / even cos( ) function or for =0+ k , i.e. r33= 1)
ccc s s cc s sc cs r11 r12 r13

R , , scc cs sc s cc s s r21 r22 r23
sc s s c r31 r32 r33
atan 2( 1 r332 , r33 ) or atan 2( 1 r332 , r33 )
If we chose the first solution for then s 0 and

= atan 2(r23 , r13 ) and atan 2(r32 , r31 )
If we chose the second solution for then s 0 and
= atan 2( r23 , r13 ) and atan 2( r32 , r31 )

Algebraic approach (Case 2 - infinite many solutions)
ccc s s cc s sc c s r11 r12 0

R , , scc c s sc s cc s s r21 r22 0
sc s s c 0 0 1
Thus only the sum ( ) can be determined as: atan 2(r21 , r11 ) atan 2( r12 , r11 )
Then atan 2( r12 , r11 )

atan 2( r22 , r21 )
11
Inverse Velocity
• Analytic formulation of the Jacobian is usually too complex and

symbolic inversion is difficult. Numerical inversion is preferred.
v0
1
1 v0
0
J 6 xN θ J 0
• J must have N=rank= 6
• If the rank of the Jacobian is less than 6 there may not be a solution.
We would like to get the “best” approximate solution.
• If the robot is redundant (i.e., N>=7) and rank(J)=6. There are

infinitely many joint velocities that will result in the desired end-
effector velocity. We want to chose the “best”.
Some Interesting Issues related to

Inverse Kinematics
• There is a upper bound on number of solutions for any open-link
(thus excl. parallel robots) spatial 3D mechanism with 6 DOF in
a non-singular configuration: 16
• Approach based on Paden-Kahan “subproblems”
• Developed a geometric algorithm to solve step-by-step inverse
kinematics problem reducing it into sub-problems whose solutions are
known, numerically stable or geometrically meaningful
• By no means exhaustive, i.e. there exists robots which cannot be
solved using this approach (decomposition into canonical sub-
problems is not completed)
• “Dialytical elimination” – procedure how to eliminate (n-1) variables in a
system of polynomials with n variables (based on elimination theory from
algebra; “brute-force version”)
• The question of how to solve the inverse kinematics problem in general (in
absence of any intersections of axes) and systematically for both planar
and spatial mechanisms is still an active research area
(incl. how to implement it in real-time)
12
Mechatronics
Redundant and Parallel Robots

ETH Zurich
1. Redundant Robots
• A robot must have enough degrees of freedom to accomplish

its tasks
• A kinematically redundant robot has more degrees
of freedom than needed (the dimension of the workspace)
• A redundant manipulator can have infinite number of
configurations for the same end-effector pose
• The extra degrees of freedom can be used:
– To avoid obstacles and singularities
– To optimize the motion of the robot (subject to minimization of a cost function)
• The human arm is redundant
– Grab the edge of a table, keep your body motionless.
Are you still able to move your elbow?
– Have you tried to fix something on the engine / inside your car?
• Disadvantages: rigidity , complexity
1
Hyper-redundant Robots
• Robots with more than 6 joints

are (usually) redundant robots
– Some joints have more than 1 DoF
(universal, ball & socket)
– Rank of the Jacobian not always more
than 6 even if you have more than six
1DoF joints
• Robots with >> 6 joints have

been termed
Hyper-redundant
• Major disadvantages:
Low rigidity & High complexity
Symmetric Tools
• A 6-DOF manipulator with a symmetric tool contains a

redundant degree of freedom
(no need to distinguish between access from left & right)
• This is common in industrial applications where
many tools are symmetric (drilling, milling, welding, gluing, etc.)
• It is possible to use simpler 5-DOF robots in these tasks,
removing the redundancy
2
Kinematics
(Redundant Robots)
• The derivation of the forward kinematics is no different than
before. Using the product of exponentials:
ˆ ˆ
g st ( ) e 1 1 e n n
g st (0)
with n > p (usually 6) p - dimension of the workspace
p = 6 for 3D spatial robots
p = 3 for 2D planar robots
• The Jacobian has more columns than rows now

– J not invertible, which implies that
– Inverse-kinematics problem is ill posed
(usually infinitely many solutions for a given
desired end-effector configuration)
Internal Motion
(Redundant Robots)
• It may be possible to move the joints of the robot without moving the
tool frame. The tool frame is motionless if
Vsts J sts ( ) 0
• This is true for any joint velocity in the null space of the Jacobian
• This is referred to as self motion or internal motion
VstS ( mx1) J stS ( m n ) ( n 1) VstS m
3
Optimizing Paths
(Redundant Robots)
• Some criteria must be used to choose the joint trajectories
• Typical solution is to go for the minimum joint velocity
(which gives us the desired workspace velocity) by choosing
J st ( )Vst
where J st J T ( JJ T ) 1 is the Moore-Penrose pseudo-inverse J
J ( n m )VstS ( m 1) VstS m
( n 1)
Minimum joint
velocity
2. Parallel Robots
(2 or more serial chains connecting the end-effector to the base)
Serial vs. Parallel Kinematics

(Open vs. Closed Kinematic Chains)
+ High accuracy, rigidity, speed, large
load carrying capability
- Complex kinematics, and
smaller workspace
4
Delta and Hexa
• 3 DOF Delta Mechanism

(Movie) (Dr. Clavel, EPFL)
6 DOF HEXA Robot From

http://www.space.mech.tohoku.ac.jp/research/hexa/hexa-e.html
Kinematics
(Parallel Robots)
• The inverse kinematics of a parallel robot is not more difficult than
in the open-chain case (often simpler).
• The forward kinematics is complicated (compared to the open-chain
case) by the closed loop nature of the mechanism we have to deal
with the constraints given by the “structure equation” , typically there
are multiple solutions.
Example: Steward platform
• forward kinematics very difficult
(due to a large number and a
complicated form of the constraints),
• whereas its inverse kinematics
is remarkably simply (computed as
simple as distances between points
in 3D).
5
Forward Kinematics
• Suppose we have a manipulator with n1 joints in the first chain

(including the end-effector) and n2 joints in the second chain
(including the end-effector)
• For parallel robots, the structure equation imposes a set of
constraints that is always satisfied
ˆ ˆ ˆ ˆ
g st e 11 11 e 1n1 1 n1
g st (0) e 21 21
e 2 n2 2 n2
g st (0)
Graphics from http://synthetica.eng.uci.edu/~curtis/project2.html

Grübler’s Formula
(Determine Mobility)
• Determining the number of degrees of freedom directly
of a parallel manipulator (or possibly serial) can be difficult
• Grübler’s formula allows us to calculate the number of degrees of

freedom of the end-effector (also called mobility):
g
F C6( N g) fi
i 1
where
– C is a constant: C = 3 for 2D (planar formula) or C = 6 for 3D (spatial)
– N is the number of links in the mechanism
not including the base link or ground (otherwise F = C ( N – g – 1) + …)
– g is the number of joints, and
– fi is the number of degrees of freedom of the ith joint
6
Grübler’s Formula
(Example 1: Stewart Platform)
• The number of joints g=18
(6 universal, 6 ball and socket, and 6 prismatic)
• The number of links is N=13
(2 parts for each actuator, and
the end-effector – top link/platform, however,
without base link/platform)
• The sum of all the joint degrees of
freedom is 36
(2 for each universal, 3 for each ball and socket,
and 1 for each prismatic in each of the 6 chains)
• The number of degrees of freedom of
this manipulator is F=6
g
F 6( N g) fi 6(13 18) 36 30 36 6
i 1
7
Mechatronics
Numerical Methods

ETH Zurich
Numerical Methods
– Linear approximation
– Solving techniques
– Calculating Jacobians
– Other concerns
• Calibration
– Methods
1
Inverse Kinematics
• Analytic solutions often not possible

– Large solution space (redundancy)
– Empty solution space (nonreachable goal)
• Need for numerical solutions
• Can be expensive
• Find numerical methods that are:

– As fast as possible
– Accurate
– Robust
Inverse Kinematics
• g d is the desired pose, and is the actual set of joint angles to get to g d
• Forward Kinematics based on POE

ˆ ˆ ˆ
g st ( ) e 1 1 e 2 2
e n n
g st (0)
gd g st ( )
g st1 ( g d )
• How?
– gst-1 is nonlinear
– find a linear approximation to gst-1
2
Linear Approximation
• Taylor series
• A first order Taylor approximation (n = 0, 1) for any Jacobian J

defined by
X V J( a)
is given by
X( ) X ( a ) J ( a )( a )
– J tells us approximately how much X will change in the world space
when we change
X J( a)
1
• Find by J( a) X
Linear Approximation
• We can also think of working directly in SE(3) using (spatial and

body) Jacobians and homogeneous transformation matrix:
Vˆ s g st ( a ) g st ( a ) 1 Vˆ b g st ( a ) 1 g st ( a )
(substitute)
Vs J s( a) Vb Jb( a)
(rewrite)
^
g st ( a ) s
J ( a) g st ( a ) g st ( a ) g st ( a ) J b ( a )
(discretize)
s ^
g J ( a) g st ( a ) g g st ( a ) J b ( a )
(rewrite)
s
J ( a) 1
g g st ( a ) 1 Jb( a) 1
g st ( a ) 1
g
(joint space) (world space)
3
Calculating Inverse Kinematics
• We can choose a value for that will move gst closer to gd

and a reasonable point to start from is
g gd g st ( a )
• We would hope then, that the corresponding value of g would
bring the end effector exactly to the goal
• Unfortunately, the nonlinearity prevents this from happening, but it
should just get us closer
• Also (“for safety”), we can take smaller steps:
g gd g st ( a )
where 0 ≤ ≤1
Basic Jacobian IK Technique

(1. Approach to IK)
• The following algorithm assumes a spatial Jacobian, but could be
easily modified for another choice of Jacobian
while (gst is too far from gd ,i.e. |gd - gst |> )
{
Compute J for the current pose a
g gd g st ( a ) // pick approximate step to take
J s( a) 1
g g st ( a ) 1
// compute change in joint DOFs
a // apply change to DOFs
Compute new gst // apply forward kinematics to

// see where we ended up
}
4
Inverting the Jacobian
• So, how do we find J-1

– J is a 6xn matrix
– Use a generalized inverse
– Generalized inverse is not unique
• X is a generalized inverse of J if it has some of the following properties
– JXJ=J
– XJX=X
– (X J)T = XJ
– (J X )T = JX
– The pseudo-inverse is when all of the above conditions hold true
• Written as X = J+
• Concerns:
– Singularities and near singularities
– Underconstrained (infinite # of solutions)
• Choose the best
– Overconstrained (no solutions)
• How close can we get?
Pseudo-Inverse
• If we have a non-square matrix (n x m) arising from an

over-constrained (non-holonomic robot) or under-constrained
system (redundant robot), we can use the pseudo-inverse:
J ( J T J ) 1 J T , (n m) J J T ( JJ T ) 1 , (n m)
• This is a method for finding a matrix that
effectively inverts a non-square matrix:
– For the over-constrained case, i.e. n>m, in which no exact solution

exists, the obtained solution always has the smallest least squares
error
– For the under-constrained case, i.e. n<m, in which an infinite

number of solutions exist, the obtained solution always has the
smallest magnitude
5
Pseudo-Inverse
• Remember the singular value decomposition 0 0 0 0 0

1
0 2 0 0 0 0
T 0 0 0 0 0
Jm n
Um m m n
Vn n
U , V are orthonormal m n
0 0 0 r 0 0
0 0 0 0 0 0
1 0 0 0 0
• The pseudo-inverse is given by: 1
1
0 0 0 0
T 2
Jn m Vn n n mU m m n m 0 0 0 0
0 0 0 1 0
r
0 0 0 0 0
0 0 0 0 0
• The pseudo-inverse has “inverse-like” properties:

JJ J J , J JJ J
T T
( JJ ) JJ , (J J ) J J
Jacobian Transpose
(2. Approach to IK)
• Another technique is to simply take the transpose
of the Jacobian matrix!
• Surprisingly, this technique actually works pretty well
• It is much faster than computing the inverse or pseudo-inverse
• Also, it has the effect of localizing the computations. To compute
for joint i, we compute the column in the Jacobian matrix Ji as
before, and then just use: (instead of the Jacobian inverse we
derived before)
JT g g st ( a ) 1
6
Nonlinear Methods
(3. Approach to IK)
• Problem recast to minimization of the following criterion:
min g d g st ( )
• Nonlinear methods
– Newton-Raphson iteration
– Gradient Decent
– etc
• Matlab tools
– fmincon(), lsqnonlin()
– Note that availability of analytical Jacobian can drastically
speed up these functions
When to Stop?
• Stopping criteria
– Finding a successful solution

• Finish when a minimum error is reached
– Local minimum
• Options for what to do next
– Accept result as best we can do
– Randomize starting point and try again
– Try a different algorithm
– Taking too long

• Limit the number of steps
7
Units & Scaling
(Potential numerical problems)
• Rotational DOFs use radians and translational DOFs use
meters
• Can we combine their derivatives into the same matrix?
• If desired, we can scale any column (weighting factors) to

adjust how much the numerical method will favor
using that DOF
Calibration
• Problem:
– Due to manufacturing tolerances, a real robot does not exactly
match that of its model.
– Thus, all forward and inverse kinematics calculated with these
models will contain inaccuracies
• Goal:
– Can we calibrate our manipulator such that these inaccuracies
will be minimized?
• How:
– Move the robot to a variety of poses and collect truth values of
where the end effector actually is for each joint value
– Use these values to correct the model
8
Methods
• Open-loop methods
– An external metrology system used to measure pose of end
effector
– The number of parameters measured can vary from 1—6
– The kinematic parameters are found from a nonlinear
optimization of the total pose set
• Closed-loop methods
– The end effector is attached to the ground and forming a mobile
closed kinematic chain
– Calibration is achieved using joint angle sensing alone
• Screw-axis measurement methods
– Identify individual joint axes as lines in space
– From this information, kinematic parameters can be found
analytically
Calibration Jacobian
• Both open and close-loop methods require numerical calibration
• We would like to determine:
y ymeasured ymodeled 1
2
y J cal ( pa ) p pa
p p pa
n
g st 0 e
• Where the Calibration Jacobian matrix Jcal maps changes in
calibration parameters p to differences between measured and
modeled end effector pose y
9
Calibration Jacobian
• After a long derivation:

k 1 1
ˆ ˆ s
Qk Ad e i i
k Ad e i i
ds
i 0 0
g st 0 e
n 1
ˆ s
QM Ad e i i
Ad e ds
i 0 0
J cal J | Q1 | Q2 | Q3 | Qn | QM
• Now, we can use the same methods as for calculating inverse

kinematics
Conclusions
• Numerical methods can generally help solve a variety of

kinematic problems
• Important points
– Speed,
– Accuracy,
– Robustness,
– Convergence rate
– Units
10
References
• K. Okamura and F. C. Park, Kinematic Calibration Using the

Product of Exponentials Formula, Robotica, 1995.
• J. M. Hollerbach and C. W. Wampler, The Calibration Index
and Taxonomy for Robotic Kinematic Calibration Methods ,
The International Journal of Robotics Research, 1996.
11
Mechatronics
Dynamics

ETH Zurich
Robot Dynamics
• “ Given a robot’s joint angles, velocities, accelerations and

inertial properties, what are the loads on the actuators? ”
• We will learn two methods

– The Newton-Euler Method
– The Lagrangian Method
1
Robot Dynamics
1 f1 ( 1,2, , N , 1,2, , N , 1,2, , N )
2 f2 ( 1,2, , N , 1,2, , N , 1,2, , N )
N fN ( 1,2, , N , 1,2, , N , 1,2, , N )
i torque/force at joint i
Newton’s Laws of Motion
• Reaction = - Action
• Time rate of change of linear momentum of a body is equal to
the net force acting on it.
• Time rate of change of angular momentum of a body is equal
to the net torque acting on it.
2
Newton-Euler Equation for a General
Rigid Body
• Motion of a rigid body subject to an external wrench applied at the
center of mass of the body
• Every quantity is expressed in body coordinates
• Inertia matrix is constant in body frame
mI 0 vb b
mv b
b b b
Fb
0
Where I is the identity matrix, is the inertia tensor, and F b is an

external wrench applied at the center of mass and specified wrt the body
coordinate frame
The Newton-Euler Method
• We write the dynamic equations for each link with the vectors
expressed in that link’s coordinate frame. Many vectors in the
equations become constants this way.
• The equations for the links will have common terms due to action-
reaction.
• By “walking” back and forth along the kinematic chain we will resolve
this coupling between the links. Y2
Link 2
Y1 Z2
Y0
Link 1 X1
X0
Z1 2
Theory of Robotics & Mechatronics — 151-0601-00

1 6
3
Forces and Moments on a Link
Zi 1 Zi
rii,Ci Ci rii 1,Ci

i
i
Yi 1 ( Rii 1 i 1
i 1 )
Yi
fi i Xi Mi gi
1 Xi
( Rii 1 fi i 11 )
Linki i 1 N
fi i : Force exerted on Link i by Link i-1, expressed in Frame i

i
i : Torque exerted on Link i by Link i-1, expressed in Frame i
rii,Ci : vector from Joint i (i.e., origin of Frame i-1)
to the center of mass of Link i expressed in Frame i. (Constant)
rii 1,Ci : vector from Joint i+1 (i.e., origin of Frame i)
to the center of mass of Link i expressed in Frame i. (Constant)
Linear Momentum
P M i vCi i , mass doesn't change with time

dP
M i aCi i fi i ( Ri1 1 fi i 11 ) M i g i
dt
M i : Total mass of Link i
vCi i : Velocity of the center of mass of Link i, expressed in Frame i
aCi i : Acceleration of the center of mass of Link i, expressed in Frame i
4
Angular Momentum
• The time rate of change of the total angular momentum of a rigid

body about an inertial (i.e., non-accelerating) frame is equal to the net
torque acting on the body.
Z0 H ri ( ri )dm
V
yi2 zi2 xi yi xi zi x
2 2
ri xi yi x
i z i zi yi y dm
X0
V
xi zi zi yi yi2 xi2 z
Y0 I xx I xy I xz x
I xy I yy I zy y I
I xz I zy I zz z
Inertia Tensor
d (I 0 0 ) 0
I 0 (t ), i.e. I 0 changes with time
dt
Angular Momentum
• The inertia tensor is constant if we express it in terms of a Frame C

rigidly attached to the object:
IC C C
(I C C
) C
: Euler's Equation
Gyroscopic term
ZC
Z0
XC
X0
YC
Y0
5
Angular Momentum
i i i i
Ii i i ( Ii i ) i Rii 1 i 1
i 1 fi i rii,Ci ( Rii 1 f i i 11 ) rii 1,Ci
I i : (Constant) Inertia tensor of Link i about a frame parallel to

Frame i with origin at the center of mass of Link i.
i
i : The angular velocity of Link i wrt Frame 0 (the inertial frame),
expressed in Frame i.
i
i : The angular acceleration of Link i wrt Frame 0 (the inertial frame),
expressed in Frame i.
• Gravity doesn’t create a torque about the center of mass
Dynamic Equations
M i aCi i fi i ( Ri1 1 fi i 11 ) M i g i
i i i i
Ii i i ( Ii i ) i Rii 1 i 1
i 1 fi i rii,Ci ( Rii 1 f i i 11 ) rii 1,Ci
• For the last link (i=N) there is no “next link” f NN 11 0, N 1
N 1 0
• If we knew the linear and angular accelerations and velocities of the
links we could go backwards from the last link and find all the force
and torques!
• On the other hand, the velocity and acceleration of each link is
“induced” by the links before it!
• Therefore, we will first move forward along the kinematic chain
(i=1 N) to determine the velocity and acceleration terms (forward
recursion) and then we will go backwards (i=N 1) to solve the force
and torques (backward recursion).
6
Angular Velocity and Acceleration
Yi 1
Y0
Joint i Xi 1
Yi Xi
X0
Link i
Z0 Zi 1
i , i, i Zi
0 0 • The velocity and acceleration of each
i i 1 Z i0 1 i Link i is related to the velocity and
i i i 1 acceleration of the previous Link i-1
i R i 1 i 1 ( Ri0 )T Z i0 1 i
• For the first link (i=1), there is no
“previous link” 0 0
0 0 0
Z i0 1 0
Z i0 1 0 0, 0 0
i i i 1 i i i
i
i ( Rii 1 )T i 1
i 1 ( Ri0 )T Z i0 1 i
i
i [( Ri0 )T Z i0 1 ] i
Linear Acceleration
Y0 Yi 1
Joint i Xi 1
Link i
Ci Yi Xi
X0 i
r
i ,Ci
Z0
vC0i =vi0 1 0
ri 0,Ci
Zi 1 rii 1,Ci
i
i , i, i Zi
vi0 =vi0 1 i
0
ri 0,Ci ri 01,Ci
vC0i : Velocity of the CM of Link i, expressed in Frame 0 • For the first link (i=1)
0
v : Velocity of the origin of Link i-1, expressed in Frame 0
i 1
a0,0 C0 0, a00 0
aC0i ai0 1 i
0
ri 0,Ci i
0
i
0
ri 0,Ci
1 T
aCi i Rii aii 11 i
i rii,Ci i
i i
i rii,Ci
1 T
aii Rii aii 11 i
i rii,Ci rii 1,Ci i
i
i
i rii,Ci rii 1,Ci
7
The Lagrangian Method
• The Lagrangian of the system is defined as
L K V,
K : Total kinetic energy of the system
V : Total potential energy of the system
• Then the dynamic load on each joint is:
d L L
i ( )
dt i i
i : Joint variable (angle or distance)

i : Load on joint (torque or force)
The Lagrangian Method for a Serial-

Chain Manipulator
• For a serial-chain manipulator, the differential equation
obtained with the Lagrangian method results in a simple
structure
M( ) C( , ) N( , )
with a mass matrix M, a Coriolis matrix C, and an
additional matrix N which includes gravity and other joint
loads like friction
• M ( ) is symmetric and positive definite
• M 2C is a skew-symmetric matrix
8
The Cartesian State-Space Equation
• It is possible to use the manipulator Jacobian to express

the dynamics in Cartesian space rather than joint space
M x( )X Cx ( , ) X Nx ( , ) F
with
M x ( ) J T ( )M ( ) J 1 ( )
Cx ( , ) J T ( ) C ( , ) M ( ) J 1 ( ) J ( )
T
Nx ( , ) J ( )N ( , )
• The force vector, position vector, and Jacobian must all

be expressed with respect to the same frame
Planar R- Robot
r (t )
• 2 DOF R- robot on horizontal plane, holding a point load. The

shoulder is Link 1, the arm is Link 2.
• The problem is “What is FA?”
9
Planar R- Robot
K KS KA K Load
I S : Moment of inertia about the axis of rotation of the shoulder
I A : Moment of inertia about the axis of rotation of the arm, I A,CM M A (r L / 2) 2
M A , M Load : Mass of the arm and the point load
1 2
KS IS
2
1 2 1
KA IA M Arm r 2
2 2
K.E. due K.E. due
to rotation to arm extension
1 2
K Load M Load r r2
2
Planar R- Robot
1 1
K I S I A,CM M A (r L / 2) 2 M Load r 2 2 M A M Load r 2
2 2
L K V , no potential energy due to horizontal plane of rotation so
V 0 and L K
• Find FA, Force load on the prismatic arm joint. Joint variable is r.
L
MA ML r
r d L L
d L i
dt
MA ML r i i
dt r
L 2
MA r L/2 M Load r
r
2
Arm FA MA ML r MA r L/2 M Load r
10
Friction: A Parasitic Force in Robotics
• The accuracy of dynamic models are always limited by

friction. Friction also limits robot precision.
• A papers of interest:
– B. Armstrong-Helouvry, P. Dupont, and C. Canudas de Wit, “A
Survey of Models, Analysis Tools and Compensation Methods
for the Control of Machines with Friction,” Automatica, vol. 30,
no. 7, pp. 1083-1138, 1994.
11
Mechatronics
Trajectory Generation and Control

ETH Zurich
Robot Trajectory Control
• “ How to define a trajectory in 3D space and how to control

the robot to follow this trajectory ”
1
Path vs. Trajectory
• A path is a locus of points in the task space (operational

space of end-effector position and orientation). It is a purely
geometric (spatial) description of motion.
• A trajectory is a path on which a time law is defined, for
instance, in terms of velocities and accelerations at each point
on the path. It is a spatio-temporal description of motion.
• There are two problems:
– How to define a trajectory.
– How to control the robot to generate the trajectory.
Trajectory Definition
• We describe the trajectory in terms of 6D points in task space

(i.e., the path) and the velocities, accelerations, etc. of the
tool frame at each point.
• Given a desired starting point and end point of motion, we
place some via points in between to better define the path.
• Most of the time we want motions to be smooth. This means
continuous derivatives (first and many times second) to
minimize rough, jerky motions between path points. To do this
we put spatio-temporal constraints on the path between the
via points.
2
Joint Space vs. Task Space
Trajectories
•
Using inverse kinematics we can
convert from task space to joint space.
The path points can be written in
terms of joint variables, describing the
trajectory in the joint space.
• Following the joint space trajectory, the end-effector will be at the desired
position and orientation at the via points. However, in between the via points
the generated path, while simple in joint space, may be complex in task
space.
• Joint space trajectory generation is the easiest to compute and inherently
avoids any singularities. However, we must make sure there are no obstacles
in the resulting
Cartesian path.
Joint Space Trajectory Definition
• Consider the case of point-to-point motion between a start position and an

end position. There are many smooth functions that may be used to
interpolate the joint values.
(t0 ) 0 , (t f ) f
• For example, a cubic polynomial function
may be used to add two constraints of
velocity to achieve a smooth (in first
derivative) function.
(t0 ) 0 , (t f ) f
(t ) a3t 3 a2t 2 a1t a0

4 equations in 4 unknowns
(t ) 3a3t 2 2a2t a1
3
Polynomial Interpolation
• Position, velocity and acceleration profile of a single cubic segment that starts and
ends in rest.
• The desired velocity at each via point may be given by the user in terms of Cartesian
(translational and angular) velocities. This definition is then translated to joint space
velocities by the inverse Jacobian.
• To add acceleration constraints, we could go up to fifth order (quintic) polynomials.
(t ) a5t 5 a4t 4 a3t 3 a2t 2 a1t a0 6 equations in 6 unknowns
(t ) 5a5t 4
4a4t 3
3a3t 2
2a2t a1 • Increasingly oscillatory path as the
order increases
3 2
(t ) 20a5t 12a4t 6a3t 2a2
Linear Segments with

Parabolic Blends (LSPB)
• Appropriate for constant velocity point to point moves.
• Between the via points, the joint velocity is constant, except at the beginning
and at the end where a quadratic polynomial is applied.
• A blend time tb is defined to begin and end the quadratic interpolation.
(t ) a2t 2 a1t a0
(t ) 2a2t a1
(t ) 2a2
0
t0 tb t f tb tf
4
Parabolic Blends
• Given the joint positions and time
at both ends, the blend time and
f the (constant) acceleration are
Half related.
point , constant acceleration
during blend
• Usually an acceleration is chosen
f
and the blending time is
computed. We cannot chose the
acceleration too low otherwise
0
t0 th t f tb tf the blending time will increase
tb
and the linear portion will “shrink”
h b 1 2 to zero.
tb tb 0 b
th tb 2 (t f t0 )
2 2
(t f t0 ) 4 ( )
f 0
Velocity at t b Position at t b tb
2 2
t f t0 2th 4( f 0)
tb 2
(t f t0 )tb f 0 0 (t f t0 )2

Parabolic Blends
• Multi-segment (i.e., with via points) linear path with blends.
• Want to keep fixed velocity during each segment.
• Does not pass exactly through the via points except at the ends. The
higher the acceleration, the closer the path to the exact via points.
5
Parabolic Blends
• If some of the via points have to be exactly passed through, we can
use two pseudo via points on either side of the exact via
(sometimes called a through point).
Task Space Trajectory Definition
• Most common type is the piecewise linear path in task space. The
same idea as the linear segments with parabolic blends is used, this
time in the task space.
• Other than the three position variables, three orientation variables
are also defined and blended at the via points.
• If the orientations of two via points are given by rotation matrices, we
can not simply interpolate the values of the corresponding elements
of these matrices between the via points. This could violate the
orthonormality constraint of the matrix during the interpolation.
RBA
B
A
6
Interpolating Orientations
• A good way to interpolate orientation is to use the angle-axis

representation of orientation.
RBA RB0
RA0
r11 r22 r33 1 • We can now interpolate

cos 1 ( ) between kA and kB just like the
2
r32 r23 k k position coordinates.
1 • Other interpolation schemes
k r13 r31
2sin( ) such as interpolating the Euler
r21 r12
angles are possible.
Geometric Problems with

Task Space Trajectories
• Intermediate points • High joint speeds • Start and goal only

unreachable near singularities. reachable in different
solutions of inverse
kinematics (i.e., right
hand elbow up—left
hand elbow down)
7
Optimization Methods for
Trajectory Planning
• Hard constraints define positions, velocities at certain via points.
• Soft constraints define an overall objective to be achieved during
trajectory planning
• Minimize (or maximize) a cost function while attaining the hard
constraints. f
2
C , minimum kinetic energy
0
f
C f ( , ,...)
f
2
C , minimum jerk
0 0
f
2
C , minimum torque change
0
Robot Control
• Most industrial robots are controlled in the joint space where

joint variables and their derivatives are sampled (i.e.,
feedback) and motor commands (desired current/torque) are
output. The quality of the kinematic model of the robot affects
the control performance.
• Task space control where some form of end-point sensing
(e.g., laser tracking, vision) is used for feedback can reduce
the effect of kinematic modeling errors. Still, the robot is
controlled by moving the joints, therefore, an inverse
kinematics calculation is necessary.
8
Independent (Decentralized) Joint
Control
• This is the simplest control scheme. Each joint is considered as an
independent single input single output (SISO) control task. The
coupling of inertial terms between the joints are neglected.
• Lowest computational demand. Suitable for slow motions and joint
motors with high gear ratios.
R
1 ,R 1
Controller Joint dynamics
1 , 1
R
2 ,R 2
Controller Joint dynamics
2 , 2
Electric Motor Dynamics
• Torque is proportional to the current through the motor. m kmiArmature

• The supplied voltage is “balanced” by the voltage drop due to resistance,
the induced voltage by the inductance due to current change and the back
emf voltage due to the “generator action” of the motor.
VA lAiA RAiA ke m
• Current amplifier motor drivers are often used that establish closed loop
control on the motor current and minimize the inductive effects so that the
motor current can be directly commanded (by a voltage signal from the
computer).
9
Effective Inertia
• The motor (rotor), the gearbox and the load present inertial loads on the
motor. Usually the moment of inertia of the gearbox as seen from the motor
shaft is specified and can be added to the rotor inertia term.
m , m 1 L , L
Motor Gearbox Load
Im IL
L m , m L • Effective inertia is the total inertia as seen
from the input or output side.
1 1
m Im m IL m • With a large gear ratio the motor inertia
becomes a more significant portion of the
IL effective inertia. In other words, the torque
I m,effective Im 2 load on the joint motor is less dependent
on the load inertia (which changes
2 depending on the current configuration of
I L ,effective IL Im
the robot)
Independent Joint Control with

Decentralized Feedforward
• With independent joint control each joint is assumed to have an effective inertia. The changes in
dynamics due to the changes in the robot’s configuration (i.e., gravity, inertial loads exerted by other
links, coriolis forces, etc.) are handled as external disturbances to the control system.
• If we had a perfect model of the system we wouldn’t need feedback. We could modify the reference
input with the inverse of the system dynamics and achieve the required result. (Given the system is
stable.)
R( s) 1/G(s) G(s)
• We can still use our “best guess” of the system model in a feedforward loop and let the feedback
loop handle the “unknown” disturbances. The feedback loop only reacts to an existing error where
as the feedforward loop predicts the necessary input to the system and acts before an error occurs.
• The feedforward loop is still decentralized in that it uses an independent model of each joint with an
effective inertia, neglecting the true dynamics of the robot.
Feedforward
Controller
R
1 ,R 1
Feedback
Controller
Joint dynamics
1 , 1
10
Computed Torque Method
• We could go another step ahead and predict the “disturbance” terms due to complete
robot dynamics that was neglected before. We can calculate the required torque load
on the actuator using the forward dynamics equations which we have studied.
• The dynamics equations are very
1 f1 ( 1,2, , N , 1,2, , N , 1,2, , N ) complicated and costly to
compute in real-time. The
1,2, , N , development of efficient recursive
Centralized
methods, such the Newton-Euler
1,2, , N , Feedforward
Controller formulation we studied, were
Decentralized partly motivated by the need to
1,2, , N
Feedforward compute the dynamics in real-
Controller time.
R
1 ,R 1
Feedback
Controller
Joint dynamics
1 , 1
Centralized Control
• Similar to the centralized feedforward idea, an inverse dynamics model of

the system is used to linearize the system at the current configuration.
• Consider a second order mass-spring-damping system but with a
non-linear spring:
mx cx kx3 f
• If we apply the following change of variables, the system is linearized:
f f x f
m • We can now develop a linear
cx kx 3 controller (i.e., a PID controller) for
this system.
11
Inverse Dynamics Control
• The whole robot is considered as a multiple input multiple output

(MIMO) system.
• Instead of a single input there is a vector of input values and a
vector of output values.
• The rigid body dynamics have this general form
M( ) V( , ) G( )
vector of Inertia vector Gravity
joint torques vector of Coriolis and
matrix centrifugal terms terms
M( )
V ( , ) G( )
, the "new" system is nonlinear and decoupled!
Cartesian-based Control Schemes
• Perform control in
Cartesian space, rather X
than joint space Xd J-1 Gains Arm
• This gives desired
response of the tool
frame, rather than the X Kin( )
individual joints
X F
Xd Gains JT Arm
• These two schemes are
not the same!
X Kin( )
• The full model-based

controller can be included
in Cartesian-based
schemes as well
12
Advanced Control Schemes
• In order for inverse dynamics method to be successful, the

inverse dynamics equations must be continuously calculated
very fast.
• The dynamic model must be accurate.
• The servo terms (the terms based on errors used in the PID
controller) must be computed very fast, but it is sometimes
possible to update the model terms (M( ), etc.) at a slower rate
• Other control topics of interest
– Robust control systems
– Adaptive control
New Work on Trajectory Tracking

in SE(3)
• Doing full trajectory tracking of a desired tool-frame pose in
SE(3), using screw theory to avoid resorting to local
coordinates, has only recently been done
• Two recent papers of interest:
– The first generalizes PD control to SE(3):
F. Bullo and R. M. Murray, “Tracking for fully actuated
mechanical systems: A geometric framework,” Automatica, vol.
35, pp. 17-34, 1999.
– The next generalizes LQR control to SE(3):
Y. Han and F. C. Park, “Least Squares Tracking on the
Euclidean Group,” IEEE Trans. Automatic Control, vol. 46, no. 7,
pp. 1127-1132, 2001.
13
Mechatronics
Force Control and Haptics

ETH Zurich
Contact Tasks
• Mechanical part mating (assembly)

• Contour Tracking
• Machining
1
Force Sensors
• Mostly at the wrist, sometimes at the joints and

grippers (fingers)
Robotics Research Corp. 7 DOF Arm
Control of Constrained Motion
• The environment sets • Trajectory control

constraints on the approach likely to fail
geometric paths that due to:
can be followed by – Inaccuracy of robot and
the end effector. FR F environment models
– Corrective action of the
position controller due to a
trajectory error can easily
create large forces, saturate
the actuators and even
crash the robot (or the
constraint!)
• Must modify the control

algorithm such that it
“respects” the constraints
2
Static Force/Torque Relationship
Fx 1
Fy
FR F FR F 1
Fz
FR 0
Y0
x J T FR0
y
z N
X0
Z1 2
• Singularities also have a
Z0 1 significance in terms of forces
that can be (actively)
generated by the robot. Cannot exert forces in this
direction
Stiffness and Compliance
• Stiffness
– Proportionality constant k that relates a static displacement to
the force due to this displacement
F k X • We cannot describe
(control) force and
displacement
independently
• Compliance
– Inverse of stiffness
– Passive compliance: Non-actuated (“internal”) tendency of a
body displaced due to external forces (e.g., compression of a
spring)
– Active compliance: Controlled compliance in response to an
external force, e.g., in order to keep the contact force at a certain
limit (“actively giving in”)
3
Compliance Frame
(Constraint Frame)
• A time-varying coordinate
frame called the compliance
frame is defined to describe Compliance frame defined at the
the contact task. end effector
• The compliance frame

decomposes the task into vx fx = 0
fy vy = rωd
directions along either a pure vz fz = 0
position command or a pure ωx τx = 0
ωy τy = 0
force command can be τz ωz = ωd
specified.
• We define natural and
artificial constraints in the Compliance frame defined at the
compliance frame. Natural axis of rotation of the crank
constraints are what the
environment imposes on the
robot, artificial constraints are vx fx = 0
how we want the robot to act. vy fy = 0
vz fz = 0
The number of both type of ωx τx = 0
constraints is equal to the ωy τy = 0
degrees-of-freedom of the τz ωz = -ωd
task space (usually six).
Compliance Frame
• We would like to have a large

stiffness along artificial velocity
constraint directions and a
vx fx = 0
smaller stiffness along the fy vy = 0
natural velocity constrained vz
ωx
fz = fd
tx = 0
zc
directions. ωy τy = 0
τz ωz = ωd
xc yc
vx
vy
fz
ωx
ωy
τz
4
Peg in Hole
Passive Compliance
• Center of compliance is the point on the tool where

a force at that point creates a pure translation and a
moment creates a pure rotation about that point.
• Remote Center Compliance (RCC) devices place
the center of compliance at the tip of the tool with
variable different stiffness along different directions
of the compliance frame.
• High stiffness for translations along the vertical axis.
5
Active Compliance
• A position controlled robot reacts to positioning errors by applying torques at the

joints such that the positioning error is reduced.
KP E KD E
• If the gain along a direction of motion is low, the robot effectively exhibits high
compliance in that direction by generating only a small opposing force to correct a
position error. By adjusting the gains of the controller we can make the robot behave
as a 6 DOF spring with controlled stiffness in each direction.
• The action of the desired 6 DOF spring can be formulated as:
F K S ,6 x 6 X , K is diagonal
• We translate this desired behavior in the task space into the joint space by:
JTF J T KS X , X J
T • This is the desired (steady-state) response of the actuators
J KS J to small joint space errors such that the robot will behave
as a 6 DOF spring with spring constant (matrix) KS
• We then modify the control law as: J T KS J E KD E

Joint space
stiffnes matrix
Compliance Control
• We can now control the force by modifying the position reference such that
the resulting position error will end in a desired force through the 6-DOF
stiffness matrix.
• Using a force sensor to measure the actual force, we can measure the error in
the desired and actual forces and adjust the position reference in real-time.
• This is a modified position control scheme to accommodate the constraints,
therefore, it can be applied with commercial robot (path) controllers by
changing the desired path on the fly.
6
Impedance Control
• Similar to compliance control but the end effector is made to behave like a
6 DOF mass-spring-damper system.
• Applied by modifying a task-space path control scheme such as inverse
dynamics control.
• Often used with robotic hands where the fingers must grasp with certain force.
The fingers must be springy enough to deform and not damage the object
while stiff enough to hold the object when subject to disturbances caused by
contact with other objects, for example, in assembly.
Hybrid Position-Force Control
1 0 0 0 • Apply position control or force control

0 0 0 0
along different degrees of freedom of
S , Selection matrix the compliance frame
0 0 1 0
0 0 0 1
7
Haptic Interfaces
• Haptic interfaces are human-computer interfaces that provide

touching sense (tactile feedback) to the user
• Force, temperature, vibration
• Games, user interfaces, training, teleoperation,
SensAble Technologies Immersion

The PHANToM
• The PHANToM by SensAble is the most

popular haptic interface
– The device has low inertia, low friction, and
is well balanced
– Feel the virtual environment, not the device
• These devices have

limits to the stiffness that
can be stably displayed
8
Parallel Haptic Interfaces
• Parallel devices increase our ability

to generate stiff environments
• This comes at the cost of greatly
reduced workspace
The SML Parallel Haptic Device (Uchiyama,Tsumaki)

www.forcedimension.com
Admittance Devices
• A haptic device can be made like a

traditional industrial robot (non-
back-drivable, strong) by attaching
a force/torque sensor where the
human user interfaces with the
robot
• Very stiff environments are easy to
generate: simply don’t command
any motion
• Displaying free motion becomes
difficult to do stably
FCS Robotics HapticMASTER
9
Haptics for Teleoperation
• Haptic devices are used for

interacting with virtual and real
environments
• To interact with real environments,
the haptic interface acts as a master
device, and a remote robot acts as a
slave device
• Force feedback has been shown to
improve user performance in certain
tasks
• Force feedback in commercial
devices (like the da Vinci) is still an
active area of research
da Vinci from Intuitive Surgical
Fundamental Papers in Haptics
• The field of haptics does not yet have a good unified text
• Three papers of interest:
– The first discusses the design of haptic devices
J. E. Colgate and J. M. Brown, “Factors Affecting the Z-Width of
Haptic Display,” IEEE Int’l. Conf. Robotics and Automation, pp.
3205-3210, 1994.
– The next discusses the interaction with virtual environments
through what is now commonly referred to as a “proxy”
C. B. Zilles and J. K. Salisbury, “A Constraint-based God-object
Method for Haptic Display,” IEEE/RSJ Int’l. Conf. Intelligent
Robots and Systems, pp. 146-151, 1995.
– The last discusses the stability of human interaction with virtual
environments
R. J. Adams and B. Hannaford, “Stable Haptic Interaction with
Virtual Environments,” IEEE Trans. Robotics and Automation,
vol. 15, no. 3, pp. 465-474, 1999.
10
Mechatronics
Introduction to Computer Vision

ETH Zurich
The Three Pillars of Robotics
Perception
Manipulation Cognition
1
Perception
• Photoreceptors
– Vision and Related Information: distance, motion, size, color, texture, location,
orientation, etc.
• Mechanoreceptors
– Sense of Feel: proprioceptive and tactile: weight, pressure, texture, shape, motion
– Sense of Hearing: direction of noise, vibration, strain sliding, impact
• Chemoreceptors
– Sensor of smell: chemical composition and reactions (fires)
• Thermoreceptors
– Locate heat sources
– Measures temperature
• Electromagnetic
– Measure of radiation
– Measure electrical component status
• Inertial
– Sense of balance
– Measure acceleration, velocity, orientation, and position
Sensing and Manipulation
• Many, if not most, of today’s limitations in robotic technology

can be traced to the problem of integrating sensors and robot
manipulators
• Sensors provide the means of adapting the robot task to:

– imprecision in part dimensions and part location
– unpredictable disturbances or modifications
2
Vision as a Sensing Mode
• Non-contact
• Large area of inspection possible
• Images are ambiguous; many variable parameters:
– Illumination
– Reflectivity, texture
– Surface orientation
– Sensor model
• Algorithms are computationally expensive
• We make it look easy but we do not understand how the
human visual system works.
• Computer technology changes continuously and so does
computer vision.
A Picture is Worth a Thousand Words
• Goal of computer vision is to

write computer programs
that can interpret images
• Can computers match
human perception?
– computer vision is still no
match for human perception
– but catching up, particularly
in certain areas
3
The Vision Problem
Given
Find
Computer Vision and

Computer Graphics
Computer World
vision model
World Computer
model graphics
• Vision and Graphics are inverse problems
4
Motion Capture, Games
Applications of Vision:
Document Analysis
2D Barcode Reading
Optical Character Recognition
5
3D Scanning
Scanning Michelangelo’s “The David”

• The Digital Michelangelo Project
• http://graphics.stanford.edu/projects/mich/
• 2 BILLION polygons, accuracy to .29mm
Medical Imaging, 3D Reconstruction,

Diagnostic Assistance
6
Face Recognition, Biometrics
Biometrics
• Authorization based on something you …

– Have (token: badge, key)
– Know (secret: PIN, mother’s maiden name)
– Are (biometrics: signature, fingerprint, DNA, gait)
• Physiological vs. behavioral
• What’s new with biometric passports?

• What are the implications of using biometrics?
– Identity theft
– ATM access
– …
7
Robotic Applications of Vision
• Automated Inspection Captured Image

(faulty)
Perfect Images
• Automated Assembly
From Production Device for Matching
• Guidance and Control Light Sensor Interface to

Sensor and
• Part Identification CHUTE

Door
18 25 Errors in
Comparison
Processor Memory > 10,
therefore
Camera rejected
Light Source
Capture Card
Viewing Area
Connector Being
Scanned
Electromagnetically
Controlled Door
Reject Bin
To Packing Machine
Automated Inspection
• Part Measurement and

Quality Control
– length, width, area
– hole diameter and position
– part profile and contour
– crack detection
– on-line assembly inspection
– verification of part features
– inspection of surface finish
8
Automated Inspection
Object inspection, sensing, counting
Automated Positioning/Inspection
Scanning
Machine Positioning/Inspection
9
Automatic Inspection of
Precision Components
• Quality assurance
• Features to be checked
– dimensions within specified tolerances
– correct positioning, orientation and alignment
– correct shape of objects and holes (especially roundness)
– whether corners are misshapen, blunted or chipped
– presence of holes, slots, screws, rivets, etc.
– presence of a thread in screws
– presence of burr and swarf
– pits, scratches, cracks, wear and other surface marks
– quality of surface finish and texture
– continuity of seams, folds, laps and other joins
Accuracy of Part Measurements by

Vision
• Given a 512 512 image, accuracy is on the order of
1/500 = 0.2%
• However, statistical techniques can improve this by an
order of magnitude to 0.02%
• Accuracy influenced by
– Lens optics
– Sensor spatial resolution
– Digitizing accuracy
– Image processing algorithm
10
Guidance and Control
• Vision-assisted robot assembly

• Vision-assisted robot material handling
• Weld seam tracking
• Part orientation and alignment systems
• Determining part position and orientation
• Monitoring high speed packaging equipment
Automated Assembly
• Part Location
– pick-and-place
• Assembly Verification
ABB
Flexpicker
11
Vision System Components
reflected/ serial stream

emitted light of charge packets
3D digitization/ computer vision useful

optics sensor information
scene hardware algorithms
2D image of 2D array of numbers

light intensities
Illumination Techniques
• An important but often overlooked part of any vision system is illumination.

• Many different techniques are applied depending on the scene and part
properties.
Ring lighting
Backlighting
12
Illumination Techniques
• Axial Illumination • Combinations

• Dome illumination
Provides diffuse
illumination without
sharp shadows
• Text on wrinkled aluminum foil with ring light and diffuse dome/axial light.
Image Formation
(Geometrical Optics)
• Light rays from a
point in the scene
“projects” to many
points on the
film/sensor. No
image is formed.
• Add a barrier to block

off most of the rays
– The opening known
as the aperture
– How does this
transform the
image?
13
Camera Obscura
• The first camera

– Known to Aristotle
• Why not make the aperture
as small as possible?
– Less light gets through
– Diffraction effects ...
Adding a Lens
“circle of
confusion”
• A lens focuses light onto the film

– There is a specific distance at which objects are “in focus”
• Other points project to a “circle of confusion” in the image
– Changing the shape of the lens changes this distance
14
Single Lens
focal point
optical center
(Center of Projection)
• A lens focuses parallel rays onto a single focal point

– focal point at a distance f beyond the plane of the lens
• f is a function of the shape and index of refraction of the lens
– Aperture of diameter D restricts the range of rays
• aperture may be on either side of the lens
– Lenses are typically spherical (easier to produce)
Depth of Field
• Changing the aperture size affects depth of field

– A smaller aperture increases the range in which the object is
approximately in focus.
15
Real Lens Systems
Variable Parameters
• Focus
– lens to sensor distance
• Focal Length
– also called zoom or magnification
• Aperture
– also called f-stop or f/number
• Depth-of-field?
Optical Distortions (Aberrations)
No distortion Pin cushion Barrel
• Distortion: Distortion is an optical error in the lens that causes

a difference in magnification at different points within the
image. Caused by imperfect lenses
– Deviations are most noticeable for rays that pass through the
edge of the lens
– Image information is not lost, can be corrected in software
through calibration
16
Chromatic Aberration
• Lens unable to focus all colors to same image point

– Wavelength-dependent refractive index
• Typically worse away from image center
Human Eye
• The human eye is a camera

– Iris: colored annulus with radial muscles
– Pupil: the hole (aperture) whose size is controlled by the iris
– What’s the “film”?
• photoreceptor cells (rods and cones) in the retina
17
Cameras

3D digitization/ computer vision Useful

optics sensor
Scene hardware algorithms Information

light intensities
CAMERAS
The Photoelectric Effect
Incoming Light
photon
- + Electrical Connection
charged plate k! charged plate Polysilicon Gate

bo
in +
Silicon dioxide
electron
+ e- e- e- e- e- e- e- e- Silicon
- + Potential Well
battery
Simple Photodetector
• Incident photons are converted into electrons and accumulated

in a charge (potential) well.
• Imaging sensors are arrays of small (typ. ~10 10 μm squares)
pixels that collect the charge. They are spatial samplers of light
intensity.
18
CCD Sensors
• The charge on each pixel is

transferred from pixel to
pixel to an amplifier.
• High sensitivity, high
dynamic range (the ratio of
a pixel’s saturation level to
its signal threshold), better
signal to noise ratio.
• Expensive, needs off-chip
circuitry for clocking and
other functions.
• Used for high quality,
scientific imaging,
photography.
CMOS Sensors
• Complementary Metal-Oxide
Semiconductor technology is
widely used for manufacturing
VLSI circuits.
• Similar to CCD in photon-to-
electron conversion but each
pixel has its own dedicated
buffer circuitry. All electronics
is integrated into a single chip.
• Better suited for complicated
functions, digital output, partial
output of the pixels
Single-chip Integrated CMOS (windowing).
sensor
• Less costly (in quantity),
13 13 mm square chip, the analog mostly used in low cost
output can directly drive a video monitor
without any other circuitry! imaging, security, web cams,
etc.
19
Linear Arrays
Photodiodes CCD
OS1
Transfer Gate
Transfer Gate
Photodiodes
Transfer Gate
OS
OS2
Linear Array CCD
Bilinear Array CCD
• A single line CCD camera
TDI (Time Delay and Integration)
• A.K.A. Line scan

• Synchronization required
time 1 time 2 time 3
Signal strength of the ball increases as it passes through each stage
20
CCD Formats, Frames/Fields and
Sampling Rates
• 1 Frame = 1 640 480 image
• Framerate = 30 Hz
• ~9 MB/s for 8 bit/pixel images (grayscale)
• 1 Frame = 2 Fields
• “Fieldrate” = 60 Hz
• Combining Fields => Interlacing
Types of Images
• Binary
• Grayscale
• Color
21
Lena Söderberg
“If a professor makes a sexist joke,

a female student might well find it so
disturbing that she is unable to listen
to the rest of the lecture. Suggestive
pictures used in lectures on image
processing are similarly distracting to
the women listeners and convey the
message that the lecturer caters to
the males only. For example, it is
amazing that the "Lena" pin-up
image is still used as an example in
courses and published as a test
image in journals today.”
Dianne O'Leary, 1999, essay on reasons for

the male predominance in computer science
May 1997
Color-depth vs. resolution
512 512, 24 bpp

22
512 512, 1 bpp 181 181, 8 bpp

103 103, 24 bpp 181 181, 8 bpp

23
Spectral Sensitivities
Color Cameras
Sensor Chip
s
f
m
m
s f
f
s
Lens
R G R G
(m) beam-splitting mirrors
(f) R, G, B color filters G B G B
(s) CCD sensors R G R G
G B G B
3 CCD color sensor Single chip color sensor
24
Foveon Sensor
• CMOS-based technology
• Layers for color capturing at each pixel
Temperature Sensors
• Infrared Imaging
– near IR (400 – 1300 nm)
– mid IR (400 – 2200 nm)
– far IR (600 – 25,000 nm)
– Pt/Si Focal Plane Arrays 256 256
– Bolometers (miniature RTDs or
thermistors)
25
Cameras


optics sensor

light intensities
CAMERAS
Signals and Digitization
• RS-170 Analog Video

– single band,
monochromatic
(intensity)
• NTSC, PAL
– Color info superimposed
on RS-170 (broadcast
standard)
• RGB
– Color, 3 separate RS-170
signals, best of analog
• DIGITAL (the best)
– CameraLink, Firewire,
USB 2.0
26
FrameGrabbers
• Computer peripheral devices that

digitize analog video.
• Some have dedicated hardware for
onboard image processing, real-
time mpeg encoding, monitor
display with overlays.
• Special digital video formats (i.e.,
CameraLink) need framegrabbers
to interface digital video as well.
• Firewire, USB 2.0 interfaces are
now usually incorporated onto
newer PC motherboards.
Camera Modeling
• Some computer vision methods, especially 3D vision, require

a mathematical model of the image formation process.
• The simple pinhole camera model is often adequate.
f XC f YC
xs , ys
sx Z C s y ZC
f : focal length
sx , s y : size of sensor pixels
27
Camera Calibration
• Camera calibration attempts to

solve two problems Image (X'u, Y'u)
– Projection (perspective) Coordinate

System
Yu
• 3D to 2D, given the 3D Yc
coordinates of a point, Xu
CCD Array
determine the location of the
Xc
corresponding image point Camera
-Zc Coordinate System
– Back-projection (inverse
perspective) Zw
• 2D to 3D, given a pixel Yw

(X'w , Y'w, Z' w)
location, determine the 3D
ray in space that defines the
“line-of-sight” of the pixel
Xw
World Coordinate System
Calibration Parameters
• Extrinsic Parameters (6 parameters)

– Position and Orientation of the camera frame with respect to
the world frame
• Intrinsic Parameters
– pixel size (sx, sy) (plus a digitization factor)
– center of image (Cx, Cy)
– focal length f
– radial distortion in the lens (k1, k2)
28
Calibration Alternative
• The best calibration would result from measuring each pixel’s

line-of-sight in the real world
• Obviously impractical because of
– too many measurements
– too much storage space required ??
• But an approximation is possible
– sample fewer points and interpolate
Interpolation
(Martins, Birk, Kelley (1981))
• Let Pj = [Xj,Yj,Zj]T be a calibration point
• Let Qj = [xj,yj,1]T be a corresponding pixel
• Assume a linear transformation: A: Q P
• Thus Pj = AQj and [P1, P2,…, PN] = A [Q1, Q2,…, QN]
• [P1, P2,…, PN] and [Q1, Q2,…, QN] are known
• We use a least-squares fit of the data to find A

– A = PQ+
– Q+ = QT(QQT)-1 is the Moore-Penrose pseudoinverse of Q
29
Other calibration issues
• Calibration may vary with many camera parameters such as

– focus
– aperture
– zoom (variable focal length)
• Pan-tilt heads must be calibrated
• We haven’t considered
– color
– intensity
30
Mechatronics
Computer Vision Algorithms

ETH Zurich
Commonly Used Algorithms


optics sensor

light intensities
ALGORITHMS
1
Commonly Used Algorithms
• Statistical Operations • Spatial Operations and

• Segmentation and Edge Transformations
Detection • Morphological Operations
• Finding Shapes • Pattern Recognition
• Frequency Domain • Labeling
Techniques
• Data Reduction
– The point of applying computer vision
algorithms is to reduce your data from a
512x512 x 8bit image to something that is
meaningful.
Example Image Processing Algorithm
Acquisition
Prefiltering
Recursive/
Adaptive
Filtering Special Purpose Image
Processing Hardware
Image
Segmentation
Geometric
Operations
Feature
Extraction
Image
Classification General Purpose CPU
Image
Evaluation
2
Image as a Function
• We can think of an image as a

function, f, from R2 to R:
– f(x, y) gives the intensity at
position (x, y)
– Realistically, we expect the
image only to be defined over a f(x,y)
rectangle, with a finite range: y
f: [a,b]x[c,d] [0,1] x
– A color image is just three

functions pasted together.
We can write this as a
vector-valued function
r (x, y )
f (x, y ) g (x, y )
b (x, y )
Image as a Discrete Function
• In computer vision we operate on digital (discrete) images:

– Sample the 2D space on a regular grid
– Quantize each sample (round to nearest integer)
• If our samples are D apart, we can write this as:
f[i ,j] = Quantize{ f(i D, j D) }
• The image can now be represented as a matrix of 8-bit integer
values
3
Digital Images
• Array of numbers (pixels)

– Typically integers 0–255 (unsigned byte)
• Pixels have neighbors
4-neighbors 8-neighbors
Statistical Operations
• Thresholding
• Histogram Equalization
• Multi-Image Operations
4
Thresholding
Histograms
• Thresholds can be chosen automatically by a technique

called histogramming
Number 40
of
Pixels 30
20
10
0
Pixel value
5
Histogramming
• Find the two (modes)

peaks and choose the
midpoint as the
threshold
• Adaptive thresholding
divides the image into
regions and computes
thresholds for individual
regions. Used when
many objects are in a
scene.
Histogram Equalization
• The objective is to create a (close to) flat histogram

• Io(x,y) = DmP{I(x,y)}
• Adaptive techniques
equalize sections of
images separately.
6
Histogram Equalization
117 123 122 117 Intensity Count Cumulative

115 1 1
116 125 137 124
116 2 3
116 125 130 122 117 2 5

119 1 6
115 121 120 119 120 1 7
121 1 8
122 2 10
68 170 153 68
123 1 11
34 221 255 187 124 1 12

125 2 14
34 221 238 153 130 1 15
137 1 16
0 119 102 85
v’ = (vcumulative – cumulativemin) / ((M*N) - cumulativemin) * 255
Multi-Image Operations
• Image subtraction/addition/averaging
- =
I1 I2 -I3
7
Background Subtraction
• Target objects present high contrast against static

background.
• Subtraction thresholding segmentation shape properties
• Fast and robust in a controlled environment
Bishop, B. and Spong, M.W., “Vision-Based

Control of an Air Hockey Robot,” IEEE Control
Systems Magazine, pp. 23–32, June 1999.
Detecting Intruders
• Image subtraction
- =
ALARM
• Identification
8
Spatial Operations and
Transformations
• Operations that involve pixels and their neighbors
– Transformations
– Image convolutions
– Edge detection
Geometric Image Transformations
original image translated
rotated stretched
9
Recording Blood Glucose Levels
Convolution
In 1D f g f*g
f g f*g
10
2D Convolution and Filters
• Mean Filter: Replaces all pixels with the mean of the

neighboring pixels
F
1 1 1
1/9 1 1 1
1 1 1
• 3×3 Mean Filtering
kernel (mask)
Image Smoothing
• Gaussian Filter gives

more weight at the 1 2 1
central pixel and less 2 4 2
weights to the 1 2 1
neighbors.
• The farther away the The std. dev., , of the
neighbors, the smaller Gaussian determines the
the weight. amount of smoothing.
Original Image Mean Filtered Gaussian Filtered

11
Median Filtering
• A Median Filter Mean Gaussian Median
operates over a window

by selecting the median 3x3
intensity in the window.
• Does not spread noise
but is more
computationally 5x5
expensive to apply
(requires sorting)
123 122 117

7x7
125 137 124
125 130 122 Filtering salt and pepper noise
117, 122, 122, 123, 124, 125, 125, 130, 170

Image Sharpening
• Surface reflectivity
• 3×3 Sharpening Kernel,
amounts to subtracting a
mean filtered (unsharpened)
copy of the image from the
original
-1 -1 -1
-1 8 -1
-1 -1 -1
12
Image Edges
surface normal discontinuity
depth discontinuity
surface color discontinuity
illumination discontinuity
Edge Detection
• What are edges?

– Rapid intensity changes in a small region of the image
– Local derivatives of the image “surface” are used to detect these
large gradient values.
F(x) Edge = sharp variation
x
x
F ’(x)
Large first derivative
13
Edge Detection
• 1D edge detection
f(x)
df ( x) f (x x) f ( x)
lim
dx x 0 x X
X-1 X+1
df ( x) f ( x 1) f ( x 1)
dx 2 Convolve with: -1 0 1
f g f*g
edge operator intensity edge edge
Edge Detection in 2D
y
1D 2D
I(x)
x
I(x) x I(x,y)
dI(x) I(x,y)=[ I(x,y)/ x, I(x,y)/ y]T

dx =[Ix(x,y), Iy(x,y)]T
dI(x) | I(x,y)|=(Ix 2(x,y) + Iy2(x,y))1/2 >Threshold

> Threshold
dx tan = Ix(x,y)/ Iy(x,y)
14
Edge Detection in 2D
0 1
f(x,y) f(x+1,y) • G1 = f(x+1,y) – f(x,y+1)
-1 0
f(x,y+1) f(x+1,y+1) 1 0
• G2 = f(x,y) – f(x+1,y+1)
0 -1
G G12 G22 G1 G2
atan2(G 2 , G1 )
G(x,y) > threshold: (potential) edge pixel found
Edge Detection Filters
• Derivatives amplify noise

G1 G2
– Must smooth the image first
0 1 1 0
Roberts -1 0 0 -1
• Other methods
-1 0 1 -1 -2 -1
– Laplacian of Gaussian (LoG)
Sobel -2 0 2 0 0 0
– Canny Edge Detection -1 0 1 1 2 1
-1 0 1 -1 -1 -1
Prewitt -1 0 1 0 0 0
-1 0 1 1 1 1
15
Edge Detection
Edge Detection:
Laplacian of Gaussian
Laplacian operator smoothed by a Gaussian filter
0 -1 0 -1 -1 -1
-1 4 -1 -1 8 -1
0 -1 0 -1 -1 -1
impulse response transfer function
16
Laplacian of Gaussian
2 2
f f 0 -1 0 -1 -1 -1
L ( x, y ) 2 2
-1 4 -1 -1 8 -1
x y 0 -1 0 -1 -1 -1
x2 y 2
1 x2 y 2 2
LoG ( x, y ) 4
1 e 2
2 2
Canny Edge Detector
1. Smooth the image with a Gaussian kernel

2. Find derivatives along x and y, compute the edge strength E(x,y) and
orientation (x,y) at each pixel.
3. Non-Maxima Suppression: Those pixels whose edge strengths are above
some threshold and whose strengths are larger than their immediate
neighbors along the edge normal direction are kept as edge candidates,
others are labeled non-edges.
Threshold x x
17
Canny Edge Detector:
Non-maximum suppression
3 1 3 5 6 0 0 0 0 6
3 5 4 6 7 0 0 0 0 7
5 6 7 7 6 0 6 7 7 0
7 6 5 6 5 7 0 0 0 0
6 5 6 3 1 6 0 0 0 0
v f(x, y)
vNW vN vNE
v vN v vS direction 0
vW v vE v v NW v vSE direction 45
f(x, y) is edge
v vW v vE direction 90
edge direction vSW vS vSE
v v NE v vSW direction 135
Canny Edge Detector
4. Apply Hysterisis
Thresholding: Instead
of only one threshold, Es(i,j)<H
there is a high threshold Es(i,j)>L
H and low threshold L.
Those pixels with Is edge
strengths above H are
labeled as an edge. If a
pixel is labeled as an
edge, its neighbors along
the edge direction ( +90) Es(i,j)>H
whose strengths are
higher than L are also Is edge
labeled as an edge. This
“chain-reaction” is
continued until all pixels
in the image are Es(i,j)<L
processed. This way
Es(i,j)>L No-edge
strong edges reinforce
adjacent weak edges. No-edge
18
Canny Edge Detection:
Hysteresis Thresholding
6
5 4
7 6
7 3 4
• High and low thresholds

(TH = 6, TL = 4)
• Start with candidates TH
• Compare neighbors along
edge direction against TL
Canny Edge Detector
Original Image Gradient Magnitude Thresholding

Non-maxima suppression,
hysteresis thresholding
19
Canny Edge Detector
color grayscale smoothed
gradient magnitude thresholded & direction shown edge-detected

Segmentation
20
Segmentation: Morphological
Operations
• Erosion: Shrinking
• Dilation: Growing
Dilation
• Set point if any neighbor is set

– Repeat if needed
8-connectivity
• How to erode? 4-connectivity
21
Segmentation:
Morphological Operations
Thinning Operations
Segmentation: Skeletonization
a.k.a. Medial axis transform and grass-fire technique
thinned skeleton
medial axis
22
Segmentation: Medial Axis Transform
Segmentation: Hough Transform
• Hough Transform for detecting line features:

– describe a line as y = mx +b
– or b = (-x)m + y
– for each edge point at (x,y), and for a given number of
allowable slopes, m, figure out what b’s are possible.
Insert these (m,b) pairs into a “Hough transform
image”
– After all edge points are processed high valued (b,m)
pairs (high intensity points in the Hough image)
describe the lines in the image
b
23
Hough Transform for Lines:
Alternative Method
• Slope-intercept form unsuitable for vertical lines
• Use normal representation of line:
x cos + y sin =r
-
0 (xmax2+ymax2)1/2
r
Hough: Circle Detection
• Hough transform can be applied to detect other shapes
• Circle
– (x-a)2 + (x-a)2 = R2
– Accumulator array increments a, b, R lying on surface of cone
– Hough Space is 3D
24
Hough Transform
Edge points
Strongest
lines
described as Set of all
slope, m, possible
and lines
y-intercept,
b
Line Segment Detection Pipeline
25
Segmentation: Edge Linking
• Join edge points into chains

• Fit Curves
Segmentation: Manipulating Images
26
Object Classification
• Object measures
– Area (number of points in region)
– Perimeter (number of points on edge)
– Length (ymax-ymin)
– Width (xmax-xmin)
– shape analysis
• rectangularity
• circularity
Object Measures
• Invariant moments
– central moments (e.g., cog)
• x=1/n (x,y)∈R x
– principal axes
• Shape descriptors
– Fourier descriptors
– medial axis transform
27
Labeling
• Labeling parts and regions
Blob Analysis
28
Pattern Recognition
• Optical Character Recognition
P
Template Matching
29
Frequency Domain Techniques
• Fourier Transform
• Convolution = Multiplication in the Fourier domain
• Low-pass filtering
Fourier Transform
30
Fourier Transform
• Fourier Series
• Fourier Transform
• Discrete Fourier Transform
• Fast Fourier Transform
• Properties of the Fourier Transform
• The Convolution Theorem
• Discrete Fourier Transform
• Discrete Cosine Transform
It Isn’t Always Perfect
In fact it never is!
31
Things We Haven’t Talked About
• Color image processing

• Motion analysis
• 3D image analysis (next class)
32
Mechatronics
Computer Vision for Robotics

ETH Zurich
Vision for Robotics
• Visual Servoing
• Visual Tracking
• 3D Vision
1
End Point Sensing
• Robot joints are always closed-loop

controlled with feedback but the robot
itself is usually “open-loop”.
• Position errors at the joints and errors
due to the finite stiffness of the structure
propagate along the kinematic chain.
• Vision can provide end-point sensing
that gives direct feedback on the final
result (i.e., end-effector position).
• High accuracy requirement on the
kinematic system changes to high
resolution which is easier to achieve.
• Requirements on part fixturing accuracy
is also lessened when the relative
position between the robot end-effector
and the manipulated objects is measured
as well.
Visual Servoing
• Visual servoing is the control of a robot using vision to close a

feedback loop.
• Dynamic look-and-move or direct visual servoing
• Position based vs. image based
• Hutchinson, S., Hager, G.D., Corke, P.I., “A Tutorial on Visual
Servo Control”, IEEE Transactions on Robotics and
Automation, vol. 12, no. 5, pp. 651–670, Oct. 1996.
2
Dynamic Position-based Look-and-
Move
• Vision loop provides position measurements which are used
as an input to the “regular” robot controller.
• In a position based scheme, the inputs from the visual system
are in 3D (task space) coordinates.
Dynamic Image-based Look-and-

Move
• In the image based approach, pixel coordinates of image
features are directly used to drive the robot controller
3
Direct Visual Servoing
• Vision loop is the only

control mechanism on
the joints.
• Slower sampling
rates, kinematic
singularity problem
must be considered.
• In practice, visually
controlled systems
mostly use the
dynamic look-and-
move approach but
are called “visual
servoing systems”
anyway.
Visual Tracking
• Following an object with a camera.

• Detect visual features in the image that belong to the object.
• Estimate the state of the object from the location and other
properties of the features.
• Use time (frame) coherence and prediction of the current
state from the previous ones to narrow your “search window”
between consecutive frames.
• Initiation of tracking: recognition/localization
4
Visual Features
• Point features: Corners, fudicial marks

• Line Features: Edges
• Color, contrast
• Blob analysis, shape properties, area, moments
• Active targets, IR LEDs
Template Matching
• Initialize tracking by storing templates of unique features on the

object.
• In every new image compare this template with the image to find the
best match.
• SSD (Sum of Squared Differences)
5
SSD Tracking
Corner Detection
• “Places where two strong edges meet”
Either Ex or Ey but not both are

large in a neighborhood of corner
Create the “local structure matrix”:
If min( 1, 2) > T
There is a corner!
Used in Harris Corner Detector and
Kanade-Lucas-Tomassi (KLT) Tracker
6
Active Contour Models (Snakes)
• M. Kass, A. Witkin and D.Terzopoulos, “Snake: Active contour model”,

Int. J. Computer Vision, vol. 1, pp. 321–331, 1987.
• An energy formulation of contour tracking problem that “locks” a contour on
to features of interest (edges, corners, etc.) in an image.
• The contour is defined as an elastic

member with stiffness and stores
energy when deflected. It can also
have some pressure inside that
increases when compressed.
External forces defined using image
intensities, image gradients, etc. act
on the contour.
• The shape of the contour defined as
the one that minimizes the total
energy of the contour.
Snakes
Start point Converged
Topologically adapting snakes

7
3D Vision
• A point in the image corresponds to a line-of-sight in the task

space. Depth information is lost during projection.
• We can use multiple views of the same point to add the
necessary constraint on position Stereo Vision
• We can use known spatial relationships between different
points as the constraint Model Based 3D
Stereo Vision
• Knowing the location of the same point in two images, let us intersect the two
lines-of-sight and determine 3D location. Relative pose and projection properties
of the cameras must be known (calibration).
• The difficulty is to find the correspondences in two images that are the projections
of the same point.
• Epipoles: Intersection points of line through focal points
with image plane
• A point in the left image is constrained to a line
in the right image. This is called the
(right) epipolar line. The search window
is narrowed.
Focal point
Epipoles
8
Stereo Matching
• Intensity Based: If the two

images are close enough, the
intensity profiles along the
epipolar lines will be similar
(i.e., shifted and scaled).
• Feature Based: First detect
“good” features in one image and
then find their correspondences
in the other image.
• In addition to epipolar constraint
other constraints (e.g., similarity,
uniqueness, continuity, order)
may also be used.
• Discontinuous depths,
occlusions and repetitivity
(regularity) of the observed
scene are problems.
Structured Light Methods
• An stripe of light is projected onto the objects. Those points on the

stripe are on to the plane defined by the light. This additional
constraint is used to determine 3D position of points.
• The stripe can scan the surface or we can use multi-stripes with
different colors
Camera
Light stripe
9
Structured Light Implementation
Options
• Pattern can be black/white, color or IR
• Multiple cameras to avoid obstructions
Kinect
10
Kinect
Model-Based Tracking
• Knowing the 3D spatial relationship (i.e., the model) between the

points in the image, we can find the position and orientation of the
rigid body constructed by those points.
• Matches between 3 non-collinear model and image points is enough
to analytically solve the 6 DOF pose.
• Usually many more points are matched and a least-squares type
solution is sought.
11
Image Jacobian
• When a rigid object moves (i.e., translates and rotates) in space, the
projections (i.e., image plane locations) of the points on this object changes.
The amount of change is dependent on the location of the point and on its
motion as well.
• For a given instant this relationship is given by the image Jacobian matrix.
Visual Resolvability
The singular value

decomposition of the
image Jacobian defines
the directional properties
of the vision system.
If we divide the Jacobian into translational and

rotational parts, the three singular values and
3V3
2V2
the corresponding right hand singular vectors of
each part define a resolvability ellipsoid in the
task space for translational and rotational
motions of the observed object.
1V1
12
Visual Resolvability
Resolvability ellipsoids for translations shown
Stereo vs. Model-Based Multi-view
Stereo Model-Based
• Matches 2D feature points • Matches different 3D model points
between images to solve depth to 2D points in separate images
• Must have the same features • Not necessary to have the same
visible in both views features visible in both views
Camera 2
Camera 1 Camera 2 Camera 1
13
Model-Based Multi-Camera Tracking
Aperture Problem
Search is
constrained to a
line along the
Only the component of motion along normal to the edge
the normal is locally observable
Model-Based Multi-Camera Tracking
Deviation of static object’s

position from the mean.
Resolvability ellipsoid for
translations is also shown.
14
Mechatronics
Micro- and Nano-robotics

ETH Zurich
1015
Nearest
1012 Stars
Edge of the
109
solar system
Earth
106 to Mars
103
1 km
m
10-3
mm
10-6
10-9
m
nm
Making intelligent machines at
Å CNT sub-millimeter scales
1
What is Micro(Nano)robotics?
• Micro(Nano)robotics is the study of robotics at the micron

(nanometer) scale, and includes robots that are microscale
(nanoscale) in size as well as large robots capable of
manipulating objects that have dimensions in the microscale
(nano) range.
– The field of microrobotics brings together several disciplines,
including microfabrication processes used for producing micro-
scale robots, microactuators, microsensors, and physical
modeling at micro scales.
– Micromanipulation technologies, including the assembly of
micron-sized parts, the manipulation of biological cells, and the
types of robots used to perform these types of tasks also form a
component of microrobotics.
Robotic Manipulation of Rigid Objects

at Micro and Nano Scales Research
• Gravity
becomes negligible as part dimensions fall below
Volume ~100μm
Surface Area
• Surface Tension
caused by adherence of H2O molecules to part
surfaces
+ +
++
• Electrostatic Forces - -
due to differences in electric potential between --
objects
• Van der Waals Forces
atomic level interactions, highly dependent on
surface smoothness
2
Microforces
Research
Pick and Place
vs.
Atomic Force Measurements

Research
3
Self Aligning and Self Assembly
Strategies Research
Fx = 8.87*10-7 N
Fy = -1.10*10-10 N
Stable equilibrium
achieved
Fx = 7.79*10-10 N
Fy = -9.98*10-13 N
Matchstick
Electromagnetics Electrostatics
Microrobotics
Research
• Microrobotics requires that we learn new ways of investigating

traditional robotics
• Fabrication of robotic components
• Sensing
– Optical microscopes
– Capacitive measurements
– Optical measurements
• Actuation
Perception
– Piezoelectric
– Capacitive
– Electromagnetic
– Ultrasonic Manipulation Cognition
• Physical modeling of the world
– New types of interactions with the world
4
Papers of Interest
• Two good papers that lay out the effects that microrobots and
nanorobots must confront
– E. M. Purcell, “Life at Low Reynolds Number,” American J.
Physics, vol. 45, no. 1, pp. 3-11, 1977.
– M. Wautelet, “Scaling Laws in the Macro-, Micro- and
Nanoworlds,” European J. Physics, vol. 22, pp. 601-611, 2001.
For Fluids, this gets really interesting:
Re = lv inertial forces
viscous forces
BioMicroRobotics
10-3
mm
10-6
m
10-9
nm
5
Microrobotics for
Handling Biological Cells Research
• In vitro fertilization (IVF)

– Intracytoplasmic sperm injection (ICSI)
• Transgenic organisms (TO)
– Embryo pronuclei DNA injection
• Cell injury studies
– Membrane properties
50 m
Typical failure

Sun and Nelson, IJRR, Oct-Nov, 2002
MEMS Based Multi-Axis

Research
Cellular Force Sensor
Deep Reactive Ion Etch

of Silicon-on-Insulator
Wafers
Force resolution
down to 10nN
Sun, Nelson, Potasek,
Theory Enikov,
of Robotics J Microeng
& Mechatronics MicroMech, Oct, 2002
— 151-0601-00 12
Potasek, Sun, Fry, Nelson, MEMS2004
6
Investigating Mouse Egg Cell and
Research
Embryo Membrane Properties
oocyte
embryo
BioMicroRobotics
10-3
mm
10-6
m
10-9
nm
7
How Does a Bug Fly?
Control SENSORY INPUT
MOTOR OUTPUT
FLIGHT MUSCLES
Daniels Lab
Zoology
U. of Washington
WING HINGE
WING MOTIONS AERODYNAMICS + FORCES
Why Look at Drosophila (the fruit fly)?
• Outstanding flight performance

• Completely autonomous
• Extreme miniaturization
• Unparalleled robustness
• Self-replicating
• Model Organism
– Decades of research
3mm
Fry
8
Fly Brain Structure
Institut für Biologie III der Albert-Ludwigs-Universität Freiburg
Fly Muscles
Dickinson
9
Neurons to Behaviors:
Measurement of Flight Dynamics: Drosophila (Fruit Fly) Research
5000 frames/s
Wing beat frequency

of 200Hz
Lift forces as
high as 35μN
Theory of Robotics & MechatronicsFry,

— 151-0601-00
Sayaman, and Dickinson, Science, 2003 19
Sun, Potasek, Bell, Fry, and Nelson, JMEMS, 2004
Micromechanical Flying Insect (MFI)

Ron Fearing UC-Berkeley
10
BioMicroRobotics
10-3
mm
10-6
m
10-9
nm
Robotic Surgery
MINIMALLY INVASIVE
11
Given Imaging M2A Camera Pill
Magnetic Steering
Research
magnetic VM B
Helmholtz Coil pair provides
uniform magnetic field at the
direction of coil current center
Direction of Magnetization
M
Magnetic
Torque
12
Magnetic Steering
Research
Fmagnetic VM B
Maxwell Coil pair provides
uniform field gradient at the
center
Magnetic Force
Microassembled Microrobots
Research
• First prototypes 800 m long, 50 m thick

• Laser micromachined steel
• Electroplated Nickel
• Silicon
13
In Vivo Magnetic MicroRobots
• A MicroRobotic Platform for

Investigating
– Microassembly
– Microfabrication
– Deposition of hard magnetic
materials for MEMS
– Microsensors and
microactuators
– Visual Servoing
– Ultrasonic Servoing
– New biomedical applications and
procedures
Wireless magnetically
guided microrobots
NanoRobotics
• Miniaturization beyond
MEMS (MicroElectro-
MechanicalSystems) is
needed
• NEMS (NanoElectro-
MechanicalSystems)
with novel nanoscale
structures will enable
many new Telescoping CNTs Nanoscrolls and
and Hybrid NEMS NEMS
nanosensors and
nanoactuators
Top electrode
Ground electrode
CNT-based NEMS
Theory of Robotics & Mechatronics — 151-0601-00

IRIS Proprietary and Confidential 28
14
Hybrid NEMS
• Top-Down Approaches
– Nanolithography, Nanoimprinting, etching
• Bottom-Up Approaches
– Self-Assembly, Directed Self-Assembly, FIB, EBID, DPL
• Hybrid Approaches to NEMS
– A combination of approaches to develop prototype NEMS
devices
Bottom-up Nanotechnology Top-down Nanotechnology
1Å 1nm 10nm 100nm 1 m
Enabling Hybrid NEMS
Manipulation Measurement
1cm 1 m
Nanomanipulation Nanoinstrumentation
100nm
NEMS
Materials
100nm
Nanomaterials NanoRobotics Nanodevices
100nm
Fabrication Assembly
100nm
100nm 100nm
Nanofabrication Nanoassembly
15
Telescoping Carbon Nanotubes
(CNT)
• MWNT opened via destructive fabrication
Multi-walled Nanotube (MWNT)
(Opened via Destructive Fabrication)
Probe Probe
EBID EBID
Field Emission of Telescoping CNTs
Substrate MWNT EBID Deposit AFM

Cantilever
l l0
g g l
G
V
V
I
16
TopDown NEMS
• Selective Growth of Carbon Nanotubes
Ni catalyst nanodots (50nm dia.)

Single CNTs selectively grown

IRIS Proprietary and Confidential
Nanoscroll Fabrication
with Dr. Grützmacher’s group at PSI
Substrate
A) Simulation Mask
Epitaxial deposition of SiGe
B)
Epitaxial deposition of Si
C)
Spin on photoresist
D)
Photolithography
Inductors
E) Chemical sensors
Dry etching Prototype Structures Force sensors
F)
Remove photoresist
G)
Wet etching
H)
I) 1.2um dia. coil 20nm thick ribbon

IRIS Proprietary and Confidential
17
Summary
• NanoTechnology: The convergence of Solid State Physics,

Biology and Chemistry
• We believe important robotics problems exist in this domain
• Our goal is to push the boundaries from a Robotics Research
perspective
mm
Solid State
m
Molecular Biology
nm Physical Chemistry Robotics
1950 2000
18
Mechatronics
Mobile Robotics

ETH Zurich
Overview
• Introduction
• Mobility
– Kinematics
– Types
• Control
– Deliberate,
– Reactive
– Hierarchical
– Behavioral
• Navigation
– Sensors
– Map Types
1
Uses: Exploration
• Space
– Voyager
– Mars Rover
• Earth
– Cliffs & Volcanoes
– Archeology
– Underwater
Uses: Search and Rescue
• Fire
• Earthquake
• Explosions
2
Uses: Human Interactions
• Tour Guide
• Entertainment
• Security
• Service
Humanoid Robotics
Sony SDR-3X
The “Killer App” ??

3
Big Dog
Control Architectures
• Approaches
– Deliberative Control: Think hard, then act.
– Reactive Control: Don’t think, (re)act.
– Behavior-Based Control: Think the way you act.
– Hybrid Control: Think and act independently, in parallel.
• Considerations
– Processing power
– Planning
– Dynamic environments
– Sensor data quality
4
Deliberative Control
— Think hard, then act.
• Uses available sensory information
• Creates a plan of action by
searching through plan space
• Pros: Can find optimal solution
Sensors
• Cons:
– Rapidly changing environment
Planner
quickly invalidates internal model
– Slow
Sensors
Actuators
Planner
Actuators
Reactive Control
— Don’t think, (re)act.
• Tightly coupled sensor inputs and effector outputs
• Quick response to changing and unstructured
environments
• Braitenberg robots
• Pros:
– Work in highly dynamic environments
– Simple to implement
• Cons:
– Little or no memory
– No internal representation of the world
Sense Sense Sense Sense Sense
Act Act Act Act Act
5
Behavior Based
— Think the way you act.
• The world is its best model
• A set of behaviors takes inputs and sends outputs to each
other
• Planning is accomplished through a network of behaviors that
talk to each other
• No single planner
Level 2
Level 1
Sensors Level 0 Actuators
Hybrid Control — Think and

act independently, in parallel
• Combine best of reactive and
deliberative control
• Often called a “three-layer” system
– Highest layer: deals with high level Plan
goal oriented plans
– Middle layer: interface between the
two (difficult) Middle
Layer
– Lowest layer: deals with immediate
sensing and reactions
Sense Act
6
Sensors
• Sensor Examples
– Odometry
– Time-of-Flight
– Heading
– Vision
• Sensor Uncertainty
Sensors: Odometry
• Most common used

navigation method
• Pros:
– Good short term accuracy
– Inexpensive
– Allows high sampling rate
• Cons:
– Errors accumulate over
time
• Typically fused with other
position measurements to
increase accuracy
7
Sensors: Time-of-Flight
• Based on elementary physics

– d=v*t
– d : round-trip distance
– v : speed of propagation
– t : elapsed time
• Considerations
– Propagation speed
– Detection uncertainties
– Timing considerations
– Surface interaction
Ultrasonic
• Sound pulses are emitted and electronics
measure the time to return.
• Since sound is relatively slow, this
method is easy to implement
• Reflectivity of the surface has a large
influence on accuracy
• Low cost
• 10m range with 2cm
resolution typical
• 30° field of view
• Crosstalk problem
8
Laser
• Known as laser radar or lidar
• Laser energy is emitted in a rapid
sequence of short bursts aimed at
the object being ranged
• The time for the pulse to reflect off
the object and return is measured
• Scanning Laser range finder
– Contain motorized scanning head to
pan the beam back and forth in the
horizontal plane
– 180° or 360° of scanning typical
– Range from .5m to 5.5m typical for
indoor use
– 10 Hz scanning speed typical
Sensors: Heading
• Compass
• Gyroscope
• Accelerometers
• Tilt
• GPS
9
Sensors: Errors
This is what we want
This is what we get
Navigation: Spatial Knowledge
• Purpose:
– Store world representation
• Possible Map types
– Quantitative
– Qualitative
Quantitative Qualitative
10
Navigation: Configuration Space
• Also known as C-Space

• A set of areas that are
“reachable” by the robot
• How?
– Abstract the center of the
robot as a point
– Expand the obstacles by the
size of the robot to account
for its footprint
• In this example, we assume
our robot is 1 square in radius
Navigation: Quantitative Maps
• Express space in terms of physical

distances of travel
• Bird’s eye view of world
• Not dependent upon perspective of
the robot
• Independent of orientation and
position of robot
• Can be used to generate qualitative
representations
• Occupancy grid
11
Path Planning Example:
Wavefront Planner
• Common algorithm to find shortest path between two points
– Similar to a breadth first search
• Simplifications:
– Use a two-dimensional grid for a map of the world
– Reduce distance to discrete intervals
• Assume uniform distances
– Direction is limited from one adjacent cell to another
4-Point 8-Point
Connectivity Connectivity

Wavefront Planner
• Label free space as 0
• Label obstacles as 1
• Label goal as 2 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1
0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1
0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1
0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 1
0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1
0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1
0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1
0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1
0 0 0 0 0 0 0 0 2 0 1 0 0 0 0 1
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
12
Path Planning Example :
Wavefront Planner
• Starting at the goal, set all adjacent cells with a “0” to the
current cell + 1
• Repeat until no 0’s are adjacent to cells with values >= 2
Iteration 1 Iteration 2
0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1
0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1
0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1
0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 1
0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1
0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1
0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 4 4 4 4 4 0 0 1 1 1
0 0 0 0 0 0 0 3 3 3 1 1 1 1 1 1 0 0 0 0 0 0 4 3 3 3 1 1 1 1 1 1
0 0 0 0 0 0 0 3 2 3 1 0 0 0 0 1 0 0 0 0 0 0 4 3 2 3 1 0 0 0 0 1
0 0 0 0 0 0 0 3 3 3 1 0 0 0 0 1 0 0 0 0 0 0 4 3 3 3 1 0 0 0 0 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
Path Planning Example :

Wavefront Planner
Final Iteration
11 11 11 11 11 11 1 1 1 1 1 1 1 1 1 1
10 10 10 10 10 10 1 10 10 10 10 11 12 13 1 1
10 9 9 9 9 9 9 9 9 9 1 1 1 1 1 1
10 9 8 8 8 8 8 8 8 8 1 1 1 1 1 1
10 9 8 7 7 7 7 7 7 7 1 0 0 0 0 1
10 9 8 7 6 6 6 6 6 6 1 1 1 1 1 1
10 9 8 7 6 5 5 5 5 5 5 5 6 1 1 1
10 9 8 7 6 5 4 4 4 4 4 5 6 1 1 1
10 9 8 7 6 5 4 3 3 3 1 1 1 1 1 1
10 9 8 7 6 5 4 3 2 3 1 0 0 0 0 1
10 9 8 7 6 5 4 3 3 3 1 0 0 0 0 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
13
Wavefront Planner
• Final result:
– Only 0’s should remain in unreachable regions
• To find the shortest path, always move toward a cell with a
lower number
– Numbers generated by the wavefront planner are roughly
proportional to the distance from the goal.
Navigation: Qualitative Maps
• Express space in terms of connections between landmarks

• Dependent upon perspective of the robot
• Orientation clues are robot centric
• Usually cannot be used to generate quantitative
representations (but the other way around works)
14
Trapezoidal Decomposition
• Method for dividing the
world into smaller regions
• Assume polygon world
• Method:
– Draw a vertical line from
each vertex until you hit an
obstacle.
– This reduces the world to a
union of trapezoid-shaped
cells

Trapezoidal Decomposition
• Each cell can be thought of as a node in a graph
15
Navigation: Maps
• Questions regarding map type:

– How accurately and efficiently does the robot need to navigate?
– Is navigation time-critical? Is a slightly sub-optimal route
acceptable?
– What are the characteristics of the environment?
– Are there landmarks to provide orientation clues?
– What are the sources of information about the environment that
specify terrains, surface properties, obstacles, etc.?
– What are the properties of the available sensors in the
environment?
Summary
• Actuators
– Frame Assignment
– Kinematics & Inverse Kinematics
– Mobility Types
• Control
– Deliberative Control: Think hard, then act.
– Reactive Control: Don’t think, (re)act.
– Behavior-Based Control: Think the way you act.
– Hybrid Control: Think and act independently, in parallel.
• Perception
– Sensors
– Mapping & Localization
16
Instant Insanity
Richard Paul, Karl Pingle,

Jerome Feldman, Alan Kay
"Instant Insanity”
Second International Joint
Conference on Artificial
Intelligence (IJCAI), 1971
A computer vision system and robotic arm solve the Instant Insanity puzzle,
which has been around for more than a century under various aliases. It
consists of a set of four cubes with one of four colors on each of their six faces.
The goal is to arrange the four cubes in a row so that all four colors appear on
each of the row's four long sides. The order of the cubes doesn't matter, but
that simplicity is deceptive. There are 41,472 different ways of arranging the
four cubes in a row, so this is not a trivial task.
17

Theory of Robotics

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Theory of Robotics

Uploaded by

Copyright:

Available Formats

Theory of Robotics and Mechatronics

Prof. Brad Nelson

Institute of Robotics and Intelligent Systems

Robots—The People’s View

• Humanoids, Evil Robots, BattleBots

Theory of Robotics & Mechatronics — 151-0601-00 2

Swiss watchmaker and

• Part 1: Get to know each other

• Part 2: What is a robot?

• Connection between sensors and actuators

Theory of Robotics & Mechatronics — 151-0601-00 7

The Scale of Robotics

Theory of Robotics & Mechatronics — 151-0601-00 8

Theory of Robotics & Mechatronics — 151-0601-00 9

Deep Space Exploration

Theory of Robotics & Mechatronics — 151-0601-00 10

Theory of Robotics & Mechatronics — 151-0601-00 13

Robotics for Exploring Life at a

Theory of Robotics & Mechatronics — 151-0601-00 14

• In vitro fertilization (IVF)

Theory of Robotics & Mechatronics — 151-0601-00 15

Theory of Robotics & Mechatronics — 151-0601-00 16

Theory of Robotics & Mechatronics — 151-0601-00 17

Atomic Force Measurements

Theory of Robotics & Mechatronics — 151-0601-00 18

A History of Robotics Research

Theory of Robotics & Mechatronics — 151-0601-00 21

What is the Future of Robotics?

• Robots for environments for which humans are ill-suited

• Robotics as a testbed for investigating intelligence

Theory of Robotics & Mechatronics — 151-0601-00 22

Medieval dentist Tele-Operation

Coronary Artery Bypass Grafting

Theory of Robotics & Mechatronics — 151-0601-00 25

Teleoperated Minimally Invasive

Theory of Robotics & Mechatronics — 151-0601-00 26

Theory of Robotics & Mechatronics — 151-0601-00 27

• National Academy of Sciences 2002 report on “Technology

Theory of Robotics & Mechatronics — 151-0601-00 28

Theory of Robotics & Mechatronics — 151-0601-00 29

DARPA’s Grand Challenge

• U.S. Congress mandates robotic forces

• 2005: 5 teams succeeded

Theory of Robotics & Mechatronics — 151-0601-00 30

Honda Asimo, P3, and P2

Theory of Robotics & Mechatronics — 151-0601-00 32

Honda Asimo, P3, and P2

The “Killer App” ??

Theory of Robotics & Mechatronics — 151-0601-00 34

Theory of Robotics & Mechatronics — 151-0601-00 35

Future Robotics Research

• Distributed robotic systems

• Fusing multiple “orthogonal” sensing modalities

• Radically new theories of intelligence

• Robots may not be what we think they are

• The future of robotics is being determined by those that have

Theory of Robotics & Mechatronics — 151-0601-00 36

• Sometimes robots are the only way

Theory of Robotics & Mechatronics — 151-0601-00 37

• Why don’t we use monkeys (humans, dolphins, …)?

• Mechanical Design of Robots

Theory of Robotics & Mechatronics — 151-0601-00 39

What we will learn

• The Mathematical Basics of Robotics Using a Classical