Theory of Robotics and Mechatronics
Course number 151‐0601‐00
Instructors Sascha Stoeter Petr Korba [email protected] [email protected]
Teaching Assistant Hong Ayoung [email protected] +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
Institute of Robotics and Intelligent Systems 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
Theory of Robotics & Mechatronics — 151-0601-00 7
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
Theory of Robotics & Mechatronics — 151-0601-00 8
4 Exploratory Robotics
Theory of Robotics & Mechatronics — 151-0601-00 9
Deep Space Exploration
Voyager I and II
Theory of Robotics & Mechatronics — 151-0601-00 10
5 Exploring the Solar System
Rover II
Beagle 2 Rover I
Mars Rovers
Theory of Robotics & Mechatronics — 151-0601-00 11
Underwater Exploration
Underwater Archeology
Theory of Robotics & Mechatronics — 151-0601-00 12
6 Robotic Surgery
Theory of Robotics & Mechatronics — 151-0601-00 13
Robotics for Exploring Life at a Cellular Level
Theory of Robotics & Mechatronics — 151-0601-00 14
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
Theory of Robotics & Mechatronics — 151-0601-00 15 Sun and Nelson, IJRR, Oct-Nov, 2002
A Microrobotic Cell Manipulation System Research
3 DOF Microrobot
Force Feedback Vision Feedback
Theory of Robotics & Mechatronics — 151-0601-00 16
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
Theory of Robotics & Mechatronics — 151-0601-00 17
Atomic Force Measurements Research
Theory of Robotics & Mechatronics — 151-0601-00 18
9 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 What is the future of Robotics and nm Å CNT Robotics Research?
Theory of Robotics & Mechatronics — 151-0601-00 19
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
Theory of Robotics & Mechatronics — 151-0601-00 20
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
Theory of Robotics & Mechatronics — 151-0601-00 21
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
Theory of Robotics & Mechatronics — 151-0601-00 22
11 Robotics in Industry: Microassembly Research
MEMS
Wafer-level Microassembly of Hybrid Microsystems
Theory of Robotics & Mechatronics — 151-0601-00 23
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
Theory of Robotics & Mechatronics — 151-0601-00 25
Teleoperated Minimally Invasive Procedures • Intuitive Surgical, Inc. – daVinci Surgical System
Theory of Robotics & Mechatronics — 151-0601-00 26
13 Teleoperated Surgery
Surgeon’s Interface
Theory of Robotics & Mechatronics — 151-0601-00 27
Military Robotics
• National Academy of Sciences 2002 report on “Technology Development for Army Unmanned Ground Vehicles” – Searcher – Donkey – Wingman – Hunter-Killer
Theory of Robotics & Mechatronics — 151-0601-00 28
14 Military Robotics: A “Searcher” Robotic System
1997–2002
Theory of Robotics & Mechatronics — 151-0601-00 29
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
Theory of Robotics & Mechatronics — 151-0601-00 30
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
Theory of Robotics & Mechatronics — 151-0601-00 32
16 Humanoid Robotics
Honda Asimo, P3, and P2
Sony SDR-3X
The “Killer App” ??
Theory of Robotics & Mechatronics — 151-0601-00 33
Nanorobotics: Manipulation of Carbon Nanotubes
Theory of Robotics & Mechatronics — 151-0601-00 34
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
Theory of Robotics & Mechatronics — 151-0601-00 35
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
Theory of Robotics & Mechatronics — 151-0601-00 36
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?
Theory of Robotics & Mechatronics — 151-0601-00 37
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.
Theory of Robotics & Mechatronics — 151-0601-00 39
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
Theory of Robotics & Mechatronics — 151-0601-00 40
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 ( 12,, N ) X Y Y Z PZ Z
Theory of Robotics & Mechatronics — 151-0601-00 41
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 P X translational and angular velocity 1 (position and orientation)? P Y 2 P Z 3 6 N X Y Z N Theory of Robotics & Mechatronics — 151-0601-00 42
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.
Theory of Robotics & Mechatronics — 151-0601-00 43
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)
Theory of Robotics & Mechatronics — 151-0601-00 44
22 What we will learn
• Dynamics Given a robot’s joint angles, velocities, accelerations and inertial properties, what are the loads on the actuators?
f (,,) 111,2,,1,2,,1,2,, NNN f (,,) 221,2,,1,2,,1,2,, NNN f (,,) NN1,2, , N 1,2, , N 1,2, , N
Theory of Robotics & Mechatronics — 151-0601-00 45
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
Theory of Robotics & Mechatronics — 151-0601-00 46
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
Theory of Robotics & Mechatronics — 151-0601-00 47
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
Theory of Robotics & Mechatronics — 151-0601-00 48
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
Theory of Robotics & Mechatronics — 151-0601-00 49
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
Theory of Robotics & Mechatronics — 151-0601-00 50
25 Robotics is …
• fun • an interdisciplinary area • unsolved
Theory of Robotics & Mechatronics — 151-0601-00 51
ICRA Video Proceedings Winner 2006
Theory of Robotics & Mechatronics — 151-0601-00 52
26 Theory of Robotics & Mechatronics
Mechanical Design of Robots
Institute of Robotics and Intelligent Systems 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 )
Theory of Robotics & Mechatronics — 151-0601-00 2
1 Common Robotic Tasks
• Manufacturing: Hard Automation vs. Flexible Automation Large Volume vs. Small Volume
• Hazardous Environments Toxic, Radioactive, Underwater, Space
• Medical Surgery, Rehabilitation
Theory of Robotics & Mechatronics — 151-0601-00 3
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
Theory of Robotics & Mechatronics — 151-0601-00 4
2 Robotics Industry
Theory of Robotics & Mechatronics — 151-0601-00 5
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 P Y Y Y
Z PZ
Theory of Robotics & Mechatronics — 151-0601-00 6
3 Degrees of Mobility (DOM)
• The number of independently controlled joints on a robot • DOM DOF
f (,xy )
2 DOM, 2 DOF
2 DOM, 1 DOF: a singularity configuration
Theory of Robotics & Mechatronics — 151-0601-00 7
Robot Types: Gantry Robots
• A.k.a. Cartesian Robots • Large workspace and heavy lifting
Theory of Robotics & Mechatronics — 151-0601-00 8
4 Robot Types: Cylindrical Robots
Theory of Robotics & Mechatronics — 151-0601-00 9
Robot Types: Spherical Robots
• A.k.a. Polar Robots
Theory of Robotics & Mechatronics — 151-0601-00 10
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
Theory of Robotics & Mechatronics — 151-0601-00 11
Robot Types: Articulated Arms
• Mostly 6 DOF • Redundant manipulators – DOM>DOF (normally DOM > 6)
www.snakerobots.com
Robotics Research Corp. 7 DOM Arm
Theory of Robotics & Mechatronics — 151-0601-00 12
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
Theory of Robotics & Mechatronics — 151-0601-00 13
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)
Theory of Robotics & Mechatronics — 151-0601-00 14
7 Industrial Applications of Robots
• Palletizing • Pick and place • Parallel robots enable very fast pick and place
Theory of Robotics & Mechatronics — 151-0601-00 15
Industrial Applications of Robots
• Assembly • Macro and Micro Scale • Flexible Feeding
Theory of Robotics & Mechatronics — 151-0601-00 16
8 Industrial Applications of Robots
• Welding • Non-contact
Theory of Robotics & Mechatronics — 151-0601-00 17
Industrial Applications of Robots
• Painting • Non-contact
ABB IRB540 Painting Robot
Theory of Robotics & Mechatronics — 151-0601-00 18
9 Industrial Applications of Robots
• Machining
Theory of Robotics & Mechatronics — 151-0601-00 19
Robot Programming
• Robots are not electric-men • Teach Pendant Low Level • Simulation and off-line programming • Path Planning, Assembly Planning • Artificial Intelligence High Level (mostly research)
Theory of Robotics & Mechatronics — 151-0601-00 20
10 Regrasping (Robot Programming Example)
Theory of Robotics & Mechatronics — 151-0601-00 21
Components of a Robot
• Joints • Sensors • Transmissions and gearboxes • Actuators • Wrists • End-effectors
Theory of Robotics & Mechatronics — 151-0601-00 22
11 Robot Joints
• Prismatic and Revolute Joints • Rigidity, backlash, friction
Theory of Robotics & Mechatronics — 151-0601-00 23
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
Theory of Robotics & Mechatronics — 151-0601-00 24
12 Robot Joints
• Universal (Cardan) and Spherical Joints
Theory of Robotics & Mechatronics — 151-0601-00 25
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
Theory of Robotics & Mechatronics — 151-0601-00 26
13 Transmissions and Gearboxes
Planetary gear drive animation From www.howstuffworks.com
Planetary gearbox
Theory of Robotics & Mechatronics — 151-0601-00 27
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 Theory of Robotics & Mechatronics — 151-0601-00 28
14 Transmissions and Gearboxes
• Tendon Drives
Theory of Robotics & Mechatronics — 151-0601-00 29
Sensors: Encoders
• Linear and Angular Encoders • Digital Output • Can also be used to sense speed
Theory of Robotics & Mechatronics — 151-0601-00 30
15 Sensors: Resolvers
• Resolvers are very robust, durable • Can sense position and velocity
from www.maxonmotor.com
Theory of Robotics & Mechatronics — 151-0601-00 31
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
Theory of Robotics & Mechatronics — 151-0601-00 32
16 Sensors: Force
• Mostly at the wrist, sometimes at the joints and grippers
Robotics Research Corp. 7 DOF Arm
Theory of Robotics & Mechatronics — 151-0601-00 33
Robot Actuators
• Hydraulic • Pneumatic • Electric • Piezo
Theory of Robotics & Mechatronics — 151-0601-00 34
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
Theory of Robotics & Mechatronics — 151-0601-00 35
Hydraulic Actuators
MTS structural testing system
Theory of Robotics & Mechatronics — 151-0601-00 36
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
Theory of Robotics & Mechatronics — 151-0601-00 37
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
Theory of Robotics & Mechatronics — 151-0601-00 38
19 Brushed-DC Motors
• Permanent magnets or coils for stator field • Carbon or metal brushes for commutation create friction and sparks, and require maintenance
Theory of Robotics & Mechatronics — 151-0601-00 39
Brushless-DC Motors
• Permanent magnet in the rotor • Contactless sensors (typically Hall-effect type) sense the position of rotor, stator coils are electronically commutated
Theory of Robotics & Mechatronics — 151-0601-00 40
20 Stepper Motors
• Driven through a train of pulses, drive electronics are more complex • Inherently position controlled if steps are not missed.
Theory of Robotics & Mechatronics — 151-0601-00 41
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 Theory of Robotics & Mechatronics — 151-0601-00 42
21 Robotic End Effectors: Grippers
Parallel jaw grip
Theory of Robotics & Mechatronics — 151-0601-00 43
Robotic End Effectors: Hands
Dr. Peter Allen Columbia Univ.
MIT-Utah Hand
Barret Hand
Robonaut hand
Theory of Robotics & Mechatronics — 151-0601-00 44
22 Robotic End Effectors: welding torches, paint nozzles, vacuum cups
Welding torch
Vacuum Cups
Theory of Robotics & Mechatronics — 151-0601-00 45
Robotic Sheep Shearing (Australia)
• http://www.mech.uwa.edu.au/jpt/shearmagic/Default.html
Theory of Robotics & Mechatronics — 151-0601-00 46
23 Tool Changers
Theory of Robotics & Mechatronics — 151-0601-00 47
Compliance Devices
• Remote Center Compliance (RCC) devices
Theory of Robotics & Mechatronics — 151-0601-00 48
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
Theory of Robotics & Mechatronics — 151-0601-00 49
Wrist Singularities
http://www.anthrobot.com/
Theory of Robotics & Mechatronics — 151-0601-00 50
25 Avoiding Singularities
• NASA 4-axis wrist
Theory of Robotics & Mechatronics — 151-0601-00 51
Precision, Accuracy
Theory of Robotics & Mechatronics — 151-0601-00 52
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
Theory of Robotics & Mechatronics — 151-0601-00 53
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
Theory of Robotics & Mechatronics — 151-0601-00 54
27 Resolution, Precision, Accuracy
Spatial Resolution
Accuracy
+ + +
Arbitrary Target Position
Precision (Repeatability)
Theory of Robotics & Mechatronics — 151-0601-00 55
Angular Resolution
• The spatial resolution due to angular joints changes by configuration
Theory of Robotics & Mechatronics — 151-0601-00 56
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
Theory of Robotics & Mechatronics — 151-0601-00 57
29 Theory of Robotics & Mechatronics
Spatial Descriptions 1 (Classical Formulation)
Institute of Robotics and Intelligent Systems 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.
Theory of Robotics & Mechatronics — 151-0601-00 2
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 0 0 X to an object to represent it? P 0 Y Y0
What about orientation?
X0
Theory of Robotics & Mechatronics — 151-0601-00 3
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
Theory of Robotics & Mechatronics — 151-0601-00 4
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
Theory of Robotics & Mechatronics — 151-0601-00 5
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)
Theory of Robotics & Mechatronics — 151-0601-00 6
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).
Theory of Robotics & Mechatronics — 151-0601-00 7
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
Theory of Robotics & Mechatronics — 151-0601-00 8
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
Theory of Robotics & Mechatronics — 151-0601-00 9
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.
Theory of Robotics & Mechatronics — 151-0601-00 10
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!
Theory of Robotics & Mechatronics — 151-0601-00 11
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
Theory of Robotics & Mechatronics — 151-0601-00 12
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)
Theory of Robotics & Mechatronics — 151-0601-00 13
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 …
Theory of Robotics & Mechatronics — 151-0601-00 14
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( ) s sin( ) Y0 Y1
011 XXsY c X1 011 cos( ) sin( ) YsXcY sin( ) X0 • Relations between two coordinate frames cos( ) • Try to derive it on your own now ! What happens if defined clockwise?
Theory of Robotics & Mechatronics — 151-0601-00 15
Rotation Matrices (Forming)
0 • Rotation matrix R1 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.
XX01 cs 01 PRP001 YY sc 1 0 R1
Theory of Robotics & Mechatronics — 151-0601-00 16
8 Rotation Matrices (Meaning)
000 cs Y0 RXY111 Y 1 sc
0 • The columns of the R1 are the X and Y X1 axes of CF1 described wrt CF0 cos( ) sin( )
sin( ) X 1 1 1 0 0 X 1 Y1 cos( ) 0 1
0 c 0 s X1 Y1 Very important for intuitive s c understanding!
Theory of Robotics & Mechatronics — 151-0601-00 17
Rotation Matrices (Properties) PRP001 • A rotation matrix is orthogonal (even ortho- 1 normal since each column and therefore each row are unit vectors, mutually orthogonal)
PP01 • Rotations do not change the vector length
1010 PRP ()1 • A rotation matrix is always invertible
1010 T RR01 () () R 1• The inverse is simply the transpose 00TT 00 ()R11RRR 11 () I 1 det R0 1 • The determinant of a proper rotation is 1 (improper rotations/reflections incl. -1)
Theory of Robotics & Mechatronics — 151-0601-00 18
9 Rotation and Translation
Given: PP10 what is ? 0010 Y PRPT 11 1 2 0 P P 21 1 Given: PP what is ? P X Y0 1 1121 Y2 PRPT 22 Given: PP20 what is ? 1 001210 0 T2 PRRPTT 12() 2 1 T1 X 0 Given: PP02 what is ? 21TT 0001 PRRPTT ()[()(21 12 ) ] X2 A 6DOF robot would have 7 such frames! Ugly!
Theory of Robotics & Mechatronics — 151-0601-00 19
Homogenous Transformation Matrices
0010 0 01 PRPT 11 Can we write this as PHP 1 ?
XcsTX001 00 1x 01 RT11 001 PP YscTY 1y 01 10011 0 H1
0110 XXsYT c Hint: 1x Points become column vectors 0110 whose last component is 1. YsXcYT 1y 11
Theory of Robotics & Mechatronics — 151-0601-00 20
10 Homogenous Transformation Matrices
001012 PHPHHP 112 Y 1 2 001 002 0 P P H 212 HH PHP 2 1 P X Y0 1 Y2 101010 PHPHP ()10
RT00 T 1 H 0 11 T 0 2 1 1 01 X0 000TT 1 ()RRT111 () H0 X 2 01
Theory of Robotics & Mechatronics — 151-0601-00 21
Homogenous Transformation (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
00 0 XY11 T 1 0 X H1 Y0 1 00 1
• Easy extension to 3D T 0 XYZ000 T 0 1 0 11 1 1 X0 H1 000 1
Theory of Robotics & Mechatronics — 151-0601-00 22
11 Rotations in 3D (Principal / Basic Rotations)
Y0 • Our former 2D rotation can be considered a 3D rotation about the Z axis! Y1 cs 0 X 1 000 cos( ) RscZ ,111 0 XYZ sin( ) sin( ) 001 X0 cos( )
10 0 cs0 Z ,Z 0 1 Rcs 0 R 010 X , Y , Advice: Derive & remember these 3 0 sc sc 0 matrices of principal rotations for the rest of this course
Theory of Robotics & Mechatronics — 151-0601-00 23
Homogenous Transformation (Example in 3D as used in robotics)
• For the rest of this course, remember all these matrix forms and their dimensions:
00 01 RT XX 0 11 H1 01 01 YY 13x 44x H 0 011 000TT Z Z ()RRT111 () H 1 11 0 01 13x 44x
Theory of Robotics & Mechatronics — 151-0601-00 24
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: r r r T 11 12 13 x r11 r12 r13 Tx r21 r22 r23 Ty r r r T H 0 21 22 23 y 1 r r r T 31 32 33 z r31 r32 r33 Tz 0 0 0 1 0 0 0 1
Theory of Robotics & Mechatronics — 151-0601-00 25
Other Transformations (Euclidean, Affine, Projective) • 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 a a a a 0 21 22 23 24 A1 a31 a32 a33 a34 0 0 0 1
Theory of Robotics & Mechatronics — 151-0601-00 26
13 Other Transformations (Euclidean, Affine, Projective) • 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 p p p p 0 21 22 23 24 P1 p31 p32 p33 p34 p41 p42 p43 p44
(observe simply that the 4th row is not [0 0 0 1] anymore)
Theory of Robotics & Mechatronics — 151-0601-00 27
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]
Theory of Robotics & Mechatronics — 151-0601-00 28
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:
Theory of Robotics & Mechatronics — 151-0601-00 29
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
001 0 PRP 1 RRR1,, X Z
• The order is important! Postmultiply for rotations about the current frame
RRX ,, ZZX RR , ,
• Description: Starting from CF0, CF1 is found by rotating along X axis by and then rotating along the original / fix Z axis by 001 0 PRP 1 RRR1,, Z X Premultiply for rotations about the original frame
Theory of Robotics & Mechatronics — 151-0601-00 30
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)
Theory of Robotics & Mechatronics — 151-0601-00 31
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
Theory of Robotics & Mechatronics — 151-0601-00 32
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:
Theory of Robotics & Mechatronics — 151-0601-00 33
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)
Theory of Robotics & Mechatronics — 151-0601-00 34
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)
Theory of Robotics & Mechatronics — 151-0601-00 35
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.
222 kkkk xyz 1
• A single vector θk can be used for direction and magnitude
Theory of Robotics & Mechatronics — 151-0601-00 36
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
RRRRRRkZYZYZ,,,,,,
Theory of Robotics & Mechatronics — 151-0601-00 37
Demo
• Case 1 – 6 Rotation of different points P1 around various axes k (always passing the common origin) by 1 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 yet another axis 0.5 0.2 0.4 0.6 1 0.8 – Case 6 P0 = [0.6 0 0]’ 1 Singularity problem with y x the employed appropriate sequence
P1 = [0 0.6 0]’
Theory of Robotics & Mechatronics — 151-0601-00 38
19 Velocity Vectors
Z1 Z0 1 00 1 Y 0 RT11 H1 X 1 01 1 13x P1 v PHP001 P0 1
X 0 001 001 vHv 1 H1
001 001 vRv 1 R1
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” Theory of Robotics & Mechatronics — 151-0601-00 39
20 Theory of Robotics & Mechatronics
Spatial Descriptions 2 (Screw Theory)
Institute of Robotics and Intelligent Systems 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.
Theory of Robotics & Mechatronics — 151-0601-00 2
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?
Theory of Robotics & Mechatronics — 151-0601-00 3
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 Theory of Robotics & Mechatronics — 151-0601-00 4
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 q x z
y A x
Theory of Robotics & Mechatronics — 151-0601-00 5
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 a b a b a 0 a b (a)^b 3 1 1 3 3 1 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)^
Theory of Robotics & Mechatronics — 151-0601-00 6
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)2 ( ˆt)3 e ˆt I ˆt 2! 3!
• Considering units of time, we compute the net rotation as R( , ) e ˆ Theory of Robotics & Mechatronics — 151-0601-00 7
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!
Theory of Robotics & Mechatronics — 151-0601-00 8
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)
Theory of Robotics & Mechatronics — 151-0601-00 9
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 q v q 2 v 2 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 !
Theory of Robotics & Mechatronics — 151-0601-00 10
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
qRpqaababb qgqaabb 1011
• 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)
Theory of Robotics & Mechatronics — 151-0601-00 11
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)
ˆ ˆt ˆt t ˆt e (I e )( v) p(t) e p(0) where e 0 1
Theory of Robotics & Mechatronics — 151-0601-00 12
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
ˆt ˆt I vt p(t) e p(0) where e 0 1
Theory of Robotics & Mechatronics — 151-0601-00 13
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
Theory of Robotics & Mechatronics — 151-0601-00 14
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’
Theory of Robotics & Mechatronics — 151-0601-00 15
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:
ˆ ˆ T ˆ e (I e )( v) 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
Theory of Robotics & Mechatronics — 151-0601-00 16
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 Theory of Robotics & Mechatronics — 151-0601-00 17
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( ^ )
Theory of Robotics & Mechatronics — 151-0601-00 18
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 Theory of Robotics & Mechatronics — 151-0601-00 19
Example: Rotation about any axis (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 gab ( ) e gab (0) where gab (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 l g ( ) 1 ab 0 0 1 0 0 0 0 1
Theory of Robotics & Mechatronics — 151-0601-00 20
10 Example: Rotation about any axis 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)
Theory of Robotics & Mechatronics — 151-0601-00 21
Example: Rotation about any axis Solution in Matlab – Useful functions 2/2
Theory of Robotics & Mechatronics — 151-0601-00 22
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 z 2
Translation 1.5
• Case 4,5 1
Twist (both, trans & rot) by different 0.5 angles theta 0 0 3 2 1 • Case 6,7,8 1 2 0 Twists around the same axis -1 3 y passing different points x
Theory of Robotics & Mechatronics — 151-0601-00 23
12 Theory of Robotics & Mechatronics
Forward Kinematics
Institute of Robotics and Intelligent Systems ETH Zurich
Robot Forward Kinematics
• “ Given the joint angles, what is the position and orientation of the end-effector? ”
Theory of Robotics & Mechatronics — 151-0601-00 2
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)
Theory of Robotics & Mechatronics — 151-0601-00 3
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)
Theory of Robotics & Mechatronics — 151-0601-00 4
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 0 Hf11 () 1 Hf22 ()
0 2 P Hf33 () 0012 HHHHf3123123 (, , )
003 P HP3
Theory of Robotics & Mechatronics — 151-0601-00 5
Kinematic Chains
0 Hf11 ()
RotZYZ 11 Trans 0,0, d Rot 90 Rot 90
Y Z 1
Y Y X1
X Z Z1
Z0 X Z d1 • Not a unique sequence d1 Y We could arrive at the Y0 final frame also through 1 a different order of X X 0 rotations and translations Theory of Robotics & Mechatronics — 151-0601-00 6
3 Kinematic Chains
0 Hf11 ()
RotZYZ 11 Trans 0,0, d Rot 90 Rot 90
cs 00 100 0 0 010 0 100 11 sc00 010 0 0 100 1 0 00 11 0010 001d1 1000 0 0 10 0001 000 1 0 001 0 0 01
sc 00 11 cs00 • Position vector 11 010d • Rotational part 1 0001
Theory of Robotics & Mechatronics — 151-0601-00 7
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. Theory of Robotics & Mechatronics — 151-0601-00 8
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:
i 1 Ai Rot Z i Trans Z d i Trans X ai Rot X i
Theory of Robotics & Mechatronics — 151-0601-00 9
D-H Convention Four Parameters 3/4
• Our 4 parameters iii(),(),(),()angle d offset a 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
Theory of Robotics & Mechatronics — 151-0601-00 10
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
Theory of Robotics & Mechatronics — 151-0601-00 11
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 • Notice that the joint variable is 2 1 Link 0 defined about the Z axis. Each Link 1 0 (base) link rotates about (or moves along) 1 the previous link’s coordinate Z1 frame.
Z0
Theory of Robotics & Mechatronics — 151-0601-00 12
6 D-H Convention – How do we proceed? 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 Z1 X 0
Z0
Theory of Robotics & Mechatronics — 151-0601-00 13
D-H Convention – How do we proceed? 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 ( If Zi intersects Zi-1 locate Oi at the X 0 Z1 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!
Theory of Robotics & Mechatronics — 151-0601-00 14
7 D-H Convention – How do we proceed? 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 Z -Z plane if Z and Z intersect. Y1 i i-1 i i-1
Y • Place Y to form a right hand 0 X i 1 coordinate frame.
2 X 0 Z1
1 Z0
Theory of Robotics & Mechatronics — 151-0601-00 15
D-H Convention – How do we proceed? Step 4: Place final coordinate frame
• The final frame (i.e., the end- effector or tool frame) is Y X 2 2 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 X 0 Z1
1 Z0
Theory of Robotics & Mechatronics — 151-0601-00 16
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 Theory of Robotics & Mechatronics — 151-0601-00 17
D-H Convention Overview
(link length)
(link offset)
(link twist)
(joint angle)
Theory of Robotics & Mechatronics — 151-0601-00 18
9 D-H Convention Table of Link Parameters
X Y2 2 Link: iii da i * 1 11 0 a 0 Y1 Z2 * 2 22 0 a 0 Y * 0 joint variable X1 a2 X Z 0 1 c s 0 a c 2 i i i i s c 0 a s i 1 i i i i 1 Ai RotZ i TransZ di TransX ai RotX i Z 0 0 1 0 0 a1 0 0 0 1
c1c2 s1s2 c1s2 s1c2 0 c1c2a2 s1s2a2 c1a1 s c c s c c s s 0 s c a c s a s a H 0 A0 A1 1 2 1 2 1 2 1 2 1 2 2 1 2 2 1 1 2 1 2 0 0 1 0 0 0 0 1 Theory of Robotics & Mechatronics — 151-0601-00 19
Three-Link Cylindrical Robot
Link: iii da i * 1 11 d 0 0 * 2 0 d2 0 -90 * 3 0 d3 0 0 * joint variable
Theory of Robotics & Mechatronics — 151-0601-00 20
10 Spherical Wrist
Link: iii da i d6 * 4 4 0 0 -90 * 5 5 0 0 +90 * 6 66 d 0 0 * joint variable
Theory of Robotics & Mechatronics — 151-0601-00 21
The Stanford Manipulator
Link: iii da i * 1 11 d 0 -90 * 2 22 d 0 +90 * 3 0 d3 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 theTheory gripper of Robotics & Mechatronics — 151-0601-00 22
11 PUMA 560 Practise more
Joint θi i ai (mm) di (mm) 190-900 0 200431.8149.09 39090-20.320 40-900433.07 50900 0 600056.25
Theory of Robotics & Mechatronics — 151-0601-00 23
PUMA 560 Resulting Description
6 61i nsap TA0 i = end-effector pose with respect to base frame 1 0001
cs1100 cs4400 sc00 sc00 1 11 3 44 A0 A4 0100 010 d4 cc12345646 cccss ssc 2356 sscccs 145646 00 01 00 0 1 n sc12345646 cccss ssc 2356 cscccs 145646 cs 0 ac cs00 scccsscsc 22 22 55 23 4 5 6 4 6 23 5 6 s cas0 sc00 2 22 22 4 55 ccccsscssssscscc A1 A5 123456462356145646 001d 2 01 0 0 s scccsscssscscscc 123456462356145646 0001 00 0 1 sccs sc c ss 23 4 56 46 2356 csac33330 cs66 00 cccs sc sss 3 s33330 cas 5 sc6600 12345235 145 A A 2 01 0 0 6 a sccs sc css 010d6 12345235 145 00 0 1 0001 scs23 4 5 cc 23 5
cd1623452352343232216452 ccssc sd ac ac sdssd p s1623452352343232216452 dccssc sd acac cdssd dcc scs cd as as 6235 23 4 5 23 4 3 23 2 2
Theory of Robotics & Mechatronics — 151-0601-00 24
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 qH 0 f (q ) n 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)
Theory of Robotics & Mechatronics — 151-0601-00 25
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.
Theory of Robotics & Mechatronics — 151-0601-00 26
13 Manipulator Workspace Example of Joint & Task Spaces
Joint space
Y1
Y0 X1
2 X 0 Z1
1 Z0
Task space
Theory of Robotics & Mechatronics — 151-0601-00 27
Manipulator Workspace 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 Theory of Robotics & Mechatronics — 151-0601-00 28
14 Manipulator Workspace 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
Theory of Robotics & Mechatronics — 151-0601-00 29
Manipulator Workspace 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
Theory of Robotics & Mechatronics — 151-0601-00 30
15 Manipulator Workspace 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
Theory of Robotics & Mechatronics — 151-0601-00 31
Manipulator Workspace 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
Theory of Robotics & Mechatronics — 151-0601-00 32
16 Manipulator Workspace 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
Theory of Robotics & Mechatronics — 151-0601-00 33
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 0 0 1 n 1 H n H1 H 2...H n 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) ˆ ˆ ˆ 1 1 2 2 n n gst ( 1, 2 , ... , n ) e e ... e gst (0) 0 • Note that the resulting POE g st ( 1 , 2 , ... , n ) H n but not its i i i 1 single elements e H i . Why? (missing initial condition!)
Theory of Robotics & Mechatronics — 151-0601-00 34
17 The Product of Exponentials 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:
ˆ 2 2 gst ( 2 ) e gst (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 ˆ ˆ ˆ 1 1 1 1 2 2 gst ( 1, 2 ) e gst ( 2 ) e e gst (0) (once composed, it doesn’t matter in what order we then move the joints later)
Theory of Robotics & Mechatronics — 151-0601-00 35
The Product of Exponentials 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 Theory of Robotics & Mechatronics — 151-0601-00 36
18 The Product of Exponentials 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 th motion of the i 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 Theory of Robotics & Mechatronics — 151-0601-00 37
The Product of Exponentials 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:
ˆ ˆ ˆ 1 1 2 2 n n gst ( ) e e e gst (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
Theory of Robotics & Mechatronics — 151-0601-00 38
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 l l g (0) 1 2 st 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 0 1 2 3 1
Theory of Robotics & Mechatronics — 151-0601-00 39
Example 1: SCARA Forward Kinematics (2 of 3) • We can choose following axis points (or any other point located at the z-axis, why?) 0 0 0 q 0 q l q l l 1 2 1 3 1 2 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 0 direction and has the associated twist 0 Theory of Robotics & Mechatronics — 151-0601-00 40
20 Example 1: SCARA Forward Kinematics (3 of 3) • The forward kinematics map has the form
ˆ ˆ ˆ ˆ R( ) p( ) 1 1 2 2 3 3 4 4 gst ( ) e e e e gst (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 ) e 1 1 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
Theory of Robotics & Mechatronics — 151-0601-00 41
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 i i • 6xR kinematics 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 l l 0 0 0 1 0 0 1 2 gst (0) 0 0 0 0 0 l l0 0 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
Theory of Robotics & Mechatronics — 151-0601-00 42
21 Example 2: Elbow Manipulator 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 l 0 l 0 0 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( ) 1 1 6 6 gst ( ) e e gst (0) 0 1
Theory of Robotics & Mechatronics — 151-0601-00 43
Example 2: Elbow Manipulator Forward Kinematics - Solution
Theory of Robotics & Mechatronics — 151-0601-00 44
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
Theory of Robotics & Mechatronics — 151-0601-00 45
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 l l 0 0 l 0 1 2 2 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
Theory of Robotics & Mechatronics — 151-0601-00 46
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 z • Case 2 1 the same SCARA robot as 0 above -1 -different positions of the 5 -2 the base frame -5 0 0 5 -5 y x
SCARA robot – in home position
Theory of Robotics & Mechatronics — 151-0601-00 47
24 Theory of Robotics & Mechatronics
Rigid Body Velocity
Institute of Robotics and Intelligent Systems 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
Theory of Robotics & Mechatronics — 151-0601-00 2
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
Theory of Robotics & Mechatronics — 151-0601-00 3
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
Theory of Robotics & Mechatronics — 151-0601-00 4
2 Translational (Linear) Velocity
• Points in space can only have a translational velocity. • A coordinate frame represents a rigid group of points (i.e., a rigid body). Z1 • 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 11 vvP P1 Y1 vP • Well-known and simple !
Theory of Robotics & Mechatronics — 151-0601-00 5
Rotational (Angular) Velocity
• When the body rotates, all its points have Z1 different velocities. However, we still can assign to all of them (to the body frame) 1 only one quantity: rotational/angular velocity vector . – The direction of the angular velocity vector X represents the instantaneous axis of rotation of 1 the body. Its magnitude is the rate of rotation (rad/s). 1 P • The translational velocity of any point on the Y1 vP rotating body is given by: 1111 vPP P • In general, a body can both rotate and translate: 1111 vvP P
Theory of Robotics & Mechatronics — 151-0601-00 6
3 Transforming Velocity Vectors
Z1 00 Z 1 0 Y 0 RT11 X 1 H 1 01 13x P1 v 001010 0 P PHPRP 1 1 T1
X 0 001 001 vHv 1 H1
001 001 vRv 1 R1
Y0 • Velocity vectors have direction and magnitude, not position: – Use only the rotation part of the transformation matrix on velocity vectors
Theory of Robotics & Mechatronics — 151-0601-00 7
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
Theory of Robotics & Mechatronics — 151-0601-00 8
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 q (t) R (t)q Z a ab b b q • Note that the description of the point q with Yb respect to the body frame does not vary in time …hence: Ya X the velocity of q in B is zero a but B is allowed to rotate X b (incl. its static point q) with respect to A Theory of Robotics & Mechatronics — 151-0601-00 9
Rotational Velocity The same equations again • So, the velocity can be computed with respect to the spatial frame A: d qa (t) Rab (t)qb v (t) q (t) R (t)q qa dt a ab b • 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 v (t) R (t)R 1(t)R (t)q qa ab ab ab b • It can be shown that, given any rotation matrix R(t) 1 1 R (t)R (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
Theory of Robotics & Mechatronics — 151-0601-00 10
5 Rotational Velocity – New Terms (Assigning names to equations) • We can represent the velocity of a rotating body by a 3-vector s • We define the instantaneous spatial angular velocity, denoted ab , as s 1 ˆ ab R ab (t)Rab (t) This corresponds to the instantaneous angular velocity of the object as seen from the spatial coordinate frame b • We define the instantaneous body angular velocity, denoted ab , as b 1 ˆab Rab (t)R ab (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 1 s and b 1 s ˆab Rab ˆab Rab ab Rab ab
Theory of Robotics & Mechatronics — 151-0601-00 11
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
v (t) ˆ s R (t)q s (t) q (t) qa ab ab b ab a v (t) RT (t)v (t) b (t) q qb ab qa ab b
• Only the first expression corresponds with our physical understanding of velocity of a point… …nevertheless both above equations are mathematically equally true
Theory of Robotics & Mechatronics — 151-0601-00 12
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 R R 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 Theory of Robotics & Mechatronics — 151-0601-00 13
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) gab (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
Theory of Robotics & Mechatronics — 151-0601-00 14
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) (4x4) T T vb RT p ˆ b 1 Rab R ab Rab p ab b ab ab ab Vab gab g ab Vab b T 0 0 ab Rab R ab • The velocity of a point q in the body frame is given by v Vˆ b (t)q b q vb qb ab b ab b ab • 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: b – vab is the velocity of the origin of the body frame (relative to the spatial frame) written in the body coordinates b – and ab is the angular velocity of the body frame, written in the body coordinates
Theory of Robotics & Mechatronics — 151-0601-00 15
Spatial Velocity
• The spatial velocity is specified with a twist (3x3) (3x1) (4x4) (6x1) T T vs R RT p p ˆ s 1 R ab Rab R ab Rab pab p ab s ab ab ab ab ab Vab g ab gab Vab s T 0 0 ab R ab Rab • The velocity of a point in the spatial frame is given by v Vˆ s (t)q s q vs qa ab a ab a ab • 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 s – vab (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… s – ab is the instantaneous angular velocity of the body as viewed in the spatial frame (intuitive)
Theory of Robotics & Mechatronics — 151-0601-00 16
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 Ad g (4x4) (6x6) 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 1 R (R p)^ R R R pˆ Ad g T T Ad g 1 0 R 0 R 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).
Theory of Robotics & Mechatronics — 151-0601-00 17
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 R RT p p Vˆ s gg 1 V s s T (R R ) and the body velocity as vb RT p Vˆ b g 1g V b b T (R R ) • They are related by the adjoint:
s b V Ad gV
Theory of Robotics & Mechatronics — 151-0601-00 18
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 l l cos (t) g(t) 1 2 0 0 1 l0 0 0 0 1 • The spatial velocity of the rigid body is
s s T l1 0 v v R R p p V s vs 0 s 0 s s T (R R ) 0 • The body velocity is l2 0 b b T b b b v v R p v 0 0 V b b T (R 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
Theory of Robotics & Mechatronics — 151-0601-00 19
Velocity of a Screw Motion
• Consider a general case where
ˆ gab ( ) e gab (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
ˆ s 1 ˆ ˆ 1 ˆ ˆ Vab g ab ( )gab ( ) e gab (0) gab (0)e
• Thus, the spatial velocity corresponding to this motion is precisely the velocity generated by the screw
Theory of Robotics & Mechatronics — 151-0601-00 20
10 Coordinate Transformations
• Consider the motion of three coordinate frames, A, B, and C. The following relation exists between their spatial velocities: V s V s Ad V s ac ab gab bc • The following relation exists between their body velocities: b b b Vac Ad 1Vab Vbc gbc
• If A and B are two inertial frames which are fixed relative to each other, then: V s Ad V s V b V b ac gab bc ac bc
Theory of Robotics & Mechatronics — 151-0601-00 21
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: 00 l 0 v 12 s vab s bc Vv 00 Vvbc bc00 bc ab ab ab ab bc 0 2 AB v-fixed – can only rotate! 0 1 • We also calculate 0 • The velocity consists of two Rab 0 Rab components, and they add Ad g ab l together linearly through 0 0 R adjoint as ab 0 l1 cos 1 0 l sin 1 1 0 0 V s V s Ad V s ac ab gab bc 1 2 0 0 0 0 1 1 Theory of Robotics & Mechatronics — 151-0601-00 22
11 Appendix (Rigid Body Velocity)
Theory of Robotics & Mechatronics — 151-0601-00 23
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!
Theory of Robotics & Mechatronics — 151-0601-00 24
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:
Theory of Robotics & Mechatronics — 151-0601-00 25
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.
Theory of Robotics & Mechatronics — 151-0601-00 26
13 How to get derivative of rotation R
• For any invertible matrix R, we can write RR 1 R 1R I Since our R matrix is a rotation then R 1 R T and we can write RRT RT R I Now, we differentiate the above equation and we get R RT RR T 0 which can also be rewritten as R RT (R RT )T 0 Recall the definition of a skew symmetric matrix now: S S T
Thus, we can write S S T 0 (which is exactly what we had above).
Therefore, we can write S R R 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 ( ) .
Theory of Robotics & Mechatronics — 151-0601-00 27
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
Theory of Robotics & Mechatronics — 151-0601-00 28
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… 100 Px 010 P H t y vPPPt xyz tt 001 Pz 000 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. xyz t
t HRotxRotyRotztt (' ', x ) (' ', y ) (' ', z )
Theory of Robotics & Mechatronics — 151-0601-00 29
Differential Rotations (Note: order is not important)
10 0 0 10 y 0 01 0 x 0100 0 Rot(' x ', x ) Rot(' y ', ) 010 y x y 010 cos( ) 1 00 0 1 0001 100 sin( ) z 100 z Rot(' z ', z ) 12*0 0010 0001 Here is a MATLAB symbolic toolbox program to test this
syms dx dy dz %differential rotation matrices Rdx=[1 0 0 0 ; 0 1 -dx 0 ; 0 dx 1 0 ; 0 0 0 1] 10 zy 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] t zx10 %different combinations H Rdx*Rdy*Rdz tt Rdx*Rdz*Rdy yx 10 Rdy*Rdx*Rdz Rdy*Rdz*Rdx 0001 Rdz*Rdx*Rdy Rdz*Rdy*Rdx
Theory of Robotics & Mechatronics — 151-0601-00 30
15 Theory of Robotics & Mechatronics
Jacobian
Institute of Robotics and Intelligent Systems 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 ?
Theory of Robotics & Mechatronics — 151-0601-00 2
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
Theory of Robotics & Mechatronics — 151-0601-00 3
The Jacobian Matrix in Robotics What do we mean? Systematically – Dimension 6 x N
• In robotics we are often interested
Y2 in the velocity of the end-effector
X 2 (tool) frame, expressed in the base (spatial) frame. Y Z 1 2 v0 1 Y 0 J6xN X1 0 N X 0 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:
Jf61xNN (, , )
Theory of Robotics & Mechatronics — 151-0601-00 4
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 a 1
X0 Observe: The dimension - not the expected matrix 6 x 2 2 For example: Where is J ? -- the part of J related to Z 1 the rotational velocity is missing completely here) 1 Z 0 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
Theory of Robotics & Mechatronics — 151-0601-00 5
The Jacobian Matrix in Robotics Structure of J in 3D space
• We can divide the Jacobian into 2 parts 3XN as the translational and rotational Jacobians. 0 1 v Jv 00 PPxx ... 0 1 N J 00 PP N yy... 1 N • Each column of the Jacobian describes the PP00 zz contribution of one joint to the final end-effector ... 1 v0 1 N velocity at a given configuration. 0 00 xx N • We can “build” the Jacobian column-by-column 1 N 00 considering each joint. yy 1 N • We must be careful that each contribution must be 00 expressed in a common coordinate frame so that zz we can add them all together 1 N J6xN • Usually, we use base frame as the common frame Theory of Robotics & Mechatronics — 151-0601-00 6
3 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 ?
Theory of Robotics & Mechatronics — 151-0601-00 7
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 Z 0 001 T • For the other joints find the forward kinematic transform up to that X joint. Y2 2 00 RT11 HA0 0 11 01 13x Y1 Z2 000 0 Y0 X XY11Z1 T 1 1 000 1 X 001 0 HAA Z1 2 212 0011i Z 1 0 Hii AA12 A
Theory of Robotics & Mechatronics — 151-0601-00 8
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.
0000 0 0 XYiiZiiT v Jv,3 1 HAAAii 12 000 1 0 di d 031
• 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 0 0 in the base coordinate frame. v Z i 1 0 di 031
Theory of Robotics & Mechatronics — 151-0601-00 9
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)
000 Z vT Ni (1) 3 X Y2 2 0 00 00 T32 Y3 3 iiZT 1(1)1(1) Ni ZT i Ni i 0 T30 Jv X 3 Y1 Z2 0 T31
Y0 X1
X 0 2
Z1
1 Z0
Theory of Robotics & Mechatronics — 151-0601-00 10
5 The Jacobian for a Revolute Joint 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
XY00ZT0 0 vZT000 HAAA0 ii i i iNii 1(1) ii12 000 1 J v 000 0 0 XYZNN NTN HAAAAN 1 2 i N 000 1
0000 vZTT iNii 11() Jv
Theory of Robotics & Mechatronics — 151-0601-00 11
The Jacobian for a Revolute Joint Part 2: Rotational Velocity – Simple!
• Each revolute joint also induces a rotational velocity about its axis of rotation:
00 Zii 1 J
We only need to write the axis in the spatial coordinates –we use the z-axis of the correct HT again:
00000 0 XYiiZiiTi HAAAii 12 000 1
Theory of Robotics & Mechatronics — 151-0601-00 12
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 ... Ji ... 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 ZTT000 () v0 Z 0 iNi 11 i 1 d J0 i 0 i or J0i i Zi 1 031
(for a revolute joint) (for a prismatic joint)
Theory of Robotics & Mechatronics — 151-0601-00 13
The Jacobian using D-H Convention Example 2: Two-Link Planar Manipulator 1/3
Forward Kinematics: 0 0 z1 T1
X Y2 2
0 T2
Z2
a 2 Y0 Y1 0 a X T2 1 1 0 T1
X 0 T = [0;0;0] 0 Z1 2
z = [0;0;1] (always when using D-H convention) Z0 1 0
Theory of Robotics & Mechatronics — 151-0601-00 14
7 The Jacobian using D-H Convention Example 2: Two-Link Planar Manipulator 2/3 • Both (2 joints) are revolute, therefore the Jacobian matrix will be (6x2) of the form: z T 0 T z0 T 0 T 0 J 0 2 0 1 2 1 0 z0 z1
0 0 st z 0 T 0 • The 1 joint is (always) a rotation about 0 at (spatial) origin 0 1 0 0 a1c1 • The 2nd joint is a rotation about z 0 0 at T 0 a s 1 1 1 1 (both in terms of spatial coordinates 1 0 taken from A ) 1 a1c1 a2c12 T 0 a s a s • The end-effector position is in spatial coordinates at 2 1 1 2 12 (taken from A1 A2 where c12 = cos( 1+ 2) etc.) 0
Theory of Robotics & Mechatronics — 151-0601-00 15
The Jacobian using D-H Convention Example 2: Two-Link Planar Manipulator 3/3 • Performing all required calculations gives: a1s1 a2s12 a2s12 a c a c a c 1 1 2 12 2 12 0 0 0 0 0 0 z0 T2 T0 z1 T2 T1 J z z0 0 0 0 1 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 0 origin of frame 2 (T2 ) 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.
Theory of Robotics & Mechatronics — 151-0601-00 16
8 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 ?
Theory of Robotics & Mechatronics — 151-0601-00 17
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
Theory of Robotics & Mechatronics — 151-0601-00 18
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 ˆ s 1 Vst g st ( )gst ( ) • Applying the chain-rule n g n g ˆ s st 1 st 1 Vst i gst ( ) gst ( ) 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 g V s J s ( ) J s ( ) st g 1 st g 1 st st st st st 1 n
s • We call Jst ( ) the spatial manipulator Jacobian
Theory of Robotics & Mechatronics — 151-0601-00 19
The Spatial Manipulator Jacobian
• Recalling the product-of-exponentials formula ˆ ˆ 1 1 n n gst ( ) e e gst (0) • After “some manipulation”, we compute gst 1 gst Ad ˆ ˆ i e 1 1 e i 1 i 1 i • The spatial manipulator Jacobian becomes s J st ( ) 1 2 n
i Ad ˆ ˆ i e 1 1 e i 1 i 1 • 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 Theory of Robotics & Mechatronics — 151-0601-00 20
10 The Spatial Manipulator Jacobian “After some manupulation …”
Theory of Robotics & Mechatronics — 151-0601-00 21
The Body Manipulator Jacobian
• It’s also possible to define a body manipulator Jacobian b b Vst J st ( )
• Calculated similar to before b J st ( ) 1 n 1 n Ad 1 i ˆ ˆ i i i n n e e gst (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 s ( ) Ad J b ( ) st gst ( ) st
Theory of Robotics & Mechatronics — 151-0601-00 22
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:
0sinsinsin() lll11 11212 qqlqll 0coscoscos() 1211311212 00 0 are points on the axes. • Calculating the twists gives
0 l1 cos 1 l1 cos 1 l2 cos( 1 2 ) 0 0 l sin l sin l sin( ) 0 1 1 1 1 2 1 2
s 0 0 0 1 J st 0 0 0 0 0 0 0 0 1 1 1 0
Theory of Robotics & Mechatronics — 151-0601-00 23
Example 3: Jacobian for a SCARA Robot (solution) Part 1/3: Solve the forward kinematics problem
Theory of Robotics & Mechatronics — 151-0601-00 24
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…
Theory of Robotics & Mechatronics — 151-0601-00 25
Example 3: Jacobian for a SCARA Robot (solution) Part 3/3: Approach 2- based on adjoints
Theory of Robotics & Mechatronics — 151-0601-00 26
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 l cos w2_=expm(skew(w1)*th1)*w2 0 1 1 q1 0 2 q1 0 1 2 1 0 2 cos 1 0 sin 1 1 0
Theory of Robotics & Mechatronics — 151-0601-00 27
Example: Jacobian for the Stanford 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 cos cos 0 1 2 zˆ xˆ 1 2 sin v e e 1 2 3 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 zˆ xˆ q 0 e 1 e 2 l (l )cos cos w 1 3 1 3 1 2 l0 0 l0 (l1 3 )sin 2
Theory of Robotics & Mechatronics — 151-0601-00 28
14 Example: Jacobian for the Stanford 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 zˆ xˆ e 1 e 2 0 c s 4 1 2 1 c2
1 c1c4 s1c2s4 zˆ xˆ zˆ e 1 e 2 e 4 0 s c c c s 5 1 4 1 2 4 0 s2s4
0 c5 (s1c2c4 c1s4 ) s1s2s5 zˆ xˆ zˆ xˆ e 1 e 2 e 4 e 5 1 c (c c c s s ) c s s 6 5 1 2 4 1 4 1 2 5 0 s2c4c5 c2s5
• We can now write the complete Jacobian:
s 0 2 q1 v3 4 q w 5 q w 6 q w J st 1 2 0 4 5 6
Theory of Robotics & Mechatronics — 151-0601-00 29
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 ?
Theory of Robotics & Mechatronics — 151-0601-00 30
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 F y FR F FR F Fz FR x Y y 0 z
X 0
Z1 2 Remark: Force is related to a translational movement Z 1 0 Torque is related to a rotational movement
Theory of Robotics & Mechatronics — 151-0601-00 31
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.
TT WFxFJtip RR WW T tip joints JF T TT R Wjoints FJR TT TT ()()FJR
(recall the dot product of two column vectors A and B) T TTT ANN 11,:BABAB ()ABBA
Theory of Robotics & Mechatronics — 151-0601-00 32
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? ” 0 1 v θ 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”).
Theory of Robotics & Mechatronics — 151-0601-00 33
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
Theory of Robotics & Mechatronics — 151-0601-00 34
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
Theory of Robotics & Mechatronics — 151-0601-00 35
Robot Manipulability
Y X
a2 Z a a a Y1 1 2 2
a1 a2 a2 Y0 X a 1 1 What tip velocities can be attained by the combination of X joint speeds such that: 0 2 θ 22 1 Z1 12 1 Z 0 Y 2 tip Manipulability ellipsoid. 1 the circle becomes through the Jacobian
J X 1 1 tip 1 X J 2 Y Tip Theory of Robotics & Mechatronics — 151-0601-00 36
18 Singular Value Decomposition
• Any m x n matrix J can be decomposed into
T JUmn mm mn VUV nn , are orthonormal
• contains the r singular values of J along its diagonal such that Singular values are non negative numbers ! 12 r 0 • For the typical cases in robotics where nm (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 00000 • r is the rank of J 00000 2 • (n-r) is the nullspace mn 00 000 nmr 000 r 00 000000
Theory of Robotics & Mechatronics — 151-0601-00 37
Singular Value Decomposition
• 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. TTT T Jmn UVuvuvuv mr rr nr 111 2 22 r r r
• Notice how each one of these terms Uuu 12 ur “works” on an “incoming” vector XJ mxn nx1 Vvv 12 vr TT T uv111 u 2 22 v urrr v
"Project onto vu111 (inner product), then scale by then multiply by this value..."
Theory of Robotics & Mechatronics — 151-0601-00 38
19 Singular Value Decomposition
TT T XJ mn n1111222 uv uv uv r r r
12 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
Theory of Robotics & Mechatronics — 151-0601-00 39
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
W
Video frames
• These measures are used for path/task planning and the optimal design of robot geometry. Theory of Robotics & Mechatronics — 151-0601-00 40
20 Lecture “the Jacobian”
Appendix
Theory of Robotics & Mechatronics — 151-0601-00 41
Singular Value Decomposition Matrix Pseudo-inverse (Moore-Penrose)
• How to use SVD for a robust matrix pseudo-inverse ? (Demo_SVD2.m)
[U,S,V'] = svd(M) % where T % M - any matrix whatsoever, Mx = b / multiply by M 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: i.e. 1/(s *s ) % inv(M’*M) = inv((USV’)’USV’) = ii ii % = 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 Theory of Robotics & Mechatronics — 151-0601-00 42
21 SVD-based Computation of the Null Space of J
J x = 0 • Recall: - Any m x n matrix J can be decomposed into T JUmn mm mn VUV nn , are orthonormal 1 00000 where the diagonal matrix (same dimension as J) 00000 2 contains r singular values of J such that mn 00 000 nmr 0 12 r 000 r 00 000000
- 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
Theory of Robotics & Mechatronics — 151-0601-00 43
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 ( ) R R T we get as
• Combining gives the relationship for both Jacobians:
Theory of Robotics & Mechatronics — 151-0601-00 44
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
Theory of Robotics & Mechatronics — 151-0601-00 45
Linear Mapping
Theory of Robotics & Mechatronics — 151-0601-00 46
23 Perpendicular Subspaces
Linear mapping:
Theory of Robotics & Mechatronics — 151-0601-00 47
Inverse Mapping
Theory of Robotics & Mechatronics — 151-0601-00 48
24 Theory of Robotics & Mechatronics
Inverse Kinematics
Institute of Robotics and Intelligent Systems 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
0 gst ( ) H N ( ) Robot sensors Position + Orientation (Pose) (joint space) of the robot in Cartesian space Inverse Kinematics
Theory of Robotics & Mechatronics — 151-0601-00 2
1 Introduction
• Varying angles (joint variables) yields an indefinite number of robot configurations.
• Recall the solution to the forward kinematics problem,(for a 0 given set of angles ) the pose of the robot is gst( ) or HN ( ).
• The solution to the more difficult inverse kinematics problem is to find the joint angles 0 gst ( ) gd H N ( )
for a given/desired configuration gd of the robot in terms of the pose of its end-effector. This is a complex problem! Why?
Theory of Robotics & Mechatronics — 151-0601-00 3
Example: Stanford arm + spherical wrist (Solving the Forward Transform)
rrr11 12 13 Tx Link: iii da i rrr21 22 23 Ty 1 * d 0 -90 11 HAAA0 NN12 rrr31 32 33 Tz * 2 22 d 0 +90 0 RN 3 0 d* 0 0 3 01 * 4 4 0 0 -90 * 5 5 0 0 +90 * 6 6 d06 0 0
ARotTransdTransaRotiZiZiXiXi () () () ( )
12 equations (not independent) 6 unknowns in 2 trigonometric functions
x0
Theory of Robotics & Mechatronics — 151-0601-00 4
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) Theory of Robotics & Mechatronics — 151-0601-00 5
Example: Stanford arm + spherical wrist (Inverse kinematics)
• Suppose that the desired position and orientation of the final frame are given by:
0 H N ( 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:
Theory of Robotics & Mechatronics — 151-0601-00 6
3 Example: Stanford arm + spherical wrist (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!
Theory of Robotics & Mechatronics — 151-0601-00 7
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:
Theory of Robotics & Mechatronics — 151-0601-00 8
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
Theory of Robotics & Mechatronics — 151-0601-00 9
Solving the Forward Transform (Try to find an algebraic solution) • There is in general no “easy way” to go back (perform the inverse transform). 0 H NN AAA123 A 10 • Useful trick: Rewrite the forward A123HAAANN transform matrices in a different order 110 AAH21NN A 3 A and look for simple/suitable terms which you can solve (algebraic approach). 11110 AAAAHANNN 1321 • Each of these give 12 (R 3x3 + T 3) equations. We usually search for expressions that can be combined (add, RT00 0 11 divide, trigonometric identities, etc.) to H 1 01 contain only one of the joint variables. 13 44
000TT 1 RRT 0 111 H1 01 13 44
Theory of Robotics & Mechatronics — 151-0601-00 10
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 0 T3 inverse orientation and inverse position problems. 0 T6
• Many industrial manipulators have spherical “wrists” (all joints in the d6 kinematic chain after the 3rd one) • Those before are called “arm”
Theory of Robotics & Mechatronics — 151-0601-00 11
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
Theory of Robotics & Mechatronics — 151-0601-00 12
6 Decoupled Wrist Kinematics
00 If RT66 and are the desired orientation and position of the tool frame: 1. Find the first three joint angles that place the wrist center at: 00 0 is the Z axis of the tool frame also known as the approach vector) TTdR3666 aa , ( 6 2. Using the joint angles found in Step 1 determine: 0 R(,,)3123 f 3. Find the wrist joint angles that create the orientation matrix: 3010 R(R)636 R
d6 0 T3
0 T6
Theory of Robotics & Mechatronics — 151-0601-00 13
Transcendental Functions
Theory of Robotics & Mechatronics — 151-0601-00 14
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 < π Theory of Robotics & Mechatronics — 151-0601-00 15
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 aa2 2
Theory of Robotics & Mechatronics — 151-0601-00 16
8 Example: Articulated Robot - 1 Geometric approach (for theta1)
T Pcccc xyz wrist center infinitely many solutions
for xcc y 0
1c atan 2(yxc , ) result in different set of 23 and 1c atan 2(yxc , ) A singularity configuration
Theory of Robotics & Mechatronics — 151-0601-00 17
Example: Articulated Robot - 2 (Avoid singularities through appropriate mechanical design)
• The shoulder offset avoids the singularity Right-arm
Left-arm
1 1 atan 2 yxc , c atan 2 yc , x c atan 2dr , 22 d 22 atan 2 dr , d atan 2dx , 222 y d cc atan 2dxyd , 222 cc Theory of Robotics & Mechatronics — 151-0601-00 18
9 Example: Articulated Robot - 3 Geometric approach (for theta2 and theta3)
Problems with cos(.) & sqrt(.)
22 2 2 rsaa 23 cos 3 2aa23 222222 xcc yd 123 zaa c 2aa23
atan 2 1 cos( )2 ,cos( ) 333
233233 atan 2(sr , ) atan 2( as , a ac )
222 atan 2(zxcc , yd c 133233 ) atan 2( asaac , )
Theory of Robotics & Mechatronics — 151-0601-00 19
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
3 RRRRR ,, ZYZ , , , 6
ccc ss ccs sc cs R ,, scc cs scs cc ss s cssc
4 Y 3 5 6 X 3 , Z 3 Theory of Robotics & Mechatronics — 151-0601-00 20
10 Example: Inverse Orientation - 2 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 ss ccs sc cs r11 r 12 r 13 Rscccsscsccssrrr , , 21 22 23 s csscrrr 31 32 33
22 atan 2( 1rr33 ,r 33 ) or atan 2( 1 33 ,r 33 )
If we chose the first solution for then s 0 and
=atan2(rr23 , 13 ) and atan2( r 32 , r 31 )
If we chose the second solution for then s 0 and
=atan2( rr23 , 13 ) and atan2( rr 32 , 31 )
Theory of Robotics & Mechatronics — 151-0601-00 21
Example: Inverse Orientation - 3 Algebraic approach (Case 2 - infinite many solutions)
ccc ss ccs sc cs r11 r 12 0 Rscccsscsccssrr ,, 21 22 0 sc ss c 00 1
Thus only the sum ( ) can be determined as: atan 2(rr21,, 11 ) atan 2( rr 12 11 )
Then atan 2( rr12, 11 )
atan 2(rr22, 21 )
Theory of Robotics & Mechatronics — 151-0601-00 22
11 Inverse Velocity
• Analytic formulation of the Jacobian is usually too complex and symbolic inversion is difficult. Numerical inversion is preferred.
0 1 0 v 1 v J θ J • J must have N=rank= 6 0 6xN 0 N
• 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”.
Theory of Robotics & Mechatronics — 151-0601-00 23
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) •“Dialyticalelimination”–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)
Theory of Robotics & Mechatronics — 151-0601-00 24
12 Theory of Robotics & Mechatronics
Redundant and Parallel Robots
Institute of Robotics and Intelligent Systems 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
Theory of Robotics & Mechatronics — 151-0601-00 2
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
Theory of Robotics & Mechatronics — 151-0601-00 3
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
Theory of Robotics & Mechatronics — 151-0601-00 4
2 Kinematics (Redundant Robots) • The derivation of the forward kinematics is no different than before. Using the product of exponentials: ˆ ˆ 1 1 n n gst ( ) e e gst (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)
Theory of Robotics & Mechatronics — 151-0601-00 5
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 s s Vst J st ( ) 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 SS VJst(1) mx st ( m n )(1) n Sm Vst
Theory of Robotics & Mechatronics — 151-0601-00 6
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
TT1 where JJJJ st () is the Moore-Penrose pseudo-inverse J
S Sm Vst (1)nnmstm JV ( ) ( 1)
Minimum joint velocity
Theory of Robotics & Mechatronics — 151-0601-00 7
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
Theory of Robotics & Mechatronics — 151-0601-00 8
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 Theory of Robotics & Mechatronics — 151-0601-00 9
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).
Theory of Robotics & Mechatronics — 151-0601-00 10
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
ˆ ˆ ˆ ˆ 11 11 1n1 1n1 21 21 2 n2 2 n2 gst e e gst (0) e e gst (0)
Graphics from http://synthetica.eng.uci.edu/~curtis/project2.html Theory of Robotics & Mechatronics — 151-0601-00 11
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 C F 6(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 th – fi is the number of degrees of freedom of the i joint
Theory of Robotics & Mechatronics — 151-0601-00 12
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
Theory of Robotics & Mechatronics — 151-0601-00 13
7 Theory of Robotics & Mechatronics
Numerical Methods
Institute of Robotics and Intelligent Systems ETH Zurich
Numerical Methods
• Inverse Kinematics – Linear approximation – Solving techniques – Calculating Jacobians – Other concerns • Calibration – Methods
Theory of Robotics & Mechatronics — 151-0601-00 2
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
Theory of Robotics & Mechatronics — 151-0601-00 3
Inverse Kinematics
• gd is the desired pose, and is the actual set of joint angles to get to gd
• Forward Kinematics based on POE ˆ ˆ ˆ 1 1 2 2 n n gst ( ) e e e gst (0)
gd gst ( )
• Inverse Kinematics g 1(g ) st d
• How? -1 – gst is nonlinear -1 – find a linear approximation to gst
Theory of Robotics & Mechatronics — 151-0601-00 4
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 )
• Find by 1 J ( a ) X
Theory of Robotics & Mechatronics — 151-0601-00 5
Linear Approximation
• We can also think of working directly in SE(3) using (spatial and body) Jacobians and homogeneous transformation matrix: ˆ s 1 Vˆ b g ( ) 1 g ( ) V g st ( a )gst ( a ) st a st a s s (substitute) b b V J ( a ) V J ( a ) (rewrite) s ^ b g st ( a ) J ( a ) gst ( a ) g st ( a ) gst ( a ) J ( a ) (discretize) s ^ b g J ( a ) gst ( a ) g gst ( a ) J ( a ) (rewrite) s 1 1 J b ( ) 1 g ( ) 1 g J ( a ) g gst ( a ) a st a
(joint space) (world space)
Theory of Robotics & Mechatronics — 151-0601-00 6
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
ggdsta g() • 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:
ggg dsta () where 0 ≤ ≤ 1
Theory of Robotics & Mechatronics — 151-0601-00 7
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
// pick approximate step to take g gd gst ( a )
s 1 1 // compute change in joint DOFs J ( a ) g gst ( a )
a // apply change to DOFs
Compute new gst // apply forward kinematics to // see where we ended up }
Theory of Robotics & Mechatronics — 151-0601-00 8
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 – J X J = J – X J X = X – (XJ)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?
Theory of Robotics & Mechatronics — 151-0601-00 9
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 TTJJnm1 J J TT(),()JJ1 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 Theory of Robotics & Mechatronics — 151-0601-00 10 5 Pseudo-Inverse • Remember the singular value decomposition 1 00000 00000 2 T mn 00 000 JUmn mm mn VUV nn , are orthonormal 000 r 00 000000 1 0000 1 • The pseudo-inverse is given by: 1 0000 T 2 JV U nm nn nm mm nm 00 00 0001 0 r 00000 00000 • The pseudo-inverse has “inverse-like” properties: JJJ J , JJJ J ()JJJJJJJJ TT , () Theory of Robotics & Mechatronics — 151-0601-00 11 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) T 1 J g gst ( a ) Theory of Robotics & Mechatronics — 151-0601-00 12 6 Nonlinear Methods (3. Approach to IK) • Problem recast to minimization of the following criterion: min gd gst ( ) • Nonlinear methods – Newton-Raphson iteration – Gradient Decent – etc • Matlab tools – fmincon(), lsqnonlin() – Note that availability of analytical Jacobian can drastically speed up these functions Theory of Robotics & Mechatronics — 151-0601-00 13 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 Theory of Robotics & Mechatronics — 151-0601-00 14 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 Theory of Robotics & Mechatronics — 151-0601-00 15 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 Theory of Robotics & Mechatronics — 151-0601-00 16 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 Theory of Robotics & Mechatronics — 151-0601-00 17 Calibration Jacobian • Both open and close-loop methods require numerical calibration • We would like to determine: 1 yymeasured y modeled 2 yJ() p p pa cal a p pp a n ge0 st • Where the Calibration Jacobian matrix Jcal maps changes in calibration parameters p to differences between measured and modeled end effector pose y Theory of Robotics & Mechatronics — 151-0601-00 18 9 Calibration Jacobian • After a long derivation: k 1 1 ˆˆ QAdeAdeds ii iis kk i 0 0 gest 0 n 1 ˆ QAdeAdeds ii s M i 0 0 JJQQQQQcal |||123 || n M • Now, we can use the same methods as for calculating inverse kinematics Theory of Robotics & Mechatronics — 151-0601-00 19 Conclusions • Numerical methods can generally help solve a variety of kinematic problems • Important points – Speed, – Accuracy, – Robustness, – Convergence rate – Units Theory of Robotics & Mechatronics — 151-0601-00 20 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. Theory of Robotics & Mechatronics — 151-0601-00 21 11 Theory of Robotics & Mechatronics Dynamics Institute of Robotics and Intelligent Systems 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 Theory of Robotics & Mechatronics — 151-0601-00 2 1 Robot Dynamics f (,,) 111,2,,1,2,,1,2,, NNN f (,,) 221,2,,1,2,,1,2,, NNN f (,,) NN1,2, , N 1,2, , N 1,2, , N i torque/force at joint i Theory of Robotics & Mechatronics — 151-0601-00 3 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. Theory of Robotics & Mechatronics — 151-0601-00 4 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 mvb F b b b b 0 WhereI 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 Theory of Robotics & Mechatronics — 151-0601-00 5 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 Y this coupling between the links. 2 Link 2 Y1 Z2 Y0 Link 1 X1 X 0 Z1 2 Theory of Robotics & Mechatronics — 151-0601-00 1 6 3 Forces and Moments on a Link Zi 1 Zi i i i r Ci riC 1, ii 1 Yi 1 iC, i i i ()Rii 11 Yi f i Mgi i X i 1 i ii 1 X i ()Rfii 11 Linki i 1 N i fi : 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 ri : vector from Joint i (i.e., origin of Frame i-1) iC, i to the center of mass of Link i expressed in Frame i. (Constant) ri : vector from Joint i+1 (i.e., origin of Frame i) iC 1, i to the center of mass of Link i expressed in Frame i. (Constant) Theory of Robotics & Mechatronics — 151-0601-00 7 Linear Momentum PMv i , mass doesn't change with time iCi dP M aiif ()R11f i M g i dt iCi i i 11 i i M i : Total mass of Link i vi : Velocity of the center of mass of Link i, expressed in Frame i Ci ai : Acceleration of the center of mass of Link i, expressed in Frame i Ci Theory of Robotics & Mechatronics — 151-0601-00 8 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. Hr ii() rdm Z0 V 22 yzii xy ii xz iix 22 r x yxz zy dm i ii i i ii y V xz zy y22 x X 0 ii i i i i z Y0 IIIxx xy xz x II I I xy yy zy y IIIxz zy zz z Inertia Tensor dI()00 0 It00(), i.e. I changes with time dt Theory of Robotics & Mechatronics — 151-0601-00 9 Angular Momentum • The inertia tensor is constant if we express it in terms of a Frame C rigidly attached to the object: CC C CC C II (): Euler's Equation Gyroscopic term ZC Z0 X C X 0 YC Y0 Theory of Robotics & Mechatronics — 151-0601-00 10 5 Angular Momentum I ii ()IRfrRfr i iiiii 11 ( ii ) i ii i ii i i 11 i i iC ,ii i 1 i 1 iC 1, Ii : (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 Theory of Robotics & Mechatronics — 151-0601-00 11 Dynamic Equations M aii f ()R11f i M g i iCi i i 11 i i I ii ()IRfrRfr i iiiii 11 ( ii ) i ii i ii i i 11 i i iC ,ii i 1 i 1 iC 1, NN 11 • For the last link (i=N) there is no “next link” fNN 11 0, 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). Theory of Robotics & Mechatronics — 151-0601-00 12 6 Angular Velocity and Acceleration Yi 1 Y0 Joint i X i 1 Y X X i i 0 Link i Zi 1 Z0 iii,, Zi 00 0 • The velocity and acceleration of each ii 11Z ii Link i is related to the velocity and iii 100 T acceleration of the previous Link i-1 iii RRZ 11 () iii 1 • For the first link (i=1), there is no “previous link” 00 000 0 00 00 0, 0 iii 11 ZZ iiiii 1 iiTiT 1100 i 00 T ii ()RRZRZ i 11 () iiiiiii [()] 1 Theory of Robotics & Mechatronics — 151-0601-00 13 Linear Acceleration Y Y0 i 1 Joint i X i 1 Link i Ci X Yi X i 0 ri Z iC, i i 0 00 00 Zi 1 r vv= r iC 1, i Ciii 1, i iC ,, iii Z vv00= 0 r 0 r 0 i ii 1,1, i iCiCii 0 v : Velocity of the CM of Link i, expressed in Frame 0 Ci • For the first link (i=1) 0 aa00 0, 0 vi 1 : Velocity of the origin of Link i-1, expressed in Frame 0 0,C0 0 aa00 000 r 0 r0 Ciii 1, iiCi iiC, i T aRaiiiiiiii 11 r r CiiiiCiiiCiii 1, , T aRiiiiii 11 a rr iiii rr iiiii iiiiCiCiiiCiC 1,1,,1, Theory of Robotics & Mechatronics — 151-0601-00 14 7 The Lagrangian Method • The Lagrangian of the system is defined as LKV , K : Total kinetic energy of the system V : Total potential energy of the system • Then the dynamic load on each joint is: dL L i () dt ii i : Joint variable (angle or distance) i : Load on joint (torque or force) Theory of Robotics & Mechatronics — 151-0601-00 15 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 Theory of Robotics & Mechatronics — 151-0601-00 16 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 N x ( , ) F with T 1 M x ( ) J ( )M ( )J ( ) T 1 Cx ( , ) J ( ) C( , ) M ( )J ( )J ( ) T N x ( , ) J ( )N( , ) • The force vector, position vector, and Jacobian must all be expressed with respect to the same frame Theory of Robotics & Mechatronics — 151-0601-00 17 Planar R- Robot rt() • 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?” Theory of Robotics & Mechatronics — 151-0601-00 18 9 Planar R- Robot KK SALoad K K I S : Moment of inertia about the axis of rotation of the shoulder 2 IIA : Moment of inertia about the axis of rotation of the arm, ACM, MrL A(/2) MMALoad,: Mass of the arm and the point load 1 KI 2 SS2 1122 K AA IMr Arm 22 K.E. due K.E. due to rotation to arm extension 2 1 2 KLoad Mrr Load 2 Theory of Robotics & Mechatronics — 151-0601-00 19 Planar R- Robot 11222 2 K IISACMA, MrL(/2) Mr Load MM A Load r 22 LKV , no potential energy due to horizontal plane of rotation so VLK 0 and • Find FA, Force load on the prismatic arm joint. Joint variable is r. L MMrAL r dL L i dL dt MMrAL ii dt r L MrL /2 M r 2 r ALoad 2 Arm FMMrMrLMr A A L A /2 Load Theory of Robotics & Mechatronics — 151-0601-00 20 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. Theory of Robotics & Mechatronics — 151-0601-00 21 11 Theory of Robotics & Mechatronics Trajectory Generation and Control Institute of Robotics and Intelligent Systems ETH Zurich Robot Trajectory Control • “ How to define a trajectory in 3D space and how to control the robot to follow this trajectory ” Theory of Robotics & Mechatronics — 151-0601-00 2 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. Theory of Robotics & Mechatronics — 151-0601-00 3 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. Theory of Robotics & Mechatronics — 151-0601-00 4 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. Theory of Robotics & Mechatronics — 151-0601-00 5 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. ()tt00 , ()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. ()tt00 , ()f f 32 ()tatatata 3210 4 equations in 4 unknowns 2 ()tatata 3321 2 Theory of Robotics & Mechatronics — 151-0601-00 6 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. 5432 ()tatatatatata 5432106 equations in 6 unknowns ()tatatatata 5432 4 3 2 • Increasingly oscillatory path as the 54321order increases 32 ()tatatata 205432 12 6 2 Theory of Robotics & Mechatronics — 151-0601-00 7 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. f 2 ()tatata 210 ()tata 2 21 ()ta 2 2 0 tt t0 tb fb t f Theory of Robotics & Mechatronics — 151-0601-00 8 4 Linear Segments with 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 • Usually an acceleration is chosen during blend f and the blending time is computed. We cannot chose the acceleration too low otherwise 0 the blending time will increase t t t ttfb t 0 b h f and the linear portion will “shrink” hb 1 2 to zero. tb tbb 0 tt 2 2 hb 2 ()tt ()4()tt f 0 f 00f Velocity at t Position at tb t b b 2 2 ttfh 0 2 t 4( ) f 0 2 ()tt2 ttttbfbf ()00 0 f 0 Theory of Robotics & Mechatronics — 151-0601-00 9 Linear Segments with 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. Theory of Robotics & Mechatronics — 151-0601-00 10 5 Linear Segments with 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). Theory of Robotics & Mechatronics — 151-0601-00 11 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. A RB B A Theory of Robotics & Mechatronics — 151-0601-00 12 6 Interpolating Orientations • A good way to interpolate orientation is to use the angle-axis representation of orientation. A 0 0 RB RB RA rrr 1 • We can now interpolate cos 1 (11 22 33 ) 2 between kA and kB just like the k k position coordinates. rr32 23 1 • Other interpolation schemes krr 13 31 2sin( ) such as interpolating the Euler rr 21 12 angles are possible. Theory of Robotics & Mechatronics — 151-0601-00 13 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) Theory of Robotics & Mechatronics — 151-0601-00 14 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 C 2 , minimum kinetic energy f 0 f Cf (,,...) 2 C , minimum jerk 0 0 f 2 C , minimum torque change 0 Theory of Robotics & Mechatronics — 151-0601-00 15 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. Theory of Robotics & Mechatronics — 151-0601-00 16 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. RR Controller Joint dynamics 11, 11, RR Controller Joint dynamics 22, 22, Theory of Robotics & Mechatronics — 151-0601-00 17 Electric Motor Dynamics • Torque is proportional to the current through the motor. mmArmature ki • 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. VliRikAAAAAem • 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). Theory of Robotics & Mechatronics — 151-0601-00 18 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. mm, 1 L , L Motor Gearbox Load Im IL LmmL , • Effective inertia is the total inertia as seen from the input or output side. 11 mmm II Lm • With a large gear ratio the motor inertia becomes a more significant portion of the effective inertia. In other words, the torque IL IImeffective, m 2 load on the joint motor is less dependent on the load inertia (which changes III 2 depending on the current configuration of Leffective, L m the robot) Theory of Robotics & Mechatronics — 151-0601-00 19 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 RR Feedback Joint dynamics 11, Controller 11, Theory of Robotics & Mechatronics — 151-0601-00 20 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. f (,,) • The dynamics equations are very 111,2,,1,2,,1,2,, N NNcomplicated and costly to , compute in real-time. The 1,2, ,N Centralized development of efficient recursive Feedforward methods, such the Newton-Euler 1,2, ,N , Controller formulation we studied, were partly motivated by the need to Decentralized 1,2, ,N Feedforward compute the dynamics in real- Controller time. RR Feedback Joint dynamics 11, Controller 11, Theory of Robotics & Mechatronics — 151-0601-00 21 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: 3 mx cx kx f • If we apply the following change of variables, the system is linearized: ff x f m • We can now develop a linear cx kx3 controller (i.e., a PID controller) for this system. Theory of Robotics & Mechatronics — 151-0601-00 22 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 MV() (,) G () vector of Inertia vector Gravity joint torques vector of Coriolis and matrixcentrifugal terms terms M () VG(,) () , the "new" system is nonlinear and decoupled! Theory of Robotics & Mechatronics — 151-0601-00 23 Cartesian-based Control Schemes • Perform control in Cartesian space, rather X than joint space X J-1 Gains Arm d • This gives desired response of the tool frame, rather than the XKin( ) individual joints X F X Gains JT Arm • These two schemes are d not the same! XKin( ) • The full model-based controller can be included in Cartesian-based schemes as well Theory of Robotics & Mechatronics — 151-0601-00 24 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 Theory of Robotics & Mechatronics — 151-0601-00 25 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. Theory of Robotics & Mechatronics — 151-0601-00 26 13 Theory of Robotics & Mechatronics Force Control and Haptics Institute of Robotics and Intelligent Systems ETH Zurich Contact Tasks • Mechanical part mating (assembly) • Contour Tracking • Machining Theory of Robotics & Mechatronics — 151-0601-00 2 1 Force Sensors • Mostly at the wrist, sometimes at the joints and grippers (fingers) Robotics Research Corp. 7 DOF Arm Theory of Robotics & Mechatronics — 151-0601-00 3 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 Theory of Robotics & Mechatronics — 151-0601-00 4 2 Static Force/Torque Relationship Fx 1 F F F F F y 1 R R Fz F R 00T x Y JFR 0 y z N X 0 Z 1 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 Theory of Robotics & Mechatronics — 151-0601-00 5 Stiffness and Compliance • Stiffness – Proportionality constant k that relates a static displacement to the force due to this displacement FkX • We cannot describe (control) force and displacement • Compliance independently – 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”) Theory of Robotics & Mechatronics — 151-0601-00 6 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 v = rω directions along either a pure fy y d vz fz = 0 position command or a pure ωx τx = 0 τ = 0 force command can be ωy y τ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 v f = 0 The number of both type of z z ωx τx = 0 constraints is equal to the ωy τy = 0 degrees-of-freedom of the τz ωz = -ωd task space (usually six). Theory of Robotics & Mechatronics — 151-0601-00 7 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 v f = f natural velocity constrained z z d ωx t = 0 zc x directions. ωy τy = 0 τz ωz = ωd xc yc vx vy fz ωx ωy τz Theory of Robotics & Mechatronics — 151-0601-00 8 4 Peg in Hole z Theory of Robotics & Mechatronics — 151-0601-00 9 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. Theory of Robotics & Mechatronics — 151-0601-00 10 5 Active Compliance • A position controlled robot reacts to positioning errors by applying torques at the joints such that the positioning error is reduced. KEPD KE • 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: FK Sx,6 6 X, K is diagonal • We translate this desired behavior in the task space into the joint space by: TT JF JKS X, X J T • This is the desired (steady-state) response of the actuators JKJS to small joint space errors such that the robot will behave as a 6 DOF spring with spring constant (matrix) KS T • We then modify the control law as: JKJE KE SD Joint space stiffnes matrix Theory of Robotics & Mechatronics — 151-0601-00 11 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. Theory of Robotics & Mechatronics — 151-0601-00 12 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. Theory of Robotics & Mechatronics — 151-0601-00 13 Hybrid Position-Force Control 1000 • Apply position control or force control 0000 along different degrees of freedom of S , Selection matrix the compliance frame 0010 0001 Theory of Robotics & Mechatronics — 151-0601-00 14 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 Theory of Robotics & Mechatronics — 151-0601-00 15 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 Theory of Robotics & Mechatronics — 151-0601-00 16 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 Theory of Robotics & Mechatronics — 151-0601-00 17 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 Theory of Robotics & Mechatronics — 151-0601-00 18 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 Theory of Robotics & Mechatronics — 151-0601-00 19 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. Theory of Robotics & Mechatronics — 151-0601-00 20 10 Theory of Robotics & Mechatronics Introduction to Computer Vision Institute of Robotics and Intelligent Systems ETH Zurich The Three Pillars of Robotics Perception Manipulation Cognition Theory of Robotics & Mechatronics — 151-0601-00 2 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 Theory of Robotics & Mechatronics — 151-0601-00 3 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 Theory of Robotics & Mechatronics — 151-0601-00 4 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. Theory of Robotics & Mechatronics — 151-0601-00 5 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 Theory of Robotics & Mechatronics — 151-0601-00 6 3 The Vision Problem Given Find Theory of Robotics & Mechatronics — 151-0601-00 7 Computer Vision and Computer Graphics Computer World vision model World Computer model graphics • Vision and Graphics are inverse problems Theory of Robotics & Mechatronics — 151-0601-00 8 4 Motion Capture, Games Theory of Robotics & Mechatronics — 151-0601-00 9 Applications of Vision: Document Analysis 2D Barcode Reading Optical Character Recognition Theory of Robotics & Mechatronics — 151-0601-00 10 5 3D Scanning Scanning Michelangelo’s “The David” • The Digital Michelangelo Project • http://graphics.stanford.edu/projects/mich/ • 2 BILLION polygons, accuracy to .29mm Theory of Robotics & Mechatronics — 151-0601-00 11 Medical Imaging, 3D Reconstruction, Diagnostic Assistance Theory of Robotics & Mechatronics — 151-0601-00 12 6 Face Recognition, Biometrics Theory of Robotics & Mechatronics — 151-0601-00 13 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 – … Theory of Robotics & Mechatronics — 151-0601-00 14 7 Robotic Applications of Vision Captured Image • Automated Inspection (faulty) Perfect Images • Automated Assembly From Production Device for Matching • Guidance and Control Light Sensor Interface to Sensor and Door • Part Identification CHUTE 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 Theory of Robotics & Mechatronics — 151-0601-00 15 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 Theory of Robotics & Mechatronics — 151-0601-00 16 8 Automated Inspection Object inspection, sensing, counting Theory of Robotics & Mechatronics — 151-0601-00 17 Automated Positioning/Inspection Scanning Machine Positioning/Inspection Theory of Robotics & Mechatronics — 151-0601-00 18 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 Theory of Robotics & Mechatronics — 151-0601-00 19 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 Theory of Robotics & Mechatronics — 151-0601-00 20 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 Theory of Robotics & Mechatronics — 151-0601-00 21 Automated Assembly • Part Location – pick-and-place • Assembly Verification ABB Flexpicker Theory of Robotics & Mechatronics — 151-0601-00 22 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 Theory of Robotics & Mechatronics — 151-0601-00 23 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 Theory of Robotics & Mechatronics — 151-0601-00 24 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. Theory of Robotics & Mechatronics — 151-0601-00 25 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? Theory of Robotics & Mechatronics — 151-0601-00 26 13 Camera Obscura • The first camera – Known to Aristotle • Why not make the aperture as small as possible? – Less light gets through – Diffraction effects ... Theory of Robotics & Mechatronics — 151-0601-00 27 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 Theory of Robotics & Mechatronics — 151-0601-00 28 14 Single Lens F 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) Theory of Robotics & Mechatronics — 151-0601-00 29 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. Theory of Robotics & Mechatronics — 151-0601-00 30 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? Theory of Robotics & Mechatronics — 151-0601-00 31 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 Theory of Robotics & Mechatronics — 151-0601-00 32 16 Chromatic Aberration • Lens unable to focus all colors to same image point – Wavelength-dependent refractive index • Typically worse away from image center Theory of Robotics & Mechatronics — 151-0601-00 33 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 Theory of Robotics & Mechatronics — 151-0601-00 34 17 Cameras reflected/ serial stream emitted light of charge packets 3D digitization/ computer vision Useful optics sensor Scene hardware algorithms Information 2D image of 2D array of numbers light intensities CAMERAS Theory of Robotics & Mechatronics — 151-0601-00 35 The Photoelectric Effect Incoming Light photon - + Electrical Connection charged plate ! charged plate Polysilicon Gate ink bo + electron Silicon dioxide + - - - - 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 μmsquares) pixels that collect the charge. They are spatial samplers of light intensity. Theory of Robotics & Mechatronics — 151-0601-00 36 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. Theory of Robotics & Mechatronics — 151-0601-00 37 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. Theory of Robotics & Mechatronics — 151-0601-00 38 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 Theory of Robotics & Mechatronics — 151-0601-00 39 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 Theory of Robotics & Mechatronics — 151-0601-00 40 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 Theory of Robotics & Mechatronics — 151-0601-00 41 Types of Images • Binary • Grayscale • Color Theory of Robotics & Mechatronics — 151-0601-00 42 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 May 1997 the male predominance in computer science Theory of Robotics & Mechatronics — 151-0601-00 43 Color-depth vs. resolution 512 512, 24 bpp Theory of Robotics & Mechatronics — 151-0601-00 44 22 Color-depth vs. resolution 512 512, 1 bpp 181 181, 8 bpp Theory of Robotics & Mechatronics — 151-0601-00 45 Color-depth vs. resolution 103 103, 24 bpp 181 181, 8 bpp Theory of Robotics & Mechatronics — 151-0601-00 46 23 Spectral Sensitivities Theory of Robotics & Mechatronics — 151-0601-00 47 Color Cameras Sensor Chip s f m m s f f s Lens (m) beam-splitting mirrors R G R G (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 Theory of Robotics & Mechatronics — 151-0601-00 48 24 Foveon Sensor • CMOS-based technology • Layers for color capturing at each pixel Theory of Robotics & Mechatronics — 151-0601-00 49 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) Theory of Robotics & Mechatronics — 151-0601-00 50 25 Cameras reflected/ serial stream emitted light of charge packets 3D digitization/ computer vision Useful optics sensor Scene hardware algorithms Information 2D image of 2D array of numbers light intensities CAMERAS Theory of Robotics & Mechatronics — 151-0601-00 51 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 Theory of Robotics & Mechatronics — 151-0601-00 52 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. Theory of Robotics & Mechatronics — 151-0601-00 53 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. ffXYCC xss , y sZxC sZ yC f : focal length ssxy,: size of sensor pixels Theory of Robotics & Mechatronics — 151-0601-00 54 27 Camera Calibration • Camera calibration attempts to solve two problems Image (X'u, Y'u) Coordinate – Projection (perspective) System • 3D to 2D, given the 3D Yu Yc X coordinates of a point, u CCD Array determine the location of the Xc corresponding image point Camera -Z Coordinate System – Back-projection (inverse c 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 Theory of Robotics & Mechatronics — 151-0601-00 55 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) Theory of Robotics & Mechatronics — 151-0601-00 56 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 Theory of Robotics & Mechatronics — 151-0601-00 57 Interpolation (Martins, Birk, Kelley (1981)) T • Let Pj = [Xj,Yj,Zj] be a calibration point T • Let Qj = [xj,yj,1] 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 Theory of Robotics & Mechatronics — 151-0601-00 58 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 Theory of Robotics & Mechatronics — 151-0601-00 59 30 Theory of Robotics & Mechatronics Computer Vision Algorithms Institute of Robotics and Intelligent Systems ETH Zurich Commonly Used Algorithms reflected/ serial stream emitted light of charge packets 3D digitization/ computer vision Useful optics sensor Scene hardware algorithms Information 2D image of 2D array of numbers light intensities ALGORITHMS Theory of Robotics & Mechatronics — 151-0601-00 2 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. Theory of Robotics & Mechatronics — 151-0601-00 3 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 Theory of Robotics & Mechatronics — 151-0601-00 4 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 rxy(, ) f (,xy ) gxy (, ) bxy(, ) Theory of Robotics & Mechatronics — 151-0601-00 5 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 Theory of Robotics & Mechatronics — 151-0601-00 6 3 Digital Images • Array of numbers (pixels) – Typically integers 0–255 (unsigned byte) • Pixels have neighbors 4-neighbors 8-neighbors Theory of Robotics & Mechatronics — 151-0601-00 7 Statistical Operations • Thresholding • Histogram Equalization • Multi-Image Operations Theory of Robotics & Mechatronics — 151-0601-00 8 4 Thresholding Theory of Robotics & Mechatronics — 151-0601-00 9 Histograms • Thresholds can be chosen automatically by a technique called histogramming Number 40 of Pixels 30 20 10 0 Pixel value Theory of Robotics & Mechatronics — 151-0601-00 10 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. Theory of Robotics & Mechatronics — 151-0601-00 11 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. Theory of Robotics & Mechatronics — 151-0601-00 12 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 Theory of Robotics & Mechatronics — 151-0601-00 13 Multi-Image Operations • Image subtraction/addition/averaging - = I1 I2 -I3 Theory of Robotics & Mechatronics — 151-0601-00 14 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. Theory of Robotics & Mechatronics — 151-0601-00 15 Detecting Intruders • Image subtraction - = ALARM • Identification Theory of Robotics & Mechatronics — 151-0601-00 16 8 Spatial Operations and Transformations • Operations that involve pixels and their neighbors – Transformations – Image convolutions – Edge detection Theory of Robotics & Mechatronics — 151-0601-00 17 Geometric Image Transformations original image translated rotated stretched Theory of Robotics & Mechatronics — 151-0601-00 18 9 Recording Blood Glucose Levels Theory of Robotics & Mechatronics — 151-0601-00 19 Convolution In 1D f g f*g f g f*g Theory of Robotics & Mechatronics — 151-0601-00 20 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) Theory of Robotics & Mechatronics — 151-0601-00 21 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 Theory of Robotics & Mechatronics — 151-0601-00 22 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 Theory of Robotics & Mechatronics — 151-0601-00 23 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 Theory of Robotics & Mechatronics — 151-0601-00 24 12 Image Edges surface normal discontinuity depth discontinuity surface color discontinuity illumination discontinuity Theory of Robotics & Mechatronics — 151-0601-00 25 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 Theory of Robotics & Mechatronics — 151-0601-00 26 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 Theory of Robotics & Mechatronics — 151-0601-00 27 Edge Detection in 2D y 2D 1D I(x) x I(x)x I(x,y) dI(x) I(x,y)=[ I(x,y)/ x, I(x,y)/ y]T T dx =[Ix(x,y), Iy(x,y)] 2 2 1/2 dI(x) | I(x,y)|=(Ix (x,y) + Iy (x,y)) >Threshold > Threshold dx tan = Ix(x,y)/ Iy(x,y) Theory of Robotics & Mechatronics — 151-0601-00 28 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 2 2 G G1 G2 G1 G2 atan2(G2 ,G1) G(x,y) > threshold: (potential) edge pixel found Theory of Robotics & Mechatronics — 151-0601-00 29 Edge Detection Filters • Derivatives amplify noise G G – Must smooth the image first 1 2 0 1 1 0 Roberts -1 0 0 -1 • Other methods – Laplacian of Gaussian (LoG) -1 0 1 -1 -2 -1 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 Theory of Robotics & Mechatronics — 151-0601-00 30 15 Edge Detection Theory of Robotics & Mechatronics — 151-0601-00 31 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 Theory of Robotics & Mechatronics — 151-0601-00 32 16 Laplacian of Gaussian 2 2 f f 0 -1 0 -1 -1 -1 -1 4 -1 -1 8 -1 L(x, y) 2 2 x y 0 -1 0 -1 -1 -1 x2 y 2 2 2 1 x y 2 LoG(x, y) 1 e 2 4 2 2 Theory of Robotics & Mechatronics — 151-0601-00 33 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 Theory of Robotics & Mechatronics — 151-0601-00 34 17 Canny Edge Detector: Non-maximum suppression 31356 00006 35467 00007 56776 067707 76565 70000 65631 60000 v f(x, y) vNW vN vNE v vN v vS direction 0 vW vvE v vNW v vSE direction 45 f(x, y) is edge v vW v vE direction 90 edge direction vSW vS vSE v vNE v vSW direction 135 Theory of Robotics & Mechatronics — 151-0601-00 35 Canny Edge Detector 4. Apply Hysterisis Thresholding: Instead of only one threshold, Es(i,j) Theory of Robotics & Mechatronics — 151-0601-00 36 18 Canny Edge Detection: Hysteresis Thresholding 6 54 7 6 7 34 5 • High and low thresholds (TH = 6, TL = 4) • Start with candidates TH • Compare neighbors along edge direction against TL Theory of Robotics & Mechatronics — 151-0601-00 37 Canny Edge Detector Original Image Gradient Magnitude Thresholding Non-maxima suppression, hysteresis thresholding Theory of Robotics & Mechatronics — 151-0601-00 38 19 Canny Edge Detector color grayscale smoothed gradient magnitude thresholded & direction shown edge-detected Theory of Robotics & Mechatronics — 151-0601-00 39 Segmentation Theory of Robotics & Mechatronics — 151-0601-00 40 20 Segmentation: Morphological Operations • Erosion: Shrinking • Dilation: Growing Theory of Robotics & Mechatronics — 151-0601-00 41 Dilation • Set point if any neighbor is set – Repeat if needed 8-connectivity • How to erode? 4-connectivity Theory of Robotics & Mechatronics — 151-0601-00 42 21 Segmentation: Morphological Operations Thinning Operations Theory of Robotics & Mechatronics — 151-0601-00 43 Segmentation: Skeletonization a.k.a. Medial axis transform and grass-fire technique thinned skeleton medial axis Theory of Robotics & Mechatronics — 151-0601-00 44 22 Segmentation: Medial Axis Transform Theory of Robotics & Mechatronics — 151-0601-00 45 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 Theory of Robotics & Mechatronics — 151-0601-00 46 23 Hough Transform for Lines: Alternative Method • Slope-intercept form unsuitable for vertical lines • Use normal representation of line: x cos + y sin = r - 2 2 1/2 0(xmax +ymax ) r Theory of Robotics & Mechatronics — 151-0601-00 47 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 Theory of Robotics & Mechatronics — 151-0601-00 48 24 Hough Transform Edge points Strongest lines described as Set of all slope, m, possible and lines y-intercept, b Theory of Robotics & Mechatronics — 151-0601-00 49 Line Segment Detection Pipeline Theory of Robotics & Mechatronics — 151-0601-00 50 25 Segmentation: Edge Linking • Join edge points into chains • Fit Curves Theory of Robotics & Mechatronics — 151-0601-00 51 Segmentation: Manipulating Images Theory of Robotics & Mechatronics — 151-0601-00 52 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 Theory of Robotics & Mechatronics — 151-0601-00 53 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 Theory of Robotics & Mechatronics — 151-0601-00 54 27 Labeling • Labeling parts and regions Theory of Robotics & Mechatronics — 151-0601-00 55 Blob Analysis Theory of Robotics & Mechatronics — 151-0601-00 56 28 Pattern Recognition • Optical Character Recognition P Theory of Robotics & Mechatronics — 151-0601-00 57 Template Matching Theory of Robotics & Mechatronics — 151-0601-00 58 29 Frequency Domain Techniques • Fourier Transform • Convolution = Multiplication in the Fourier domain • Low-pass filtering Theory of Robotics & Mechatronics — 151-0601-00 59 Fourier Transform Theory of Robotics & Mechatronics — 151-0601-00 60 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 Theory of Robotics & Mechatronics — 151-0601-00 61 It Isn’t Always Perfect In fact it never is! Theory of Robotics & Mechatronics — 151-0601-00 62 31 Things We Haven’t Talked About • Color image processing • Motion analysis • 3D image analysis (next class) Theory of Robotics & Mechatronics — 151-0601-00 63 32 Theory of Robotics & Mechatronics Computer Vision for Robotics Institute of Robotics and Intelligent Systems ETH Zurich Vision for Robotics • Visual Servoing • Visual Tracking • 3D Vision Theory of Robotics & Mechatronics — 151-0601-00 2 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. Theory of Robotics & Mechatronics — 151-0601-00 3 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. Theory of Robotics & Mechatronics — 151-0601-00 4 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. Theory of Robotics & Mechatronics — 151-0601-00 5 Dynamic Image-based Look-and- Move • In the image based approach, pixel coordinates of image features are directly used to drive the robot controller Theory of Robotics & Mechatronics — 151-0601-00 6 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. Theory of Robotics & Mechatronics — 151-0601-00 7 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 Theory of Robotics & Mechatronics — 151-0601-00 8 4 Visual Features • Point features: Corners, fudicial marks • Line Features: Edges • Color, contrast • Blob analysis, shape properties, area, moments • Active targets, IR LEDs Theory of Robotics & Mechatronics — 151-0601-00 9 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) Theory of Robotics & Mechatronics — 151-0601-00 10 5 SSD Tracking Theory of Robotics & Mechatronics — 151-0601-00 11 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 Theory of Robotics & Mechatronics — 151-0601-00 12 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. Theory of Robotics & Mechatronics — 151-0601-00 13 Snakes Start point Converged Topologically adapting snakes Theory of Robotics & Mechatronics — 151-0601-00 14 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 Theory of Robotics & Mechatronics — 151-0601-00 15 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 Theory of Robotics & Mechatronics — 151-0601-00 16 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. Theory of Robotics & Mechatronics — 151-0601-00 17 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 Theory of Robotics & Mechatronics — 151-0601-00 18 9 Structured Light Implementation Options • Pattern can be black/white, color or IR • Multiple cameras to avoid obstructions Theory of Robotics & Mechatronics — 151-0601-00 19 Kinect Theory of Robotics & Mechatronics — 151-0601-00 20 10 Kinect Theory of Robotics & Mechatronics — 151-0601-00 21 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. Theory of Robotics & Mechatronics — 151-0601-00 22 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. Theory of Robotics & Mechatronics — 151-0601-00 23 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 V the corresponding right hand singular vectors of 2 2 each part define a resolvability ellipsoid in the task space for translational and rotational motions of the observed object. 1V1 Theory of Robotics & Mechatronics — 151-0601-00 24 12 Visual Resolvability Resolvability ellipsoids for translations shown Theory of Robotics & Mechatronics — 151-0601-00 25 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 Theory of Robotics & Mechatronics — 151-0601-00 26 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 Theory of Robotics & Mechatronics — 151-0601-00 27 Model-Based Multi-Camera Tracking Deviation of static object’s position from the mean. Resolvability ellipsoid for translations is also shown. Theory of Robotics & Mechatronics — 151-0601-00 28 14 Theory of Robotics & Mechatronics Micro- and Nano-robotics Institute of Robotics and Intelligent Systems ETH Zurich 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 10-9 m Making intelligent machines at nm Å 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. Theory of Robotics & Mechatronics — 151-0601-00 3 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 Theory of Robotics & Mechatronics — 151-0601-00 4 2 Microforces Research Pick and Place vs. Theory of Robotics & Mechatronics — 151-0601-00 5 Atomic Force Measurements Research Theory of Robotics & Mechatronics — 151-0601-00 6 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 Theory of Robotics & Mechatronics — 151-0601-00 7 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 – Piezoelectric Perception – Capacitive – Electromagnetic – Ultrasonic Manipulation Cognition • Physical modeling of the world – New types of interactions with the world Theory of Robotics & Mechatronics — 151-0601-00 8 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 = l v inertial forces viscous forces Theory of Robotics & Mechatronics — 151-0601-00 9 BioMicroRobotics 10-3 mm 10-6 m 10-9 nm Theory of Robotics & Mechatronics — 151-0601-00 10 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 Theory of Robotics & Mechatronics — 151-0601-00 11 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,Theory Potasek, of Robotics Enikov, & Mechatronics J Microeng — 151-0601-00MicroMech, Oct, 2002 12 Potasek, Sun, Fry, Nelson, MEMS2004 6 Investigating Mouse Egg Cell and Research Embryo Membrane Properties oocyte embryo Theory of Robotics & Mechatronics — 151-0601-00 13 BioMicroRobotics 10-3 mm 10-6 m 10-9 nm Theory of Robotics & Mechatronics — 151-0601-00 14 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 Theory of Robotics & Mechatronics — 151-0601-00 15 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 Theory of Robotics & Mechatronics — 151-0601-00 16 8 Fly Brain Structure Institut für Biologie III der Albert-Ludwigs-Universität Freiburg Theory of Robotics & Mechatronics — 151-0601-00 17 Fly Muscles Dickinson Theory of Robotics & Mechatronics — 151-0601-00 18 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-00Sayaman, and Dickinson, Science, 2003 19 Sun, Potasek, Bell, Fry, and Nelson, JMEMS, 2004 Micromechanical Flying Insect (MFI) Ron Fearing UC-Berkeley Theory of Robotics & Mechatronics — 151-0601-00 20 10 BioMicroRobotics 10-3 mm 10-6 m 10-9 nm Theory of Robotics & Mechatronics — 151-0601-00 21 Robotic Surgery MINIMALLY INVASIVE Theory of Robotics & Mechatronics — 151-0601-00 22 11 Given Imaging M2A Camera Pill Theory of Robotics & Mechatronics — 151-0601-00 23 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 Theory of Robotics & Mechatronics — 151-0601-00 24 12 Magnetic Steering Research Fmagnetic VM B Maxwell Coil pair provides uniform field gradient at the center Magnetic Force Theory of Robotics & Mechatronics — 151-0601-00 25 Microassembled Microrobots Research • First prototypes 800 m long, 50 m thick • Laser micromachined steel • Electroplated Nickel • Silicon Theory of Robotics & Mechatronics — 151-0601-00 26 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 Theory of Robotics & Mechatronics — 151-0601-00 27 NanoRobotics • Miniaturization beyond MEMS (MicroElectro- MechanicalSystems) is needed • NEMS (NanoElectro- MechanicalSystems) with novel nanoscale structures will enable many new Telescoping CNTs Nanoscrolls and nanosensors and and Hybrid NEMS NEMS 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 Theory of Robotics & Mechatronics — 151-0601-00 29 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 Theory of Robotics & Mechatronics — 151-0601-00 30 15 Telescoping Carbon Nanotubes (CNT) • MWNT opened via destructive fabrication Multi-walled Nanotube (MWNT) (Opened via Destructive Fabrication) Probe Probe EBID EBID Theory of Robotics & Mechatronics — 151-0601-00 31 Field Emission of Telescoping CNTs Substrate MWNT EBID Deposit AFM Cantilever l l0 g g l G V I Theory of Robotics & Mechatronics — 151-0601-00 32 16 TopDown NEMS • Selective Growth of Carbon Nanotubes Ni catalyst nanodots (50nm dia.) Single CNTs selectively grown Theory of Robotics & Mechatronics — 151-0601-00 33 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 Theory of Robotics & Mechatronics — 151-0601-00 34 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 Theory of Robotics & Mechatronics — 151-0601-00 35 18 Theory of Robotics & Mechatronics Mobile Robotics Institute of Robotics and Intelligent Systems ETH Zurich Overview • Introduction • Mobility – Kinematics – Types • Control – Deliberate, – Reactive – Hierarchical – Behavioral • Navigation – Sensors – Map Types Theory of Robotics & Mechatronics — 151-0601-00 2 1 Uses: Exploration • Space – Voyager – Mars Rover • Earth – Cliffs & Volcanoes – Archeology – Underwater Theory of Robotics & Mechatronics — 151-0601-00 3 Uses: Search and Rescue • Fire • Earthquake • Explosions Theory of Robotics & Mechatronics — 151-0601-00 4 2 Uses: Human Interactions • Tour Guide • Entertainment • Security • Service Theory of Robotics & Mechatronics — 151-0601-00 5 Humanoid Robotics Honda Asimo, P3, and P2 Sony SDR-3X The “Killer App” ?? Theory of Robotics & Mechatronics — 151-0601-00 6 3 Big Dog Theory of Robotics & Mechatronics — 151-0601-00 7 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 Theory of Robotics & Mechatronics — 151-0601-00 8 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 quickly invalidates internal model Planner – Slow Sensors Actuators Planner Actuators Theory of Robotics & Mechatronics — 151-0601-00 9 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 Theory of Robotics & Mechatronics — 151-0601-00 10 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 Theory of Robotics & Mechatronics — 151-0601-00 11 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 – Lowest layer: deals with immediate Layer sensing and reactions Sense Act Theory of Robotics & Mechatronics — 151-0601-00 12 6 Sensors • Sensor Examples – Odometry – Time-of-Flight – Heading – Vision • Sensor Uncertainty Theory of Robotics & Mechatronics — 151-0601-00 13 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 Theory of Robotics & Mechatronics — 151-0601-00 14 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 Theory of Robotics & Mechatronics — 151-0601-00 15 Sensors: Time-of-Flight 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 Theory of Robotics & Mechatronics — 151-0601-00 16 8 Sensors: Time-of-Flight 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 Theory of Robotics & Mechatronics — 151-0601-00 17 Sensors: Heading • Compass • Gyroscope • Accelerometers • Tilt • GPS Theory of Robotics & Mechatronics — 151-0601-00 18 9 Sensors: Errors This is what we want This is what we get Theory of Robotics & Mechatronics — 151-0601-00 19 Navigation: Spatial Knowledge • Purpose: – Store world representation • Possible Map types – Quantitative – Qualitative Quantitative Qualitative Theory of Robotics & Mechatronics — 151-0601-00 20 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 Theory of Robotics & Mechatronics — 151-0601-00 21 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 Theory of Robotics & Mechatronics — 151-0601-00 22 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 Theory of Robotics & Mechatronics — 151-0601-00 23 Path Planning Example: Wavefront Planner • Label free space as 0 • Label obstacles as 1 • Label goal as 2 0000001111111111 0000001000000011 0000000000111111 0000000000111111 0000000000100001 0000000000111111 0000000000000111 0000000000000111 0000000000111111 0000000020100001 0000000000100001 1111111111111111 Theory of Robotics & Mechatronics — 151-0601-00 24 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 0000001111111111 0000001111111111 0000001000000011 0000001000000011 0000000000111111 0000000000111111 0000000000111111 0000000000111111 0000000000100001 0000000000100001 0000000000111111 0000000000111111 0000000000000111 0000000000000111 0000000000000111 0000004444400111 0000000333111111 0000004333111111 0000000323100001 0000004323100001 0000000333100001 0000004333100001 1111111111111111 1111111111111111 Theory of Robotics & Mechatronics — 151-0601-00 25 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 Theory of Robotics & Mechatronics — 151-0601-00 26 13 Path Planning Example: 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. Theory of Robotics & Mechatronics — 151-0601-00 27 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) Theory of Robotics & Mechatronics — 151-0601-00 28 14 Path Planning Example: 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 Theory of Robotics & Mechatronics — 151-0601-00 29 Path Planning Example: Trapezoidal Decomposition • Each cell can be thought of as a node in a graph Theory of Robotics & Mechatronics — 151-0601-00 30 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? Theory of Robotics & Mechatronics — 151-0601-00 31 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 Theory of Robotics & Mechatronics — 151-0601-00 32 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. Theory of Robotics & Mechatronics — 151-0601-00 33 17