7.3 Mixing Discrete and Continuous Spaces

Many important applications involve a mixture of discrete and continuous vari- ables. This results in a state space that is a Cartesian product of the C-space and a finite set called the mode space. The resulting space can be visualized as having layers of C-spaces that are indexed by the modes, as depicted in Figure

7.11. The main application given in this section is manipulation planning; many others exist, especially when other complications such as dynamics and uncertain- ties are added to the problem. The framework of this section is inspired mainly from hybrid systems in the control theory community [409], which usually model mode-dependent dynamics. The main concern in this section is that the allowable robot configurations and/or the obstacles depend on the mode.

• • 1. Hybrid Systems Framework

As illustrated in Figure 7.11, a hybrid system involves interaction between dis- crete and continuous spaces. The formal model will first be given, followed by some explanation. This formulation can be considered as a combination of the components from discrete feasible planning, Formulation 2.1, and basic motion planning, Formulation 4.1.

Formulation 7.3 (Hybrid-System Motion Planning)

• • • 1. The W and C components from Formulation 4.1 are included.
      2. A nonempty mode space, M that is a finite or countably infinite set of modes.
      3. A semi-algebraic obstacle region O(m) for each m ∈ M.

m = 4

m = 3

m = 2

m = 1

Modes Layers

Figure 7.11: A hybrid state space can be imagined as having layers of C-spaces that are indexed by modes.

• • • 1. A semi-algebraic robot (m), for each m M. It may be a rigid robot or a collection of links. It is assumed that the C-space is not mode-dependent; only the geometry of the robot can depend on the mode. The robot, trans-

A ∈

formed to configuration q, is denoted as A(q, m).

• • • 1. A state space X is defined as the Cartesian product X = C × M. A state is represented as x = (q, m), in which q ∈ C and m ∈ M. Let

Xobs = {(q, m) ∈ X | A(q, m) ∩ O(m) /= ∅}, (7.13) and Xfree = X \ Xobs.

• • • 1. For each state, x ∈ X, there is a finite action space, U(x). Let U denote the set of all possible actions (the union of U(x) over all x ∈ X).
      2. There is a mode transition function fm that produces a mode, fm(x, u) ∈ M, for every x X and u U(x). It is assumed that fm is defined in a way that does not produce race conditions (oscillations of modes within an instant of time). This means that if q is fixed, the mode can change at most once. It then remains constant and can change only if q is changed.

∈ ∈

• • • 1. There is a state transition function, f, that is derived from fm by changing the mode and holding the configuration fixed. Thus, f(x, u) = (q, fm(x, u)).
      2. A configuration xI ∈ Xfree is designated as the initial state.
      3. A set XG Xfree is designated as the goal region. A region is defined instead of a point to facilitate the specification of a goal configuration that does not depend on the final mode.

∈

• • • 1. An algorithm must compute a (continuous) path τ : [0, 1] → Xfree and an

action trajectory σ : [0, 1] → U such that τ (0) = xI and τ (1) ∈ XG, or the

algorithm correctly reports that such a combination of a path and an action trajectory does not exist.

The obstacle region and robot may or may not be mode-dependent, depending on the problem. Examples of each will be given shortly. Changes in the mode depend on the action taken by the robot. From most states, it is usually assumed that a “do nothing” action exists, which leaves the mode unchanged. From certain states, the robot may select an action that changes the mode as desired. An interesting degenerate case exists in which there is only a single action available. This means that the robot has no control over the mode from that state. If the robot arrives in such a state, a mode change could unavoidably occur.

The solution requirement is somewhat more complicated because both a path and an action trajectory need to be specified. It is insufficient to specify a path because it is important to know what action was applied to induce the correct mode transitions. Therefore, σ indicates when these occur. Note that τ and σ are closely coupled; one cannot simply associate any σ with a path τ ; it must correspond to the actions required to generate τ .

Example 7.2 (The Power of the Portiernia) In this example, a robot, , is modeled as a square that translates in = R2. Therefore, = R2. The obstacle region in is mode-dependent because of two doors, which are numbered “1” and “2” in Figure 7.12a. In the upper left sits the portiernia,2 which is able to give a key to the robot, if the robot is in a configuration as shown in Figure 7.12b. The portiernia only trusts the robot with one key at a time, which may be either for Door 1 or Door 2. The robot can return a key by revisiting the portiernia. As

W

W C

A

shown in Figures 7.12c and 7.12d, the robot can open a door by making contact with it, as long as it holds the correct key.

The set, M, of modes needs to encode which key, if any, the robot holds, and it must also encode the status of the doors. The robot may have: 1) the key to Door 1; 2) the key to Door 2; or 3) no keys. The doors may have the status: 1) both open; 2) Door 1 open, Door 2 closed; 3) Door 1 closed, Door 2 open; or 4) both closed. Considering keys and doors in combination yields 12 possible modes. If the robot is at a portiernia configuration as shown in Figure 7.12b, then its available actions correspond to different ways to pick up and drop off keys. For example, if the robot is holding the key to Door 1, it can drop it off and pick up the key to Door 2. This changes the mode, but the door status and robot configuration must remain unchanged when f is applied. The other locations in which the robot may change the mode are when it comes in contact with Door 1 or Door 2. The mode changes only if the robot is holding the proper key. In all other configurations, the robot only has a single action (i.e., no choice), which keeps the

mode fixed.

The task is to reach the configuration shown in the lower right with dashed lines. The problem is solved by: 1) picking up the key for Door 1 at the portiernia;

2This is a place where people guard the keys at some public facilities in Poland.

• • • • 1. (b)

(c) (d)

Figure 7.12: In the upper left (at the portiernia), the robot can pick up and drop off keys that open one of two doors. If the robot contacts a door while holding the correct key, then it opens.

1. opening Door 1; 3) swapping the key at the portiernia to obtain the key for Door 2; or 4) entering the innermost room to reach the goal configuration. As a final condition, we might want to require that the robot returns the key to the

portiernia. ．

Example 7.2 allows the robot to change the obstacles in . The next example involves a robot that can change its shape. This is an illustrative example of a reconfigurable robot. The study of such robots has become a popular topic of research [209, 385, 552, 990]; the reconfiguration possibilities in that research area are much more complicated than the simple example considered here.

O

Example 7.3 (Reconfigurable Robot) To solve the problem shown in Figure 7.13, the robot must change its shape. There are two possible shapes, which correspond directly to the modes: elongated and compressed. Examples of each

are shown in the figure. Figure 7.14 shows how Cfree(m) appears for each of the

Figure 7.13: An example in which the robot must reconfigure itself to solve the problem. There are two modes: elongated and compressed.

Elongated mode Compressed mode

Figure 7.14: When the robot reconfigures itself, free(m) changes, enabling the problem to be solved.

C

two modes. Suppose the robot starts initially from the left while in the elongated mode and must travel to the last room on the right. This problem must be solved by 1) reconfiguring the robot into the compressed mode; 2) passing through the corridor into the center; 3) reconfiguring the robot into the elongated mode; and

4) passing through the corridor to the rightmost room. The robot has actions that directly change the mode by reconfiguring itself. To make the problem more interesting, we could require the robot to reconfigure itself in specific locations (e.g., where there is enough clearance, or possibly at a location where another robot can assist it).

The examples presented so far barely scratch the surface on the possible hybrid motion planning problems that can be defined. Many such problems can arise, for example, in the context making automated video game characters or digital actors. To solve these problems, standard motion planning algorithms can be adapted if they are given information about how to change the modes. Locations in X from which the mode can be changed may be expressed as subgoals. Much of the planning effort should then be focused on attempting to change modes, in addition to trying to directly reach the goal. Applying sampling-based methods requires the definition of a metric on X that accounts for both changes in the mode and the configuration. A wide variety of hybrid problems can be formulated, ranging from those that are impossible to solve in practice to those that are straightforward

extensions of standard motion planning. In general, the hybrid motion planning model is useful for formulating a hierarchical approach, as described in Section

1.4. One particularly interesting class of problems that fit this model, for which successful algorithms have been developed, will be covered next.

• • 1. Manipulation Planning

This section presents an overview of manipulation planning; the concepts explained here are mainly due to [16, 17]. Returning to Example 7.2, imagine that the robot must carry a key that is so large that it changes the connectivity of free. For the manipulation planning problem, the robot is called a manipulator, which interacts with a part. In some configurations it is able to grasp the part and move it to other locations in the environment. The manipulation task usually requires moving the part to a specified location in , without particular regard as to how the manipulator can accomplish the task. The model considered here greatly simplifies the problems of grasping, stability, friction, mechanics, and uncertainties and instead focuses on the geometric aspects (some of these issues will be addressed in Section 12.5). For a thorough introduction to these other important aspects of manipulation planning, see [681]; see also Sections 13.1.3 and 12.5.

C

W

Admissible configurations Assume that , , and from Formulation 4.1 are used. For manipulation planning, is called the manipulator, and let refer to the manipulator configuration space. Let denote a part, which is a rigid body modeled in terms of geometric primitives, as described in Section 3.1. It is assumed

a

P

A C

W O A

that P is allowed to undergo rigid-body transformations and will therefore have its own part configuration space, C = SE(2) or C = SE(3). Let q ∈ C denote a part configuration. The transformed part model is denoted as P(q ).

p

p p p p

The combined configuration space, C, is defined as the Cartesian product

C = C × C , (7.14)

a p

in which each configuration q is of the form q = (q , q ). The first step is to remove all configurations that must be avoided. Parts of Figure 7.15 show examples of these sets. Configurations for which the manipulator collides with obstacles are

a p

∈ C

Cobs = {(q

a

a

, qp

) ∈ C | A(q

) ∩ O /= ∅}. (7.15)

The next logical step is to remove configurations for which the part collides with obstacles. It will make sense to allow the part to “touch” the obstacles. For example, this could model a part sitting on a table. Therefore, let

a

p

Cobs = {(q

p

a

, qp

) ∈ C | int(P(q

)) ∩ O /= ∅} (7.16)

denote the open set for which the interior of the part intersects O. Certainly, if the part penetrates O, then the configuration should be avoided.

q ∈ Cobs

a

q ∈ Cobs

q ∈ Cobs

p

ap

q ∈ Cgr q ∈ Csta q ∈ Ctra

Figure 7.15: Examples of several important subsets of C for manipulation planning.

Consider C (Cobs

a

∪Cobs

). The configurations that remain ensure that the robot

and part do not inappropriately collide with . Next consider the interaction

p

O

between and . The manipulator must be allowed to touch the part, but penetration is once again not allowed. Therefore, let

A P

a

p

Cobs = {(q

ap

a

, qp

) ∈ C | A(q

) ∩ int(P(q

)) /= ∅}. (7.17)

Removing all of these bad configurations yields

a p ap

Cadm = C \ (Cobs ∪ Cobs ∪ Cobs), (7.18)

which is called the set of admissible configurations.

Stable and grasped configurations Two important subsets of Cadm are used

p

in the manipulation planning problem. See Figure 7.15. Let Csta denote the set

of stable part configurations, which are configurations at which the part can safely

rest without any forces being applied by the manipulator. This means that a part cannot, for example, float in the air. It also cannot be in a configuration from which it might fall. The particular stable configurations depend on properties such as the part geometry, friction, mass distribution, and so on. These issues are not considered here. From this, let sta adm be the corresponding stable configurations, defined as

C ⊆ C

p

p

Csta = {(q

a

, qp

) ∈ Cadm | q

∈ Csta}. (7.19)

The other important subset of adm is the set of all configurations in which the

C

robot is grasping the part (and is capable of carrying it, if necessary). Let this denote the grasped configurations, denoted by Cgr ⊆ Cadm. For every configuration, (qa, qp) ∈ Cgr, the manipulator touches the part. This means that A(qa)∩P(qp) /= ∅ (penetration is still not allowed because Cgr ⊆ Cadm). In general, many configura- tions at which A(qa) contacts P(qp) will not necessarily be in Cgr. The conditions

for a point to lie in gr depend on the particular characteristics of the manipulator, the part, and the contact surface between them. For example, a typical manipula- tor would not be able to pick up a block by making contact with only one corner of it. This level of detail is not defined here; see [681] for more information about grasping.

C

We must always ensure that either x ∈ Csta or x ∈ Cgr. Therefore, let Cfree =

sta gr, to reflect the subset of adm that is permissible for manipulation planning. The mode space, M, contains two modes, which are named the transit mode and the transfer mode. In the transit mode, the manipulator is not carrying the

C ∪ C C

part, which requires that q ∈ Csta. In the transfer mode, the manipulator carries the part, which requires that q ∈ Cgr. Based on these simple conditions, the only

way the mode can change is if q sta gr. Therefore, the manipulator has two available actions only when it is in these configurations. In all other configurations the mode remains unchanged. For convenience, let tra = sta gr denote the set of transition configurations, which are the places in which the mode may change.

∈ C ∩ C

C C ∩ C

Using the framework of Section 7.3.1, the mode space, M, and C-space, C, are combined to yield the state space, X = C × M. Since there are only two modes, there are only two copies of C, one for each mode. State-based sets, Xfree, Xtra,

Xsta, and Xgr, are directly obtained from free, tra, sta, and gr by ignoring the mode. For example,

C C C C

Xtra = {(q, m) ∈ X | q ∈ Ctra}. (7.20)

The sets Xfree, Xsta, and Xgr are similarly defined.

The task can now be defined. An initial part configuration, qp ∈ Csta, and

init

a goal part configuration, qp ∈ Csta, are specified. Compute a path τ : [0, 1] →

X such that τ (0) = qp

goal

d τ (1) = qp

. Furthermore, the action trajectory

free

init an

goal

o : [0, 1] → U must be specified to indicate the appropriate mode changes whenever

τ (s) Xtra. A solution can be considered as an alternating sequence of transit paths and transfer paths, whose names follow from the mode. This is depicted in Figure 7.16.

∈

Transfer

Transit

Figure 7.16: The solution to a manipulation planning problem alternates between the two layers of X. The transitions can only occur when x ∈ Xtra.

Manipulation graph The manipulation planning problem generally can be solved by forming a manipulation graph, Gm [16, 17]. Let a connected compo-

nent of Xtra refer to any connected component of tra that is lifted into the state space by ignoring the mode. There are two copies of the connected component of

C

Ctra, one for each mode. For each connected component of Xtra, a vertex exists in

Gm. An edge is defined for each transfer path or transit path that connects two

connected components of Xtra. The general approach to manipulation planning then is as follows:

1. Compute the connected components of Xtra to yield the vertices of Gm.
2. Compute the edges of Gm by applying ordinary motion planning methods to each pair of vertices of Gm.
3. Apply motion planning methods to connect the initial and goal states to every possible vertex of Xtra that can be reached without a mode transition.
4. Search m for a path that connects the initial and goal states. If one exists, then extract the corresponding solution as a sequence of transit and transfer paths (this yields σ, the actions that cause the required mode changes).

G

This can be considered as an example of hierarchical planning, as described in Section 1.4.

Figure 7.17: This example was solved in [244] using the manipulation planning framework and the visibility-based roadmap planner. It is very challenging because the same part must be regrasped in many places.

Multiple parts The manipulation planning framework nicely generalizes to multiple parts, 1, . . ., k. Each part has its own C-space, and is formed by taking the Cartesian product of all part C-spaces with the manipulator C- space. The set adm is defined in a similar way, but now part-part collisions also have to be removed, in addition to part-manipulator, manipulator-obstacle, and part-obstacle collisions. The definition of sta requires that all parts be in stable configurations; the parts may even be allowed to stack on top of each other. The definition of gr requires that one part is grasped and all other parts are stable.

C

C

C

P P C

There are still two modes, depending on whether the manipulator is grasping a part. Once again, transitions occur only when the robot is in Ctra = Csta ∩ Cgr.

Figure 7.18: This manipulation planning example was solved in [915] and involves 90 movable pieces of furniture. Some of them must be dragged out of the way to solve the problem. Paths for two different queries are shown.

The task involves moving each part from one configuration to another. This is achieved once again by defining a manipulation graph and obtaining a sequence of transit paths (in which no parts move) and transfer paths (in which one part is carried and all other parts are fixed). Challenging manipulation problems solved by motion planning algorithms are shown in Figures 7.17 and 7.18.

Other generalizations are possible. A generalization to k robots would lead to 2k modes, in which each mode indicates whether each robot is grasping the part. Multiple robots could even grasp the same object. Another generalization could allow a single robot to grasp more than one object.