Integrated Single-arm Assembly and Manipulation Planning using Dynamic Regrasp Graphs

Transcription

1 Proceeding of The 2016 IEEE International Conference on Real-time Computing and Robotic June 6-9, 2016, Angkor Wat, Cambodia Integrated Single-arm Aembly and Manipulation Planning uing Dynamic Regrap Graph Weiwei Wan and Kenuke Harada Abtract Thi paper preent an integrated ingle-arm aembly and motion planning algorithm to recurively find how to aemble two object with the help of a plenary urface a the upporting fixture. The algorithm i done in both aembly level and motion level. In the aembly level, the algorithm check all combination of the aembly and get a et of candidate aembly order. For each aembly order, it perform motion planning to pick-and-place the bae object and aemble the other object to the bae. In the motion level, the algorithm integrate the order computed in the aembly level incrementally and recurively with graph earching and motion planning, and find a motion equence to finih aembly tak. The propoed algorithm can find a feaible olution to aemble two object with completene. It i practical and i ready to be integrated with force control to perform real-world aembly tak. be, and how to aemble the econd object to the firt one, could be found. (a) The naphot of ingle-arm aembly. In ubfigure (1-2), the robot pick up the firt object and place down it on the table a the bae. In ubfigure (3-4), the robot pick up the econd object and aemble it to the bae. I. Introduction Thi paper tudie the integrated ingle-arm aembly and manipulation planning uing regrap graph and a table urface. Given two part and their relative poition in an aembled tructure, our integrated planning algorithm decide (1) which part i ued a the bae, (2) how to place it, and (3) how to aemble the econd part to the bae. In tate-of-the-art robotic aembly ytem, the manipulation order and equence are pre-defined manually, which ignificantly impair the automation of next-generation manufacturing. Take Fig.1(a) for example. To aemble two object, technician program the aembly plan manually: They make the robot to firt pick up the firt object, place it down on a fixture uing a pre-defined poition and orientation, and pick up the econd object and aemble it to firt one uing the given relative poe between them. The aembly order i decided by the intelligence of the technician: There are everal other poible olution hown in Fig.1(b) but the technician manually elect one for the robot uing their experience. In thi paper, we would like to automate the manual proce performed by the technician. We propoe an integrated aembly and planning algorithm which automatically generate the aembly order and find a motion equence to finih aembly tak. Lot of tudie are available on how to intelligently find the poe of the two aembly part and how to pick up and place down them a well a avoiding colliion with the environment. But to the bet knowledge of u, no tudy about the aembly order, namely which object to pick firt, which poition and orientation hould the object Weiwei Wan i affiliated with National Intitute of Advanced Indutrial Science and Technology (AIST), Japan. Kenuke Harada i with Oaka Univerity, Japan. wan-weiwei@ait.go.jp /16/$ IEEE (b) The poible aembly order. The econd object i aembled to the bae following the arrow. Fig. 1. The aembly order in tate-of-the-art robotic aembly ytem i manually pecified. A robot pick up and place down the bae, and aemble the econd object to the bae following (a). There are lot of other candidate hown in (b) but the technician manually elect one for the robot uing their experience. Thi paper propoe an algorithm to find a feaible aembly automatically uing integrated aembly and manipulation planning. In repective field, aembly planning and motion planning are well tudied. In motion planning, [1], [2] are the leading tudy that compared the workpace and joint pace approache. [3], [4] and [5] preented the probabilitic approache to find colliion free motion in the joint pace. [6], [7], and [8] repreent the more recent tudie that ue hitoric data to improve algorithm performance. In aembly planning, early work like [9] and [10] were devoted to ymbolic planning. [9] preented the AND/OR graph approach to analyze aembly tructure. [10] preented the eminal work that ue Non-Directional Blocking Graph (NDBG) to generate aembly equence. [11] preented a phere aembly method which i eentially a path planning problem. More recent work like [12] and [13] concentrate on earching the feaible grap and manipulation motion with repect to a fixed aembly poe. Other work like [14] convert aembly planning to a emi-automatic proce and learn the aembly equence from human being. The adopted method avoid the contraint from the robot bodie and the other part of the aembled tructure, intead of uing them. Comparing with our work, it did not integrate the aembly and motion. 174

2 Beyond motion planning and aembly planning, there are tudie about integrated tak and motion planning [15], [16], [17], [18], [19], [20] where the planning i divided into two level. In the tak level, the robot divide the tak into meta primitive. In the motion level, the robot plan motion to implement the primitive. The tak level planning i done incrementally and recurively along with the motion level planning. For example, [19] ue geometric backtracking in tak level to decompoe and plan the motion in the motion level. Integrate tak and motion planning hare the baic principle with our work, except that we olve the pecific tak of aembly, and are ubject to contraint from aembly order and configuration. In the aembly level, we check all combination of the aembly and get a et of candidate aembly order. For each order, we perform motion planning in the motion level to pick-and-place the bae object and aemble the other object to the bae. The aembly level planning i integrated with the planning in the motion level and i done incrementally and recurively with graph earching and motion planning. If the planning algorithm cannot find a path in the motion level, we roll back to the aembly level and try another candidate aembly order until a olution i found or failure i reported. We implemented the algorithm uing Choreonoid and grapplugin, and executed the implementation on a Kawada Nextage Robot uing ome wooden block. The execution demontrate the applicability of our propoal. The imulation, the correpondent regrap graph, the recurive planning proce, and the final execution on real robot are hown in detail in the experimental ection. The propoed algorithm can find a feaible olution to aemble two object with completene. It i practical and i ready to be integrated with force control to perform real-world aembly tak like napping [21], crewing, ringto [22], etc. II. Integrated Aembly and Manipulation Planning We olve the aembly planning problem incrementally and recurively uing dynamic regrap graph at the motion level. The ketch of the planning flow i hown in Fig.2. In the firt tep hown in the upper part of the figure, the algorithm elect a bae, compute it placement on the table and check all the order to aembly the econd object to it placement. The output of the firt tep i a et of candidate aembly order. In the econd tep hown in the lower part, the algorithm check each aembly order equentially uing regrap graph. Each object ha an initial configuration and a goal configuration. The algorithm compute all the table placement of the object, connect them with the initial and goal configuration, and build a regrap graph. It earche the regrap graph to ee if there i a direct motion or a equence of regrap motion that the robot can ue to pick up the object from the initial configuration and place it down to the goal configuration. The regrap graph i dynamically built and earched for each object. If the earch fail, the algorithm tart over to chooe a different candidate order or to elect a different bae. Fig. 2. The ketch of the planning flow. The figure can be divided into two tep. In the firt tep, the algorithm elect a bae, compute it placement on the table and check all the method to aembly the econd object to it placement. The elected bae here i the L hape and it table placement are hown in the middle framebox. The output of the firt tep i the candidate et. In the econd tep, algorithm check each aembly order equentially uing dynamic regrap graph. The algorithm connect the initial and goal configuration uing the dynamic regrap graph and earche the grap to find aembly motion. The regrap graph are repeated built for each element in the candidate et and are hown in the bottom framebox. The detail of the algorithm will be explained in Section III and IV. Before that, we lit the ymbol to facilitate reader. p X R X The poition of object X on a plenary urface. We ue A and B to denote the two object and conequently ue p A and p B to denote their poition. It could denote the initial poition or the poition of table placement on a plenary urface, depending on the context. The orientation of object X on a plenary urface.

3 p X a R X a p X R X f Like p X, X i to be replaced by A or B. Like p X, It could denote the initial orientation or the orientation of table placement on a plenary urface, depending on the context. The poition of object X in the aembled tructure local coordinate ytem. The orientation of object X in the aembled tructure local coordinate ytem. The poition of object X in the aembled tructure in the world coordinate ytem. The orientation of object X in the aembled tructure in the world coordinate ytem. The force-cloure grap of object X. The letter f indicate the object i free, and i not in an aembled tructure or laying on omething. The force-cloure grap of object X on a plenary urface. It i aociated with p X and R X. The colliion-free and IK (Invere Kinematic) feaible grap of object X on a plenary urface. It i alo aociated with p X and R X. The force-cloure grap of object X in an aembled tructure, decribed in the world coordinate ytem. It i aociated with p X and R X. The colliion-free and IK (Invere Kinematic) feaible grap of object X in the aembled tructure, decribed in global coordiante ytem. It i aociated with p X and R X. III. The Aembly Planning Apect The given parameter of the ytem before the planning are: (1) The relative poe of the two part, (2) the geometric model, and (3) the poition on the upporting table to do aembly. Moreover, we aume the econd object i aembled to the bae from a direction perpendicular to the urface normal of the upporting table. (1) mean the value of p a B -p a A and R Ba (R a A ) are known. (3) mean the poition of the aembled bae (x a, y a ) on the table are known. By etting p a A a the original point and etting R a A a the orientation of the aembled tructure local coordinate ytem, the variable p a X and R a X can be computed following p B a = p B a p A a, R B a = R Ba (R A a ) (1) p A a = [0, 0, 0], R B a = I (2) We do aembly planning baed on the placement of a bae object. There are two phae in the aembly planning. In the firt phae, the aembly planning elect a bae object (Object A for intance) and compute all it table placement on the table {(p Ap, R Ap )}. For each (p Ap, R Ap ), it compute the aembly candidate C uing: where C = { (p A, R A ), (p B, R B ), (3) ([p B k n t ], R B ) } (4) p A = [x a, y a, p Ap.z + h t ], R A = R Ap (5) p B = p A + p B a, R B = R A R B a (6) and CD(objB(p B, R B, plenary ur f ace) } (7) In thee equation, h t i the height of the plenary urface. It i a contant value. n t i the normal of the plenary urface. (x a, y a ) i the poition on the plenary urface to do aembly. The bae object i aumed to be A. Take the L object in the middle part of Fig.2 for example. It ha five (p Ap, R Ap ) which are plotted in a framebox. (p B, R B ) i the configuration of object B in the aembled tructure in the world coordinate ytem. ([p B k n t ], R B ) i the configuration of object B after retracting along the normal of the plenary urface. CD(objX( p, R), environment) i a function that enure the object objx at poition p and orientation R doen t collided with the environment. The et C i a et of triple where the firt two element correpond to the poe of a placement of the bae object and the poe the econd object aembled to it, the third element i the retraction of the econd object from the bae object along the urface normal of the upporting table. The econd object i enured to not collide with the environment. In the econd phae, the aembly planning interact with regrap planning and loop through all element in C and different bae to find the feaible olution to aemble the object. Firt, we et the two object at free pace and compute the force-cloured and colliion-free grap. Each grap i repreented uing f ={p 0, p 1, R} where p 0 and p 1 are the contact poition of the finger tip, R i the orientation of the palm. The whole et i repreented by f, which include many f. Namely, f = { f }. Given a triple in C, the IK-feaible and colliion-free grap that the robot can ue to aemble it i computed following g X = IK ( g X ) CD ( g X, environment & ob ject ) (8) where g X = R X g f X + p X (9) If none of the computed by the three p X and R X in the triple i empty, the triple will be ued to build the regrap graph and do aembly planning. Or ele, our algorithm examine another triple or examine a different placement. IV. The Manipulation Planning Apect In the motion planning level, the algorithm know the initial configuration of the object through viion ytem and the goal configuration of the two object through the et C. It build a regrap graph uing the initial configuration, the goal configuration, and the placement, earche the graph to find a equence of grap, and connect the equence uing Tranition-RRT [23] motion planning. Given the initial configuration or the placement of an object on the plenary urface, ay p X and R X, the IKfeaible and colliion-free grap that the robot can ue to pick up the object i computed following = IK ( ) CD (, planery ur f ace ) (10)

4 where = R X g f X + p X (11) Here the equation (10-11) are imilar to the one in (8-9). Fig. 4. The grap graph built uing g X and g p(g) X. It ha three layer where the top layer encode the grap aociated with the initial configuration, the middle layer encode the grap aociated with placement on planery uface, and the bottom layer encode the grap aociated with the aemble poe. The left image how one g p X (g) (the virtual grap illutrated in cyan). It correpond to a node in the bottom layer. The ubimage in the frame box illutrate the placement (yellow) and their aociated grap (green). Fig. 3. The flow of the motion planning. Given the initial and goal poe of an object (the yellow block), we earch it available initial and goal grap and ue the common grap and IK to get the initial and goal configuration of the robot arm. Then, we do motion planning repeatedly between the initial and goal configuration to find a olution to the deired tak. The green hand denote acceible grap. The red one and blue one denote the colliding grap and the IK-infeaible grap. The pink color denote robot motion. The motion planning build a graph uing the element in g X and g X. Fig.3 how the flow. The object X in thi graph i a wooden block hown in the left image of the upper-left frame box. The image alo how the poe of thi object on the planery urface, p X and R X. When the object i aembled in the tructure, it poe p X and R X i hown in the left image of the bottom-left frame box. The grap aociated with the poe are hown in the right image of the two frame boxe. They are rendered uing the colored hand model where green, blue, and red denote IKfeaible and colliion free, IK-infeaible, and collided grap repectively. We build a graph to find the common grap and employ Tranition-RRT to find the motion between the initial configuration and goal configuration. Note that the planning i done between the grap aociated with the initial configuration and the third element of the triple in C. Directly planning to the grap aociated with the firt element i a narrow paage problem [24][25] and hould be avoided. Fig.4 how the regrap graph. It i baically the ame a [20], but ha three layer. The top layer ha only one circle and i mapped to the initial configuration. The bottom layer alo ha only one circle and i mapped to the goal configuration. The middle layer are compoed of everal circle where each of them map a poible placement on a planar urface. Each node of the circle repreent a grap: The one in circle of the upper layer are from g X, and the one in the circle of the bottom layer are from g X. The one from the circle in the middle layer are the grap aociated with the placement. The orientation of the placement are evenly ampled online. Their poition are fixed to the initial poition p A. If the circle hare ome grap (grap with the ame p 0, p 1, R value in the object local coordinate ytem), we connect them at the correpondent node. We earch the graph to find the initial and goal configuration and a equence of high-level keyframe, and perform motion planning between the keyframe to perform deired tak. If no keyframe were found, we try a different triple in C or ue a different baed. The detail of the earching and recuring flow are ummarized in Fig.5. For each element in the candidate et, the algorithm build the regrap graph and perform motion planning. If the planning ucceed, the algorithm report found, other wie it trie a different element from the candidate et. If all element in the candidate et are viited, the algorithm trie a different bae. Exemplary reult will be hown in the experiment ection. Fig. 5. Detail of the earching and recuring flow. The algorithm ue each element in the candidate et to do motion planning. If fail, it trie a different element or a different poe until all poibility i teted. V. Experiment and Analyi We perform both imulation and real-world experiment to analyze the integrated aembly and motion planning

5 Fig. 6. Two failure cae during the recurive earching. Thee two cae ue the firt and econd element in the C of the L bae to build the graph and do motion planning. Both of them failed ince there i no acceible grap to place down the econd object at the goal. Fig. 7. The ucceful plan. The third element in the C of the L bae lead to a ucceful plan. The imulated motion equence are hown in (1-8). The correpondent regrap graph and the path on the graph are hown in (1-8 ). (1-8 ) how the correpondent robot execution. The earching proce took about 30 econd on the Xeon E CPU. and demontrate the applicability of the algorithm. The computer ytem ued to compute the grap and motion i a Dell T7910 worktation (Proceor: Intel Xeon E v3 with 8CHT, 20MB Cache, and 2.4GHz Clock, Memory: 32G 2133MHz DDR4). The relative poe between the two object are obtained in advance uing a teaching ytem1. Fig.6 how the reult of the dynamic graph building and the recurive earching. In the upper row of thi figure, the algorithm elect the L object a the bae and trie the aembly order hown in the left in aembly planning, and build the regrap graph and earche it to do motion planning in (1-6). The algorithm check the IK-feaible and colliion free grap (the green plot) aociated with the initial and goal configuration, and ue them to build the regrap graph in (1). Then, the algorithm earche the graph and doe motion planning to tranfer the L object from the initial configuration to the goal in (2-4). In (5-6), the robot check the aembly tructure, and the IK-feaible and colliion free grap aociated with the econd object. Since 1 The baic principle i human being teach the relative poe of the two object in front of a camera uing AR marker. The detail can be found in [26]. Reader may aume the relative poe to be known. there i no acceible grap (no green plot) aociated with the goal configuration, the robot cannot build the third layer of the graph and cannot find a path. It report failure and requet to ue a different aembly order. In the lower row, the algorithm ue the ame bae but trie a different aembly order in aembly planning, and build and earche the graph and perform motion planning in (7-12). Like (1), the algorithm check the grap and build a graph in (7). Then, it earche the graph, perform motion planning, and uccefully find a way to tranfer the firt object to the goal. In (11-12), the algorithm check the grap of the econd object and found all grap at the goal are not available. It report failure and requet another aembly order. The proce continue incrementally until the algorithm a olution i found or failure i reported. Fig.7 how a ucceful cae. In thi cae, the robot trie a different aembly order. Like the flow in Fig.6, the robot compute the grap aociated with the initial configuration and goal configuration of the firt object in (1). The available grap are rendered in green. They correpond to the top and bottom layer of the grap hown in (1 ). In (2), the robot chooe one feaible (IK-feaible and colliion-free) grap 178

6 from the aociated grap and doe motion planning to pick up the object. The elected grap correpond to one node in the top layer of the graph, which i marked with red color in (2 ). In (3), the robot pick up object A and tranfer it to the goal poe uing a econd motion planning. Thi correpond to an edge in (3 ) which connect the node in one circle to the node in another. The edge directly connect to the goal in thi example and there i no intermediate placement. After that, the robot move it arm back at (4), which correpond to a node in the bottom layer of the graph hown in (4 ). In (5), the robot compute the grap aociated with the initial and goal configuration of the econd object. The available grap are rendered in green and correpond to the top and bottom layer of the grap hown in (5 ). Both initial and goal configuration have aociated grap, and it i poible to build the regrap graph for the econd object thi time. In (6), the robot chooe one feaible grap and doe motion planning to pick up the object. The elected grap correpond to the marked node in (6 ). In (7), the robot pick up object B and aemble it to the goal poe uing a econd motion planning which correpond to an edge in (7 ). Finally, the robot move it arm back at (8) and (8 ). The ubfigure (2 )-(4 ) and (6 )-(8 ) in the third row how how the robot execute the planned motion. They correpond to (2)-(4), (6)-(8) and (2 )-(4 ), (6 )-(8 ) in the firt two row. The whole proce are divided at 4/4 /4 ) and (5/5 /6 ) where (1/1 /2-4/4 /4 ) are to pick and place the firt object and (5/5 /6-8/8 /8 ) are to aemble the econd object to the firt one. A video clip that how both the imulation and real-world execution i available online at: VI. Concluion and Future Work Thi paper preented an integrated ingle-arm aembly and motion planning algorithm to recurively find how to aemble two object with the help of a plenary urface a the upporting fixture. The algorithm decide which object to place on the urface firt, how to place the object, and how to aemble the econd object to the firt one. It can find a feaible olution to aemble two object with completene. The propoed algorithm i expected to help robot perform aembly tak automatically, and take the place of the technician who manually pecify the aembly order uing their experience. It i practical and i ready to be integrated with force control to perform real-world aembly tak. The work i kinematic. In the future, we will integrate the algorithm with force control to perform real-world tak. Reference [1] Y. Koga and J.-C. Latombe, Experiment in Dual-arm Manipulation Planning, in Proceeding of IEEE International Conference on Robotic and Automation (ICRA), pp , [2] Y. Koga and J.-C. Latombe, On Multi-Arm Manipulation Planning, in Proceeding of IEEE International Conference on Robotic and Automation (ICRA), pp , [3] L. Kavraki, P. Svetka, J. C. Latombe, and M. Overmar, Probabilitic Roadmap for Path Planning in High-Dimenional Configuration Space, IEEE Tranaction on Robotic and Automation, vol. 12, pp , [4] T. Simeon, J.-P. Laumond, J. Corte, and A. Shbani, Manipulation Planning with Probabilitic Roadmap, International Journal of Robotic Reearch, [5] S. M. Lavalle and J. J. Kuffner, Rapidly-Exploring Random Tree: Progre and Propect, in Proceeding of International Workhop on the Algorithmic Foundation of Robotic, pp , [6] N. Vahrenkamp, D. Berenon, T. Afour, J. Kuffner, and R. Dillmann, Humanoid Motion Planning for Dual-arm Manipulation and Regrap Tak, in Proceeding of IEEE/RSJ International Conference on Intelligent Robot and Sytem (IROS), [7] D. Berenon, P. Abbeel, and K. Goldberg, A Robot Path Planning Framework that Learn from Experience, in Proceeding of IEEE International Conference on Robotic and Automation (ICRA), [8] M. Phillip and M. Likhachev, Speeding up Heuritic Computation in Planning with Experience Graph, in Proceeding of IEEE International Conference on Robotic and Automation (ICRA), [9] L. H. de Mello, AND/OR Graph Repreentation of Aembly Plan, IEEE Tranaction on Robotic and Automation, [10] R. Wilon and J.-C. Latombe, Geometric Reaoning About Mechanical Aembly, Artificial Intelligence, [11] H. I. Bozma and D. E. Koditchek, Aembly a a Noncooperative Game of it Piece: Analyi of 1D Sphere Aemblie, Robotica, vol. 19, no. 01, pp , [12] R. Knepper, T. Layton, J. Romanihin, and D. Ru, IkeaBot: An Autonomou Multi-robot Coordinated Furniture Aembly Sytem, in Proceeding of IEEE International Conference on Robotic and Automation (ICRA), [13] M. Dogar, A. Spielberg, S. Baker, and D. Ru, Multi-Robot Grap Planning for Sequential Aembly Operation, in Proceeding of IEEE International Conferene on Robotic and Automation (ICRA), [14] R. Lioutikov, G. Neumann, G. Maeda, and J. Peter, Probabilitic Segmentation Applied to an Aembly Tak, in Proceeding of International Conference on Humanoid Robot (Humanoid), [15] J.-P. Saut, M. Gharbi, J. Corte, D. Sidobre, and T. Simeon, Planning Pick-and-place Tak with Two-hand Regraping, in Proceeding of IEEE/RSJ International Conference on Intelligent Robot and Sytem (IROS), [16] J. King, M. Klingenmith, and C. Dellin, Regrap Manipulation a Trajectory Optimization, in Proceeding of Robotic: Science and Sytem (RSS), [17] T. Lozano-Perez and L. P. Kaelbling, A Contraint-baed Method for Solving Sequential Manipulation Planning Problem, in Proceeding of IEEE/RSJ International Conference on Robot and Sytem (IROS), [18] S. Nedunuri, S. Prabhu, and M. Moll, SMT-baed Synthei of Integrated Tak and Motion Plan from Plan Outline, in Proceeding of IEEE International Conference on Robotic and Automation (ICRA), [19] J. Bidot, L. Karlon, F. Lagriffoul, and A. Saffiotti, Geometric Backtracking for Combined Tak and Motion Planning in Robotic ytem, Artificial Intelligence, [20] W. Wan and K. Harada, Developing and Comparing Single-arm and Dual-arm Regrap, IEEE Robotic and Automation Letter, [21] J. Roja, K. Harada, and H. Onda, A Relative-change-baed Hierarchical Taxonomy for Cantilever-nap Aembly Verification, Proceeding of IEEE/RSJ International Conference on Intelligent Robot and Sytem (IROS), [22] I. G. Ramirez-Aplizar, K. Harada, and E. Yohida, Motion Planning for Dual-arm Aembly of Ring-haped Elatic Object, in Proceeding of IEEE International Conference on Humanoid Robot, [23] L. Jaillet, J. Corte, and T. Simeon, Tranition-baed RRT for Path Planning in Continuou Cot Space, in Proceeding of IEEE/RSJ International Conference on Intelligent Robot and Sytem (IROS), [24] D. Hu, T. Jiang, J. Reif, and Z. Sun, The Bridge Tet for Sampling Narrow Paage with Probabilitic Roadmap Planner, in Proceeding of IEEE International Conference on Robotic and Automation (ICRA), [25] H. Liu, D. Ding, and W. Wan, Predictive Model for Path Planning uing K-near Dynamic Brdige Builder and Inner Parzen Window, in Proceeding of IEEE/RSJ International Conference on Intelligent Robot and Sytem (IROS), [26] W. Wan, F. Lu, Z. Wu, and K. Harada, Teaching Robot to Do Object Aembly uing Multi-modal 3D Viion, ArXiv e-print, 2016.