Page 236 - Sensing, Intelligence, Motion : How Robots and Humans Move in an Unstructured World

PLANAR REVOLUTE–REVOLUTE (RR) ARM 211

The RR-Arm Algorithm procedure is as follows:

1. All four complementary M-lines, M k ,k = 1, 2, 3, 4, are defined according
to the formulae (5.2) and (5.5) and are ordered as follows: M 1 is the short-
est; M 2 complements M 1 over the angle θ 2 [as in (5.2)]; M 3 complements
M 1 over θ 1 ; M 4 complements M 1 over both θ 1 and θ 2 .GotoStep2.
2. M 1 -line is designated as M-line. Set the flag down. Set j = 1. Go to Step 3.
3. Counters C 1 and C 2 are set to zero. From point L j−1 , the arm moves along
M-line until one of the following takes place:
(a) Target T is reached. The procedure stops.
(b) An obstacle is encountered and a hit point, H j , is defined. Go to Step 4.
4. Counters C 1 and C 2 are turned on. The arm follows the obstacle boundary
in the chosen local direction, until one of the following takes place:
(a) Target T is reached. The procedure stops.
(b) M-line is met at a distance d from T such that d< d(H j ,T ); point
L j is defined. Increment j.Go toStep3.
(c) The arm returns to position H j (which completes a closed curve along
the virtual boundary) without ever meeting the M-line. Go to Step 5.
5. Examine the obstacle range accumulated in counters C 1 and C 2 . One of
the following takes place:
(a) The range is (0, 0), which means it is a Type I obstacle. Target cannot
be reached. The procedure stops.
(b) The range is not (0, 0) and the flag is up, which means it is the second
closed curve of the virtual boundary of a Type II obstacle. The target
cannot be reached. The procedure stops.
The remaining three events relate to the case when the range is not (0,
0) and the flag is down (which means that the current virtual boundary
is the first closed curve of the virtual boundary of a Type II obstacle).
(c) The range is (0, n 2 ), |n 2 |≥ 1; designate the shorter of M 3 and M 4 as
the M-line. Go to Step 6.
(d) The range is (n 1 ,0), |n 1 |≥ 1; designate the shorter of M 2 and M 4 as
the M-line. Go to Step 6.
(e) The range is (n 1 ,n 2 ), |n 1 |≥ 1, |n 2 |≥ 1; designate the shortest of M 2 ,
M 3 ,and M 4 as the M-line. Go to Step 6.
6. The arm moves back to Start S. Set the flag up. Set j = 1. Go to Step 3.

5.2.3 Step Planning
Physical realization of the above motion planning algorithm requires a small
additional piece that addresses the following question: With the overall path
being generated by the motion planning algorithm, how does one plan every
step along that path? While this question is somewhat outside our main topic of