1. ROBOT VISION Lesson 9: Robot Kinematics Matthias Rüther
Kawada Industries Inc. has introduced the HRP-2P for Robodex 2002.  This humanoid appears to be very impressive. It is 154 cm (60") tall, weighs 58kg (127 lbs) and has 30 DOF. Here is a news release. Notice the LACK of a battery pack.  Here is a new story about HRP2. 

2. Contents Introduction Geometry Direct Kinematics Inverse Kinematics
Overview
Basic Terms
Typical Configurations
Geometry
Homogeneous Transformations
Representing Orientation
Direct Kinematics
Open Chain
Denavit-Hartenberg
Inverse Kinematics
Problem
Numerical Algorithms

3. Overview An industrial robot consists of
A manipulator that consists of a sequence of rigid bodies (links) connected by means of articulation (joints). It is characterized by an arm (for mobility), a wrist (for dexterity) and an end effector (to perform a task).
Actuators that set the manipulator in motion by actuating the joints (electric, hydraulic or pneumatic)
Sensors that measure the manipulators status (proprioceptive sensors) or the status of the environment (exteroceptive sensors).
A control system that controls and supervises manipulator motion.

4. Overview An industrial robot has three fundamental capacities
Material handling: objects are transported from one location to another, their physical characteristics are not altered (e.g. pallettizing, warehouse loading/unloading, sorting, packaging)
Material Manipulation: physical characteristics of objects are changed or their physical identity is lost (e.g. welding, painting, gluing, cutting, drilling, screwing, assembly, …)
Measurement: object properties are measured by sensors on the end effector, so they are able to explore a 3D environment (e.g. quality control by image processing, tactile sensors, …)

5. Basic Terms
Kinematic chain: sequence of links and joints connecting the end effector to the base. Can be open (single chain) or closed (loop within the chain).
Joints:
Revolute: rotational joint
Prismatic: translational joint
Degree of mobility: each joint typ. provides an additional DOM (e.g. 6DOM necessary for 3D motion)
Accuracy: „How close does the robot get to a desired point?“, distance between desired point and actual point. (off line)
Repeatability: „How close does the robot get to a position it has reached before?“ (on line)

6. Repeatability and Accuracy
Repeatability: the measurement of how closely a robot returns to the same position n number of times.
Accuracy: the measurement of how closely the robot moves to a given target coordinate.
Where: Pt is the target position,
Pa the average achieved position,
R is the repeatability, and
A is the accuracy.

7. Many factors can effect the accuracy of a robot such as:
environmental factors (eg temperature, humidity and electrical noise);
kinematic parameters (eg robot link lengths etc);
dynamic parameters (eg structural compliance, friction);
measurement factors (eg resolution and non-linearity of encoders and resolvers);
computational factors (eg digital round off, steady-state control errors); and
application factors (eg installation errors and errors in defining work-piece coordinate frames).

8. Why? Each of these can effect the robot kinematics
depending on the robot model design and manufacturing process the kinematics can be the most significant source of error.
if the internal kinematic model of the robot does not match the actual model we can not accurately predict where the end-effector is. 
Make them match by either
tighter manufacturing tolerances; or
by measuring actual kinematics and incorporating this information into the kinematic model.

9. Basic Terms
Joint space: end effector position and orientation given in joint coordinates.
Operational space: end effector position and orientation given in world coordinates (e.g. cartesian coordinate system)
Workspace:
Reachable WS: set of positions the EE can reach
Dexterous WS: set of positions the EE can reach with a given orientation
Redundancy: more DOM than needed are available

10. Basic Joints Revolute Joint 1 DOF ( Variable - ) Prismatic Joint
1 DOF (linear) (Variables - d)

11. An Example - The PUMA 560 2 3 4 1 The PUMA 560 has SIX revolute joints
There are two more joints on the end effector (the gripper)
The PUMA 560 has SIX revolute joints
A revolute joint has ONE degree of freedom ( 1 DOF) that is defined by its angle

12. Manipulator Structures

13. Manipulator Structures

14. Forward and Inverse Kinematicsxw
Joint 1
Joint 2
Joint 3
Link 1 z1
World (Base) Coordinate Frame
Tool Coordinate Frame q1 q2 zw yt zt
Link Space
n variables (q1 … qn)
Tool Space
6 variables (x,y,z,qx,qy,qz)
Forward K
Another way to think of this is a transform between Link space (i.e. variables such as angles and distances) to Tool space (i.e. the x,z coordinates and orientation angles of the tool frame), and back again.
Inverse K

15. Co-ordinate Frames z y x z 2 x 2 right handed coordinate frames
used as reference frames x z y xw
World (Base) Coordinate Frame
Link frame zw
x 2
z 2
Tool Coordinate Frame yt zt
The coordinate frames are a set of three orthogonal unit vectors.
We use them as reference frames and attach them to each link on the robot, so that each link has its own reference point.

16. F o r w a r d K i n e m a t i c s
F o r w a r d K i n e m a t i c s

17. The Situation:
You have a robotic arm that starts out aligned with the xo-axis.
You tell the first link to move by 1 and the second link to move by 2.
The Quest:
What is the position of the end of the robotic arm?
Solution:
1. Geometric Approach
This might be the easiest solution for the simple situation. However, notice that the angles are measured relative to the direction of the previous link. (The first link is the exception. The angle is measured relative to it’s initial position.) For robots with more links and whose arm extends into 3 dimensions the geometry gets much more tedious.
2. Algebraic Approach
Involves coordinate transformations.

18. Example Problem:
You are have a three link arm that starts out aligned in the x-axis. Each link has lengths l1, l2, l3, respectively. You tell the first one to move by 1 , and so on as the diagram suggests. Find the Homogeneous matrix to get the position of the yellow dot in the X0Y0 frame. Y3 3 2 3 Y2 X3 2 X2
H = Rz(1 ) * Tx1(l1) * Rz(2 ) * Tx2(l2) * Rz(3 )
i.e. Rotating by 1 will put you in the X1Y1 frame.
Translate in the along the X1 axis by l1.
Rotating by 2 will put you in the X2Y2 frame.
and so on until you are in the X3Y3 frame.
The position of the yellow dot relative to the X3Y3 frame is
(l1, 0). Multiplying H by that position vector will give you the
coordinates of the yellow point relative the the X0Y0 frame. Y0 1 X1 1 Y1 X0

19. Slight variation on the last solution:
Make the yellow dot the origin of a new coordinate X4Y4 frame Y3 Y4 3 2 3 Y2 X3 2 X2 X4
H = Rz(1 ) * Tx1(l1) * Rz(2 ) * Tx2(l2) * Rz(3 ) * Tx3(l3)
This takes you from the X0Y0 frame to the X4Y4 frame.
The position of the yellow dot relative to the X4Y4 frame is (0,0). Y0 1 X1 1 Y1 X0
Notice that multiplying by the (0,0,0,1) vector will equal the last column of the H matrix.

20. Denavit - Hartenberg Parameters
More on Forward Kinematics…
Denavit - Hartenberg Parameters

21. Denavit-Hartenberg Notation
Z(i - 1)
Y(i -1)
Y i
Z i
X i
a i
a(i - 1 )
d i
X(i -1)
 i
( i - 1)
IDEA: Each joint is assigned a coordinate frame. Using the Denavit-Hartenberg notation, you need 4 parameters to describe how a frame (i) relates to a previous frame ( i -1 ).
THE PARAMETERS/VARIABLES: , a , d, 

22. The Parameters
You can align the two axis just using the 4 parameters
Z(i - 1)
Y(i -1)
Y i
Z i
X i
a i
a(i - 1 )
d i
X(i -1)
 i
( i - 1)
1) a(i-1)
Technical Definition: a(i-1) is the length of the perpendicular between the joint axes. The joint axes is the axes around which revolution takes place which are the Z(i-1) and Z(i) axes. These two axes can be viewed as lines in space. The common perpendicular is the shortest line between the two axis-lines and is perpendicular to both axis-lines.

23. a(i-1) cont...
Visual Approach - “A way to visualize the link parameter a(i-1) is to imagine an expanding cylinder whose axis is the Z(i-1) axis - when the cylinder just touches the joint axis i the radius of the cylinder is equal to a(i-1).” (Manipulator Kinematics)
It’s Usually on the Diagram Approach - If the diagram already specifies the various coordinate frames, then the common perpendicular is usually the X(i-1) axis. So a(i-1) is just the displacement along the X(i-1) to move from the (i-1) frame to the i frame.
If the link is prismatic, then a(i-1)
is a variable, not a parameter.
Z(i - 1)
X(i -1)
Y(i -1)
( i - 1)
a(i - 1 )
Z i
Y i
X i
a i
d i
 i

24. Technical Definition: The displacement
Technical Definition: Amount of rotation around the common perpendicular so that the joint axes are parallel.
i.e. How much you have to rotate around the X(i-1) axis so that the Z(i-1) is pointing in the same direction as the Zi axis. Positive rotation follows the right hand rule.
3) d(i-1)
Technical Definition: The displacement
along the Zi axis needed to align the a(i-1)
common perpendicular to the ai common
perpendicular.
In other words, displacement along the
Zi to align the X(i-1) and Xi axes.
4) i
Amount of rotation around the Zi axis needed to align the X(i-1) axis with the Xi axis.
Z(i - 1)
X(i -1)
Y(i -1)
( i - 1)
a(i - 1 )
Z i
Y i
X i
a i
d i
 i

25. The Denavit-Hartenberg Matrix
Just like the Homogeneous Matrix, the Denavit-Hartenberg Matrix is a transformation matrix from one coordinate frame to the next. Using a series of D-H Matrix multiplications and the D-H Parameter table, the final result is a transformation matrix from some frame to your initial frame.
Z(i - 1)
Y(i -1)
Y i
Z i
X i
a i
a(i - 1 )
d i
X(i -1)
 i
( i - 1)

26. Denavit-Hartenberg Link Parameter Table
3 Revolute Joints Z0 X0 Y0 Z1 X2 Y1 Z2 X1 Y2 d2 a0 a1
Denavit-Hartenberg Link Parameter Table
Notice that the table has two uses:
1) To describe the robot with its variables and parameters.
2) To describe some state of the robot by having a numerical values for the variables.

27. d2 a0 a1 Note: T is the D-H matrix with (i-1) = 0 and i = 1. Z0 X0 Y0

28. This is just a rotation around the Z0 axis
This is a translation by a0 followed by a rotation around the Z1 axis
This is a translation by a1 and then d2 followed by a rotation around the X2 and Z2 axis

29. Review of Direct Kinematics
What we have:
Geometric layout of robot (constant under robot motion)
Joint positions qnx1 (variable under robot motion)
What we want:
End Effector (EE) position and orientation wrt base coordinate frame (variable under robot motion)
A coordinate transformation T from EE coordinates to base coordinates, where T is a function of q:
E.g. position of EE coordinate origin is:

30. Review of Direct Kinematics
How we get there:
Place local coordinate frame at each joint i, i=1..n:
Determine common normal (CN) between joint i and i-1 (i.e. line connecting both joint axes in the shortest possible way)
Place coordinate origin oi-1 at intersection of CN and joint axis i-1
Attention Sciavicco/Siciliano: oi-1 at intersection of CN and joint axis i!
Set z-axis in direction of joint axis
Set x-axis in direction of CN
Model robot arm using Denavit-Hartenberg (DH) parameters:
a: shortest distance between consecutive joint axes (length of common normal CN).
: angle between consecutive joint axes around CN.
d: distance between consecutive coordinate origins along i-th joint axis (distance between i-th coordinate origin and intersection of CN with i-th joint axis.
y or : angle between consecutive CN‘s around joint axis.
Depending on joint type, either d or y are variables.

31. Review of Direct Kinematics
From DH-table, calculate T recursively:

32. From Position to Angles
Inverse Kinematics
From Position to Angles
K-1
(q1 … qn)
(x,y,z,qx,qy,qz)
We do inverse kinematics unwittingly, our eyes can determine where an object is in 3D space, and our sub-sub-conscious can figure out the variables required to move our hand to that position (or the tool in our hand to that position…gosh we are clever!)
But for robots we have to think a bit harder and mathematically, not intuitively!

33. Inverse Kinematics What we have: What we want: Problematic, because:
Geometric layout of robot (constant under robot motion)
End Effector (EE) position and orientation wrt base coordinate frame (variable under robot motion)
What we want:
Joint coordinates qnx1 (variable under robot motion).
A mapping f from EE position and orientation to joint coordinates:
Problematic, because:
EE position may be out of reachable workspace (no solution)
EE position and orientation may be out of dexterous workspace (no solution)
EE position may be reachable in several ways (multiple solutions, redundancy)
EE position may be reachable in infinitely many ways (infinite number of solutions, redundancy)
f is highly nonlinear, no closed form solution for complicated canfigurations.

34. Inverse Kinematics How we get there:
Calculate closed-form solution, if possible
fast
accurate
unique for every arm configuration (no general concept)
Use Iterative Method based on differential kinematics
slower
gives approximate solution
converges to a single solution (even if multiple solutions exist)
general algorithm, applicable to all arm configurations

35. Multiple Solutions
·       Because we are dealing with transcendental functions there is often more then one solution.
eg: cos-1(0.5) = 60º and 300º

36. Insoluble

37. Inverse Kinematics x4 x3 z4 y3 x6 x5 z6 y5
Solution easier if the three joint axes intersect at one point => a closed form or analytical solution possible (Pieper’s solution).
Otherwise use a numerical (iterative) solution where:
 speed is poor; and
we may not be able to calculate all solutions. x4 z4 x3 y3 x5 y5 x6 z6

38. A Simple Example Finding : Revolute and Prismatic Joints Combined
More Specifically:
(x , y)
arctan2() specifies that it’s in the first quadrant Y
Finding S: S 1 X

39. Inverse Kinematics of a Two Link Manipulator
Given: l1, l2 , x , y
Find: 1, 2
Redundancy:
A unique solution to this problem does not exist.
Sometimes no solution is possible.
(x , y) 2 l2 l1 1 l2 l1
(x , y) l2 l1

40. The Geometric Solution
Using the Law of Cosines: 2
(x , y) l1  1
Using the Law of Cosines:
Redundant since 2 could be in the first or fourth quadrant.
Redundancy caused since 2 has two possible values

41. The Algebraic Solution
(x , y) l2  2 l1 1
Only Unknown

42. Substituting for c1 and simplifying many times
We know what 2 is from the previous slide. We need to solve for 1 . Now we have two equations and two unknowns (sin 1 and cos 1 )
Substituting for c1 and simplifying many times
Notice this is the law of cosines and can be replaced by x2+ y2

43. A General Solution
A general solution to the IK problem is obtained using differential kinematics.
There exists a linear mapping between EE velocity and Joint velocity.
Joint positions can be computed by integrating velocities over time.

44. Differential Kinematics
What we have:
Geometric layout of robot (constant under robot motion)
Joint velocities
What we want:
Linear velocity
Angular velocity
A mapping J from joint velocities to EE velocities:

45. The Jacobian
Jacobian matrix J(q) gives linear and angular velocity (p‘, ) as a function of joint velocity (q‘)
Depends on the location in joint space, hence J(q)
Can be computed via closed formula.

46. Derivative of a Rotation Matrix
Consider rotation matrix R(t), what is its derivative?
Interpretation of S: S is the cross-matrix of angular velocities 

47. Example: rotation about z-axis
We want to compute the time-derivative of Rz(t)

48. Velocity composition rule
Consider the coordinate transformation of a point P from coordinate frame 1 to frame 0:
point wrt 0
origin of 1 wrt 0
rotation of 1 wrt 0
point wrt 1
If p1 is fixed:

49. Link Velocity
Consider link i connecting joints i and i+1 with their coordinate frames
Let pi and pi+1 be the coordinate frame origins wrt base coordinate frame
Let rii,i+1 be the coordinate frame origin of i+1 wrt coordinate frame i, expressed in coordinate frame i
Lin. velocity of i
Lin. velocity of i+1 wrt i
Angular velocity of i
Lin. velocity of i+1
… angular velocity (without derivation)
Angular velocity of i+1
Angular velocity of i
Angular velocity of i+1 wrt i

50. Link Velocity
For prismatic joint:
For revolute joint:

51. Computation of Jacobian
Let the Jacobian J be partitioned into the following 3x1 vectors:
Prismatic joint:
Revolute joint:
… contribution of joint i to EE linear velocity
… contribution of joint i to EE angular velocity

52. Computation of Jacobian
… if prismatic
… if revolute

53. Example: three link planar arm
si…j, ci…j denote sin(qi+…+qj), cos(qi+…+qj)

54. Iterative Inverse Kinematics
Specify target transformation T you want to reach.
Make an initial guess for joint coords at time tk: q(tk).
Compute T(tk) using forward kinematics.
Compute differential motion required to move from T(tk) to T:
Compute pseudo-inverse of Jacobian J-1 at the position of q(tk).
Add Joint motion to initial guess, make result new initial guess.
Repeat 2-6 until differential motion is satisfactorily small.

55. Example: Robotics Toolbox