A Planning Framework for Non-Prehensile Manipulation under Clutter and Uncertainty

Abstract

Robotic manipulation systems suffer from two main problems in unstructured human environments: uncertainty and clutter. We introduce a planning framework addressing these two issues. The framework plans rearrangement of clutter using non-prehensile actions, such as pushing. Pushing actions are also used to manipulate object pose uncertainty. The framework uses an action library that is derived analytically from the mechanics of pushing and is provably conservative. The framework reduces the problem to one of combinatorial search, and demonstrates planning times on the order of seconds. With the extra functionality, our planner succeeds where traditional grasp planners fail, and works under high uncertainty by utilizing the funneling effect of pushing. We demonstrate our results with experiments in simulation and on HERB, a robotic platform developed at the Personal Robotics Lab at Carnegie Mellon University.

Appendix: Computing Conservative Capture Regions

We compute the capture region for a push-grasp using a pushing simulation. This simulation assumes that an object’s limit surface can be approximated with an ellipsoid in the force-moment space as in Howe and Cutkosky (1996).

As described in Sect. 2.2.2 two parameters of this simulation affect the boundaries of the computed capture region: the pressure distribution between the object and the support surface, ρ; and the coefficient of friction between the robot finger and the object, μ _c . These values are difficult to know and we assume that our robot does not know the exact values for any object.

When we run our simulation we generate capture regions that are conservative with respect to these parameters; i.e. capture regions that work for a range of reasonable values of μ _c and ρ. For μ _c such a reasonable region is given by the values between a very small value (a very slippery contact between the robot finger and the object), and a very high value (a high friction contact). The pressure distribution ρ can take any rotationally symmetric shape, but it is limited by the boundaries of the object making contact with the support surface.

One way to achieve a conservative capture region is by discretizing the values a parameter can take, running the simulation for each of the values, and then intersecting the resulting capture regions to find a capture region that works for all the values. But for certain object shapes we can do better.

For a cylindrical object the conservative capture region can be found simply by running the simulation for specific values of μ _c and ρ. For μ _c if we choose a very high value the computed capture region will also be valid for any lower value. For ρ if we choose a pressure distribution that is completely at the periphery of the cylinder (like a rim), the capture region will be valid for any other rotationally symmetric pressure distribution that is closer to the center of the cylinder. This is equivalent to saying that, as μ _c gets smaller or as ρ gets concentrated around the center, the required pushing distance for the push-grasp will decrease. In this section we prove this claim.

In Fig. 17 we present some of the force and velocity vectors that determine how a cylinder moves under quasi-static pushing. The following notation will be used:

• $\hat{\mathbf{n}}$ is the unit normal at the contact between the finger and the object.

• f _L and f _R are the left and right edges of the friction cone. The friction cone is found by drawing the vectors that make the angle α=arctanμ _c with $\hat{\mathbf{n}}$.

• f is the force applied to the object by the finger. The direction of f is bounded by the friction cone.

• v and ω are the linear and angular velocities of the object at its center. v and ω can be found by computing the force and moment at the center of the object due to f, finding the corresponding point on the limit surface, and taking the normal to the limit surface. Our ellipsoid approximation of the limit surface dictates that v∥f (Howe and Cutkosky 1996).

• m _L and m _R are the left and right edges of the motion cone. The edges of the motion cone are found by:

  • taking one of the edges of the friction cone, say the left edge;

  • computing the force and moment it creates at the object center;

  • using the limit surface to find the corresponding linear and angular velocity of the object, in response to this force and moment;

  • using the linear and angular velocity at the center of the object to find the velocity at the contact point. This gives the left edge of the motion cone; the right one is found by starting with the right edge of the friction cone.

• $\hat{\mathbf{v}}_{\mathbf{d}}$ is the unit vector pointing in the opposite direction of ω×r where r is the vector from the center of the object to the contact; $\hat{\mathbf{v}}_{\mathbf{d}} = \frac{(\boldsymbol {\omega} \times\mathbf{r})}{|\boldsymbol{\omega} \times\mathbf{r}|}$.

• v _p is the velocity of the pusher/finger at the contact point. The voting theorem (Mason 1986) states that v _p and the edges of the friction cone votes on the direction the object will rotate. For a cylinder the friction cone edges always fall on different sides of the center of the object, and v _p alone dictates the rotation direction; v _p .(ω×r)>0. In terms of $\hat{\mathbf{v}}_{\mathbf{d}}$ this means $\mathbf {v}_{\mathbf{p}}.\hat{\mathbf{v}}_{\mathbf{d}} < 0$.

• v _c is the velocity of the object at the contact point; v _c =v+ω×r.

• The grasp-line is the line at the fingertip orthogonal to the pushing direction. The push-grasp continues until the object center passes the grasp-line.

Fig. 17

Some of the force and velocity vectors we will be using in our proof. In the figure the finger is pushing the cylinder towards right

During quasi-static pushing, the contact between the finger and the object can display three different modes: separation, sticking, or sliding. The case of separation is trivial, in which the finger moves away from the object resulting in no motion for the object. In the case of sticking contact the contact point on the finger and the contact point on the object moves together, i.e. v _p =v _c . This happens when f falls inside the friction cone, and correspondingly when v _c falls inside the motion cone. In sliding contact the object slides on the finger as it is being pushed. In this case f aligns with the friction cone edge opposing the direction the object is sliding on the finger. Similarly v _c aligns with the motion cone edge opposing the direction the object is sliding on the finger. v _p  is outside of the motion cone.

What follows is a series of lemmas and their proofs; which we then use to prove our main theorem.

Lemma 1

During sticking and sliding contact $\mathbf{v}.\hat{\mathbf{n}} = \mathbf{v}_{\mathbf{p}}.\hat{\mathbf{n}}$.

Proof

During sticking contact v _p =v _c , which implies

$$\mathbf{v}_{\mathbf{p}}.\hat{\mathbf{n}}=\mathbf{v}_{\mathbf {c}}.\hat{\mathbf{n}} $$

We know that v _c =v+ω×r. Then,

$$\mathbf{v}_{\mathbf{p}}.\hat{\mathbf{n}}=(\mathbf{v} + \boldsymbol{\omega} \times\mathbf{r}).\hat{\mathbf{n}} $$

Since $(\boldsymbol{ \omega} \times\mathbf{r}).\hat{\mathbf{n}} = 0$,

$$\mathbf{v}_{\mathbf{p}}.\hat{\mathbf{n}}=\mathbf{v}.\hat{\mathbf{n}} $$

During sliding contact, the relation between v _c and v _p is given by

$$\mathbf{v}_\mathbf{c} = \mathbf{v}_{\mathbf{p}} + \mathbf {v}_{\mathbf{slide}} $$

where v _slide is the velocity the object slides on the finger. Taking the projection of both sides along the contact normal gives

$$\mathbf{v}_\mathbf{c}.\hat{\mathbf{n}} = (\mathbf{v}_{\mathbf{p}} + \mathbf{v}_{\mathbf{slide}}).\hat{\mathbf{n}} $$

Sliding can only happen along the contact tangent. For a cylindrical object, this means $\mathbf{v}_{\mathbf{slide}}.\hat{\mathbf{n}} = 0$. Then,

$$\mathbf{v}_{\mathbf{p}}.\hat{\mathbf{n}} = \mathbf{v}_{\mathbf {c}}.\hat{ \mathbf{n}} $$

This is also illustrated in Fig. 18. The rest of the proof proceeds the same as the sticking contact case.

Fig. 18

The relation between v, v _p , and v _c during sliding contact

 □

Lemma 2

During sticking contact, as we change ρ such that it is concentrated closer to the center of the cylinder, the magnitude of the angular velocity, |ω|, will get larger.

Proof

As ρ concentrates at the center, the limit surface ellipsoid gets a more flattened shape at the top and bottom. This implies that for the same force and moment applied to the object, the ratio |ω|/|v| will get larger (Howe and Cutkosky 1996).

We can express |v| as,

$$|\mathbf{v}| = \sqrt{(\mathbf{v}.\hat{\mathbf{v}}_\mathbf{d})^2 + (\mathbf{v}.\hat{\mathbf{n}})^2} $$

and using Lemma 1,

$$ |\mathbf{v}| = \sqrt{(\mathbf{v}.\hat{\mathbf{v}}_\mathbf{d})^2 + (\mathbf{v}_{\mathbf{p}}.\hat{\mathbf{n}})^2} $$

(2)

During sticking contact v _p =v _c , hence

$$\mathbf{v}_{\mathbf{p}} = \mathbf{v} + (\boldsymbol{ \omega} \times\mathbf{r}) $$

Since $(\boldsymbol{ \omega} \times\mathbf{r}) = -|\boldsymbol{ \omega}||\mathbf{r}|\hat{\mathbf{v}}_{\mathbf{d}}$ we have

$$\mathbf{v}_{\mathbf{p}} = \mathbf{v} - |\boldsymbol{ \omega }||\mathbf{r}| \hat{\mathbf{v}}_\mathbf{d} $$

Rearranging and projecting both sides onto $\hat{\mathbf{v}}_{\mathbf{d}}$ gives:

$$\mathbf{v}.\hat{\mathbf{v}}_\mathbf{d} = \mathbf{v}_{\mathbf {p}}.\hat{ \mathbf{v}}_\mathbf{d} + |\boldsymbol{ \omega}||\mathbf{r}| $$

Inserting this into Eq. (2),

$$|\mathbf{v}| = \sqrt{\bigl(\mathbf{v}_{\mathbf{p}}.\hat{\mathbf {v}}_\mathbf{d} + |\boldsymbol{\omega}||\mathbf{r}|\bigr)^2 + ( \mathbf{v}_{\mathbf {p}}.\hat{\mathbf{n}})^2} $$

Except |ω|, the terms on the right hand side are independent of ρ. Since $\mathbf{v}_{\mathbf{p}}.\hat{\mathbf{v}}_{\mathbf{d}} < 0$, as |ω| increases |v| decreases, and vice versa. Then the only way |ω|/|v| can increase is when |ω| increases. □

Lemma 3

For a given configuration of the object and the finger, if we change the pressure distribution ρ such that it is concentrated closer to the center of the object, the change in v will have a non-negative projection on $\hat{\mathbf{v}}_{\mathbf{d}}$.

Proof

We will look at different contact modes separately. The separation mode is trivial. The object will not move for both values of ρ. The change in v will have a null projection on $\hat{\mathbf{v}}_{\mathbf{d}}$.

Assume that the contact mode is sliding. Then f will be aligned with one of the friction cone edges; let’s assume f _R without loss of generality. Since v∥f, then v is also a vector with direction f _R

$$\mathbf{v} = |\mathbf{v}|\hat{\mathbf{f}}_\mathbf{R} $$

where $\hat{\mathbf{f}}_{\mathbf{R}}$ is the unit direction along f _R . Inserting this into the result from Lemma 1 we have

$$|\mathbf{v}|\hat{\mathbf{f}}_\mathbf{R}.\hat{\mathbf{n}} = \mathbf{v}_{\mathbf{p}}.\hat{\mathbf{n}} $$

Then

$$|\mathbf{v}| = \frac{\mathbf{v}_{\mathbf{p}}.\hat{\mathbf {n}}}{\hat{\mathbf{f}}_\mathbf{R}. \hat{\mathbf{n}}} $$

Multiplying both sides with ${\hat{f}_{R}}$ we have

$$\mathbf{v} = \frac{\mathbf{v}_{\mathbf{p}}.\hat{\mathbf{n}}}{\hat {\mathbf{f}}_\mathbf{R}.\hat{\mathbf{n}}} \hat{\mathbf{f}}_\mathbf{R} $$

None of the terms get affected by a change in ρ, i.e. the change in v will have a null projection on $\hat{\mathbf{v}}_{\mathbf{d}}$.

As ρ concentrates at the center, |ω|/|v| will get larger. The motion cone edges will then get more and more aligned with the direction of ω×r, making the motion cone wider. At the point when the motion cone edge reaches v _p the contact is no more a sliding contact but a sticking one.

When the contact is sticking we have

$$\mathbf{v}_{\mathbf{p}}=\mathbf {v}_{\mathbf{c}}= \mathbf{v}+\boldsymbol{\omega} \times\mathbf{r}. $$

Then

$$\mathbf{v} = \mathbf{v}_{\mathbf{p}} - (\boldsymbol{ \omega} \times\mathbf{r}) $$

If we rewrite (ω×r) using the direction $\hat{\mathbf{v}}_{\mathbf{d}}$, we get

$$\mathbf{v} = \mathbf{v}_{\mathbf{p}} + |\boldsymbol{\omega }||\mathbf{r}|\hat{ \mathbf{v}}_\mathbf{d} $$

Except |ω|, the terms on the right hand side are independent of ρ. By Lemma 2, we know that as ρ concentrates around the center of the object, |ω| increases; i.e. the change in v has a positive projection on $\hat{\mathbf{v}}_{\mathbf{d}}$. □

Now we look at the effect of μ _c , the friction coefficient between the object and the finger.

Lemma 4

For a given configuration of the object and the finger, if we decrease the value of μ _c , the change in v will have a non-negative projection on $\hat{\mathbf{v}}_{\mathbf{d}}$.

Proof

Again we look at the two contact modes separately.

The sticking contact case is trivial. f is inside the friction cone. If we decrease μ _c , the friction cone will get narrower, but as long as it does not get narrow enough to leave f outside, the contact is still sticking. There is no effect to the velocities of the motion, including v. The change in v has a null projection on $\hat{\mathbf{v}}_{\mathbf{d}}$.

If we continue to decrease μ _c at one point the contact will become sliding. f will be at the edge of the friction cone and the friction cone will get narrower as we decrease μ _c . Without loss of generality, let’s assume f is along f _R . Since v∥f, v will also move with f _R . Pictorially v will change as in Fig. 19, resulting in a change along $\hat{\mathbf{v}}_{\mathbf{d}}$. Formally, in the proof of Lemma 3 we showed that during sliding contact

$$\mathbf{v} = \frac{\mathbf{v}_{\mathbf{p}}.\hat{\mathbf{n}}}{\hat {\mathbf{f}}_\mathbf{R}.\hat{\mathbf{n}}} \hat{\mathbf {f}}_\mathbf{R} $$

By the definition of the friction cone we have

$$\hat{\mathbf{f}}_\mathbf{R} = \cos{(\arctan{\mu_c})}\hat{ \mathbf {n}} - \sin{(\arctan{\mu_c})}\hat{\mathbf{v}}_\mathbf{d} $$

Replacing this into the equation above and noting that $\hat{\mathbf {v}}_{\mathbf{d}}.\hat{\mathbf{n}}=0$ we have

$$\mathbf{v} = \frac{\mathbf{v}_{\mathbf{p}}.\hat{\mathbf{n}}}{\cos {(\arctan{\mu_c})}} \bigl(\cos{(\arctan{\mu_c})}\hat{ \mathbf{n}} - \sin{(\arctan{\mu_c})}\hat{\mathbf{v}}_\mathbf{d} \bigr) $$

Then we have

$$\mathbf{v} = (\mathbf{v}_{\mathbf{p}}.\hat{\mathbf{n}})\hat {\mathbf{n}} - ( \mathbf{v}_{\mathbf{p}}.\hat{\mathbf{n}}) \mu_c \hat{ \mathbf{v}}_\mathbf{d} $$

Except μ _c itself, the terms on the right hand side are independent of μ _c . The contact mode requires that $\mathbf{v}_{\mathbf {p}}.\hat{\mathbf{n}} > 0$. Hence, as μ _c decreases the change in v will be positive in the direction of $\hat {\mathbf{v}}_{\mathbf{d}}$.

Fig. 19

v changes along with the edge of the friction cone as μ _c is decreased

 □

Now we are ready to state and prove our main theorem.

Theorem 1

For a cylindrical object under quasi-static pushing, where the quasi-static motion is approximated by the ellipsoid limit surface (Howe and Cutkosky 1996), as μ _c gets smaller or as ρ gets concentrated around the center, the required pushing distance for a push-grasp will decrease or stay the same (but not increase).

Proof

The push-grasp starts at a certain configuration between the finger and the object, and continues until the object’s center passes the grasp-line at the fingertip and orthogonal to the pushing direction (Fig. 20). Since we assume that μ _c is uniform all around the object, we can ignore the rotation of the cylinder and simply consider its position relative to the finger. Then, independent of ρ or μ _c , the finger and the object will go through all the configurations between r _start to r _final during the push-grasp. We will show below that the velocity the object center moves towards the grasp-line never decreases as μ _c gets smaller or as ρ gets concentrated around the center.

Fig. 20

Independent of μ _c and ρ, the finger and the object goes through the same set of relative configurations during the push-grasp

For a given configuration of the object and the finger, the object center’s velocity is given by v (ω does not have an effect). We can express v using its components

$$\mathbf{v} = (\mathbf{v}.\hat{\mathbf{n}})\hat{\mathbf{n}} + (\mathbf{v}.\hat{ \mathbf{v}}_\mathbf{d})\hat{\mathbf{v}}_\mathbf{d} $$

Lemma 1 tells us that the component of v along $\hat{\mathbf{n}}$ is fixed for different ρ or μ _c :

$$\mathbf{v} = (\mathbf{v}_{\mathbf{p}}.\hat{\mathbf{n}})\hat {\mathbf{n}} + ( \mathbf{v}.\hat{\mathbf{v}}_\mathbf{d})\hat{\mathbf{v}}_\mathbf{d} $$

Hence, the only change in the object center’s motion happens along $\hat{\mathbf{v}}_{\mathbf{d}}$. Lemmas 3 and 4 states that the change in v will be non-negative along $\hat{\mathbf{v}}_{\mathbf{d}}$. □