ROBOTICS LECTURE

Chapter 3 and 4

Manipulator Kinematics
- DOF (degree of freedom)

Point (3dof) single body (bdof)

Constrained single body (1dof) ball joint (3dof)

- Mobility criteria (# of dof)

= 6( 1) (6 )
Where:
n: number of rigid bodies (including ground)
m: number of joints
: number of dof at jth joint

= 3( 1) (3 )

- Joint type
 FIGURE 3.1 The six possible lower pair joints

FIGURE. 12. Undriven structural joints

FIG. 16. Two-DOF knuckles with perpendicular axes (a-c, left to right):
glimbal-based knuckle; exterior frame knuckle (based on universal joint);
interior cross knuckle (based on universal joint)
- Example for mobility of system
= 3( 1) (3 )
N=4 including ground
M=3
, , =1
M=3(4 1) 3 x 2 = 3dof

On the plane
= 6( 1) (6 )
= 6(7 1) 6 x 5 = 6dof

In space

n=4
m=4
=1
M = 3(4 1) 4 x 2 = 1dof
On the plane

n=?
m=?
M=?

M=?

: dof ball joint

: 2dof kmickle joint

: 1dof prismatic joint

M=?
Link Description

Link length (a)

: mutually perpendicular
line always exists and is
unique
Link twist (a)
: relative location of two
axes

Link offset (d)

Joint variable ()
=> 4 parameter
convention

: Danavit-Hartenburg
parameter
 Affixing frames to links

Fig 3.5 Link frames are attached so that frame {i} is attached rigidly to link i.

Definition of the link parameters : Craigs way

= the distance from to measured along ;

= the angle between and measured about

= the distance from to measured along ;and

= the angle between and measured about .

 Link frames attachment procedure

1. Identify the joint axes and imagine (or draw) infinite lines along them.
For steps 2 through 5 below, consider two of these neighboring lines
(at axes and + 1 ).
2. Identify the common perpendicular between them, or point of
intersection. At the point of intersection, or at the point where the
common perpendicular meets the ith axis, assign the link frame origin.
3. Assign the axis pointing along the ith joint axis.
4. Assign the axis pointing along the common perpendicular, or of the
axes intersect, assign to be normal to the plane containing the two
axes.
5. Assign the axis to complete a right-hand coordinate system.
6. Assign {0} to match {1} when the first joint variable is zero. For {N}
choose an origin location and direction freely, but generally so as
to cause as many linkage parameters as possible to become zero.
 Example 1)

Fig. 3.6 A three-link planar arm. On the right we show the same
manipulator by means of a simple schematic notation. Hash marks on the
axes indicate that they are mutually parallel.

Fig 3.7 link frame assignments.

 Fig 3.8 Link parameters of the three-link planar manipulators.

Ex 2)

Fig 3.9 manipulator having three degrees of freedom and one prismatic
joint.
 Fig 3.10 Link frame assignments.

Fig 3.11 Link parameters for the RPR manipulator of Example 3.4.
Ex 3) Demonstration of non-uniqueness of frame assignments

Fig 3.12 Three-link, non-planar manipulator.

Fig 3.13 Two possible frame assignments.

Fig 3.14 Two more possible frame assignments.
 Derivation of link transformation

Fig 3.15 Location of intermediate frame {P}, {Q}, and {R}.

= , (3.1)
Or
= , (3.2)
where
= . (3.3)
Considering each of these transformations, we see that (3.3) may be
written:
= ( ) ( ) ( ) ( ), (3.4)
or
= ( , ) ( , ), (3.5)
Where the notation ( , ) stands for a translation along an axis
by a distance , and a rotation about the same axis by an angle .
Multiplying out (3.4) we obtain the general form of :
0

= . (3.6)
0 0 0 1
 Pauls way for link frame attachment

For revolute joint:

For prismatic joint

Definition of the link parameters: Pauls way
Rotate about zn-1 an angle n;
Translate along zn-1, a distance dn;
Translate along rotated xn-1 = xn a length an;
Rotate about xn, the twist angle n.

Link transformation
= ( , ) (0,0, ) ( , 0,0) ( , )
0 0 1 0 0 1 0 0 0
0 0 0 1 0 0 0 0
=
0 0 1 0 0 0 1 0 0
0 0 0 1 0 0 0 1 0 0 0 1


=
0
0 0 0 1

Example 1
Denavit-hanrtenburg parameter
3.11)

z6
z5
z4
y6 y4

y5

Fig. 3R non-orthogonal axis robot

a d
. . . 4
- 0 0 5
0 0 6
3.12) No, need ? parameters
3.15)

Z3

Y3 Z4

Z1 d3 X4
X1

Z5

X5

Z2
Y2

Fig 3.34 Schematic of a 2RP2R manipulator

 Concatenating link transformation:
{0} frame {n} frame
= ..

Space:
Actuator space Joint space Cartesian space
Task Oriented space
Operational Space

Actualtor Joint variable Cartesian

parameter vector Vector
vector X
Y
n Z
x
y
z
N > 6 : redundant manipulator
 Kinematics for Puma 560 manipulator

FIGURE 3.18. Some kinematic parameters and frame assignments

for the Puma 560 manipulator

i i-1 ai-1 di i

1 0 0 0 1

2 -900 0 0 2

3 0 a2 d3 3

4 -900 a3 d4 4

5 900 0 0 5
 6 -900 0 0 6

FIGURE 3.19. Kinematic parameters and frame assignments for the

forearm of the PUMA 560 manipulator
 FIGURE 3.20 Schematic of a 3R wrist in which all three axes
intersect at a point and are mutually orthogonal. This design is used
in the PUMA 560 manipulator and many other industrial robots.

0 0
0 0
=
0 0 1 0
0 0 0 1
0 0
0 0 1 0
=
0 0
0 0 0 1
0
0 0
=
0 0 1
0 0 0 1
0
0 0 1
=
0 0
0 0 0 1
0 0
0 0 1 0
=
0 0
0 0 0 1
0 0
0 0 1 0
=
0 0
0 0 0 1

0
0 0
= =
0
0 0 0 1
Where c5 is the shorthand for cos(5), s5 for sin(5) and so on. Then we
have


= =
0
0 0 0 1
0
0 0 1
= =
0
0 0 0 1
Where we have used the sum of angles formulas(from appendix A)
=
= +
Then we have

= =

0 0 0 1

= [ ] ,
= ,
= [ ] ,
= [ + ]+ ,
= ,
= [ + ]+ ,
= ,
= ,
= ,
= + ,
= ,
= ,

Finally, we obtain the production of all six link transforms

 = = ,
0 0 0 1
Where
= [ ( ) ]+ ( + ),
= [ ( ) ] ( + ) ,
= ( ) ,
= [ ( )+ ]+ ( ),
= [ ( )+ ] ( ),
= ( )+ ,

= ( + ) ,
= ( + )+ ,
= ,

= [ + ] ,
= [ + ]+ ,
= ,
 Frames with standard names

Base Frame {B} : {0} link frame

Wrist Frame {W} : {N} link frame =
Tool Frame {T} : the end of any tool
Station Frame {S} : always known
Goal Frame {G} : tool frame should coincide with goal
frame

=
=
 3.10 Computational consideration
Factorization
Table lookup implementations of transcendental functions
(sine, cosine) sine need 2 or 3 mult. times or less
Rotation matrix with 9 elements

HW 3.3, HW3.4, HW 3.8, HW 3.9, HW 3.11, HW 3.13, HW 3.15, HW

3.16, HW 3.17, HW 3.18, HW 3.19, HW 3.20, HW 3.21, HW 3.22
Inverse Manipulator Kinematics

Generally, {W} = {T}

= = for 6 dof manipulator

Inverse Kinematics problem?

Find the set of joint angles

Desired position and
which will achieve this desired
Orientation of the tool
result

= : known 12 numeric values

0 1

R: 9 eqs. 3 ind.eqs. 6eqs.

P: 3 eqs. 6 unknowns

6 unknown joint angle set

 6 eqs. 6 unknown: Inverse Kinematics Problem: highly non-linear,
involves transcendental eqs. (Korean)

(Korean)
(1) Existence of solutions
(2) Multiple solutions
(3) Method of solution

1) Existence of solution

Workspace: Volume of space which the end-effector of manipulator

can reach
(1) Dextrouswork space: reach with all orientations
(2) Reachable work space: reach in at least one orientation
 28

If =

Reachable work space

Dextrous work space (single

point)

If , >
Reachable work space
No destrous work space

2 orientations possible

Only one orientation

Joint limit
Manipulators of less than 6dof
Specified orientation and position
Existence of solution
 29

2) Multiple Solutions

3 links: 2 possible
solutions

Choice:
Closest solution in
the absence of obstacle

Need define Closest

solution

Number of solution depends on

(1) Number of joints
(2) Function of link parameters
(3) Allowable range of motion of the joints
 30

FIGURE 4.4 Four solution of the PUMA 560

= + 180 ,

= ,

= + 180 .
Generally, 6 revolute robot with nonzero link parameters has 16
possible solutions
 31

3) Method of Solution:
Solvable:
A given Position All sets of
and orientation Joint variables

Cf) numerical algorithm get one set of solutions

Closed form solution. VS. Numerical Solution

Analytic expression Iterative approach, time consuming

Algebraic Geometric

All system with revolute and prismatic joints having a total of 6

degrees of freedom in a single series chain are now solvable
Numerial approach.

Sufficient condition for a closed-form soluability in case of 6R

manipulator:
Joint axes of three consecutive revolute joints interest at a single
point for all arm configuration.
 32

Algebraic Solution

0.0 +
0.0 +
= = , (1)
0.0 0.0 1.0 0.0
0 0 0 1

3R planar Manipulator specification of the goal point with (x,y,)

0.0
0.0
= , (2)
0.0 0.0 1.0 0.0
0 0 0 1
 33

(1)=(2),
= , = + ,
= , = + ,
+ = + +2 (3)
Where we have made use of
= ,
= .
Solving (3) for we obtain

= (4)

1 1 check the existence of solution

= 1 .
Finally, we compute using the two-argument arctangent routine
= 2( , )
For ,
= ,
= + .
Where
= + ,
= .
If

=+ +
And
= 2( , ),
Then
= ,
 34

= .
Equation (4.17) and (4.18) can now be written
= ,

= + ,

or
cos( + )= ,

sin( + )= .

Using the two-argument arctangent we get

+ = 2 , = 2( , ).

And so
= 2( , ) 2( , ).

for ,
+ + = 2( , ) = .
 35

Geometric Solution

We can apply the law of cosines to solve for :

+ = + +2 cos (180 + ).
Since cos(180 + ) = cos ( ), we have

= + + : Existence of solve

180 0
= by symmetry

For , 2: table loop-up for computational solving

= 2( , ).

We again apply the law of cosines to find

= + + 2 +
+ +
=
2 +

= ,
= .
 36

Algebraic solution by reduction to polynomial

Transcendental eqs. Polynomial in forms of single

(cos, cosine) variable

= ,

= ,

= ,
 37

Example 4.3
Convert the transcendental equation
+ = (4.36)
Into a polynomial in the tangent of the half angle and solve for .
Substituting from (4.35) and multiplying through by 1 + we have
(1 )+2 = (1 + ). (4.37)
Collecting powers of u yields
( + ) +2 +( )=0 (4.38)
Which is solved by the quadratic formula to be

= (4.39)

Hence,

=2 ( ) (4.40)
 38

Piepers solution when three axes intersect

0

= ,

0 0 0 1
When the last three axes intersect, the origins of link frames {4}, {5}, and {6}
are all located at this point of intersection. This point is given in base
coordinates as

= (4.41)
or, using the fourth column of (3.6) for i=4.

= , (4.42)
1
or as
( )
( )
= , (4.43)
( )
1
Where

= (4.44)
1 1

Using (3.6) for in (4.44) yields the following expressions for

= + + , Function of

= , (4.45)
= + + .
 39

Using (3.6) for and in (4.43) we obtain

0 0
+ 0 0
= = , (4.46) =
0 0 1 0
1 1 0 0 0 1
where = + ,
= + , (4.47)
= = =0
= + + + ,

We now write an expression for the magnitude squared of , which is

seen from (4.46) to be
= + + , (4.48)
or, using (4.47) for the ,we have | |=| |

= + + + + +2 +2 ( ) (4.49)

We now write this equation, along with the component equation from
(4.46), as a system of two equations in the form
=( + )2 +
= ( ) + (4.50)
where
=
=
= + + + + +2 (4.51)
= +
Equation (4.50) is useful because dependence on hase been
eliminated and dependence on takes a simple form.
Check 3 cases:
 40

1. If = 0 then we have = where r is known. The right-hand

side is a function of only. After making the substitution (4.35), a

quadratic equation in may be solved for .

2. If = 0 then we have = where z is known. Again, after

substituting (4.35) a quadratic equation arises which may be solved for .
3. Otherwise, eliminate and from(4.50) to obtain

( ) ( )
+ = + .
4
Where

=
2
1
=
1+
2
=
1+

For , and , given

= ( ) ( )

= ( )

can be solved for by using exactly Z-Y-Z Euler angle solution.

Euler angle solution, 2 solution set
Position solution, 8 solution set
General 6 DOF manipulator inverse kinematic solution set 16

The PUMA 560

We wish to solve
 41

=
0 0 0 1
= ( ) ( ) ( ) ( ) ( ) ( ) (4.54)
For when is given as numeric values.
A restatement of (4.54) which puts the dependence on on the left-
hand side of the equation is
[ ( )] T= ( ) ( ) ( ) ( ) ( ). (4.55)
Inverting we write (4.55) as
0 0
0 0
= (4.56)
0 0 1 0
0 0 0 1 0 0 0 1
Where is given by equation (3.13) developed in Chapter 3. This
simple technique of multiplying each side of a transform equation by an
inverse is often used to advantage in separating out variables in search of
a solvable equation.
Equating the (2, 4) elements from both sides of (4.56), we have
+ = . (4.57)
To solve an equation of this form, we make the trigonometric
substitutions
=
= (4.58)
where

= +

= 2 , (4.59)
Substituting (4.58) into (4.57), we obtain
 42

= (4.60)

Using the difference of angles formula:

( )= (4.61)

Hence

( )= 1 (4.62)

And so

= 2 , 1 (4.63)

Finally, the solution for may be written:

= 2 , 2 , + (4.64)

Note that we have found two possible solutions for corresponding to

the plus-or-minus sign in (4.64). Now that is known, the left-hand side of
(4.56) is known. If we equate the (1 ,4) elements from both sides of (4.56)
and also the (3, 4) elements, we obtain
+ = +
p = + (4.65)
If we square equations (4.65) and (4.57) and add the resulting
equations, we obtain
= (4.66)
where

= (4.67)

Note that dependence on has been removed from (4.66). Equation

(4.66) is of the same from as (4.57) and so may be solved by the same
kind of trigonometric substitution to yield a solution for :
 43

= 2( , ) 2 , + (4.68)

The plus-or-minus sign in (4.68) leads to two different solutions for .

If we consider (4.54) again, we can now rewrite it so that all the left-
hand side is a function of only knowns and :

[ ( )] = ( ) ( ) ( ) (4.69)


= (4.70)
0
0 0 0 1 0 0 0 1
Where is given by equation (3.11) developed in Chapter 3. Equating
the (1, 4) elements from both sides of (4.70), as well as the (2, 4) elements,
+ =
+ = (4.71)
These equations may be solved simultaneously for and , resulting
in
( ) + + ( )
=
+ +
( ) ( )
= (4.72)

Since the denominators are equal and positive, we solve for the sum of
and as
= 2 ( ) + ( + ), (
) ( + ) + (4.73)
Equation (4.73) computes four values of according to the four
possible combinations of solutions for and . Then, four possible
solutions for are computed as
 44

= (4.74)
Where the appropriate solution for is used when forming the
difference.
Now the entire left side of (4.70) is known. Equating the (1, 3) elements
from both sides of (4.70), as well as the (3, 3) elements, we get
+ =
+ = (4.75)
As long as 0, we can solve for as
= 2( + , + ) (4.76)
When = 0 the manipulator is in a singular configuration in which joint
axes 4 and 6 line up and cause the same motion of the last link of the robot.
In this case, all that matters (and all that can be solved for) is the sum or
difference of and . This situation is detected by checking whether both
arguments of the Atan2 in (4.76) are near zero. If so, is chosen arbitrarily,
and when is computed later, it will be computed accordingly.
If we consider (4.54) again, we can now rewrite it so that all the left-
hand side is a function of only knowns and by rewriting it as
[ ( )] = ( ) ( ), (4.77)
Where [ ( )] is given by
+ +
+ + +

0 0 0 1
(4.78)
And is given by equation (3.10) developed in Chapter 3. Equating
the (1, 3) elements from both sides of (4.77), as well as the (3, 3) elements,
we get
( + )+ ( ) ( )=
 45

( )+ ( )+ ( )= (4.79)

So we can solve for as:

= 2( , ) (4.80)
Where s and c are given by (4.79) above.
Applying the same method one more time, we compute ( ) and
write (4.54) in the form
( ) = ( ). (4.81)
Equating the (3, 1) elements from both sides of (4.77), as well as the (1,
1) elements as we have done before, we get
= 2( , ), (4.82)
where
s = r ( )r ( + )+ ( )
= [( + ) ]+ [( ) ]
( + )
Because of the plus-or-minus signs appearing in (4.64) and (4.68),
these equations compute four solutions. Additionally, there are four more
solutions obtained by flipping the wrist of the manipulator. For each of the
four solutions computed above, we obtain the flipped solution by
= + 180
= (4. 83)
= + 180
After all eight solutions have been computed, some or all of them may
have to be discarded because of joint limit violations. Of the remaining valid
solutions, usually the one closest to the present manipulator configuration
is chosen.
Standard frames
=
 46

{ }={ }
=
=
 47

1) Pay load
2) Speed
3) Resolution
4) Accuracy
5) Repeatability
6) Workspace
7) Cost
48
 49

4.6) Describe a simple algorithm for choosing the nearest solution from a set
of possible solution
Solution:
To derive the nearest solution, we would like to minimize the rotation of each
joint. Denote the starting angle of each joint as (for i-th joint), and the final
position of each joint as . For each propose solution compute

And choose the nearest as the one which minimize S. Sometimes a weighting
factor is used (to penalize motion of large joints, for example) and so the score for
each propose solution is

4.7) Make a list of factors which might affect the repeatability of a

manipulator. Make a second list of additional factors which affect the accuracy of a
manipulator.
Soluyion:
Repeatability us affected by:
Steady state error is
Flexibility of links
Backlash in gears
Looseness in bearings
Noise in sensing readings
Thermals effects
Accuracy is affected by all of the above, plus
Imprecise knowledge of DH parameters
 50

4.8) Given a desired position and orientation of the hand of a three-link

planar rotary jointed manipulator, there are two possible solutions. If we add one
more rotational joint (in such a way that the arm is still planar), how many
solutions are there?
Solution:
There are an infinite number of solutions. Imagine fixing the last link in
position and orientation

Then the first 3 links from a 4 bars linkage which can take on infinity of
positions since it has a decree of freedom.
4.12) in Fig 4.14 two 3R mechanisms are pictured. In both cases, the three
axes intersect at a point (not that over all congiguration, this point remains fixed in
space). The mechanism in Fig. 4.14a has link twists ( ) of magnitude 90 degrees.
The mechanism in Fig. 4.14b has one twist of in magnitude and the oder of
180 4 in magnitude

The mechanism in Fig. 4.14a can be seen to be in correspondance with Z-Y-Z

Euler angles, and therefor we khow that it suffices to orienr link 3 (with arrow in
figure) arbitrarily with resoect ti the fixed link 0, because is not equal to 90
degress, it turns out that the other mechanism cannot orient link 3 arbitrarily.
 51

Solution:
Starting with joint 3, spinning it, frame 3 can attain any orientation about its
axis, pointing upward, the problem thus reduces to finding restrictions on the
pointing direction of

But spinning about joint , can spin any orientation on the surface of the
cone of apex angle
Spinning this cone about joint 1, can spin any orientation expect in a
cone taped zone of apex angle 360 4