Task-contrained planning

Since a motion model is now available, we can formally introduce the task. Its definition is very similar to the one given in Section 4.1.2 with some minor modifications. Here, we want to generalize the concept of a task. Suppose that a task is described as the trajectory for the position (and possibly orientation) of a specific point (body) of the humanoid, as in Section 4.1.2. For instance, a manipulation task may be specified as a trajectory assigned to one hand, while a navigation task may be assigned in terms of motion of the midpoint between the feet. This simple viewpoint makes it very easy to translate a task from natural language (‘pick up that object and bring it to me’) to an assignment that can be directly used by our planner. We emphasize that the task may be a geometric path rather than a trajectory, and in particular it may reduce to a goal position in task space. The proposed planner works without any modification in these cases, as shown in Section 5.4.

Then, the task is generalized in the sense that it might be expressed as a trajectory (as for the motion planner described in the previous chapter) but also as a geometric path or even a goal position in task space. Moreover, we will show how composite tasks can be handled in our

Chapter 5. TCMP for humanoids based on CoM movement primitives Marco Cognetti

framework, by combining tasks of different nature (e.g., manipulation and navigation tasks. See Section 5.4 for an example of a composite task).

Formally, collect the task coordinates in a vector y taking values in an appropriate space. The following kinematic map links task coordinates with configuration coordinates

y= f (q_CoM, q_jnt).

Suppose that a desired task trajectory y∗(t), t ∈ [ti, tf], is assigned. In case the trajectory is

assigned via a geometric path, one has to replacet with a path parameter s. On the other side, in

case one wants to assign the task as a desired task set-point, its definition degenerates to a single

point y∗(tf). Our framework is able to handle without any modifications all these definitions of

the assigned task.

To summarize, the planning problem considered in this chapter is to find a feasible whole-

body motion of the humanoid over[ti, tf] that realizes the assigned task while avoiding collisions

with workspace obstacles, whose geometry is known in advance. In our approach, a solution is identified by a concatenation of CoM movement primitives that has been ‘fleshed out’ by defining collision-free whole-body motions which realize such movements while complying with the task. In the end, however, the solution can be directly described in terms of joint motions.

Formally, a solution for our TCMP problem is a trajectory q_jnt(t), t ∈ [ti, tf] that satisfies

three requirements

1. the assigned task trajectory is exponentially realized; in formula, lim

t→∞(y(t) − y

∗_{(t)) = 0}

with an exponential rate of convergence;

2. self-collisions and collisions with workspace obstacles are avoided;

3. position and velocity of the joints are within their bounds, respectively in the form q_jnt,m<

q_jnt< q_jnt,Mand vjnt,m< vjnt< vjnt,M;

Note that if y(ti) = y∗(ti) (matched initial configuration, or ‘the robot starts on the task’),

the first requirement automatically becomes

y(t) = y∗_{(t), ∀t ∈ [t}

i, tf],

i.e., the assigned task must be exactly realized at all times.

The first requirement is another small improvement w.r.t. the framework proposed in the previous chapter. In fact, the initial configuration must be on the task in the formulation of Section 4.1.2 while here we do not require it. This is due to the CoM movement primitives and

Figure 5.2 Example of the CoM movement selection procedure. (left) Once qnearhas been

selected, the CoM movement selector has to choose between the various primitive (depicted each with a different color). Each primitive embeds a duration and a trajectory for the CoM and possibly other points (here swing foot). (right) A primitive has been selected, uniquely identifying the portion of the task to be fulfilled (red).

further details), leading also to recover an initial error in a natural way. On the other side, this is not trivial to obtain by using the framework described in Chapter 4, since the time interval (for

each iteration of the planner) is defined by the planner itself (theTkin Section 4.3) and the sum

of the durations might be not compatible with the total duration of the assigned task.

5.2

Motion generation

The proposed planner works in an iterative fashion by repeated calls to a motion generator. That is the reason for which we present first this module and then we describe the planner in Section 5.3. As for the motion generation scheme presented in Section 4.2, we use two interleaved pro- cedures that reflects the way the configuration vector q is composed in eq. (5.1). First, the

CoM movement selectorchooses a particular CoM movement from the set of primitives (that we assumed to be available to the planner). Then, the joint motion generator computes feasible collision-free joint motions that realize the chosen primitive as well as the corresponding por- tion of the assigned task trajectory. The definition of feasible motion is the same as given in Section 4.2, that we repeat here for completeness. A motion is feasible if the configurations that compose the motion are collision-free. Moreover, none of these configurations has to exceed the joint limits nor the joint velocity limits.

In Section 5.2.1 we describe the CoM movement selection mechanism while the joint motion generation is depicted in Section 5.2.2.