COLLISIONFREE PATH FINDING
FOR
TWO COOPERATIVE SPATIAL MANIPULATORS
BY
JAULIANG CHEN
A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL
OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT
OF THE REQUIREMENTS FOR THE DEGREE OF
DOCTOR OF PHILOSOPHY
UNIVERSITY OF FLORIDA
1990
TO MY WIFE MEIHSIANG AND DAUGHTER KATHERINE
ACKNOWLEDGEMENTS
The author would like to express his sincere appreciation
to his adviser Dr. Joseph Duffy for his guidance and support
of this work. He would like to thank Dr. Ali Seireg, Dr. John
Staudhammer, Dr. Gary K. Matthew and Dr. Carl D. Crane for
their help and participation in his committee.
The author would also like to thank all his colleagues
in CIMAR for sharing their knowledge and pleasure.
Special thanks are extended to Mr. and Mrs. Edward
Gilbert, without them this work cannot be presented.
Finally, the author wishes to thank his parents, parents
inlaw, and wife, Mei, for their encouragement, spirit and
patience.
iii
TABLE OF CONTENTS
page
ACKNOWLEDGEMENTS.. ..... ......................... iii
ABSTRACT........................................... .. vi
CHAPTERS
I INTRODUCTION...................................... 1
1.1 Literature Review........................... 2
1.1.1 Obstacle Avoidance................... 3
1.1.2 Multirobot System................... 6
1.2 Objective of Research....................... 7
1.3 Problem Statement........................... 7
1.4 Assumptions................................. 7
1.5 Methodology.................................. 8
II OBSTACLE EXPANSION... ............................. 12
2.1 Expansion of Objects........................ 12
2.1.1 Expanding the Vertical Sides.......... 13
2.1.2 Expanding the Top Surfaces............ 17
2.2 Identification of Obstacles................. 23
III WRIST CENTER LOCALE FINDING..................... 42
3.1 Construction of Vertical Planes............. 42
3.2 Determination of CollisionFree Area (CFA).. 45
3.3 Determination of WristCenter Area (WCA).... 54
3.3.1 WCA for the First Robot.............. 57
3.3.2 WCA for the Second Robot............. 59
IV COLLISIONFREE PATH FINDING...................... 71
4.1 Collision Between End Effectors, Grasped
Workpiece and Obstacles..................... 72
iv
4.1.1 Identification of Obstacles........... 73
4.1.2 Perspective Projection............... 75
4.2 Collision Checking Between Two Robots Links. 77
4.2.1 Two Planes Intersect at One Line..... 78
4.2.2 Two Planes are Coincident............ 83
4.2.3 Two Planes are Parallel.............. 84
4.3 CollisionFree Path Generation.............. 85
4.4 Computer Graphics Simulation................ 87
V CONCLUSIONS AND RECOMMENDATIONS.................. 109
REFERENCES............................................. 111
BIOGRAPHICAL SKETCH................................... 116
Abstract of Dissertation Presented to the Graduate School
of the University of Florida in Partial Fulfillment of the
Requirements for the Degree of Doctor of Philosophy
COLLISIONFREE PATH FINDING
FOR
TWO COOPERATIVE SPATIAL MANIPULATORS
By
iJauLiang Chen
December, 1990
Chairman: Dr. Joseph Duffy
Major Department: Mechanical Engineering
This study describes the development of finding
collisionfree paths for a pair of cooperative spatial robot
manipulators as their end effectors grasp a workpiece in an
obstaclestrewn environment. After the initial and goal
positions of the wrist center are specified, a collisionfree
path for this pair of manipulators to move the workpiece
safely to the final destination is generated.
It is assumed that the surrounding objects are composed
of vertical sides with either a flat top surface or an
inclined top surface, and their locations, sizes and shapes
are known. The objects are then expanded according to the
dimension of the links of the manipulators so that all the
links can be treated as straight line segments. Objects which
lie within the swept volume are identified as obstacles to the
movement of these two manipulators.
A series of vertical planes for each individual
manipulator is established to reduce the geometric problem
from three dimensions to two dimensions. Each of the planes
contains a vertex of the top surface of the expanded obstacles
and is offset a fixed distance from the first joint axis of
the robot. These planes generate crosssections of the
obstacles and wrist reachable workspace.
In each crosssection of the wrist reachable workspace,
a wnst center locale (WCL) is determined so that an intermediate
position of the wrist center can be selected within the WCL.
During the selection of the intermediate position, the
collision between end effectors, the grasped workpiece and
the obstacles need to be considered. The intersection between
the two manipulators links is also taken into account during
the selecting procedure.
After all the intermediate positions are found, a path
which connects these intermediate positions together with the
initial and goal positions is modified to produce a smooth
collisionfree path. Here, the algorithm is applied to the
PUMA robots and is demonstrated via computer graphics
animation on a Silicon Graphics IRIS 4D/70GT workstation.
vii
CHAPTER I
INTRODUCTION
Automation has become a necessary element in modern
manufacturing processes. Many of these processes are complex,
such as multipart assemblies, fabrication of difficult
geometric shapes, and the handling of very heavy parts. Along
with the development of high level machine intelligence for
second and third generation robots, there is need to develop
robotic systems with multiarm configurations for better
manipulation.
If we examine the way the human body adapts to
manipulative activity, we find that each arm and hand can be
used independently with skill and efficiency. Both sets of
arms and hands, however, can accomplish much more than double
the work of one. Working together, the two have a qualitative
effect that makes it possible to perform tasks impossible to
execute with a single arm and hand.
In similar fashion, a multirobot system (MRS) offers
flexibility and an ability to process parts which would be too
heavy to be handled by a single arm. Multirobot systems increase
2
productivity through parallelism, and provide flexibility to
accommodate diversified applications.
MRS offers other advantages, making it worthy of
consideration, even when the conventional single arm system
could be used. The multiple grasping of a workpiece by
several arms transforms the serial configuration of individual
robots into a closed kinematic chain. This creates a
structure far more rigid than its openchain counterpart,
thereby increasing the operational reliability of a system.
In the previous work [Chen and Duffy 1988a, 1988b], the
motion capability and ranges of end effector orientations for
two cooperative robots, which grasp a workpiece and move it
along a straight line path, were discussed. This research is
a continuation of that study, and focuses on obstacle
avoidance.
1.1 Literature Review
In Section 1.1.1, a brief review of the literatures on
obstacle avoidance is presented. As far as the author aware,
no other work has appeared in literature concerning the
problems associated with two cooperative robots grasping a
workpiece while avoiding collision with the obstacles in the
environment. Most of the studies on the multirobot system can be
divided into two groups. The first group considered the
collision avoidance between robots. Each robot performs
3
different tasks. The second group discussed multiple robots
grasp a workpiece, but only deals with the control issue. The
discussion of those studies on the multirobot system is presented
in Section 1.1.2.
1.1.1 Obstacle Avoidance
The most common robot motion is transfer motion. In the
execution of transfer motion, the robot and the workpiece it
carries must be able to avoid collision with any obstacle that
may lie within the robot working area. The capability to plan
motions that avoid such obstacles is, therefore, essential to
task planning. Several obstacle avoidance algorithms have
been proposed. In this section, those studies which dealing
with obstacle avoidance in three dimensions are reviewed.
Until recently, most of this research could be
characterized as a "hypothesize and test" algorithm [Pieper
and Roth 1968, Boyse 1979, Ahuja et al. 1980]. Such an
algorithm uses three steps repeatedly: first, it hypothesizes
a candidate path; second, it checks this path for possible
collision; third, if a collision can occur, the path is then
modified. The main advantage of the hypothesize and test
technique is its simplicity. This method works fairly well
in rather uncluttered environments where crude heuristics can
be used to generate candidate paths. When the space is
4
cluttered, however, attempts to avoid collision with one
obstacle will typically lead to collision with another.
A more recent approach is based on the "configuration
space" first introduced by Udupa [1977] and then developed by
LozanoPerez and Wesley [1979], LozanoPerez [1987, 1986,
1983, 1981]. Basically, this approach consists of shrinking
the moving object to a point, while expanding obstacles
inversely to the original shape of the moving object. Then,
finding a path for the moving object is equivalent to finding
a path for a point among the expanded obstacles. Path finding
can be accomplished with a search algorithm operating on a
graph that represents interconnections between freespace
cells. This method is well suited for Cartesian manipulators,
but its extension to rotating movement is complicated [Chien
et al. 1984, Brooks and LozanoPerez 1985]. If the
manipulator has many rotational degrees of freedom, selection
of a trial collisionfree path is complicated. In the case
of a manipulator with many rotational links, this algorithm
utilizes many rough approximations and may, therefore, fail
to find collisionfree paths even when they exist.
Another method, by Brooks [1983], represents free space
by a structured set of "freeways." Each freeway is modeled
using generalized cones. Then, "legal motion segments"
(possible collisionfree paths) for the moving object are
defined by translations and possible reorientations along the
"freeway spines" (outer boundary of the cone). This method
5
has been extended to find collisionfree paths involving four
degrees of freedom [Brooks 1984]. The method works fast in
a relatively uncluttered environment. This approach, like the
"configuration space" solution, has difficulty in a
complicated environment and may fail to uncover a possible
collisionfree path.
Khatib [1985] proposed a penalty function approach which
satisfies the definition of a "potential field." The gradient
of this field, at a selected point on the robot, is
interpreted as a repelling force acting on that point. The
destination, on the other hand, is assumed to generate an
attracting force to the selected point. A collisionfree path
is found after accounting for interactions between these two
forces, subject to the kinematic constraints. The penalty
function approach finds safe paths based on local information.
The procedure does not account for global information, thus
leading to situations where further progress is impossible.
This approach is more suitable for applications that require
only small modifications to a known path.
Different approaches to this task can also be found in
[Faverjon 1984, Wong and Fu 1985, Laugier and Germain 1985,
Canny 1986, Young 1986, Hasegawa and Terasaki 1988, Jun and
Shin 1988, Cameron 1990, Choi 1990].
1.1.2 Multirobot System
There is growing interest in the development of multirobot
systems. The potential of such systems covers a wide range of
applications extending from assembly tasks in automated
manufacturing to deepsea exploration and service tasks in
outer space. These applications are complex and require
dexterous manipulation. Robot systems with two or more arms
are necessary.
Robot systems with two arms working in the same space
have been investigated by several researchers [Acker et al.
1985, Freund and Hoyer 1986, 1985, Lee 1986, Lee and Lee 1987,
Roach and Boaz 1987, Nagata et al. 1988]. In these studies,
each robot performed an independent working sequence rather
than a coordinated task. The primary interest of these
studies was collision avoidance between robots. Because each
of the two robots had an independent working sequence, there
was no kinematic relationship between their motion vectors.
If one robot had a closely coordinated motion with the other,
an important kinematic relationship would exist.
Since the 1970s, several researchers [Fujii and Kurono
1975, Ishida 1977, Alford and Belyeu 1984, Luh and Zheng 1987,
1986, Tarn et al. 1986, Tournassoud 1987, Lee and Lee 1988,
Benhabib et al. 1988, Tricam et al. 1988, Tao and Luh 1989,
Tao et al. 1990] have focused attention on the use of two
manipulators to handle a single workpiece. However, they have
7
almost exclusively dealt with control issues, leaving the
kinematic and geometric aspects virtually untouched.
This research will study collision avoidance for two
cooperative robots handling a workpiece within an obstacle
strewn environment.
1.2 Objective of Research
The objective of this research is using a simple
geometric model to produce an algorithm which can be executed
quickly to find collisionfree paths for two cooperating robot
manipulators. The algorithm will be applied to two PUMA
robots in an obstaclestrewn environment.
1.3 Problem Statement
The problem of this study can be stated as follows:
Given: The wrist center positions, and the end effector
orientations at the initial and the final positions
of a pair of cooperative robots.
Find: Paths for this pair of robots to move a workpiece
safely to a destination position without colliding
with obstacles in the environment.
1.4 Assumptions
Several assumptions have been made in order to simplify
the problem.
8
1) The surrounding objects have vertical sides with an
inclined top surface. The location and geometric description
of each object are known.
2) The initial and goal positions of each wrist center
are obstacle free.
3) The end effectors firmly grasp the workpiece, and
no slippage will occur during movement. The orientation of
the workpiece is the same as that of the end effector.
4) The joint angle limitations are considered
throughout the analysis.
5) The grasped workpiece is a long rectangular bar and
its crosssection dimensions are smaller than those of the
robot arm.
1.5 Methodology
Several terms are used repeatedly throughout the study:
Collisionfree area (CFA):
An area within which the wrist center of an
individual robot can be located to avoid collision
between the robot arm and obstacles.
Wrist center area (WCA):
An area within which the wrist centers of the
cooperative robots can be located.
Wrist center locale (WCL):
An area within which the wrist centers of the
cooperative robots can be located to avoid collision
between the robot arms and obstacles.
Swept volume:
The space under the area swept by a robot arm along
a predetermined path.
The method for determining a collisionfree path can be
summarized as follows:
1) Expand the surrounding objects.
2) Construct a straight line path for the wrist center
of the first robot, from the initial to the final positions.
The wrist center path of the second robot is determined
according to the end effector orientation and wrist center
position of the first robot.
3) Identify those obstacles that fall within or
intersect the swept volume of both robots.
4) Establish a series of vertical planes* for each
robot, to reduce the problem from three dimension to two
dimensions. Each vertical plane includes a vertex of the
obstacle's top surface and is offset a fixed distance from the
first joint axis of the robot.
5) Identify the collisionfree area (CFA) on each vertical
plane.
6) Determine the wrist center area (WCA), keeping the
distance between the two wrist centers constant.
For the PUMA robot, those vertical planes are parallel
to the central axis of the first joint.
10
7) Superimpose (logical intersection) the CFA (5
above) and the WCA (6 above) to find the wristcenter locale (WCL).
8) Locate intermediate wrist center positions by
examining the intersection between the end effectors, the
grasped workpiece, and the obstacles.
9) Reexamine for possible collision between
manipulators' links when selecting intermediate positions.
10) Connect initial, intermediate, and goal positions
to produce a collisionfree path, after locating intermediate
positions.
The two cooperative robots are positioned in locations
to facilitate development of the algorithm. The coordinate
system is constructed with its origin at the center of the
first robot's base. The Zaxis coincides with the central
axis of the first joint of the first robot, and the center of
the second robot's base is on the positive Xaxis (see Fig.
1.1).
An algorithm for expanding objects and identifying those
objects which are obstacles is developed in Chapter II.
Chapter III describes the procedure of finding a wrist center locale
(WCL) on each vertical plane. Chapter IV describes the
determination of intermediate positions of wrist paths. That
chapter also examines the possibility of collision between two
manipulators links. Finally, some recommendations for future
study are listed in Chapter V.
z
1'
Relative position of the PUMA robots.
Figure 1.1
CHAPTER II
OBSTACLE EXPANSION
Collision avoidance between a solid link and objects
scattered within a robot's working area is a complex problem.
To simplify the problem, each object is expanded by a factor
derived from the thickness and width of the link, so that all
the links can be treated as line segments. The problem can
then be reduced to locating intersections of the line segments
with expanded objects. Section 2.1 describes the expansion
in detail.
Section 2.2 develops an algorithm to determine whether
an expanded object will be an obstacle to the movement of each
robot. A straight line is used as a predetermined path for
the wrist center movement of each of the two cooperating
robots, moving from an initial position to the target
position.
2.1 Expansion of Objects
Figure 2.1 shows a PUMA 600 robot and the crosssection
of an object. In the case of a single robot, each object is
expanded according to the relationship between the links of
the robot and sides of the object. For example, if any
13
portion of the sides of an object can be reached by the upper
arm, these sides are expanded by a factor derived from the
dimensions of the upper arm. Otherwise, they are expanded by
a factor derived from the dimension of the forearm.'
For two cooperative robots, each object is expanded by
an amount calculated from the thickness and width of the upper
arm. Figure 2.2 illustrates the upper arm of the PUMA robot.
Because of its irregular form, it is difficult to use this
shape in calculating the expansion. To reduce complexity,
this arm is treated as a parallelepiped, utilizing its largest
dimension (see Fig. 2.2).
Figure 2.2 shows the crosssection of the upper arm.
Point P is the intersection point of the crosssection AA and
the upper arm axis. This axis connects the centers of joints
2 and 3. H is the distance from point P to the lower corner
of the crosssection. V is the perpendicular distance from
point P to the base.
The expansion of the vertical sides of an object is
discussed in Section 2.1.1. The expansion of the top surface
of an object is discussed in Section 2.1.2.
2.1.1 Expanding the Vertical Sides
The vertical sides of an object are expanded by H. Each
vertex is then expanded to form two corners (see Fig. 2.3).
SFor details see Tseng 1987 pp. 1124.
14
The coordinates for the new corners are calculated in two
steps. The first is to find the X and Y coordinates, and the
second is to find the Z coordinate.
2.1.1.1 Determine the X and Y coordinates
To facilitate calculation, the object is projected onto
the XY plane as shown in Fig. 2.4. Points A, B, C, and D are
the projections of the vertices, indexed sequentially in a
counterclockwise direction. The vertical sides are projected
as the line segments AB, BC, CD, and DA.
The line equations for lines AB and BC are given as
Li := X mY = n, (2.1)
Le := X pY = q, (2.2)
where m = (XA XB) / (YA YB)
n = XA m YA
p = (Xc XB) / (Yc YB),
q = XB p YB,
and (XA, YA), (XB, YB), and (Xc, Yc) are the X and Y coordinates
of points A, B, and C.
When the direction of the cross products OA x AB and OB
x BC point to the positive Zaxis (see Fig. 2.5a), the
equations for lines DE and EF are given by
15
L, := X mY = n + H (1 + m2)i/2, (2.3)
L4 := X pY = q + H (1 + p2)1/2. (2.4)
Otherwise (see Fig 2.5b), the equations for lines DE and
EF are given by
L, := X mY = n H (1 + m2)1/2, (2.3a)
L4 := X pY = q H (1 + p2)1/2. (2.4a)
Solving either pair of the simultaneous equations, the
coordinates of the intersection point E are
XE = (a, p b m) / (p m), (2.5)
YE = (a b) / (p m), (2.6)
where a, = n + H (1 + m2)/2,
b, = q H (1 + p2)12.
The () sign is used when lines DE and EF are defined by
Eqs. (2.3a) and (2.4a).
The equation for line BE can be expressed as
L, := X tY = s, (2.7)
where t = (XE XB) / (YE Y)
s = X, t YB.
16
Point I lies on line BE with a distance H from point B
and its coordinates are calculated as follows:
X, = X, + (XB XE)R, (2.8)
Y, = YB + (YB YE)R, (2.9)
where R = H [(XB XE)2 + (YB YE)2]1/2
The equation for line L6, which passes through point I
and is perpendicular to line L5, can be expressed as follows:
L6 := tX + Y = c, (2.10)
where c = t X, + Y,.
The coordinates of point N, (N2) are found by solving Eqs.
(2.3) and (2.10) ((2.4) and (2.10))
XN = (a, + c m) / (1 + t m), (2.11)
YNI = (c a, t) / (1 + t m), (2.12)
and XN2 = (bI + c p) / (1 + t p), (2.13)
YN2 = (c bi t) / (1 + t p). (2.14)
2.1.1.2 Determine the Z coordinate
Since the new corners lie on the same plane as the
original before expansion of the top surface, the Z coordinate
of each new corner can be computed as follows:
17
a) The equation of the plane containing all these
vertices can be expressed by
LX + MY + NZ P = 0, (2.15)
where L = (YA YB) x (Zc Z) (Yc YB) x (ZA Z),
M = (ZA ZB) x (Xc XB) (Zc ZB) x (XA XB),
N = (XA XB) x (Yc Ya) (Xc XB) x (YA YB),
P = LXA + MYA + NZA,
and (XA, YA, ZA) (XB, YB, Z) and (Xc, Yc, Zc) are the
coordinates of any three vertices A, B and C on the top
surface of the original object.
b) The Z coordinate of each new corner can then be
calculated as follows:
Z, = (P LXNi MYN) / N, i = 1, 2 (2.16)
where (XNi, Y,) are the X and Y coordinates of each new corner
found by Eqs. (2.11), (2.12), (2.13) and (2.14).
2.1.2 Expanding the Top Surface
In the previous section, coordinates for the corners of
the expanded side surface were calculated. The next step is
to expand the top surface along the Zdirection. This
expansion must recognize the position of the upper arm axis
relative to the top surface of the objects.
18
Figure 2.6 shows positions of the upper arm relative to
the top surface of two different objects. In both (a) and
(b), the axis of the upper arm is parallel to the slope of the
top surface of the object. If each top surface is expanded
by the amount of V/cosa, where a is the angle of the slope of
the top surface, the relative position the upper arm and the
object's top surface will remain the same.
Figure 2.7 shows two examples where the axis of the upper
arm is not parallel to the slope of an object's top surface.
When the top surface is expanded by V/cosa, a leading angle
0 is formed, due to side expansion. Therefore, the line
segment representing the upper arm will strike the expanded
object before it hits the real object.
According to the relative position of the upper arm to
the object, there are three different cases:
Case 1The upper arm collides with the near corner2 of
the object.
Case 2The upper arm collides with the far corner3 of
the object.
Case 3The upper arm collides with the top surface of
the object.
2 The corner on the side closest to the robot's first joint.
3 The corner on the side away from the robot's first joint.
19
In the following discussion, the angle 0 and points O,
P, R, P' and R' are defined as follows:
0  Rotational angle of the upper arm, measured from the
Xaxis.
O  Center of joint 2.
P  Point where the upper arm hits the original object.
R  Point on the central axis of the upper arm
corresponding to point P.
P'  Point where the line segment representing the upper
arm hits the expanded object.
R'  Intersection point between the central axis of the
upper arm and the expanded top surface.
Case 1. The upper arm collides with the near corner of the
object.
Figure 2.7 shows two examples of case 1. In Fig. 2.8,
the X coordinate of points P' and R' are calculated as
follows:
X, = Xp PR'sin[(O+a)/2], (2.17)
Xp, = Xp H, (2.18)
where PR' = V / cos[(Oa)/2],
and Xp is the X coordinate of point P.
If X, is less than Xp,, the upper arm will hit the
original object before the line segment, representing the
20
upper arm, hits the expanded object as shown in Fig. 2.9.
From Eqs. (2.17) and (2.18),
H < V a / b, (2.19)
where a = sin[(8+a)/2],
and b = cos[(6a)/2].
Expressing Eq. (2.19) as a function of 6 yields
tan(0/2) > (a' / b'), (2.20)
where a' = Hcos(a/2) Vsin(a/2),
and b' = Vcos(a/2) Hsin(a/2).
Solving Eq. (2.20) for 0 yields
0 > 2 arctan(a'/b'). (2.21)
Figure 2.10 shows the boundary value of 0 as a changes
from 90 degrees to 90 degrees according to Eq. (2.21).
Figure 2.10 also shows that the boundary value of 0 is always
greater than 90 degrees. If 0 is greater than 90 degrees, the
upper arm will not hit the object. However, if the upper arm
hits the object, 0 must be less than 90 degrees, and X, must
be greater than X,. From the sine rule,
P'R' OR'
S (2.22)
sing sin(70+a0)
Solving Eq. (2.22) for 0 yields
P'R'sin(0a)
S= arctan[ ]. (2.23)
OR' P'R'cos(Oa)
Case 2. The upper arm collides with the far corner of the
object.
Figure 2.11 shows two examples of case 2. In Fig. 2.12,
the X coordinates of points R' and P' are calculated as
follows:
X, = Xp + PR'sin[(O+a)/2], (2.24)
Xp, = Xp + H, (2.25)
where PR' = V/cos[(Oa)/2].
If X, is greater than X,, the upper arm will hit the
original object before the line segment hits the expanded
object as shown in Fig. 2.13. From Eqs. (2.24) and (2.25),
H < V a / b, (2.26)
where a = sin[(8+a)/2],
b = cos[(8a)/2].
22
Expressing Eq. (2.26) as a function of 0 yields
tan(0/2) > (a'/b'), (2.27)
where a' = Hcos(a/2) Vsin(a/2),
b' = Vcos(a/2) Hsin(a/2).
Solving Eq. (2.27) for 6 yields
0 > 2 arctan(a'/b'). (2.28)
The result (2.28) is the same as in Case 1. Hence a
leading angle 0 exists and is calculated by the same method
as described in Case 1.
Case 3. The upper arm collides with any part of the top
surface of the object.
Figure 2.14 shows an example of case 3. In Fig. 2.15,
the X coordinates of points R and R' can be calculated as
follows:
X, = Xp a V, (2.29)
X, = Xp b V, (2.30)
where a = sinO,
b = sin[ (+a)/2]/cos[(Oa)/2].
23
If Xr is less than X,,, the upper arm will hit the object
before the line segment hits the expanded object, as shown in
Fig. 2.16. From Eqs. (2.29) and (2.30), this situation will
happen only when a is greater than b. Table 2.1 shows the
values of (a b) for different values of a and 0.4 From this
table, a is always less than b. Hence a leading angle must
exist.
All three cases demonstrated that V/cosa can be used as
the expansion dimension for the top surface of the object.
This value is added to the Z coordinate calculated by Eq.
(2.16). The Z coordinate of each vertex, after expansion, is
then given by
ZN = Zn + V / cosa. (2.31)
Figure 2.17 illustrates an example of the object before
and after the expansion. The upper arm and forearm become two
line segments that lie on the same plane, with an offset to
the first joint (see Fig. 2.18).
2.2 Identification of Obstacles
This study assumed that the wrist center of the first
robot moves along a straight line path, from initial to final
4 Note that in this case n/2 < 0 < a.
24
position. Further, the study recognizes that there may be a
number of points along this path that are inaccessible to a
wrist center and/or points where an upper arm or forearm may
collide with an obstacle. In these cases, the wrist center
position needs to be relocated such that it can be reached by
the wrist center and neither the forearm nor the upper arm
collides with the obstacle. The relocating of the wrist
center position will be discussed in Chapter IV.
Objects in the environment were expanded in Section 2.1.
Only those objects which lie within the sweptvolume formed by
the robots will be considered as obstacles in path finding.
The following procedure is used to determine which
objects are classified as obstacles:
1. Project the expanded object and the swept volume onto
the XY plane. Each object is evaluated in terms of two pairs
of angles on this plane. For example, [Oi, Of] and [08, 8!]
are used to evaluate object A (see Fig. 2.19). These angles
are measured between the offset of the robot and the Xaxis
as shown in Fig. 2.19. The angles Bi and O8 indicate contact
with object A (as the robots approach A), where O, and 08
indicate the robots are clear of object A. The two pairs of
angles are used to denote the angular range of object A.
25
The 0 angles (0 and 0') are calculated by
0 = 6 + 4, (2.32)
where 6 = arctan(Y,/Xr), (2.33)
S= arccos[S/(X +Y2)12], (2.34)
and (Xr, Y,) are the X and Y coordinates of the vertex, and S
is the offset.
The angular range of the swept volume of the left hand
robot is represented by [a, B], and the angular range of the
swept volume of the right hand robot is represented by [B', a'].
2. In order to be classified as an obstacle, the angular
range of an object must satisfy one of the following
conditions:
a) Lie within the angular range of the swept volume of
both robots (for example, objects A, B, and C in Fig 2.19).
b) Lie within the angular range of the swept volume of one
of the robots and overlap with another (for example, object
D in Fig. 2.19).
c) Overlap the angular range of the swept volume of both
robots (for example, object E in Fig. 2.19).
Example: In Fig. 2.19, object F does not satisfy any of
the three conditions. Therefore, it is not counted as an
obstacle.
26
The objects in the environment were expanded according
to the dimensions of the robot. The upper arm and the forearm
of the robot becomes two line segments after the expansion of
objects. Those expanded objects which affect the motion of
the two cooperative robots were identified as obstacles. The
collision checking can then be accomplished by checking the
intersection between the line segments with the obstacles.
It will be discussed in the next chapter.
Table 2.1 Values of (ab) for different a and 8.
aUl 85 80 70 60 50 40 30 20 10
80 0.0038 0
70 0.0115 0.0152 0
60 0.0193 0.0306 0.0299 0
50 0.0275 0.0465 0.0603 0.0437 0
40 0.0361 0.0632 0.0916 0.0882 0.0562 0
30 0.0454 0.0810 0.1245 0.1340 0.1133 0.0670 0
20 0.0555 0.1003 0.1595 0.1820 0.1722 0.1351 0.0758 0
10 0.0669 0.1216 0.1975 0.2332 0.2340 0.2053 0.1527 0.0822 0
0 0.0799 0.1457 0.2395 0.2886 0.2997 0.2788 0.2321 0.1657 0.0862
10 0.0951 0.1737 0.2870 0.3501 0.3711 0.3572 0.3152 0.2518 0.1737
20 0.1136 0.2070 0.3420 0.4196 0.4501 0.4423 0.4038 0.3420 0.2639
30 0.1368 0.2480 0.4076 0.5000 0.5394 0.5364 0.5000 0.4382 0.3584
40 0.1674 0.3008 0.4885 0.5959 0.6428 0.6428 0.6064 0.5425 0.4592
50 0.2104 0.3724 0.5924 0.7141 0.7660 0.7660 0.7267 0.6580 0.5686
60 0.2764 0.4771 0.7334 0.8660 0.9180 0.9129 0.8660 0.7885 0.6896
70 0.3931 0.6481 0.9397 1.0723 1.1133 1.0940 1.0321 0.9397 0.8264
80 0.6620 0.9848 1.2764 1.3737 1.3785 1.3268 1.2368 1.1199 0.9848
Table 2.1 Continued.
a e 0 10 20 30 40 50 60 70 80
0 0
10 0.0875 0
20 0.1763 0.0861 0
30 0.2680 0.1737 0.0822 0
40 0.3640 0.2639 0.1657 0.0758 0
50 0.4663 0.3584 0.2518 0.1527 0.0670 0
60 0.5774 0.4592 0.3420 0.2321 0.1351 0.0562 0
70 0.7002 0.5686 0.4382 0.3152 0.2053 0.1133 0.0437 0
80 0.8391 0.6957 0.5425 0.4038 0.2788 0.1722 0.0882 0.0299 0
Figure 2.1 PUMA and the crosssection of an object.
A ~
2
A No
3
P
H
]
.4.
AA
crosssection
Figure 2.2 The upper arm of PUMA robot.
11.75"
11
I ~ I
L /U A I
H F
M E
Figure 2.3 Expansion of vertical side.
Figure 2.4 Projection onto the XY plane.
AC
Figure 2.5 Determination of points N1 and N2.
(b)
Figure 2.6 Expansion of the top surface
(a)
(b)
Figure 2.7 Upper arm hits the near corner of the object.
2
2
Figure 2.8 Calculate 0 for Case 1.
Figure 2.9 Xr, < X,.
240 
230 
220
210
200 
190
180
170 
9 160
150
140
130
120
110
100 
90
90 70 50 30 10 10 30 5 70 90
90 70 50 30 10 10 30 50 70 90
Figure 2.10 Boundary values of 0.
Figure 2.11 Upper arm hits the far corner of the object.
2_
2
2e
2 0
Figure 2.12 Calculate 0 for Case 2.
Y
0 x
R
P
Figure 213 X > X
Figure 2.13 Xr, > 0.
Figure 2.14 The upper arm hits the top surface.
Figure 2.15 Calculate 0 for Case 3.
40
Y
X
R R'
P'
PFigure 2.16 X
Figure 2.16 X, > X,,.
Figure 2.17 Object before and after expansion.
41
i e 2 Lie s ffret S
Figure 2.18 Line segments represent the PUMA robot.
wrist center
Figure 2.19 Identification of obstacles.
CHAPTER III
WRIST CENTER LOCALE FINDING
In the preceding chapter, objects were expanded and
obstacles were identified. In this chapter, a series of
vertical planes will be used to determine the wrist center locale
(WCL) for each robot. Section 3.1 describes how to construct
these vertical planes. The determination of a collisionfree area
(CFA) for each individual robot on each vertical plane will be
discussed in Section 3.2.
For two cooperative robots which grasp a common
workpiece, the relative distance between the two wrist centers
is invariant. To determine the position for each wrist
center, this relative distance is considered to be a
constraint. In Section 3.3, this constraint will be applied
to find the wrist center area (WCA). The WCL will then be found by
superimposing the CFA and the WCA.
3.1 Construction of Vertical Planes
A series of vertical planes is formed for each of the two
cooperative robots. Each plane will include a vertex of the
43
obstacle's top surface and an offset, S, to the axis of the
robot's first joint (as shown in Fig. 3.1). The equation of
each plane is given by
Y = mX + b, (3.1)
where m = tanS, (3.2)
b = Yr mXr, (3.3)
and (Xr, Yr) are the X and Y coordinates of the vertex.
The angle 6 is computed as follows (see Fig. 3.2):
6 = B + a 7/2, (3.4)
where B = arctan(Y,/Xr), (3.5)
a = arccos(S/dr), (3.6)
dr = (X2 + Y2)2. (3.7)
For each vertical plane established for the first robot,
a corresponding plane can be found for the second robot
according to the predetermined path of the wrist center.
The equation for wrist center path of the first robot is
given as follows:
R = P + X(Q P), 0 \ 1. (3.8)
where P and Q are the coordinates of the initial and final
positions of the wrist center of the first robot.
44
The equation for wrist center path of the second robot
is given by
R' = R + r S, (3.9)
where r is the distance between two wrist centers, and S6 is
the end effector orientation of the first robot.
If the end effector orientations at the initial and final
positions are the same, a constant end effector orientation
of the first robot is assumed throughout the motion. In that
case, the wrist center path is a straight line.
If the end effector orientation at the initial and final
positions are different, the end effector orientation will be
varied throughout the motion. It is possible to determine
the axis and net angle of rotation about this axis that will
cause the end effector of the first robot to change
orientation as desired. According to the axis and net angle
of rotation, the end effector orientation, S6, at each
intermediate position can be determined [see Crane 1987 pp.
103111]. In this case, the wrist center path of the second
robot becomes a curve as shown in Fig 3.3.
For each vertical plane, a corresponding plane for the
second robot can be found as follows (see Fig. 3.3):
(1) Determine the intersection point I of the plane and
path of the wrist center.
(2) Compute the end effector orientation, S6, at point I.
45
(3) Substitute the coordinates of point I from (1) and
the end effector orientation, S, from (2) into Eq. (3.9), to
find the corresponding point I' on the path of the second
robot.
(4) Use the point I' and the offset S, to construct the
corresponding plane of the second robot.
Following this construction, these planes are arranged
in a sequence according to the angle formed by the offset of
the first robot and the Xaxis. During this arranging
process, any plane which lies outside the work area, or is a
duplicate of previous plane is deleted from the arrangement.
3.2 Determination of CollisionFree Area (CFA)
Each vertical plane cuts through obstacles and the
reachable workspace of the wrist center, and thus generates
crosssections of obstacles and the reachable workspace of the
wrist center. The crosssections are mapped via a rotational
transformation onto the XZ plane. A CFA for each individual
robot can then be found on each transformed crosssection,
according to the relative positions of the obstacles and the
work area of the robot.
Two terms, "principal upper arm obstacle" and )principal forearm obstacle,"
have been established to locate the lower boundary of the
CFA:
46
1. The )principal upper arm obstacle" is defined as the first
obstacle that would be hit by the upper arm if it was rotated
downward from its vertical position (for instance, obstacle
A in Fig. 3.4a).
2. The 'principalforearm obstacle" is defined as the first
obstacle that would be hit by the forearm if it was rotated
downward from its extreme position and the upper arm and the
forearms are collinear (for instance, obstacle B in Fig.
3.4b). However, if the upper arm is halted by a principal upper
arm obstacle, the forearm may rotate further before it strikes an
obstacle (for instant, obstacle B in Fig. 3.4a). This
obstacle is also defined as the principalforeann obstacle.
According to the location of the principal obstacle,
determination of a lower boundary of the CFA will fall into
the following three groups.
Group 1: The angle from joint 2 to any vertex of the
principal obstacles is less than the joint
limitation of joint 2.
In this group, the lower boundary is determined by
rotating the forearm around the lowest position of joint 3.
According to the location of the principalforearm obstacle, there are
two different cases.
Case 1. The X coordinate of the far corner of the
obstacle is greater than the X coordinate of joint 3 (see Fig.
3.5). The actual curve of the lower boundary of the CFA is
a sixth degree polynomial [see Tseng 1987 pp. 121123]. As
suggested, the straight line PR' rather than the sixth degree
polynomial curve, is used to represent the lower boundary.
Point R' denotes the bounding point of the CFA. Point P is
the vertex hit by the forearm. The X and Z coordinates of
point R' are derived as follows:
Xr, = /icos92min + 12COS83, (3.10)
Zr, = H + l1sin02min + 12sinO3, (3.11)
where 11 and 12 are the link length of the upper arm and the
forearm respectively. H is the height of joint 2, and 02min is
the angular limitation of joint 2. The angle 03 is derived as
follows:
03 = arctan[(Zp Zo3)/(Xp Xo3)], (3.12)
where Xo3 = 1cos2min,
Zo3 = H + lisinO2min,
and (Xp, Zp) are the X and Z coordinates of vertex P.
Case 2. The X coordinate of the far corner of the
obstacle is less than the X coordinate of the joint 3 (see
48
Fig. 3.6). In this case, the lower boundary of the CFA is
the far vertical side of this obstacle.
Group 2: The straight line O2R* hits the principalupperarm
obstacle first.
In this group, the lower boundary of the CFA is
determined by the forearm as it rotates around the joint 3
while the upper arm is fixed at its lowest position.
According to the location of the principalforearm obstacle, there are
three different cases.
Case 1. The forearm hits the far corner of the obstacle
(see Fig. 3.7). In this case, the lower boundary of the CFA
is formed by a line PR' and a circular arc R'R. Point R is
the intersection point of the straight line and the outer
boundary of the wrist work area. The X and Z coordinates of
point R are derived as follows:
Xr = (11 + 12)COS2, (3.13)
Zr = H + (11 + 12)sin02, (3.14)
where 02 = arctan[(Zq,H)/Xq,], (3.15)
and (Xq,, Zq,) are the X and Z coordinates of vertex Q'.
* The line 02R represents the upper arm and the forearm
when they are collinear.
49
From Fig. 3.7, the X and Z coordinates of point R' can
be derived as expressed as
X,, = licos02 + 12CosO3, (3.16)
Z, = H + /isinO2 + 2sin03, (3.17)
where 83 = arctan[ (Zpllsin2H)/(X,l1cos8) ] (3.18)
Case 2. The forearm hits the top surface of the
obstacle (see Fig. 3.8). In this case, the lower boundary of
the CFA is form by a circular arc R'R. Point R is defined as
the same in Case 1. The X and Z coordinates of point R is
computed by Eqs. (3.13) and (3.14). Point R' is the
intersection point of the forearm and the top surface. From
Fig. 3.8, the X and Z coordinates of point R' are derived as
follows:
The equation of line PIP2, which represents the top
surface of the obstacle, is given by
S = P1 + Ap(2 P1), (3.19)
where Pi and P2 are the coordinates of the two vertices.
The coordinates of the intersection point are given by
Xr = Xo3 + 12cos3,,
(3.20)
Zr = Z3 + /1sin03. (3.21)
where Xo3 = 11cos02, (3.22)
Zo = H + 11sin02, (3.23)
and 02 is calculated by Eq. (3.15).
Substituting Eqs. (3.20) and (3.21) into Eq. (3.19)
yields
Acos03 + Bsin03 + C = 0, (3.24)
where A = 12(Zp2 Zpl),
B = 12(Xp2 Xpl),
C = (Zp2Zp,) (Xo3Xpl) (Z03Zpl) (Xp2Xpl)
Solving Eq. (3.24) yields two values for 03.
Substituting 03 into Eq. (3.20) gives a pair of values for Xr,.
The value of X, which lies between Xp, and Xp2 is selected.
Case 3. The principal forearm obstacle is the same as the
principal upper arm obstacle (see Fig. 3.9). There are three
subcases:
Case 3.1. The upper arm hits the far corner of the obstacle,
and the forearm hits the far vertical side of the obstacle
(see Fig. 3.9a). In this case, the lower boundary of the CFA
51
is formed by a circular arc R'R. The X and Z coordinates of
point R are computed by Eqs. (3.13) and (3.14). Point R' is
the intersection point of obstacle and the forearm. The X and
Z coordinates of point R' can be derived as follows:
Xr, = Xq, (3.25)
Zr, = H + lsin82 + 2sinO3, (3.26)
where B3 = arccos[(Xq l1cos02) / 2], (3.27)
and 02 is calculated by Eq. (3.15).
Case 3.2. The upper arm hits the near corner of the obstacle,
and the forearm hits the top surface of the obstacle (see Fig.
3.9b). In this case, the lower boundary is formed by a
circular arc R'R. Point R' is the intersection point of the
forearm and the top surface. The X and Z coordinates of point
R' are derived as follows:
The equation of line PQ, which represents the top surface
of the obstacle, is given by
S = P + ( P), (3.28)
where P and Q are the coordinates of the two vertices.
52
The X and Y coordinates of the intersection point R' are
given by
X, = X03 + 12cos83, (3.29)
Z,, = Zo3 + l2sin03. (3.30)
where Xo3 and Zo3 are calculated by Eqs. (3.22) and (3.23).
Substituting Eqs. (3.29) and (3.30) into Eq. (3.28)
yields
Acos03 + Bsin03 + C = 0, (3.31)
where A = 2(Zq Zp),
B = 2(Xq Xp),
C = (ZqZp) (Xo3Xp) (Zo3Zp) (XqXp) .
Solving Eq. (3.31) yields two values for 83.
Substituting 03 into Eq. (3.29) gives a pair of values for Xr,.
The value of X,, which lies between Xp and Xq is selected.
Case 3.3. The upper arm hits the near corner of the obstacle,
and the forearm hits the far corner of the obstacle (see Fig.
3.9c). In this case, the lower boundary is formed by a line
QR' and a circular arc R'R. Point Q is the far corner of the
obstacle. The X and Z coordinates of point R' are derived as
follows:
X,, = Xo3 + 12COS03, (3.32)
Zr, = Zo3 + 12sinO3, (3.33)
where 83 = arctan[(Zq Zo3)/(Xq Xo) ], (3.34)
X,3 and Zo3 are computed by Eqs. (3.22) and (3.23).
Group 3: The straight line 02R hits the principalforeann
obstacle first.
In this group, the lower boundary of the CFA is given by
line PR (see Fig. 3.10). The X and Z coordinates of point R
are derived as follows:
Xr = (lI + 12)cos02, (3.35)
Zr = H + (l1 + 12)sinO2, (3.36)
where 02 = arctan[(ZpH)/Xp]. (3.37)
In all three groups, the lower boundary of the CFA on
the right side of the principal obstacle was determined. The
lower boundary between the principal obstacle and the left
most obstacle was formed by connecting the vertices of each
obstacle with a straight line (for instance, the line QQ' as
shown in Fig. 3.7).
54
3.3 Determination of Wrist CenterArea (WCA)
After the position of the wrist center of the first robot
is selected, a sphere is constructed with its center located
at this position, and with a radius equal to the distance
between the wrist centers of the two robots (see Fig. 3.11).
The intersection of the sphere and the vertical plane of the
second robot is a circle. The wrist center will then lay in
that part of the circumference of this circle which lies in
the work area.
The wrist center position of the second robot is
determined as follows:
1) The vertical plane 7rI which forms an angle 86 with
the Xaxis (see Fig. 3.12) is considered. The normal vector
of this plane is nj = (sin61, cos61, 0).
A new coordinate system OXYZl is constructed with the Y1
axis parallel to this normal vector n, with an offset S. The
relationship between the original OXYZ and the new OXYZ1
coordinate systems is derived as follows:
l.a) Rotating the coordinate system about Zaxis through
angle 6, yields
X Ci si 0 x
Y = si ci 0 y (3.38)
Z 0 0 1 z
where ci = cos51,
s, = sin6,.
l.b) A translation S along the yaxis is expressed by
x X, 0
y = YI + S (3.39)
z Zi 0
where S is the offset of the joint 2.
Hence, the transformation is given by
X ci si 0 X,
Y s= s c 0 Yi+S (3.40)
Z 0 0 1 Z,
In the OXiYZ, coordinate system, a point on plane 71 can
be designated by (Xi, 0, ZJ). According to Eq. (3.40), the
same point in the OXYZ coordinate system is designated by
(cX1I sIS, siX1 + ciS, ZI) .
2) For the second robot, the normal vector of the
corresponding plane I, is n2 = (sinS2, coss2, 0) (see Fig.
3.10). A coordinate system O2X2Y2Z2 is constructed with the Y2
axis parallel to n2. The origin 02 has an offset with the base
of the first joint, and X2Z2 plane coincides with the plane r2
(see Fig. 3.12). The relationship between the OXYZ and 02X2Y2Z2
coordinate systems can be obtained by the following steps:
56
2.a) A translation d along the Xaxis is expressed by
X X' d
Y = Y' + 0 (3.41)
Z Z' 0
where d is the distance between two robots.
2.b) Rotating the coordinate system about the Z'axis
through the angle 62 yields
X' c2 S2 0 x"
Z' 0 0 1 z"
where C2 = cos62,
s2 = sin62.
2.c) A translation S along the y"axis is expressed
by
x" X2 0
z" Z2 0
The relationship between the two coordinate systems is
therefore
X c2 S2 0 X2 d
Y S2 c2 0 Y2+S + 0 (3.44)
Z 0 0 1 Z2 0
In the O2X2Y2Z2 coordinate system a point in plane 72 can
be designated by (X2, 0, Z2). According to Eq. (3.44), the
same point in the OXYZ coordinate system is designated by (cX2
 s2S + d, sX2 + c2S, Z2).
3) For two cooperative robots, the distance between the
two wrist centers is constant and equal to R. This condition
can be expressed mathematically as
[(c2X2S2S+d) (cXlslS) ]2 + [(S2X2+C2S) (sXl+CiS) ]2
+ [Z2Z1]2 = R2. (3.45)
From Eq. (3.45), for given coordinates of either (Xj, Zj)
or (X2, Z2), the WCA for each robot can be found.
3.3.1 WCA for the First Robot
For a given position of the wrist center of the first
robot, given X, and Z,, Eq. (3.45) can be rewritten
(X2 a)2 + (Z2 Zi)2 = r2, (3.46)
where a = C12X 1.2S c2d, (3.47)
r = (R2 2)1/2, (3.48)
58
and p = (s2X2[ (s.2c12S.2) S+ (cic2c.2) d] X,
+ (lci) 2S2+2 [ SS2C2S12]Sd+s d2)1/2, (3.49)
c.2 = cos(61 62),
s,2 = sin(61 62).
If r < 0, there is no intersection between the sphere and
plane T2. This means that a wrist center position cannot
exist for the second robot. Hence, r must be greater than
zero, thus p must be less than R. From Eq. (3.49),
X 2aiXi + a2 < 0, (3.50)
where al = [ (cc2c2) d+ (s12c1.2s12) S]/s2,
a2= [(1c12) 2S2+2 (s1S22S12) Sd+s2d2R2]/s.2.
By solving Eq. (3.50)
AI < XI, A2, (3.51)
where A, = a, (al a2)1/2,
A2 = a, + (a 2 a2)1/2
The reachable limits of the PUMA robot in the X direction
are
34.1" < X, < 34.1".
(3.52)
59
From Eqs. (3.51) and (3.52),
X1Iow < Xi < Xiup. (3.53)
Figure 3.13 shows four different cases of WCA for the
first robot.
3.3.2 WCA for the Second Robot
Similarly, for a given set of coordinates of the wrist
center position of the second robot, Eq. (3.45) can be written
as a function of X1 and Z,:
(Xi b)2 + (Z1 Z2)2 = r'2, (3.54)
where b = c12X + cd + s,2S, (3.55)
r' = (R2 12)1/2 (3.56)
p = { s2X +2 [ (c2CIc12) d+ (s.2c2s1.2) S ]X2
+ (1C12)2S2+2 (s1s2cS1.2) Sd+s2d21/2. (3.57)
For the existence of the wrist center of the first robot,
r' must be greater than zero. From Eq. (3.56),
X2 2bX2 + b2 < 0, (3.58)
where bi = [(cici2c2) d (s,12c2S2) S]/s 2,
b2 [(c.2) 2S2+2 (s,s2c.12) Sd+sd2 R2]/s 2.
By solving Eq. (3.58),
BI < X2 < B2, (3.59)
where B, = b, (b b2)1/2,
B2 = b, + (b2 b2)12.
Due to the reachable limitation, X2 must also satisfy the
boundary conditions as defined in Eq. (3.52).
In the previous, the collisionfree area (CFA) for each
individual robot and the wrist center area (WCA) for two robots
under cooperative motion, were determined. The wrist center locale
(WCL) for both robots is found by superimposing these two
areas. In the next chapter, determination of intermediate
positions from the WCL will be discussed.
Figure 3.1 Series of vertical planes.
Figure 3.1 Series of vertical planes.
(Xr, Yr)
Figure 3.2 Determination of angle 6.
Q'(Xq', Yq')
P(Xp, Yp)
P'(Xp', Yp')
FIgure 3.3 Determination of corresponding plane.
Q (Xq, Yq)
Figure 3.4 Principal obstacles.
Figure 3.5 Group 1 Case 1.
R2
Figure 3.6 Group 1 Case 2.
Group 2 Case 1.
Group 2 Case 2.
Figure 3.7
Figure 3.8
(b)
Figure 3.9 Group 2 Case 3.
Figure 3.9 Continued
Figure 3.10 Group 3.
Figure 3.11 Determination of WCA.
YY
Y
x
0 / 0 X,
Figure 3.12 Projection onto XY plane.
69
70 
7 0 20,0
60
50 
40 
30 
20 
10
0
10
40 20 0 20 40
xl
(a)
70
60 
50 
40 
W 30 
20 
10 
0
10
40 20 0 20 40
Xl
(b)
Figure 3.13 Examples of WCA.
70
70
60 I I I I
50 
40 
30
20 
10 
10
10 
40 20 0 20 40
X1
(C)
70
60 
50 
40 
30 
20 
10 
10
40 20 0 20 40
XI
(d)
Figure 3.13 Continued.
CHAPTER IV
COLLISIONFREE PATH FINDING
Path finding is accomplished by locating the intermediate
positions within the wrist center locale (WCL) and joining these
positions from the initial position to the final destination.
As mentioned in Section 2.2, a straight line path, from
the initial position to the final position of the wrist center
for the first robot, was assumed as a predetermined path.
This straight line intersects each vertical plane at one
point. The intersection point is called the "recommended wrist
center position." If this point lies outside the WCL, it is
necessary to shift the point into the WCL for further
consideration.
For example, in Fig. 4.1a, the recommended wrist centerposition,
E, lies under the WCL. In this case, it is shifted along the
Zdirection until it falls within the WCL, E'. In a second
example, point E lies outside the WCL (see Fig. 4.1b). It is
shifted first along the Xdirection and then along the Z
direction until it reaches the region of the WCL, E'.
72
Because the two robots are working in cooperation, it is
necessary to check the possibility of collision between the
end effectors, the grasped workpiece, and the obstacles during
the selection of the intermediate wrist center position. This
will be discussed in Section 4.1.
The collision between links of the two cooperative robots
also needs to be taken into account during the selection of
the intermediate wrist center positions. This will be
discussed in Section 4.2.
4.1 Collision Between End Effectors,
Grasped Workpiece and Obstacles
In Chapter III, the wrist centerlocale (WCL) on each vertical
plane was determined. During the determination of the WCL,
only the collision between the upper arm and the forearm with
respect to the obstacles has been discussed. It is now
desired to check the possibility of collision between the end
effectors and the grasped workpiece with the obstacles.
Since the end effector grasps the workpiece firmly and
no slippage occurs during the motion, the end effectors and
the grasped object can be treated as a rigid body. A line
segment PQ is used to represent this rigid body, with the
endpoints P and Q located at the wrist center position of each
robot respectively (see Fig. 4.2).
For a given position of the wrist center, P, the
corresponding position, Q, will lie on a circle. Thus, the
73
possible position for the end effectors and grasped workpiece
will form a coneshaped region with the vertex at point P (see
Fig. 4.2). The collision checking can be simplified as the
determination of the intersection between this region (cone)
and the obstacles.
In Section 4.1.1, obstacles which affect the movement of
the end effectors and the grasped workpiece is identified.
Then in Section 4.1.2 a perspective projection is applied to
determine the possible position for point Q.
4.1.1 Obstacles Identification
To check if the obstacles affect the movement of the end
effectors and the grasped workpiece, the cone and the
obstacles are projected onto the plane Z = 0 (see Fig. 4.3).
Triangle PiQ1R1 is the image of the cone on the projection
plane. The image of any obstacle on the projection plane is
a polygon. If the polygon touches the triangle, the obstacle
will affect the movement of the end effectors and the grasped
workpiece.
Triangle PiQR can be represented by two sets of angular
ranges [a, B] and [B', a'] with respect to Pi and O'. The
point 0' is the center of the right hand robot's base. Angles
a and B are computed as follows:
a = arctan[(Yql Ypi)/(Xqi Xp)], (4.1)
B = arctan[(Y, Ypi)/(Xi Xp,)], (4.2)
74
where (Xp1, Yp,), (Xqi, Yqi), (Xr,, Yrj) are the coordinates of
points P1, Q, and Ri. Angles a' and B' are computed by Eq.
(2.32).
The angular range of each polygon with respect to O' is
represented by [0 O8]. Angles O8 and 6 are computed by Eq.
(2.32).
The detection of a possible intersection is achieved as
follows:
Step 1. Check the angular range [80, 0f] of each polygon
with respect to [B', a']. If it falls into one of the
following conditions, the polygon intersects the cone.
1. [68, Of] contains [B', a'] (for example, polygon A
in Fig. 4.4a).
2. [80, Of] overlaps with [B', a'] (for example, polygon
B in Fig. 4.4b).
3. [O8, Of] lies within [B', a'] (for example, polygon
C in Fig. 4.4c).
In any of the cases, it is necessary to proceed with Step
2.
Step 2. Exclude the portion of the polygon which is
outside the area formed by lines MP1 and MQ, (see Fig. 4.5).
The new vertices of the remaining portion can be found by
checking the intersection points) of each edge with line MP,
and MQ1. For example, polygon A becomes polygon A1A2A3A4A,6A as
shown in Fig. 4.5.
75
Step 3. Find the angle, Oi, between Xaxis and the line
from P, to each vertex of the new polygon (see Fig. 4.6). This
angle is computed as follows:
0i = arctan[(Y, Yp1)/(Xi Xp,)], (4.3)
where (Xi, Yi) are the coordinates of i'h vertex of the polygon.
The angular range, [minf, Omax], for each polygon with
respect to PI can be calculated. The same conditions for
finding affected obstacle in Step 1 is applied here to exclude
those unaffected obstacles.
Step 4. Exclude the portion of polygon which lies
outside the area of triangle P1Q1Rj.
From the above steps, obstacles which affect the movement
of the end effectors and the grasped workpiece have been
identified. It is now desired to check the height of each
obstacle. If the height is lower than the lower point of the
cone, this obstacle will not affect the movement. In this
case, this obstacle is excluded from further consideration.
4.1.2 Perspective Projection
In the previous section, obstacles which will affect the
movement of the end effectors and the grasped workpiece have
been identified. It is now desired to know the effect of
these obstacles. For a given wrist center position, P, it is
76
necessary to determine if a corresponding wrist center Q
exists.
A perspective projection from point P to plane f' is
shown in Fig. 4.7. The coordinate system O2X2Y2Z2 on the plane
7' is used to find the transformation matrix. The
relationship between coordinate systems 02X2Y2Z2 and OXYZ is
given by'
x c2 S2 0 Xd 0
y = S2 C2 0 Y S (4.4)
z 0 0 1 Z 0
where d is the distance between the bases of the two robots,
and S is the offset of joint 2 (see Fig. 3.12).
The transformation of this perspective projection is
given by
x' yp Xp 0 x
y' 0 0 0 y (4.5)
Y Yp
z' 0 z, y, z
1 This relationship is obtained from Eq. (3.46).
77
where (xP, yp, zP) are the coordinates of P on the O2X2Y2Z2
coordinate system.
In Fig. 4.8, polygon A is the image of an obstacle on
plane 7'. Circle C is generated by the possible positions of
wrist center Q for a given position of P. From the relative
positions among circle C, polygon A and the CFA, the
existence of wrist center Q can be determined. If any portion
of C lies outside the polygon A and within the area of CFA,
as shown in Fig. 4.8, the wrist center Q can be located on
that portion. Otherwise, a wrist center Q cannot exist. In
this case, it is necessary to reselect a position of P and
repeat the procedure until a corresponding wrist center Q is
found.
4.2 Collision Checking Between Two Robots Links
It is necessary to check the possibility of collision
between two robots links during the motion. Since the upper
arm and the forearm of each robot lie on a vertical plane at
each interval, a collision will occur on the intersection line
of the two planes. Section 4.2.1 discusses the case when two
planes intersect in one line. The cases when two planes are
coincident or parallel are discussed in Sections 4.2.2 and
4.2.3.
78
4.2.1 Two Planes Intersect at One Line
The equation of the vertical plane which contains the
upper arm and the forearm of the left hand robot is given by
Y = m1X + bl, (4.6)
where m, = tan(6,),
bi= Y1 miX,
and X, = S cos(80),
Y, = S sin(01),
08 = 61 + 7r/2.
Further, S is the offset of joint 2, and 6, is calculated by
Eq. (3.4) (see Fig. 3.2).
For the right hand robot, the plane equation is given by
Y = m2X + b2, (4.7)
where m2 = tan(62),
b2 = m,X,,
and X2 = S cos(82) + d,
Y2 = S sin(02),
02 = 62 + 7/2.
Further, d is the distance between the base of the two robots.
The equation of the line of intersection, L, can be
obtained from these two expressions:
b2 b,
X = (4.8a)
m, m2
mlb2 m2bb
Y = (4.8b)
mI m2
According to the relative position between the wrist
center (P or Q) and the line of intersection, L, the
possibility of collision can be detected.
If both (or one of the two) wrist centers locate(s)
between line L and the first joint axis of each robot, the
links between the two robots will not collide (see Fig. 4.9).
The coordinate system OXYZ, attached to the plane which
contains the upper arm and the forearm of the left hand robot,
has been discussed in Section 3.3. The relationship between
the coordinate systems OXYZ, and OXYZ is given by2
X, ci si 0 X 0
YI = si c, 0 Y S (4.9)
Zi 0 0 1 Z 0
The equation of the line of intersection, L, in the
coordinate system OXYiZ can be obtained from these two
expressions:
2 This relationship is obtained from Eq. (3.42).
Xi = c, X + s, Y, (4.10a)
YI = s, X + c, Y S = 0. (4.10b)
The coordinate system 2X2Y2Z2 attached to the plane which
contains the upper arm and the forearm of the left hand robot,
has been discussed in Section 3.3. The relationship between
the coordinate systems O2X2Y2Z2 and OXYZ is given by3
X2 c2 S2 0 Xd 0
Y2 = s2 2 Y S (4.11)
Z2 0 0 1 Z 0
The equation of the line of intersection, L, in the
coordinate system 02X2Y2Z2 can be obtained from these two
expressions:
X2 = C2 (X d) + 2 Y, (4.12a)
Y2 = s2 (X d) + c2 Y S = 0. (4.12b)
For the first robot, the wrist center position is denoted
by P, and the elbow position is denoted by P' (see Fig. 4.9).
For the second robot, the wrist center position is denoted by
Q, and the elbow position is denoted by Q'. When Xp > X, and
3 This relationship is obtained from Eq. (3.46).
81
X2q > X2, there exists the possibility that the two robots
links will collide with each other.
To determine which link will collide with another link,
it is necessary to calculate the X coordinate of the elbow
position. The X and Z coordinates (X1p, and ZI,,) of the elbow
position, P' on the coordinate system OXYZil, can be obtained
by solving the following equations:
(Xp,, Xp)2 + (Zip, ZP,)2 = l2, (4.13)
X, = 11 cos8, (4.14a)
and Zip, = H + 11 sinO, (4.14b)
where H is the height of the joint 2, 11 and 12 are the
dimensions of the upper arm and the forearm respectively.
Substituting Eq. (4.14) into Eq. (4.13) yields
AcosO + Bsin8 + C = 0, (4.15)
where A = 2 Xip 11,
B = 2 (H /i li Zip),
C = H2 + l + X[2 + Z2 1 2 H /l.
Solving Eq. (4.15) yields two values for 0. Substituting
0's into Eq. (4.14b) gives a pair of values for Zi,,. The value
82
of ZIp, which gives the robot an elbow up configuration4 is
selected. The procedure for determining the coordinates of
Q' is the same as for P'.
If X,, is greater than X1, there is a possibility that the
upper arm will collide with either the forearm or the upper
arm of the right hand robot (see Fig. 4.10a). If Xp, is less
than X,, there is a possibility that the forearm will collide
with either the forearm or the upper arm of the right hand
robot (see Fig. 4.10b). The Z coordinate of the intersection
point I, between the link (upper arm or forearm) and line L
can be computed as follows:
Case 1. The upper arm passes through the line L. In
this case, the Z coordinate of I, is given by
Z, = H + (ZIp, H), (4.16)
where X = Xi / X1p,.
Case 2. The forearm passes through the line L. In this
case, the Z coordinate of point I, is given by
Zil = Zip, + A(ZIp Zip,), (4.17)
where A = (X, Xp,) / (XP Xlp,)
4 In this study, only the elbow up configuration for the
robot is considered.
83
Similarly, the Z coordinate of the intersection point 1,,
Z,, of the second robot with the line L can be determined.
If the difference between Zi and Z2 is less than the
width of the two links, these two links will collide. In this
case, it is necessary to reselect the wrist center positions
for both robots.
4.2.2 Two Planes Are Coincident
In this case, it is necessary to check the intersection
between each link and the end effectors to detect the
collision. The coordinate of the elbow position for each
robot is computed as described in Section 4.2.1. Since these
two planes are coincident, it is necessary to transfer the
coordinates of points Q and Q' from the coordinate system
O2X2Y2Z2 to O1X1Y1Z1. The relationship between these two
coordinate systems is given by (see Fig. 4.11)
X, 1 0 0 X2 D
Y = 0 1 0 Y2 + 0 (4.18)
Zi 0 0 1 Z2 0
where D = (d2 4S2)1/2
If the X coordinate of point P, XP, is less than the X
coordinate of point Q, Xq, there will be no collision between
these two robots (see Fig. 4.12).
84
If X, > Xq, it is necessary to check the intersection
between the links and end effectors together with the grasped
workpiece. The types of intersection can be classified into
the following six different cases:
Case 1. The upper arm of the first robot collides with
the end effectors and grasped workpiece (see Fig. 4.13a).
Case 2. The upper arm of the first robot collides with
the forearm of the second robot (see Fig. 4.13a).
Case 3. The upper arm of the first robot collides with
the upper arm of the second robot (see Fig. 4.13b).
Case 4. The forearm of the first robot collides with
the forearm of the second robot (see Fig. 4.13c).
Case 5. The forearm of the first robot collides with
the upper arm of the second robot (see Fig. 4.13d).
Case 6. The upper arm of the second robot collides with
the end effectors and the grasped workpiece (see Fig. 4.13b).
If any one of the above six cases occurs, the two robots
collide. It is thus necessary to assign another set of wrist
center positions and repeat the checking procedure until none
of the above six cases occurs.
4.2.3 Two Planes Are Parallel
When two planes are parallel to each other, it is
necessary to calculate the distance between these two planes.
If the distance between these two planes is greater than the
85
thickness of the links, the two robots links will not collide
with one another.
If the distance is less than the thickness, it is
necessary to find the positions of the elbow and the wrist
center for each robot. These points are projected onto a
plane Y = mX, which is parallel to both planes. From the
relative position of P, P', Q and Q' on this projection plane
(see Fig. 4.14), the collision between the two robots can be
determined. The same procedure which is used for the case in
Section 4.2.2, can be applied to determine if these two robots
collide.
4.3 CollisionFree Path Generation
In the previous two sections, an algorithm was developed
to select the intermediate positions of the wrist center. The
collisionfree wrist center path for each robot can thus be
generated by connecting these intermediate positions, from the
initial position to the final destination.
In general, the path which sequentially connects all the
intermediate positions is not a smooth one. For example, the
path which passes through point a, b, c, ..., 1, in Fig. 4.15
is generated by connecting the intermediate position. It is
desirable to shorten and to smooth this path.
The path modification is accomplished by the following
steps:
86
Step 1. Compute the slope of each line segment which
connects the starting position to every intermediate position.
Step 2. Find the line segment which has the greatest
slope; for example, this is the line segment ae in Fig. 4.15.
Step 3. Shift every intermediate position between the
starting position and the end position of the line segment
found in Step 2. For example, positions b, c, and d are
shifted to b', c', and d' in Fig. 4.15.
Step 4. Use the end position of the line segment found
in Step 2 as the new starting position and repeat the
procedure until reach the final position.
During the shifting, it is necessary to check if the new
positions lie outside the outer boundary of the wrist center
workspace. If a position lies outside the workspace, it must
be moved down until it reaches the workspace.
The path modification is based on the original path of
the first robot. Due to the cooperative operation of the two
robots, the path of the second robot is changed following the
modification of the first robot as shown in Figs 4.16 and
4.17. However, in some cases, the modified path of the second
robot cannot be followed by its wrist center. Figure 4.18
shows an example that part of the modified path of the second
robot lies outside the upper limit of the wrist center. In
this case, the original generated path is used without any
modification.
87
During the path modification, it is also necessary to
check again the collision between the two robots links. After
shifting the wrist center position, the two robots links may
collide with each other. In this case, the original generated
paths are used as the final wrist center paths. It should be
noted that the generated path of the second robot can also be
used to perform the modification. The path of the first robot
is then changed according to it.
4.4 Computer Graphics Simulation
The algorithms developed in this work were implemented
on the Silicon Graphics IRIS 4D/70GT workstation. The
programs were written in C Language and a builtin graphics
library on the system was used for basic graphics functions
such as line draw, polygon fill, and so on. The surface
models for the robot working environment and the PUMA robots
were built by utilizing a Zbuffer algorithm provided by the
system.
After data input, the collisionfree paths were found
within several seconds. This included generating graphics
objects, and modification of the path. During the
computation, if collisionfree path could not be generated,
the program would give an error message and stop running the
simulation. For example, as shown in Fig. 4.19, part of the
generated path lies outside the upper limit. Thus a
collisionfree path cannot be found.
88
Figure 4.20 shows two PUMAs grasp a workpiece in an
obstaclestrewn environment. Figure 4.21 shows the obstacles
after expansion, and the upper arm and the forearm are
represented by two line segments. The generated path and the
path after modification are shown in Figs. 4.22 and 4.23.
Figures 4.24 4.26 show the consecutive configurations of the
two PUMAs as they move along the collisionfree paths.
(b)
Figure 4.1 Shifting recommended wrist center position.
Figure 4.2 Wrist center positions.
RI
77C x
Figure 4.3 Projection onto the plane Z = 0.
Figure 4.4 Identification of obstacle (step 1).
Figure 4.4 Continued.
Figure 4.5 Identification of obstacle (step 2).
Identification of obstacle (step 3).
Figure 4. 6
