Manipulation with Shared Grasping
MManipulation with Shared Grasping
Yifan Hou
Carnegie Mellon [email protected]
Zhenzhong Jia
Southern University of Science and [email protected]
Matthew T. Mason
Carnegie Mellon [email protected]
Abstract —A shared grasp is a grasp formed by contactsbetween the manipulated object and both the robot hand andthe environment. By trading off hand contacts for environmentalcontacts, a shared grasp requires fewer contacts with the hand,and enables manipulation even when a full grasp is not possible.Previous research has used shared grasps for non-prehensilemanipulation such as pivoting and tumbling. This paper treats theproblem more generally, with methods to select the best sharedgrasp and robot actions for a desired object motion. The centralissue is to evaluate the feasible contact modes: for each contact,whether that contact will remain active, and whether slip willoccur. Robustness is important. When a contact mode fails, e.g.,when a contact is lost, or when unintentional slip occurs, theoperation will fail, and in some cases damage may occur. Inthis work, we enumerate all feasible contact modes, calculatecorresponding controls, and select the most robust candidate.We can also optimize the contact geometry for robustness. Thispaper employs quasi-static analysis of planar rigid bodies withCoulomb friction to derive the algorithms and controls. Finally,we demonstrate the robustness of shared grasping and the useof our methods in representative experiments and examples. Thevideo can be found at https://youtu.be/tyNhJvRYZNk I. I
NTRODUCTION
Grasping is widely adopted in automation industry be-cause of its robustness, especially when compared with non-prehensile manipulative actions. A force-closure grasp canneutralize disturbance forces in any direction up to a mag-nitude determined by the gripping force. When graspinglocations are not available on the object, the robot can onlymove the object with the help of external force resources [7],e.g. pivot or tumble an object on a table [1, 38, 30]. Un-like grasping, these are usually non-prehensile manipulationswhich are less robust against disturbance forces.Salisbury [29] introduced the idea of “whole arm manipula-tion”, suggesting using all available surfaces on the robot formore ways to manipulate an object. Following this thought,if we treat the environment as another finger of the robot,manipulation under external contacts is like grasping from theobject’s point of view. We call it shared grasping . With sharedgrasping, the object can stay in a force closure even whenno force closure grasps are available. Under suitable contact modes, the hand can move the object while maintaining theforce closure, which opens possibilities for robust manipula-tion beyond grasping alone. This view motivates us to designa more general framework for manipulation under externalcontacts that can: • Analyze the feasibility and robustness of a shared grasp-ing system. • Find robust robot actions to achieve a desired objectmotion and/or contact mode.Although grasping and shared grasping are conceptuallysimilar, traditional analyses for grasping stability do not workfor shared grasping. In grasping, all contact points are assumedto be sticking [26], while in shared grasping all contact modesare considered to allow for more motion possibilities. Withmultiple possible contact modes, traditional force-based grasp-ing stability analysis [32] cannot distinguish some contactmodes from each other.To overcome the mode ambiguity, and also to control theexact motion of the shared-grasped object, our approach mod-els the robot action by
Hybrid Force-Velocity Control (HFVC) .Force control and velocity control are special cases of HFVC.We use the velocity portion of HFVC to filter out undesiredcontact modes and maintain the desired object velocity. Notethat the traditional force-based grasping analysis cannot modelHFVC properly, because the forces in the velocity-controlleddirections are not controllable.In this work, we provide a method to analyze the quasi-static stability of planar shared grasping systems in any contactmode under HFVC. The foundation of our stability analysis isa distinguishable cone representation for the object wrenchesfrom different contact modes. In the object wrench space,we illustrate how the change of robot action and modelparameters can cause mode transitions, so as to quantifythe stability margin , i.e. how much disturbance a contactmode can handle. To get around the force indeterminacy invelocity-controlled directions, we project the wrench spaceonto the force-controlled subspace and pick a force controlthere. This is why we call our approach wrench stamping . a r X i v : . [ c s . R O ] J un uring the computation of stability margin, wrench stampingalso computes a robust HFVC action to achieve the desiredobject motion.We demonstrate how to use wrench stamping in two prob-lems: 1) How to select a contact mode for a multi-contactmanipulation task; 2) Given a contact mode, how to choosefinger placement locations on the object. In the first problem,we pick the contact mode with highest stability margin. In thesecond problem, we compute finger locations by maximizingthe stability margin for the given mode. This is possiblebecause we can compute the analytical gradient of the stabilitymargin about the contact geometry parameters. Note that wecan also optimize over the environmental contact locations andfriction coefficients, which could be useful for system designin industrial applications.The paper is organized as follows. After introducing relatedwork in Section II, we first analyze the feasibility of contactmodes in a shared grasping system in Section III. Based on theanalysis, we derive the stability margin for any contact modein Section IV. Section V uses our wrench stamping to solvemode selection and contact point optimization. We provideexperimental results in several representative shared graspingproblems in Section VI, and conclude with a discussion onlimitations and future work in Section VII.II. R ELATED W ORK
A. Manipulation Modeling with External Contacts
There is lot of prior work on manipulation with externalcontacts. By pressing a grasped object against a support, therobot can do in-hand manipulation [33, 12, 15, 4] or transportthe object without bearing its full weight [1, 38]. Without agrasp, a finger can still move an object by planar pushing[19, 41, 11], pivoting or tumbling [30, 14]. Dafle et al. [7]demonstrated that a simple robot hand can do many tasks bysequencing multiple open loop actions with external contacts.Several of these examples were prehensile, such as movinga block on a surface by rotating the block about one ofits corners [1, 38] or edges on the surface [30]. The objectis in a force closure formed by two point-finger contactsand a point or edge table contact. These works computedthe conditions of maintaining static balance throughout themotion, including minimal friction coefficient (all contactswere sticking) and range of finger locations. Planar pushingcan also be prehensile, examples are rotating a paper card ona table by pressing on it with two fingers [17], and in-handmanipulation with a parallel gripper by external contacts [4, 6].
B. Robust Control of Contact-Rich Manipulation
Manipulation control under external contacts is difficultbecause a control designed for one contact mode can resultin another mode. Previous work on manipulation controlwere mainly concerned with switching to the right controllerfor the right mode. It is popular to do mode scheduling(either planned or hand-coded) before computing a feedbackcontroller [11, 10]. Using hybrid force-velocity control (HFVC, also called hy-brid ’force-position’ or ’position-force’ control) brings robust-ness to manipulation, because the force control and velocitycontrol can handle model uncertainties and force disturbances,respectively. There is a long thread of work on HFVC formanipulation in a rigid environment [20, 39, 28, 37], andstability analysis for the whole robot-contact system underHFVC [22]. Uchiyama and Dauchez [35] computes HFVCfor manipulating an object with two fingertips.Hou and Mason [14] considered the case when the objectmay have multiple contacts with both the robot hand and theenvironment. They introduced the hybrid servoing algorithm tocompute a HFVC action, and provided criteria to evaluate therobustness of a multi-contact system against several kinds offailures [13]. This work extends previous work by consideringall possible contact modes, as well as proposing a simplerrobustness score with clear physical meanings.
C. Motion Planning with Contacts
The model of rigid body contacts is discontinuous. Gradi-ents do not exist at the making or breaking of contacts, therebymaking powerful continuous solvers unusable. A popular wayaround is to model the contacts by complementary constraintson continuous variables [27]. However, these constraints makethe optimization problem close to ill-condition and sensitiveto the choice of initial trajectory. Another direction is toapproximate the contact dynamics with continuous models[23, 24, 34]. So far there have been limited success [25] intransferring motions planned with simplified dynamics intoexperiments. Sampling based planning methods handle dis-crete states naturally. They find solutions by solving steeringproblems under contact constraints [36, 5], or by projectingthe sampled motion into the constrained manifold [2].A key to successful experiments is the ability to handleuncertainties. Object pose uncertainties can be reduced bycontacts [18, 42, 31]. However, there is little work on makingthe contact modes robust against modeling uncertainties andforce disturbances. This paper lays the foundation for suchplanning by providing fast solutions for robustness-basedmode selection and contact point selection.In recent years, learning based methods have also enabledrobots to do manipulation tasks with external contacts, suchas opening doors [9] and pushing in clutter for grasping[8, 40]. However, these learned tasks are intrinsically stable:temporarily falling into a bad contact mode will not fail thetask. We have not seen work on learning shared grasping taskssuch as pivoting or tumbling. The reason could be the difficultyin simulating contacts and the inefficiency/danger of actionexploration under holonomic constraints.III. F
EASIBILITY OF A C ONTACT M ODE
To begin with, in this section we derive the mechanicalcondition under which a contact mode is feasible and unique.This section lies the base for our robustness analysis in thenext section. First, we introduce the symbols.onsider a robot hand, a rigid object and a rigid environ-ment. Denote
H, O, W as the hand frame, object frame andworld inertia frame, respectively. We use the symbol A c B todescribe the quantity c of frame B measured in frame A , where c could be position p , normal n , force f , etc. Denote N D asthe degree-of-freedom of a rigid body, N d as the dimensionof space ( N d = 2 for planar systems). We make the followingassumptions: • The system is quasi-static: inertia forces are ignored. • Friction obeys Coulomb’s Law; the maximum stictionequals the sliding friction. • The hand joints are locked. • Gravity is not significant comparing to contact forces.A description of a shared grasping system includes: • Robot hand pose and object pose. • A list of contact points and normals on the object. • Coulomb friction coefficient µ for each contact. A. Newton’s Law for Wrench Space Analysis
Define the generalized velocity vector V and force vector F of the system in the robot hand frame H : V = (cid:34) H v OH v H (cid:35) , F = (cid:34) H f OH f H (cid:35) . (1) H f H is the robot control force, H f O is always zero since theobject is free. We choose the hand frame because it is easierto describe HFVC later. The Newton’s Second Law for thehand-object system takes the form: (cid:20) J Te − J Th J Th (cid:21) (cid:34) τ e τ h (cid:35) + (cid:34) H f OH f H (cid:35) = 0 . (2)where τ e , τ h ≥ are vectors of contact wrench magnitudes;each row of the contact Jacobian J e and J h represents thewrench of a friction cone edge of an environment contactand hand contact, respectively. The derivation of the contactJacobians can be found in [26]. Each contact point contributesup to two rows to the contact Jacobian, depending on its mode:( s ) Separation: no wrench from the contact;( f ) Fixed: wrenches for both the left and right edges;( l ) Left sliding: only the wrench for the right edge;( r ) Right sliding: only the wrench for the left edge.Following the convention in [21], we use a string of theabbreviation letters to describe a contact mode, e.g. ‘ffsl’. B. A Distinguishable Cone Representation for Modes
There are many ways to check the feasibility of (2), i.e.whether a set of feasible forces exist. It is well-known that allthe contact wrenches on the object forms a polyhedral convexcone (PCC) in the object wrench space; if the cone spans thewhole space, the contacts on the object are in static balanceand the object is in force closure [26]. This way of usingPCC is not helpful for shared grasping, because it cannotdistinguish different contact modes and every force closurecontact mode looks the same (span the whole wrench space).We must modify the representation. Rewrite equation (2) using H f O = 0 : J Te τ e = J Th τ h = − H f H . (3)This equation maps the environmental contact force τ e and thehand contact force τ h into the generalized force space of thehand, which is the space where the robot action H f H livesin. Since τ e ≥ , τ h ≥ , J Te τ e and J Th τ h each again formsa polyhedral convex cone in this wrench space, call them the environment cone C e and the hand cone C h : C e = { J Te τ e | τ e ≥ } C h = { J Th τ h | τ h ≥ } (4)An example is shown in Figure 1. Rows of J e and J h are
12 3
X Y H Fig. 1: Left: a shared grasping system with a cube object(bottom) and a robot palm (top). The circled numbers showthe ordering of contact points. Right: the hand cone (purple)and environment cone (blue) for the “ffff” mode.generators of the cones. Our condition of force balance is:
The existence of a nonzero intersection between C e and C h is equivalent to the existence of a non-trivial (all zero) solution ( τ e , τ h , H f H ) to the static balance equation (3) . Now we explain how this new intersection-based conditionmakes a difference in distinguishing contact modes. We denotethis intersection as C m and call it the cone of the mode m .We can draw C m for all the modes in the same space, as inFigure 2. We use the contact mode enumeration algorithm byHuang et al. [16] to get a list of modes. Note that these cones llfs fffs fsfsllffslff sfff sfsf ffsf rrsfrrfffsffffff rsff Fig. 2: A closer look at the cones of the example in Figure 1with annotation. In this example, each non-zero face of the3D cone is the cone of a contact mode.are distinguishable, although most contact modes have forceclosure. This is because different contact modes always havedifferent contact wrenches.rom (3) we know the robot can directly apply a wrench w = − H f H in this wrench space, then as long as w ∈ C m , (5)mode m is feasible under robot action w . We can make theclaim stronger: If the robot action w is contained only in C m , the systemmust be in mode m . Now it seems straightforward to enforce a contact mode m :pick a robot wrench w in C m while staying away from thecones of other modes. The system will be in mode m if such w exists. However, this approach lacks robustness because thecones of different modes are not well separated from eachother. For example, in Figure 2 every facet of the “ffff” modedenotes another mode. If we apply w in one of those modes,a tiny disturbance force could push the actual applied wrenchinto the “ffff” mode. This shows a limitation of purely force-based method in multi-contact manipulation. We show how toovercome it in section III-C shortly.Before we move on, another thing we can learn is the rangeof wrenches the robot may apply. Denote U AF as the unionof the hand cone and environment cone of the all-fixed mode.Denote W as the whole N D dimensional wrench space. Wecan tell the result of applying a wrench w as follows. • w ∈ C AF : the object can have static force balance; • w ∈ U AF − C AF : the contact forces cannot balance thewrench, the object is accelerating; • w ∈ W − U AF : (impossible) some contact forces areoutside of their friction cones.Depending on the existence of C m , we call mode m F-feasible or F-infeasible . C. Feasibility under Hybrid Force-Velocity Control(HFVC)
In this subsection we explain how to uniquely select onemode by replacing pure force control w with HFVC. HFVCdecompose the robot action space into a velocity controlsubspace and force control subspace. Denote n af and n av asthe dimension of force and velocity control subspace in HFVC, n af + n av = N d . Define transformation R a ∈ R N d × N d , wecan describe a HFVC by: (cid:20) ω af ω av (cid:21) = R aH v H , (cid:20) η af η av (cid:21) = R aH f H , (6)where η af ∈ R n af and ω av ∈ R n av are the force and velocitycontrol, respectively. η av , ω af denote the uncontrollable vari-ables: force/velocity in the velocity/force-controlled direction.The HFVC imposes a velocity constraint: (cid:104) R ( n av ) a (cid:105) V = ω av . (7) R ( n av ) a is the last n av rows of R a . Denote C v = [0 R ( n av ) a ] and write the velocity constraint as C v V = ω av .Given a desired contact mode, we use hybrid servoing [14]to compute the force-velocity decomposition R a and velocitycontrol ω av . Hybrid servoing quickly finds a velocity controlthat is compatible with the contact constraints. We don’t use the second half of hybrid servoing which computes the forcecontrol η af , because the original algorithm was not designedfor handling sliding contacts. The inputs to hybrid servoinginclude a description of the goal velocity: G V = b G , (8)and contact constraints on velocity N m V = 0 , as explainedbelow. In the following we analyze the system under a HFVCand use a new approach to compute the force control η af .
1) Model filtering by velocity control:
Denote the velocityconstraint imposed by the contacts in contact mode m by: N m V = 0 M m V ≤ , (9)including non-penetration constraint, sticking constraint andsliding direction constraint. We call mode m V-feasible if thevelocity constraints (7) and (9) have a solution,
V-infeasible otherwise.Before we can safely ignore all the V-infeasible modes,we need to consider the possibility of crashing [13], i.e. thesituation where a small robot motion causes huge internalforce. Crashing can be dangerous and must be avoided. Incrashing, the system can remain in static balance when theforce in the velocity-controlled directions becomes arbitrarilylarge. In other words, C m must intersect the set of positiveforces in the velocity-controlled directions, denoted as C v : C m ∩ C v (cid:54) = { } . (10)This is a necessary condition. C v here can be derived from(4) and (6): C v = {− R − av τ v | τ v ≥ } , (11)where R − av represents the last n av columns of R − a . C v isagain a PCC, we call it the velocity cone .Remember C AF contains all C m . Using (10) we can derivethe sufficient condition for crashing-avoidance: C AF ∩ C v = { } , (12)This condition ensures no crashing in all possible modes. If itis satisfied, we can safely ignore all the V-infeasible modes,which makes it easier to separate the desired mode from therest. This is illustrated in the middle figure of Figure 3. D. Mode Selection by Force Control
There is usually more than one V-feasible mode. To securethe desired mode, the robot still needs to apply suitable force.Equation (6) and (3) gives: J Te τ e = J Th τ h = w = − R − a (cid:34) η af η av (cid:35) , (13)so the force-velocity decomposition happens directly in thewrench space where the hand and environmental cones live.Note again that we can not control η av , so we cannotdirectly place the wrench vector w in the cone of the desiredmode. To resolve the difficulty, we project all V-feasible cones C m into the force-controlled subspace. Those projections ¯ C m lffsfffsfsf ffsfrrsf Fig. 3: Procedures of wrench stamping for the problem inFigure 1. The torque is scaled into Newton by the objectlength. Top: All nonempty cones. Middle: the cones of V-feasible modes (one or two generators). The gray plane isthe force-controlled subspace. Bottom: Projection of feasiblecones onto the force-controlled subspace. The red ray is thechosen wrench for mode “sfff”.are PCCs in a lower dimensional space. Then we pick theforce control η af to be within the projection of the desiredmode, while staying away from any other projections: η af ∈ ¯ C goal − ¯ C others . (14)The minus sign means set difference. If η af exists, the actual w has to be in the desired mode because projections aresurjective. An example of is shown in Figure 3, bottom.Because of the projection, we call this approach WrenchStamping .If the set ¯ C goal − ¯ C others is empty, we call this mode F-indistinguishable since there is no force action the robot cantake to avoid other feasible modes. IV. R
OBUSTNESS OF A C ONTACT M ODE
In this section, we show how to evaluate the robustness ofa contact mode by looking at two causes of mode transitions:
A. Perturbations on Contact Geometry
Contact geometries plus friction coefficients determine theshape of hand and environment cones C e , C h . Modelinguncertainties change the shape of these cones, which couldcause their intersection to disappear and the correspondingmode to be F-infeasible. For planar systems, we define astability margin Φ g ( C e , C h ) that describes the “depth” of theintersection, i.e. the minimal angular rotation C e needs to taketo eliminate non-trivial intersections with C h . Note that thevolume (solid angle) of the intersection C m is NOT the rightmetric. Cones of most modes have zero volume. To introduceour metric, we first define a distance function ∆( F, E ) whichcomputes the angular distance between ray E and hyperplane F : ∆( F, E ) = arcsin( n F · E ) , (15)where n F denotes the normal of the hyperplane F . Both n F and E are unit vectors. Next, define a function that evaluatesthe “depth” of a cone inside another cone. Consider two PCCs C a , C b that satisfies C b ⊆ C a . Define σ ( C a , C b ) = min j max i ∆( F j , E i ) , (16)where F j is the j th facet of C a , E i is the i th edge of C b . Afacet F j defines a hyperplane with the positive normal pointinginside of the cone. We reduce dimension whenever possible,for example, if C a has two generators, a facet only has oneray and defines a one-dimensional hyperplane. If C a only hasone generator, we define σ ( C a , C b ) = 0 . Finally, define the geometrical stability margin Φ g ( C e , C h ) as: Φ g ( C e , C h ) = min( σ ( C e , C e ∩ C h ) , σ ( C h , C e ∩ C h )) . (17) B. Perturbations on Force Control
Disturbance forces can move the force control η af out of theregion defined by equation (14). We define the control stabilitymargin Φ c as the angular distance from η af to the closest coneprojections of other modes ¯ C others . Figure 4 shows how thecontrol in Figure 3 can fail under disturbance force.Fig. 4: Left: A disturbance force (bold red arrow) changesthe contact mode. Right: illustration of the control stabilitymargin. c and Φ g each describes the minimal disturbance forcerequired to trigger mode transition in terms of angular distancein object wrench space. Define the stability margin Ψ : Ψ = min(Φ c , Φ g ) . (18)With proper scaling between force and torque, as well as anominal force magnitude K F , the product K F Ψ represents theminimal magnitude of the disturbance force that can changecontact mode m . Just like grasping, we can make the systemmore robust simply by applying larger force.We summarize wrench stamping in Procedure Wrench-Stamping. Line 2 of WrenchStamping calls 2D contact modeenumeration algorithm implemented in [16]. Line 1 uses thefirst half of the hybrid servoing algorithm [14]. Procedure
WrenchStamping(Mode m ∗ ) Input: N , M , C e , C h for all-fixed mode (9)(4) Input:
G, b G , set of modes M Output: Ψ , HFVC ( n af , n av , R a , w av , η af ) [ n af , n av , R a , w av ] = hybridServoing( N m ∗ , G , b G ); if Crashing happens ( (12)(11) ) then return ø ; Initialize empty set ˆ M ← ø ; // Mode filtering by F-feasibility foreach m ∈ M do Assemble C ( m ) e , C ( m ) h from columns of C e , C h ; if C ( m ) e ∩ C ( m ) h = ø then Continue; Compute stability margin Φ ( m ) g ( C ( m ) e , C ( m ) h ) (17); if Φ ( m ) g = 0 then Continue; Assemble N m , M m from rows of N , M ; ˆ M ← { ˆ M , m } ; end M ← ˆ M , ˆ M ← ø ; // Mode filtering by V-feasibility foreach m ∈ M , m (cid:54) = m ∗ do if (7)(9) are feasible then ˆ M ← { ˆ M , m } ; end // Projections ¯ C goal ← R − Taf C m , R − af is the first n af cols of R − a ; ¯ C others ← ø ; foreach m ∈ ˆ M do ¯ C others ← { ¯ C others , R − Taf C m } end ¯ C remain ← ¯ C goal − ¯ C others ; if ¯ C remain = ø then return ø ; Pick η af ∈ ¯ C remain , compute Φ c ; Ψ ← min(Φ c , Φ g ) ( Φ g was computed in line 7);V. A LGORITHMS FOR TWO PROBLEMS
In this section, we provide algorithms for two problems inshared grasping.
A. Shared Grasping Control with Mode Selection
We provide algorithm 1 to solve robust control for sharedgrasping problem. Given a desired velocity, the algorithm findsthe most robust contact mode and computes a hybrid force-velocity control.If a desired contact mode is given, the control problembecomes a hybrid servoing problem [14]. Procedure Wrench-Stamping itself can solve it better than the original hy-brid servoing algorithm: users do not need to provide theguard conditions, WrenchStamping determines the conditionof mode transition by itself. Also WrenchStamping can handlerobust control under sliding contacts.
Algorithm 1:
Shared Grasping Control
Input: N , M , C e , C h for all-fixed mode (9)(4), G, b G Output:
The chosen contact mode m , stability margin Ψ , HFVC ( n af , n av , R a , w av , η af ) Compute the cone of all-fixed mode: C AF ← C e ∩ C h ; M all ← all contact modes, M ← ø ; Line 4 - 11 of Procedure WrenchStamping for m ∈ M all ,save results in M ; Initialize the solution set: S ← ø ; foreach m ∈ M do S ← { S , WrenchStamping( m ) } end return the s ∈ S with highest stability margin Ψ B. Geometry Optimization
We can optimize contact geometry parameters to maximizethe geometrical stability margin Φ g for a given mode. Notethat the function Φ g ( C e , C h ) boils down to the ∆ distancebetween a facet F and an edge E . The facet has either twogenerators C i , C j or just C i , we can backtrack its location in C e and C h . The edge is a generator of C e ∩ C h , which couldbe either an edge C k of C e or C h , or an intersection of a facet ( C , C ) in C e and a facet ( C , C ) in C h . In the latter case,the expression of the edge is ( C × C ) × ( C × C ) . So wecan explicitly write down Φ g as a function of contact screws: Φ g ( C e , C h ) = arccos (cid:18) C i || C i || · C k || C k || (cid:19) or= arccos (cid:18) C i || C i || · ( C × C ) × ( C × C ) || ( C × C ) × ( C × C ) || (cid:19) . (19)for one-dimensional facet F , or Φ g ( C e , C h ) = arcsin (cid:18) C i × C j || C i × C j || · C k || C k || (cid:19) or= arcsin (cid:18) C i × C j || C i × C j || · ( C × C ) × ( C × C ) || ( C × C ) × ( C × C ) || (cid:19) (20)for two-dimensional F . We can further expand the contactscrews into contact geometry parameters and compute theradient of Φ g in analytical form. We outline a gradientdescent algorithm in Algorithm 2. The parameter p maycontain contact locations, normals and friction coefficients.Note that computation in (19) and (20) only involves contactscrews that are the current bottleneck of stability margin.This set of contact screws will change between iterations. Weobserved this phenomenon in the experiments section.In order to get analytical expression of the gradient of Φ g ,we ignore the normalizations in (19) and (20) when computingtheir derivatives. To obtain the correct gradient about a contactscrew, we just project the gradient computed this way onto thetangent plane of the contact screw, as shown in line 7. Algorithm 2:
Geometrical Parameter Optimization
Input:
Contact geometry parameter p Output:
Optimized contact geometry parameter p ∗ repeat // 1. Evaluate Φ g Compute C e , C h (4) using p ; Compute C m = C e ∩ C h , record the mapping M between columns of C m and columns of C e , C h .; Compute Φ g (17), record the mapping M between C i,j,k, , , , and columns of C e , C h , C m ; Use M and M to compute the mapping M between C i,j,k, , , , and columns of C e , C h ; // 2. Compute gradient Compute Φ t = ∂ Φ g /∂C t , t ∈ { i, j, k, , , , } bydifferentiating (19)(20) without denominators; Remove the components of Φ t that scales C t : Φ t = Φ t − Φ t · C t , t ∈ { i, j, k, , , , } ; Use M to construct Φ C = ∂ Φ g /∂C from Φ t , C ∈ columns of C e , C h ; Compute the Jacobian C p = ∂C/∂ p bydifferentiating the expression of C e , C h (4).; Φ p ← (cid:80) C (Φ C · C p ) ; // 3. Update p ← p + d Φ p , d is a step length; until converge ; VI. EXPERIMENTSWe implement our algorithms in MATLAB and test them onseveral tasks in experiments. We use an ABB IRB120 robotin section VI-A and VI-B, a UR5e robot in section VI-D,both are 6-axis robot and position-controlled; we implementa hybrid force-velocity controller with a wrist-mounted ATIMini-40 FT sensor. The communication rate is 250Hz for theABB robot and 500Hz for the UR5e robot. In both cases, theforce control bandwidth is roughly 1Hz. A. Robust Control of Desired Contact Modes
First, we validate the ability of our method in computingrobust actions under different contact modes. Consider theshared grasping system in Figure 1 as an example. The friction Fig. 5: Different contact modes and their stability margins.The order of contacts follows Figure 1.coefficients are estimated to be 1.2 and 0.25 for hand-objectand table-object contacts, respectively.There are 81 contact modes for the four contact points.Algorithm 1 computes 13 modes with positive geometricalstability margin Φ g . We hand-pick six modes with objectmotion and solve them with Procedure WrenchStamping. Theresults are shown in Figure 5. B. Contact Mode Selection and Control
Fig. 6: Flipping a cube on its corners.Using the same object and robot, we demonstrate Algo-rithm 1 in the task of cube rotation with a palm, as shown inFigure 6. The task is challenging because the object could fallat any time, the control must always be robust. The task hasfour stages. The goal velocity for the first two stages are: G = [0 0 1 0 0 0] , b G = − . . (21) G = (cid:20) Adj
W H I (cid:21) , b G = [0 0 0 − . T . (22)First three rows of G , b G constrain the object to be static.Stage three and four are the same but in a different plane. Eachstage is divided into eight time steps, at which Algorithm 1decides which mode and what action to take. There is no visionfeedback, the object pose is estimated from robot hand poseand has an accumulating error. We execute the four-stage taskeight times and have four complete successes. The failures allhappen in the last two stages. C. Finger Placement Optimization
We test Algorithm 2 on a point finger manipulation task,as shown in Figure 7, left. The width and height of thelock are both 0.1m. We optimize finger location for twotasks: 1. pivoting about the left corner with mode “sff”; 2.Sliding to the left with mode “llf”. We show the result of200 iterations of gradient descent in Figure 8, right, whichtakes 0.45s in MATLAB. The stability margin increases andreaches a plateau. Note that on the plateau the solution isstill zig-zagging by a small magnitude, because the set ofcontact screws being optimized is varying between iterations,as explained in section V-B.Fig. 7: Left: the Finger-block example and the ordering ofcontacts. Right: Experiment setup. The yellow block is fixed.
Time steps
Stability Margin (Unitless)
PivotingSliding 0 50 100 150 200
Time steps -0.05-0.03-0.010.010.03
Finger Position X (m)
PivotingSliding
Fig. 8: The evolution of finger position and stability marginduring the optimization.
D. Compute the Range of Feasible Parameters
Consider again the task of block sliding (Figure 7). Usingthe optimized finger location and a HFVC computed fromAlgorithm 1, we compute the range of table contact locationsthat produce a positive stability margin for the desired mode“llf”. We randomly sample 1000 pairs of object right and leftcorner locations within the range ( − . m, . m ) . The resultis shown in Figure 9. A point is marked green if the stabilitymargin is positive. We can see there is roughly a safety rangeof . m around the nominal point, i.e. the sliding motioncan still be successful if the actual environmental contactlocation changes by 0.05m. We can do the same computationfor varying object height, friction coefficient, etc.We implement this sliding action with a wide range ofobjects and also with gravity as disturbance force. The resultsare shown in Figure 10.VII. CONCLUSION AND DISCUSSIONTo conclude, we provide stability analysis method for sharedgrasping problems that involves multiple contact modes. Wedemonstrate how to find the most robust contact mode andcompute robot actions to achieve the desired system velocity, -0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2 Right Corner Location (m ) -0.2-0.100.10.2 L e f t C o r n e r L o ca ti on ( m ) StableUnstable
Fig. 9: Feasibility of different model parameters. The controlis computed for the black dot.Fig. 10: The same control law works for different objects.as well as how to refine finger placements to improve stability.We are working on generalizing the method to 3D problems.We need to revisit the design of stability margins ((17) and(18)), which currently relies on 3D geometry.We demonstrate the robustness of our method in a varietyof experiments. The speed of robot motion is limited by thebandwidth of the robot. Recent progress on direct-drive robothands [3] opens possibilities for faster shared grasping.Although shared grasping is designed for making environ-mental contacts, the stability analysis and algorithms also workfor real in-hand manipulation problems if there are two (groupsof) fingers.Shared grasping complements grasping as another categoryof manipulations with good robustness. This work showsits potential for contact-rich manipulations that are reliableenough for real-world applications.A
CKNOWLEDGMENTS
We would like to thank Eric Huang and Xianyi Cheng forinsightful discussions. We also want to thank Nikhil Chavan-Dafle and Ankit Bhatia for their suggestions in the writing ofthe paper. R
EFERENCES [1] Yasumichi Aiyama, Masayuki Inaba, and Hirochika In-oue. Pivoting: A new method of graspless manipulationf object by robot fingers. In
Proceedings of 1993IEEE/RSJ International Conference on Intelligent Robotsand Systems (IROS’93) , volume 1, pages 136–143. IEEE,1993.[2] Dmitry Berenson, Siddhartha S Srinivasa, Dave Fergu-son, and James J Kuffner. Manipulation planning onconstraint manifolds. In
Robotics and Automation, 2009.ICRA’09. IEEE International Conference on , pages 625–632. IEEE, 2009.[3] Ankit Bhatia, Aaron M. Johnson, and Matthew T. Mason.Direct drive hands: Force-motion transparency in gripperdesign. In
Proceedings of Robotics: Science and Systems ,June 2019.[4] Nikhil Chavan-Dafle and Alberto Rodriguez. Prehensilepushing: In-hand manipulation with push-primitives. In , pages 6215–6222, 2015.[5] Nikhil Chavan-Dafle and Alberto Rodriguez. Sampling-based planning of in-hand manipulation with externalpushes. arXiv preprint arXiv:1707.00318 , 2017.[6] Nikhil Chavan-Dafle, Rachel Holladay, and AlbertoRodriguez. In-hand manipulation via motion cones.
Robotics: Science and Systems , 2018.[7] Nikhil Chavan Dafle, Alberto Rodriguez, Robert Paolini,Bowei Tang, Siddhartha S Srinivasa, Michael Erdmann,Matthew T Mason, Ivan Lundberg, Harald Staab, andThomas Fuhlbrigge. Extrinsic dexterity: In-hand manip-ulation with external forces. In , pages1578–1585. IEEE, 2014.[8] Chelsea Finn and Sergey Levine. Deep visual foresightfor planning robot motion. In , pages2786–2793. IEEE, 2017.[9] Shixiang Gu, Ethan Holly, Timothy Lillicrap, and SergeyLevine. Deep reinforcement learning for robotic ma-nipulation with asynchronous off-policy updates. In , pages 3389–3396. IEEE, 2017.[10] Weiqiao Han and Russ Tedrake. Local trajectory stabi-lization for dexterous manipulation via piecewise affineapproximations. arXiv preprint arXiv:1909.08045 , 2019.[11] Franc¸ois Robert Hogan and Alberto Rodriguez. Feedbackcontrol of the pusher-slider system: A story of hybridand underactuated contact dynamics. arXiv preprintarXiv:1611.08268 , 2016.[12] Anne Holladay, Robert Paolini, and Matthew T Mason. Ageneral framework for open-loop pivoting. In , pages 3675–3681, 2015.[13] Yifan Hou and Matthew T Mason. Criteria for main-taining desired contacts for quasi-static systems. In
Proceedings of IEEE/RSJ International Conference onIntelligent Robots and Systems . IEEE, November 2019.[14] Yifan Hou and Matthew T Mason. Robust executionof contact-rich motion plans by hybrid force-velocity control. In
International Conference on Robotics andAutomation (ICRA) 2019 . IEEE Robotics and Automa-tion Society (RAS), May 2019.[15] Yifan Hou, Zhenzhong Jia, and Matthew T Mason. Fastplanning for 3d any-pose-reorienting using pivoting. In , pages 1631–1638. IEEE, 2018.[16] Eric Huang, Xianyi Cheng, and Matthew T Mason.Efficient contact mode enumeration in 3d. In
Workshopon the Algorithmic Foundations of Robotics , 2020.[17] Imin Kao and Mark R Cutkosky. Quasistatic manipu-lation with compliance and sliding.
The Internationaljournal of robotics research , 11(1):20–40, 1992.[18] Michael Koval.
Robust Manipulation via Contact Sens-ing . PhD thesis, Carnegie Mellon University, Pittsburgh,PA, September 2016.[19] Kevin M Lynch and Matthew T Mason. Stable pushing:Mechanics, controllability, and planning.
The Inter-national Journal of Robotics Research , 15(6):533–556,1996.[20] Matthew T Mason. Compliance and force control forcomputer controlled manipulators.
IEEE Transactions onSystems, Man, and Cybernetics , 11(6):418–432, 1981.[21] Matthew T Mason.
Mechanics of robotic manipulation .MIT press, 2001.[22] N Harris McClamroch and Danwei Wang. Feed-back stabilization and tracking of constrained robots.
IEEE Transactions on Automatic Control , 33(5):419–426, 1988.[23] Igor Mordatch, Zoran Popovi´c, and Emanuel Todorov.Contact-invariant optimization for hand manipulation.In
Proceedings of the ACM SIGGRAPH/Eurographicssymposium on computer animation , pages 137–144. Eu-rographics Association, 2012.[24] Igor Mordatch, Emanuel Todorov, and Zoran Popovi´c.Discovery of complex behaviors through contact-invariant optimization.
ACM Transactions on Graphics(TOG) , 31(4):43, 2012.[25] Igor Mordatch, Kendall Lowrey, and Emanuel Todorov.Ensemble-cio: Full-body dynamic motion planning thattransfers to physical humanoids. In
Intelligent Robotsand Systems (IROS), 2015 IEEE/RSJ International Con-ference on , pages 5307–5314. IEEE, 2015.[26] Richard M Murray, Zexiang Li, S Shankar Sastry, andS Shankara Sastry.
A mathematical introduction torobotic manipulation . CRC press, 1994.[27] Michael Posa, Cecilia Cantu, and Russ Tedrake. Adirect method for trajectory optimization of rigid bodiesthrough contact.
The International Journal of RoboticsResearch , 33(1):69–81, 2014.[28] Marc H Raibert and John J Craig. Hybrid position/forcecontrol of manipulators.
Journal of Dynamic Systems,Measurement, and Control , 103(2):126–133, 1981.[29] J Kenneth Salisbury. Whole-arm manipulation. In
Proc.of the 4th Int. Symp. of Robotics Research . Published bythe MIT Press, 1987.30] Naoyuki Sawasaki, Masayuki Inaba, and Hirochika In-oue. Tumbling objects using a multi-fingered robot.In
Proceedings of the 20th International Symposium onIndustrial Robots and Robot Exhibition , pages 609–616,1989.[31] Arne Sieverling, Clemens Eppner, Felix Wolff, andOliver Brock. Interleaving motion in contact and infree space for planning under uncertainty. In , pages 4011–4073. IEEE, 2017.[32] Ra´ul Su´arez, Jordi Cornella, and M´aximo Roa Garz´on.
Grasp quality measures . Citeseer, 2006.[33] Hajime Terasaki and Tsutomu Hasegawa. Motion plan-ning of intelligent manipulation by a parallel two-fingered gripper equipped with a simple rotating mech-anism.
IEEE Transactions on Robotics and Automation ,14(2):207–219, 1998.[34] Marc Toussaint, Kelsey Allen, Kevin A Smith, andJoshua B Tenenbaum. Differentiable physics and sta-ble modes for tool-use and manipulation planning. In
Robotics: Science and Systems , 2018.[35] Masaru Uchiyama and Pierre Dauchez. A symmetrichybrid position/force control scheme for the coordinationof two robots. In
Robotics and Automation, 1988.Proceedings., 1988 IEEE International Conference on ,pages 350–356. IEEE, 1988.[36] Dustin J Webb and Jur Van Den Berg. Kinodynamic rrt*:Asymptotically optimal motion planning for robots withlinear dynamics. In , pages 5054–5061. IEEE,2013.[37] Harry West and Haruhiko Asada. A method for thedesign of hybrid position/force controllers for manipu-lators constrained by contact with the environment. In
Proceedings. 1985 IEEE International Conference onRobotics and Automation , volume 2, pages 251–259.IEEE, 1985.[38] Eiichi Yoshida, Mathieu Poirier, Jean-Paul Laumond,Oussama Kanoun, Florent Lamiraux, Rachid Alami, andKazuhito Yokoi. Pivoting based manipulation by ahumanoid robot.
Autonomous Robots , 28(1):77–88, 2010.[39] Tsuneo Yoshikawa. Dynamic hybrid position/force con-trol of robot manipulators–description of hand constraintsand calculation of joint driving force.
IEEE Journal onRobotics and Automation , 3(5):386–392, 1987.[40] Andy Zeng, Shuran Song, Stefan Welker, Johnny Lee,Alberto Rodriguez, and Thomas Funkhouser. Learn-ing synergies between pushing and grasping with self-supervised deep reinforcement learning. 2018.[41] Jiaji Zhou, Robert Paolini, J Andrew Bagnell, andMatthew T Mason. A convex polynomial force-motionmodel for planar sliding: Identification and application.In , pages 372–377. IEEE, 2016.[42] Jiaji Zhou, Robert Paolini, Aaron M Johnson, J AndrewBagnell, and Matthew T Mason. A probabilistic planning framework for planar grasping under uncertainty.