IROS2019国际学术会议论文集0896

A Convex-Combinatorial Model for Planar Caging Bernardo Aceituno-Cabezas1, Hongkai Dai2, and Alberto Rodriguez1 1Department of Mechanical Engineering Massachusetts Institute of Technology 2Toyota Research Institute ,hongkai.daitri.global AbstractCaging is a promising tool which allows a robot to manipulate an object without directly reasoning about the contact dynamics involved. Furthermore, caging also provides useful guarantees in terms of robustness to uncertainty, and often serves as a way-point to a grasp. However, caging is traditionally difficult to integrate as part of larger manipulation frameworks, where caging is not the goal but an intermediate condition. In this paper, we develop a convex-combinatorial model to characterize caging from an optimization perspective. More specifically, we derive a set of sufficient constraints to enclose the configuration of the object in a compact-connected component of its free-space. The convex-combinatorial nature of this approach provides guarantees on optimality and convergence, and its optimization nature makes it versatile for further applications on robot manipulation tasks. To the best of our knowledge, this is the first optimization-based approach to formulate the caging condition. I. INTRODUCTION A cage is an arrangement of obstacles that bounds the mobility of an object. The connection of cages to invariant regions and robustness has attracted the attention of the manipulation community for a long time. A cage can be used as a waypoint to a grasp 1, 2, providing a guarantee that the object will not escape in the process. A cage can also be used to manipulate without rigid immobilization, alleviating common issues from jamming, wedging, and general over-constrained interactions (e.g. turning the handle of a door). Thereality,however,isthatthepracticalapplicationsofexisting caging algorithms have been limited. In this work we propose to rethink the conventional topologic/geometric approach to characterize cagingfocused on developing complete algorithms to analyze the configuration space of an object surrounded by a set of obstacles. Here, we develop an approach aimed at the synthesis of manipulation strategies that can incorporate and exploit the caging condition, the same way that we normally use grasping for immobilization. Figure 1 describes the motivation and the long-term goal of this project: How do we synthesize a manipulation plan to cage an object while exploiting environment constraints and respecting the kinematic constraints of the robot? The work in this paper is a first step in that direction. We present a reformulation of the caging condition as a set of mixed-integer convex constraints that provide: Versatilityto incorporate the caging condition in the context of a larger manipulation planning framework. Guaranteesof the optimization framework that steams from the convex-combinatorial nature of the model. If a cage exists within the set of conditions described by the Fig. 1:Examples of caging in the context of robot manipulation: trapping against a wall and caging under kinematic constraints. mixed-integer problem, the optimization algorithm will find it. If it does not exist, it will report so. These come at the expense of exponential bounds on compu- tation time, and resolution completeness of the algorithm. In par- ticular, this paper focuses on caging polygonal objectsdescribed as the union of convex polygonswith a manipulator described by an arbitrary number of point fingers in the plane. To do this, the proposed approach provides a set of sufficient conditions to cage such object. The main contributions of this paper are: Cage optimizationalgorithm based on the proposed convex-combinatorial caging model. This can be computed efficiently with off-the-shelf mixed-integer solvers yielding global convergence guarantees. Validationof the proposed cage synthesis algorithm on random planar polygons. Applicationof the caging algorithm to find cages that either exploit the environment (walls) or take into account kinematic limits in finger motions. In Section VII, we discuss the possibility of using the approach to synthesize cage-reach-grasp motions with formal guarantees of convergence, and its application to design gripper kinematics and shapes defined by a set of points. A. Related Work Algorithms for caging have been studied since Rimon and Blake 3 introduced this notion to the robotics community. The first caging algorithms proposed were focused on characterizing the set of point-finger configurations capable of caging a polygon 3, 4, 5. Since then, cage-finding algorithms have continuously improved. Some works have formulated the problem in contact0space, providing better computational complexity 6, 7. Some other works have shown how tools from computational topology can be used to find and verify cages on 2D and 3D 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Macau, China, November 4-8, 2019 978-1-7281-4003-2/19/$31.00 2019 IEEE2378 objects 8, 9. Furthermore, the work of Varava and Carvalho et. al. 10 showed how sampling-based methods could efficiently find cages of 2D and 3D objects with arbitrary manipulator geometries. In similar spirit to our work, Pererira et. al. and Varava et. al. 11, 12 showed the application of caging as a tool for multi-robot transportation of polygonal objects. In comparison to prior work, our model is less computationally efficient and more limited, as the conditions derived are only sufficient to guarantee a cage. However, this approach offers significantly more flexibility, as it can be easily included as constraints within trajectory or shape optimization, while still characterizing a large set of cages. II. PRELIMINARIES In this section, we introduce the notation that we will use through this paper and define the idea of caging. A. Notation Given an objectOon a workspaceW, we will denote its Configuration Space 13 asC SO(2). We refer to a plane ofCwith fixed orientation componentsas aCslice, denotedC(s). We refer to an arrangement of point fingers as the manipulatorM. We assumeMhasNpoint fingers with positions M=p1,.,pNWN. At eachCslice, we refer to the set of configurations where the object penetrates a finger asC-obstacles. Then, the free-space of the objectCfree(O)corresponds to the spaceCnot intersecting any of theC-obstacles, namely the set of object configurations that does not penetrate any of the fingers. B. Caging The caging problem, based on the original formulation by Kuperberg 14, can be stated as: For a planar objectOat configurationq = qx,qy,qTand a manipulatorM, find a configuration ofMsuch thatqlies in a compact connected component ofCfree(O), denoted as Ccompact free (O). This formulation of the problem, based on topology, is equivalent to the more traditional geometric condition that there exists no continuous path that will drive the object arbitrarily far from the manipulator, as illustrated in Fig. 2. III. APPROACHOVERVIEW Inthissection,weprovideanoverviewontheproblemofcaging a planar object and describe the conditions to characterize it. A. Conditions for Caging The set of compact-connected components in free-space can be too general to explore. Here, we introduce a set of conditions that characterizearichsetofcages,wherethecomponentCcompact free (O) has a single local maxima and minima over the orientation axis. For this, let us first define the idea of limit orientations: Definition 1 (Limit Orientation): Given a compact-connected componentAC, its limit orientationsU,Lcan be defined as U=supA and L=infA. Fig. 2:Overview of the conditions for caging. Each plane shows a Cslice during a cage between two limit orientations (blue) with a compact-connected component of free-space (red). Note that in the limit orientations the object is constrained to a line segment of translational motion by theCobstacles. Also, the object is only caged if the component remains compact and connected between slices. Then, the following conditions generate any component Ccompact free (O) with at most two global limit orientations: 1)The component is bounded in the orientation axis by two limit orientations, otherwise it infinitely repeats along such axis with period 2. 2)In allCslices, between two limit orientations when these exist, there is a loop ofCobstacles enclosing a segment of free-space. Such loop enclosesqat the slice withs=q (as illustrated in the middle column of Fig. 2). These loops must be connected, enclosing a component of free-space in between adjacent slices. 3)At theCslice of the limit orientations, if these exist, the free-space component enclosed by the loop has zero area. Thus, getting reduced to a line segment or a singleton. Fig. 2 illustrates some slices between limit orientations where these conditions hold. While these conditions might seem restrictive, these can be used to represent a very large set of cages. We provide a geometric intuition of the different type of cages we can characterize in Fig. 3. We will derive a set of convex-combinatorial constraints to impose these conditions and yield an optimization formulation of the caging problem. B. Model Overview We make the following assumptions: 1)The objectOis represented as the union ofMconvex polygons, with a boundary that consists of L facets. 2)The manipulatorMis represented as a set ofNpoint- fingers. (a)(b)(c) Fig. 3:Different types of compact connected components. The conditions described in this paper can fully describe components (a) and (b), as both of these components have a pair of limit orientations where the component opens and closes (green stars). However, these conditions are not sufficient to create a cage with component (c), as there are two local minima in the orientation of the component (red stars). 2379 1 2 p1 p2 p3 12 22 13 23 11 21 11 21 12 22 13 23 H1(2,1)H2(1,2) G1(1,2)G3(2,1) H3(1,1) Fig. 4:The caging loop for a slice with three fingers inW(left),C(s) (center) and its corresponding connection graph (right) To include the conditions above, we discretizeCinS C-slices, similar to 10, and impose that the manipulator bounds a component of free-space in each slice. Moreover, we make sure that the cage is not broken between slices by imposing continuity conditions between slices. We note that formulating this model requires continuous variables to represent the position of the manipulator, and binary variables to represent the discrete connectivity relation between C-obstacles. Therefore, we introduce two sets of constraints to our model: At each slice:We require that a subset of theCobstacles forms a loop around the configuration of the objectqx,qyT. This ensures that there is a compact-connected component at each slice, as illustrated in Fig. 4. For all orientations:we constrain that either the object is caged for all360or thatCcompact free (O)is bounded by two limit orientations. Moreover, we require that the loop at each Cslice maps continuously, without breaking, into the loop of its adjacentCslices. Through this, we can ensure that all components are connected betweenCslices, forming the component Ccompact free (O) that encloses q. In sections IV and V, we will show that these conditions are sufficient to guarantee thatqis caged byM. Furthermore, the use of convex-combinatorial constraints allows us to provide guarantees in terms of optimality and convergence. IV. CONSTRUCTING LOOPS AT EACH SLICE Here, we describe the set of conditions required to create a compact-connected component of free-space at oneCslice. For notation convenience, we will refer to the free-space ofC(s) asCfree(O,s), and its enclosed component, whereqis, as Cc(O,s). To createCc(O,s), we require that all theCobstacles inC(s)form a loop. Since the object is decomposed intoM convex polygons, the problem of enforcing such a loop reduces to constructing a directed graph with the edges representing polygons intersection betweenC-obstacles. Finally, we must also require that such loop encloses the configurationqx,qyTat the slice with fixed orientation q. 1) Existence of a loop: In order to compose an enclosing piecewise polygonal loop, we must determine which of the polygons on eachCobstacles are part of the loop and their direction in the directed graph. To this end, we represent each polygonasanode,andaddanedgebetweeneachpairofpolygons thatmustintersect.Fig.4illustratesthisconstruction.Fornow,and for simplicity, we assume that all fingers must be part of this loop. Foreachfinger,letusintroduceabinarymatrix Hn 0, 1MM, whereHn(i, j) = 1if theithpolygon onCobstaclenintersects with thejthpolygon onCobstacle n+1. Mathematically if we denote theithpolygon in thenth finger as Pi,n, then we impose the constraint: Hn(i,j) point rnR2s.t. rnPi,nPj,n+1(CT1) where we transcribe the(implies) operator as linear constraints through big-M formulation115. Furthermore, to make each finger a part of the loop, we enforce the constraint: X i,j Hn(i,j)=1, n(CT2) meaning that there is one and only one directed edge from C-obstaclentoC-obstaclen+1. Since these connections are enforced forn = 1,.,N, there is a directed loop of obstacles inC(s). In addition to usingHnto represent the connectivity between two different C-obstacles, we introduce a matrix Gn 0, 1MMto denote if an edge in theCobstacle connection graph is in the loop. In the case thatGn(i,j)=1the graph has an edge going from polygonito polygonjon thenth Cobstacle. Then, the fingers create a closed loop if: Hn1(i,j)k,l s.t. Gn(j,k)+Hn(j,l)=1(CT3) Gn(p,q)s,r6=p s.t. Gn(q,r)+Hn(q,s)=1(CT4) (CT3)and(CT4)combined guarantee that for each node with an inbound edge, there is one and only one outbound edge, thus we have a loop inC(s). In the special case of a two-finger manipulator, sinceHn,n+1andHn1,nhave the same value, we need to further constrain that l6=i in (CT3). 2) Configuration enclosing: The previous constraints ensure the existence of a compact-connected componentCc(O, s) in each slice. It is important to note that the enclosing loop implies the existence of piecewise-polygonal curve that lives in theCobstacles. In our case, we define this curve by connecting points that live in the intersection of polygons in the loop. However, this does not guarantee thatqis contained in Ccompact free (O). For this, we note that enclosingqinCcompact free (O) requires enclosingqx,qyTinCc(O,q). In order to incorporate this constraint to the model, we introduce Remark 1: Remark 1 16:If a linear ray that originates from a pointr has an odd number of intersections with a closed curve, then the point r falls in the interior of such curve. This remark is valid except for degenerate cases, when the ray is parallel to a segment of the curve or the area enclosed by the curve is a single point. However, such scenarios can be easily avoided in our analysis, as the enclosing curve is contained within theCobstacles. To incorporate this condition, we constrain the number of intersections between the ray and the line segments as a convex-combinatorial constraint. For this, we decompose the area that covers each possible line segment of the loop into 4 1For a binaryB, we haveBAxbis equivalent toAxb+M(1B) withMbeing a large positive number. This allows us to represent conditionals within the optimization model through linear constraints. 2380 square regions, parallel to the ray, and introduce a binary decision matrix F 0,1NM5, such that: 1) F(n,m,1)throughF(n,m,4)assignqx,qyto one of four rectangular regions enclosing the line segment starting in the mthpolygon of the nthfinger. 2) F(n,m,5)is set to1if themthpolygon of thenthfinger is not part of the loop. Here, we assignF(n,m,1)to the region that is parallel to the ray and below the line segment. Because of this,F(n,m,1)=1 implies that the ray intersects the segment. A visualization of this can be seen in Fig. 7. Then, we introduce the following pair of sufficient constraints: (P n,mF(n,m,1) is an odd number P5 i=1F(n,m,i)=1 n,m (CT5) To transform (CT5) into a set of linear constraints, we introduce the following lemma: Lemma 1.The summation of binary variables Pn i=1bi is an odd number if and only if b1XOR b2. XOR bn=1. Where theXORoperator can be transcribed as linear constraints on the binary variables 17. 3) Non-Penetration Constraints:Additionally, we must prevent the fingers from penetrating the object. For this, we partition the 2D collision free workspaceWOinto a set ofNr convex regions, represented as: Ri=xR2|Aixbi and then constrain that eachpnfinger lies in one of these regions. To do this, we introduce a binary decision matrixR0,1NrN such that: Rr,nAipnbiand Nr X r=1 Rr,n=1,n(CT6) Where we again transcribe theoperator via big-M formulation. This ensures that each finger lies in only one of the regions. Note that this constraint also ensures thatqlies in the interior of Cc(O,s)without penetrating anyCobstacle. Sect. V discusses how these loops must interconnect in between slices to create a compact-connected component Ccompact free (O) in C. 12 22 11 21 12 22 11 21 Fig. 5:A point lies within a loop if a ray originating from that point has an odd number of intersections (blue) with the edges of the loop. Left: the point lies inside the polygon, and the ray has an odd number of intersections (one). Right: the point lies outside the polygon, and the ray has an even number of intersections (two). Fig. 6:Region assignment ofq(red dot) depending on the value of F(n,m)and the direction of the linear ray (red arrow), for a line segment oftheenclosingloop(blue).Notethat,sincetheregionisparalleltotheray, the ray always intersects the segment whenqis assigned withF(n,m,1). V. CONSTRUCTING A CAGE FROM LOOPS In order for an object to be caged, the compact-connected components