Control Synthesis for Dynamic Contact Manipulation

We explore the control synthesis problem for a robot dynamically manipulating an object in the presence of multiple frictional contacts. Contacts occur both between the object and the robot, and between the object and the environment. Two sets of constraints govern the evolution of the system — contact velocity constraints that prevent separation and cause rolling, and, contact force constraints that arise from Coulomb friction. We combine the constraints in the space of contact accelerations, obtaining bounds on the robot acceleration as a function of the system state. We solve the motion planning problem by providing a feasible path for the system and generating the controls and the system trajectory by time-scaling the feasible path. We provide examples that illustrate the merits and limitations of our technique and discuss some of the open problems.


I. INTRODUCTION
When a robot manipulates an object, the contacts act as the coupling between the object and the robot; they transmit forces from the robot to the object, and impose motion constraints on the object.The evolution of the system depends on the laws governing motion and force at the contacts, and the dynamics of the object and the robot.In this paper, we focus on synthesizing controls for the robot that move the robot and the object from a start location to a goal while satisfying the constraints imposed at the contacts.

A. Dynamics
In this paper, when we refer to the term dynamic manipulation, we mean "controlling the motion of the manipuland during its dynamic phase" as opposed to "having plans where the manipuland moves dynamically".In general, planners of the latter variety treat statically stable configurations of the manipuland as the nodes of a planning graph and use discrete actions that involve dynamics to transition between nodes.The key point here is that once a discrete action is initiated, it is terminated only at the statically stable goal state, there is no feedback during the action.There have been many elegant demonstrations of such planners [1], [2], [3].However, there are many situations where controlling the motion of the manipuland during its dynamic phase might be useful or essential: • Error recovery -Imagine sliding a book about an edge along a rough horizontal surface with a palm tilted at some angle.If the variation in the friction coefficient is large, the book can slip out of the palm.
However, if it is possible to act upon the book as it is falling down, by decreasing the palm angle, the fall can be stopped.• Richer actions -By applying time-varying forces on the manipuland during its dynamic phase, it is possible to perform tasks that are otherwise impossible.A simple example is the tipping of a block resting on a palm.With the right conditions, it is possible to tip the block by accelerating the palm upwards and to one side.We provide another motivating example in §I-C.• Necessity -All of the planners mentioned [1], [2], [3] assume the existence of statically stable configurations.These configurations are either natural to the task at hand, or artificially constructed to simplify the problem.However, there might be cases where a statically stable configuration might not exist, or the object might take a long time to settle into one.

B. Environmental contacts
Contacts with the environment occur all the time.They occur in assembly in peg-in-hole insertion when the peg contacts the hole, and in part fixturing where the goal is to immobilize the part against the remote fixtures.Such contacts occur when we manipulate objects too large and heavy to lift; when we push on them or topple them.Here, the environment provides a helping hand, a remote contact that bears most of the heavy load that we would not be able to bear by ourselves.Environmental contacts also occur in mobile manipulation, where a robot uses its wheels to move the manipuland.Here, the environment provides an opposing force, remote contacts that together with the wheel contact, help maintain control of the manipuland and prevent it from scooting away from the robot.Hence these contacts are either inevitable, as in the case of assembly, or essential, as in the case of large object manipulation or mobile manipulation.

C. A motivating example
Here we consider an example that highlights two characteristics of the problems we are interested in solvingcontact with the environment, and the use of dynamics to find a solution where a quasi-static analysis fails.
Consider the problem shown in Fig. 1(i) where an arm with one prismatic and one revolute joint (henceforth referred as a PR arm) manipulates a block resting on a step.The arm contacts the block at C and the block contacts the environment at the apex(A) and base(B) of the step.As shown in the figure, the orientation of the block is denoted by θ.We control the motion of the robot via its joint (i) (ii) (iii) (iv) acceleration.The goal is to synthesize controls to stand the block up against the step (θ = π 2 ) while maintaining sliding contacts at A and B and a fixed contact at C.
It can be shown that if the object starts from rest in the configuration shown in Fig. 1(i), any force exerted by the robot at C causes one of three object motions.Depending on the direction of the force, the object either loses contact with B and starts tipping about A, remains immobile, or starts sliding backward.Hence, quasi-statically, there is no control that increases θ.
However, we can use the dynamics of the object to accomplish the task.Intuitively, this involves sliding the block backward a distance that is sufficient to accelerate the block forward in such a way that the centripetal force generated by the motion prevents the block from tipping.Snapshots of the resulting dynamic trajectory are shown in Fig. 1(iv).We explore this example in detail in §VI.

D. Outline
Motion at the contacts imposes two sets of constraints on the system.Sliding or rolling at the contacts imposes velocity constraints on the system.For example, in Fig. 1(i), for the object to remain in contact at A and B, the velocity of the object normal to the contact must be zero.Likewise, for the contact C to remain fixed, the velocity of the arm at C must equal the velocity of the object at C. The laws of Coulomb friction impose constraints on the contact forces.For example, the contact force at A and B must lie on the left edges of the respective friction cones.
In §IV, we combine the two sets of constraints to obtain a constraint on the control.We show that a function of the feasible robot joint acceleration must lie within a convex cone in the contact acceleration space, as illustrated in Fig. 1(ii).In §V, we use the technique of time-scaling to simplify the problem.We project the constraints on to a lower dimensional phase space and solve for a feasible trajectory in this space.The arrows in Fig. 1(iii) denote the constraints on the tangents to the trajectories in phase space and the solid curve denotes a feasible trajectory.We use the phase space trajectory to synthesize the controls and the dynamic trajectory of the system (shown in Fig. 1(iv)).We provide a detailed example in §VI and discuss the limitations of our technique in §VII.

II. BACKGROUND
Early work on dynamic manipulation focused on dexterous manipulation where a robot hand manipulated an object with multiple frictional fingers.In Cole et al. [4], [5], some fingers were designated to slide on the object's surface while others were designated to roll.The authors provided a control law that achieved simultaneous tracking of a pre-planned object trajectory together with the desired motion at the fingertips using the location of the contacts and the relative velocity at the contacts as feedback.
There are two key assumptions in Cole et al. .The first assumption is that the object is held in a force closure grasp throughout its motion.In such a grasp, contact forces can be combined to exert an arbitrary wrench on the object.The second assumption is that each finger is fully actuated.As a result of these two assumptions the hand can exert an arbitrary wrench on the object, all object trajectories are feasible and there are no constraints on the robot joint accelerations.
In this paper we do not make either assumption.The object need not be in force closure.We also have contacts between the object and the environment which are unactuated, where the robot cannot actively control the contact force exerted.
Brook et al. [6] studied the manipulation of objects in equilibrium grasps.They showed that most equilibrium grasps were locally controllable, and stabilizable under suitable feedback control.They showed that manipulation from one equilibrium grasp to another was possible if there was a continuity of equilibrium grasps between them.
Trinkle and Hunter [7] provided a framework for manipulation planning.They defined a contact formation (CF) as a qualitative description of a grasp based on contacts between the vertices, edges and surfaces of the robot and the object.To plan a motion, they constructed CF-trees with the start and goal CFs as the parent nodes, the child nodes being the CFs the parents could transition to, and the arcs being controls that caused the transitions.A plan existed if the the start and goal CF-trees had a common node.
Erdmann [3] explored the nonprehensile manipulation of planar convex objects using two flat palms.He restricted the actions that could be performed by the palms to four primitives which caused the object to tilt, rotate, slide, or be released.Based on a static analysis, he decomposed the configuration space of the object into regions of invariant dynamics, and searched for plans in this simplified space.
Song et al. [8] studied the design of fixtures where the part was subjected to frictional contacts and impact.For a given set of design parameters and initial conditions, they computed the dynamic motion of the part using a linear complementarity formulation.Ideal design parameters for a given cost function were obtained by solving a constrained optimization problem.
Sarkar et al. [9] studied the control of a robot in a cooperative manipulation task ordered to compliantly follow the motion of an object.They designed a nonlinear feedback controller that maintained rolling contact.
This paper is motivated by the work of Lynch and Mason [10].They studied the planning and control of dynamic nonprehensile manipulation.They computed controls for a one degree of freedom arm performing dynamic tasks such as snatching an object from a table, rolling the object on the arm, and throwing and catching.They used a nonlinear optimization technique to search for feasible controls in a low dimensional state space.
The idea of decoupling path generation and trajectory planning was first explored in the context of robot arms.Bobrow et al. [11] and Shin and McKay [12] studied the problem of moving a manipulator in minimum time along a specified geometric path subject to constraints on the actuator torque.Both papers analyzed the problem in the phase space of the parametrized geometric path and provided algorithms to compute time optimal trajectories.Subsequent authors have worked both on improving the efficiency of the algorithms [13] and on sub-optimal efficient solutions [14].

III. PROBLEM STATEMENT
We denote the configuration of the object by its pose q o and the configuration of the robot by its joint angles q r .We denote the configuration of the system by q = [q o , q r ] T , the velocity of the system by q = [ qo , qr ] T and the state of the system by the pair [q, q] T .We define the dimension of the configuration space as n q = dim(q).The dimension of the state space is n s = 2n q .
Motion at the contacts between the object and the robot gives rise to constraints on the relative velocity between the object and the robot.These can be written as: Likewise, motion at the contacts between the object and the environment gives rise to constraints: Combining the above two constraints: Constraints on the contact force f ci at the ith contact are: where F i is the contact friction cone, i = [1, . . ., n] and n is the number of contacts.
We can combine the force constraints at all contacts as: where We control the motion of the robot via its joint acceleration qr .The motion of the object is: where M is the mass matrix of the object, G is the grasp map which relates contact forces to wrenches on the object, and n o is the wrench on the object due to gravity.The problem can be stated as: Given a start q s and a goal q g , find the robot joint acceleration qr (t) that will move the system from the start to the goal without violating the contact velocity constraints (Eqn.3) or the contact force constraints (Eqn.4).

IV. COMBINING CONSTRAINTS
In this section, we combine the velocity and the force constraints of the system.This gives us a range of robot joint acceleration that does not violate either constraint.

A. The contact acceleration constraints
Theorem 1.For a given configuration q = [q o , q r ] T and velocity q = [ qo , qr ] T , the allowable joint acceleration of the robot qr that satisfies both the velocity and the force constraints is constrained to lie within the cone: Proof: The constraints on the system are: Since the velocity constraint is valid over an interval of time, its derivative wrt.time is also valid over the interval of time.We can, thus, differentiate Eqn.7 : We can rewrite the dynamics of the object as: Multiplying the equation by G T c and substituting for G T c qo from Eqn.9, we get: Incorporating the force constraint from Eqn.8, we can write the above equation as: where V is a collection of terms comprising of the system velocity and the weight of the object.Eqn.6 represents a balance of accelerations in contact space.The LHS of Eqn.6 represents contact accelerations that can be produced by the robot.As per the laws of Coulomb friction, the acceleration must lie within the cone of contact accelerations that can be produced by forces belonging to the contact friction cone.The RHS of Eqn.6 maps the contact friction cone into the space of contact accelerations:

Contact space friction cone
Object space friction cone Object space acceleration cone Contact space acceleration cone (12)

B. Internal forces
n n q 1 q 2 q 3 q 4 no Fig. 2

. An example of the insufficiency of the contact acceleration constraints
Eqn.6 is a necessary but not a sufficient condition for the constraints to be satisfied if the system has internal forces -forces that produce no acceleration of the system.
Internal forces can result if the object is in force closure.For example, in the system shown in Fig. 2, contact forces pointing inwards do not accelerate the object.
When we apply the contact acceleration constraint for the system initially at rest and moving to the right, we get: Eqn.13 prohibits the contacts from separating or penetrating into the object.However Eqn.13 does not tell us how much we should squeeze the object in order to balance its weight.This is because the squeezing force does not change the acceleration of the robot and hence does not appear in the contact acceleration constraint.

V. CONTROL SYNTHESIS
In this section, we synthesize controls that move the system from a start q s to a goal configuration q g while satisfying the contact acceleration constraints.

A. Time-scaling
Our motivation is to search for controls in a space that has a lower dimension than n s .We simplify the problem into two subproblems.The first problem is finding a feasible path q(s), where s is a parametrization of the path, from start to goal.This involves searching the configuration space of the system which has a dimension of ns 2 .Once a feasible path is found, the second problem is finding a feasible trajectory along the path.This involves searching the two dimensional phase space [s, ṡ] for trajectories that do not violate the contact acceleration constraints.The choice of ṡ and s for a path is called the time-scaling of the path.

Definition 1. A feasible path is one that does not violate the contact kinematic constraints for any time-scaling.
Theorem 2. Every feasible path q(s) = [q o (s), q r (s)] T satisfies the constraint: We omit the proof of Thm.2 due to lack of space.Once a feasible path is generated, we can can apply the contact acceleration constraint to obtain constraints on the phase space trajectories.Using Eqn.14 in Eqn.6 we obtain phase space constraints of the form:

B. Trajectory generation
In planar contact problems we leverage the linearity of the contact friction cone to obtain a computationally efficient representation of Eqn.15.Since the mapping from contact force space to contact acceleration space is linear, a linear F maps to a linear A. The face normal representation of A is given by: where " " and " " denote componentwise inequality.
Using the face normal representation in Eqn.15, we get: At each point [s, ṡ] in phase space, Eqn.16 gives us a set of constraints on s.We can combine the constraints to obtain bounds on the acceleration s ∈ [s min , smax ].
At each point in phase space, acceleration bounds can be visualized as cones in the tangent space.The tangent to a feasible phase space path must lie within the cones.When the constraints are inconsistent, the cone disappears and no feasible path can pass through such a point.
For our problem, we pick a phase space acceleration s1 and compute the forward trajectory from the start.We pick another phase space trajectory s2 and compute the backward trajectory from the goal.At the point where the two trajectories intersect, we switch from s1 to s2 .
Once a feasible phase space path is computed, we use Eqn.6 to compute the desired controls.

VI. AN EXAMPLE
In this section, we describe in detail the example from §I.
In the example, a PR arm manipulates a block, as shown in Fig. 1(i).The configuration of the block is q o = (x, y, θ) T , where (x, y) is the location of the center of mass of the block and θ is its orientation.We describe the configuration of the robot by its joint angles q r = (q 1 , q 2 ) T .
Our goal is to manipulate the block from a start orientation θ s to θ g = π 2 , while maintaining sliding contact at A and B, and a fixed contact at C.

A. The contact acceleration constraints
The arm Jacobian is given by: where l r is th length of the second link.
Vectors from the center of mass to each contact point are: where w is the half-width of the block, l is the half-length of the block, h is the height of C measured along the block, and d is the height of the step.Rolling at C enforces the constraint: Maintaining contact at A and B enforces the constraint: n A and n B are the contact normals given by: , n B = 0 1 Combining both the constraints, we get: As the block is sliding to the right at contacts A and B, the contact force is constrained to lie on the left edge of the friction cone at the contacts.The contact at C is fixed, so the contact force can range anywhere inside the friction cone at C. The contact force constraints are: α is the angle of repose at each contact.
F A and F B are the left edges of the friction cones at A and B respectively and F C is the friction cone at C.
Substituting the above data into Eqn.6,we compute the contact acceleration constraints of the system.

B. Time-scaling
With the imposed contact velocity constraints the system has one degree of freedom.We compute a feasible path q parametrized by the orientation of the block θ: Using Eqn.16, we compute the constraints on the trajectories in the phase space [θ, θ] of the form: Furthermore, we impose a limit on the maximum possible acceleration of the block θ ∈ [−Θ, Θ].

C. Results
The phase plots and phase space trajectories for two instantiations of the problem are shown in Fig. 3 and Fig. 4. The arrows represent tangents.Orientations of the block that correspond to values in the horizontal axis are shown.
In Fig. 3 contact C is at h = 0.3l and the starting orientation of the block θ s = 0.7.In Fig. 4 contact C is at h = l and the starting orientation of the block is θ s = π 3 = 1.05.For both problems Θ = 20.In Fig. 3, the arm is able to push the block forward and make it stand.The arm accelerates the block until the switching point is reached after which the arm decelerates the block to rest.A different strategy is needed in Fig. 4.Here the arm moves the block backward for a duration of time and then pushes the block forward and stands it up.The different strategies are a result of the phase space acceleration constraints at the start of motion.For Fig. 3, the phase space acceleration θ is constrained to lie in the range [−20, 20].As this range includes a positive acceleration, the arm can push the block forward from rest.For Fig. 4, the phase space acceleration θ is constrained to lie in the range [−20, −10.83].Hence the only motion at the start that does not violate any of the contact constraints is one that moves the block backward.The block moves backward until the constraints allow a positive acceleration.The arm then decelerates the backward motion to rest, accelerates the block forward and stands it up.
There is a critical start angle of the block θ crit at which the switch from the strategy in Fig. 3 to Fig. 4 occurs.The critical event that causes the switch is when the left edge of the contact friction cone at C passes through the contact A. Start angles up to θ crit will follow the strategy in Fig. 3 and start angles in [θ crit , π 2 ] will follow the strategy in Fig. 4. The solid vertical lines in Fig. 3 and Fig. 4 show the critical angles for the two problems.As we lower the contact C along the block, θ crit decreases.This matches our intuition, as the higher C is along the block, the greater is the risk of breaking contact at B and tipping the block if the arm pushes forward.

VII. DISCUSSION
By separating path generation and trajectory generation, we have simplified a n s dimensional problem into a 2 dimensional problem.However, finding paths that can be time-scaled is not easy.The problem of testing if a path can be successfully time-scaled without having to compute the phase space constraints is still open.Shin and McKay [15] showed that time-optimal paths for robot arm dynamics were geodesics in joint configuration space.We will work on using some of the ideas in that paper to characterize paths that can be successfully time-scaled.
The contact friction cone in 3D is nonlinear.Our result for combining the constraints is general; it is valid both for planar and 3D contact problems.However, our implementation uses the linearity of the planar contact acceleration cone to efficiently compute acceleration bounds.For 3D problems, Eqn.15 gives us a set of nonlinear constraints on the allowable s at each [s, ṡ].A simple way to solve for the bounds is to perform a 1D search in s.
A drawback of the Coulomb friction model is the possibility of the existence of multiple motions for a given control.Hence, to obtain a particular motion with certainty we must ensure that the control is inconsistent with any other motion.We will work on choosing phase space trajectories that satisfy this requirement.