An Iterative Diffusion Algorithm for Part Disassembly - CiteSeerX

An Iterative Diffusion Algorithm for Part Disassembly Etienne Ferré

Jean-Paul Laumond

Kineo CAM BP 27-01, 31312 Labege, France [email protected]

LAAS-CNRS 7 avenue du Colonel Roche, 31077 Toulouse, France [email protected]

Abstract— The paper presents an efficient collision-free path planner for mechanical part disassembly simulation. The principle of the algorithm consists in computing a first path while allowing some penetration within the obstacles. Then the current paths are iteratively re-shaped by decreasing the allowed penetration threshold. The process is controlled by a dynamic collision checker. The cases of failure of the iterative process are automatically detected and solved.

I. P ROBLEM STATEMENT,

RELATED WORK AND CONTRIBUTION

Is the windscreen wiper motor well designed to be mounted within the car? How easy will be the maintainability of the driver sit? How to take advantage of the existing numerical data and 3D CAD models to reduce the costs of the manufacturing process at any stage of product lifecycle? PLM (Product Lifecycle Management) is today an active sector of process simulation aiming at providing software solutions to problems formerly addressed from costly physical mockups. With the current stage of the technology offered by software providers it appears that part disassembly path simulations take several tedious hours of an experienced operator even with the help of efficient geometric tools (e.g., collision-checkers and automated local path computations). Difficult problems such as the two benchmarks presented in this paper take few days to be solved1 . This paper aims at providing an efficient and practical solution to the part disassembly problem. Efficiency should be evaluated in terms of computation time and solution quality. Practicality requires a solution easy to use for non expert operators: the solution should not impose any parameter tuning. The paper benefits from the current active research area in probabilistic motion planning. It gathers and improves several published ideas in an integrated solution giving rise to an original parameter free algorithm for part disassembly. After the pioneering work [3] that combines random walks and gradient descents in the configuration space (CS), two kinds of CS search paradigms are investigated with success. The sampling approaches introduced in [13] consist in computing a so-called roadmap whose nodes are collision free configurations chosen at random and whose edges model the existence of collision free local paths between two nodes. 1 Both benchmarks have been submitted by the automotive French company Renault.

Sampling approaches aim at capturing the topology of the collision free configuration space (CS f ree ) both in terms of covering and connectiveness in a learning phase. The diffusion approaches introduced in [9], [14] consist in solving single queries by developing a tree rooted at the start configuration towards the goal to be reached. How to sample or diffuse within CS f ree efficiently? Such questions give rise to numerous variants of the original algorithms (e.g., [2], [5]–[8], [10]– [12], [15]–[17], [19], [21], [24]). As part disassembly is concerned, diffusion techniques clearly behave better than random sampling ones. Indeed, the solution space of a disassembly problem has the shape of a long thin tube. Random sampling within a tube requires a high density of points while tree diffusion benefits from the shape of the tube that naturally steers the diffusion process. On the basis of this simple statement, several technical issues remain to be solved. How to control the diffusion process? For efficiency purpose, it should progress fast in empty rooms and more slowly in constrained spaces. How to guarantee the safeness of the solution path under an imposed user defined clearance threshold? Such a constraint depends on the problem and should be automatically taken into account by the method. This paper addresses all these issues. The principle of the algorithm is illustrated in Figure 4. A first path is computed while allowing some penetration within the obstacles. Then the current paths are iteratively re-shaped by decreasing the allowed penetration threshold. The cases of failure of the iterative process are automatically detected and solved (see Figure 6). Our approach benefits from several sources and ideas already explored as such but never combined: • As collision checking is concerned, a critical problem is to perform efficient collision checking not only for configurations (see overviews in [18], [22]) but also for local paths. Exact collision checking along computed paths has been recently addressed in [20]: path collision checking is performed with a static collision checker while the (usually costly) iterative process is speeded thanks to distance computations. Here we show how the approach may be adapted to account for user-defined imposed clearance constraint. • To overcome the expansive cost of configuration and path collision checking, some approaches have been defined to

put back the tests and then to avoid useless computations. This is the case of the lazy approaches [6], [19] where the algorithms put back collision checking as long as the probability of failure is high. The algorithm presented in this paper starts from a rough solution path and iteratively refines it. The iterative procedure is based on an original penetration distance control. When the refinement procedure fails (i.e. when the current path cannot be locally re-shaped into a collision-free one), then the search re-starts with a roadmap composed of the portions of the path that are collision-free. That kind of procedure has been recently introduced in [1] to improve the connectiveness of roadmaps. • Another key point is the control of the diffusion process: how to steer the diffusion process without introducing useless side effects? For instance, defining a new diffusion direction at random by fixing a new configuration goal (as in [14]) gives rise to a bias in introducing implicit bounding boxes on the translation parameters. This problem has been already noticed as critical for assembly problems in [9]. The solution in [9] depends on a local grid whose resolution appears as a parameter to be tuned. The solution we propose is parameter free. • Finally our refinement procedure for path reshaping follows the same idea as the variation approach (introduced in [4]) where the search is performed by iteratively growing formerly shrunk obstacles. In our approach, the growing process is automatically controlled. Moreover the failures due to the closure of passages at some stage of the growing are automatically solved. In the following section we describe our dynamic collision checker that allows to account for any effective clearance imposed by the user on the desired solution path. Then Section III focuses on the diffusion process. The iterative algorithm is presented in Section IV. We conclude on experimental results obtained from two industrial benchmarks. II. DYNAMIC C OLLISION C HECKING A. Importance of the Operator Any path planner requires a procedure to check whether a path is collision free or not. This operation is crucial, especially for part disassembling. In this case, the solution path is very close to the obstacles and the algorithm must guarantee the safeness of the result. This question is solved by the following Dynamic Collision Checker (DCC). B. Commonly Use Approach The commonly used approach is the test of discrete points up to a fixed resolution ∆s. If each point is tested collision free, the algorithm answers that the whole path is collision free: (∀i, q(i × ∆s) ∈ CS f ree ) ⇒ path ⊂ CS f ree The main drawback of this solution is the non guaranty of collision avoidance. Hence, even if no tested points on the path are colliding, pieces of the path in between are not checked and

Fig. 1. Penetration control discretization: the step in CS is computed with respect to the maximal allowed penetration in the workspace

a collision may occur. Moreover this collision is not quantified: it can be just a small touch of the part or the part traversing thought a wall. Finally, the fixed resolution is an abstract value and may be hard to well choose. C. Parasitic Collision Control Our DCC method is similar to [20]. Even if it is impossible to avoid collisions by testing discrete points on the path, the collision can nevertheless be quantified. Hence, our Dynamic Collision Checker computes the step between two tested points relative to the maximal swept motion of points of the moving object. The input parameter is then a distance in the workspace and the step ∆si between two tested points is computed in such a way that no points of the object move along a distance larger than 2. Once the step has been computed, the part is moved to the corresponding point on the path and a collision test is performed: (∀i, q(si ) ∈ CS f ree ) ⇒ path ⊂ CS f ree si = si−1 + f (q(si−1 , )) The function f is determined is such a way that (if A is the set of points of the part): Z si+1 dP (q(s))kds < 2 ∀P ∈ A/ k ds si The method does not avoid the problem of potential collisions between two tested points (Figure 1). Nevertheless if a collision occurs, it can now be quantified. Indeed, in the worst case, the traversed distance of one point is 2. Then the penetration inside obstacles is bounded by (Figure 2). This point can so penetrate an obstacle of and must go back because the next tested point is collision free. The resulting penetration can then be controlled and bounded by a so-called ”dynamic penetration” user-defined parameter . D. Path Quality Control This Dynamic Collision Checker algorithm fully controls the path quality. By setting the dynamic penetration to a large value, the path is authorized to penetrate the obstacle. It can be a way to shrink the obstacle in order to make the path planning problem easier to solve. This idea is the core of the iterative diffusion described in IV.

Fig. 2. Penetration control discretization: the control of the motion in the workspace bounds the parasite penetration in case of collision between two tested points.

Finally, as we control the maximal penetration along the path, the collision can be maintained outside the real obstacle by setting a static tolerance of the Collision Checker to a value larger than the dynamic penetration. If the distance between the body and the obstacles is smaller than the static tolerance, the configuration is considered colliding as if the obstacles were virtually enlarged. Thus, when a collision appears due to the parasitic penetration side effect, the movable part penetrates the enlarged area keeping the real obstacles safe. The path is then guaranteed to be continuously collision free. Moreover, the algorithm assures that the path is not closer to the obstacles than the so-called effective clearance which is imposed by the user and which appears as the difference between the static tolerance and the dynamic penetration. Notice that the approach is applicable as soon as the collisionchecker integrates the notion of static tolerance. This is the case of almost all the marketed collision-checkers. III. ROADMAP N EIGHBORHOOD O RIENTED RRT- BASED DIFFUSION

A. Part Disassembling Problem The part disassembling problem is specific: the starting configuration is the mounted position in a highly constrained area inside the mechanical assembly and the ending configuration lies in a free area outside the assembly. The goal is then to map out the neighborhood of the starting configuration because the difficulty of the problem is focused on this area. It is useless to explore the whole configuration space. Hence, diffusion is the most adapted approach for path planning in this context. Moreover, a simple diffusion instead of a double diffusion is more suitable as the tree diffusing from the goal configuration needlessly explores a free and large space area. B. Translation and Rotation Behavior Discrepancy The RRT algorithm [14] is one of the most efficient diffusion algorithm. Some adaptations have been made to improve the performances of the RRT on the part disassembling problem. In the classical RRT, one shots a configuration at random in the configuration space and tries to extend the exploring tree toward this configuration. However, this sampling technique implicitly introduces the bounds of the degrees of freedom

Fig. 3.

Roadmap neighborhood oriented sampling.

(DOF) as new parameters. The part has indeed six DOFs, three for the position and three for the attitude of the rigid body. Whereas the rotating DOFs are not bounded, the algorithm is very sensitive to the bounds of the translation. If they are too large, the algorithm tries to link one node of the tree to a very distant position of the part. Locally in the neighborhood of the tree where the problem is constrained, the rotations have no influence and the motion appears as a translation. As a result, the tree is diffusing with only translating motions whereas the part has to rotate to escape from the narrow passage.

C. Roadmap Neighborhood Oriented Sampling The randomization has then been adapted to focus the sampling on the neighborhood of the tree where the problem is. Our method is similar to the method used in [9]. One existing node is chosen at random with a uniform distribution and the extending configuration is shot according to a normal distribution (Figure 3). The parameters of this distribution are dynamically tuned during the search: they are implemented as an increasing function of the diffusion process velocity (they do not depend on a grid as in [9]). As an effect, the diffusion process then covers empty rooms quickly and faster than the classical RRT approaches (see Figures 5 and 6).

D. Making Grow the RRT with a Former Roadmap Whereas the classical RRT deals with no more than two trees, our modified RRT can take as input a roadmap with several connected components. When a node is added to the tree, the algorithm explores each connected component, finds the nearest node and tries to link both nodes (see algorithm below). In case of success, the connected components are merged and the search continues until the starting configuration and the ending one are in the same component. This light modification allows using a former roadmap to speed up the progression of the tree exploration.

Algorithm 1 Tree expansion: RNOS-RRT Tree T //tree to be developed Nrand ← chooseNodeAtRandom(T ) qrand ← shootNearNode(Nrand ) Nnear ← getNearestNode(T , qrand ) Nnew ← extendTree(Nnear ,qrand ) if Nnew VALID then addNodeToTree(Nnew ,T ) for i = 1 to ntree do linkTree(Nnew , Ti ) //link to the other trees end for end if

(a) Start and Goal

(b) Basic RRT behavior

Fig. 5. Extraction of a simple translating square: Basic RRT fails after more than 10000 nodes inserted and 1224s.

IV. I TERATIVE D IFFUSION A LGORITHM A. Diffusion in narrow tunnels The diffusion in a narrow tunnel is often problematic. Most of the time, the range of the progression directions is limited and the probability to shoot the right direction is almost null. Moreover, diffusion algorithms have difficulty in finding narrow passages because they spend a lot of time to explore the free area while the problem is focused on a small fraction of the configuration space. Hence, the classical RRT often fails to find a solution for part disassembling in a reasonable running time despite its probabilistic completeness. One solution is to enlarge the tunnel in order to find a draft solution and then to refine the path by removing the collisions. Some incremental algorithms has been proposed [19]. Most of these algorithms solve the problem thanks to grid techniques with the side effect of adding several parameters that may be hard to well set. Here, we propose a new iterative algorithm dedicated to part disassembling with no sensitive parameters. B. Iterative Diffusion Algorithm 2 Iterative Path Planner roadmap G ← ∅ dynamic penetration δ ← distance(qinit , qgoal ) while δ > do path Γ ← diffuseRRT(qinit ,qgoal ,G) δ ←δ÷α G ← removeCollidingPart(Γ) end while path Γ ← diffuseRRT(qinit ,qgoal ,G) Our iterative path planner uses both algorithms described in II and III. The algorithm is illustrated in Figure 4. Thanks to the penetration control, we enlarge the narrow passage by setting a very large dynamic penetration value and we find a draft path with the dedicated RRT 4(a). This draft solution may be colliding due to the dynamic penetration effect but it represents a good approximation of the final path. Once this path is found, the dynamic penetration δ is decreased by a given scale factor α. All the local paths are then retested and the colliding ones (according to the new dynamic penetration)

are removed from the path 4(b). The result is a set of collision free nodes linked together with collision free edges. This object is then a roadmap given as input to a second run of a RRT. This second run is strongly speeded up thanks to the progressive merge of the former connected components during the search (Figure 4(c)). This iteration is repeated until the dynamic penetration reaches the user-defined value. V. E XPERIMENTAL R ESULTS All experiments were performed on a 2GHz Pentium IV PC with 512 MB RAM. The algorithm has been implemented in KineoWorksTM2 . The collision tests were performed using KCD [23]. A. Simple Example: Translating Square The first experiment (Figure 5(a)) deals with a simple translating square with 2 DOFs. The environment contains a narrow passage. After the insertion of 10000 nodes, the classical RRT fails to find the entrance of the tunnel; it only explores the free area of the interior of the cage (Figure 6(a)). The iterative path planner finds a solution in 3.89s. In this example, the first search is inefficient (Figure 6(a)). Because of the thinness of the cage wall, the first dynamic penetration value make the RRT diffuse through the obstacle. The second dynamic penetration value is small enough to prevent the wall crossing. The second search (Figure 6(b)) finds a good draft path even if some collisions remain inside the tunnel. The third diffusion uses this lead to find a collision free path. B. Industrial Use-cases Both next examples are real use-cases from the automotive industry. The problem is to find a collision free path dismounting a part of a car to check the manufacturing process or the maintainability of the assembly. The first one (Figure 7) proves the maintainability of the windscreen wiper motor and the second one (Figure 8) proves the process of assembling a seat inside the car body. Each example is solved within few minutes. The results are exposed in Table I. Each running time is a mean of 100 runs. For each example, the first line indicates the running time 2 KineoWorks is the path planning dedicated Software Development Kit developed by Kineo CAM.

Fig. 4.

(a) Step 1a

(b) Step 1b

(c) Step 2a

(d) Step 2b

(e) Step 3a

(f) Path

Iterative Diffusion: each iteration is a RRT diffusion and a path to roadmap conversion with a reduced dynamic penetration value.

(a) Step 1, 126 nodes

(b) Step 2, 164 nodes

(c) Step 3, 15 nodes

Fig. 6. Extraction of a simple translating square from a 2D cage. The path is computed in 3.89s: Step 1 is useless due to the thinness of the walls, Step 2 finds a first good draft solution (collisions remain at the entrance of the tunnel), Step 3 finds a collision free path

with a null static tolerance and the second line with a static tolerance equal to the dynamic penetration assuring a null effective clearance and so the guaranty of non-collision for the resulting path. Setting a non-null value to the static tolerance often slows down the resolution. Indeed, the enlarged obstacles narrow the passage. This is the reason why the problem is more difficult and the running time longer. Movies of the simulations are available on www.kineocam.com.

TABLE I RESULTS ON INDUSTRIAL USE - CASES

Nb of facets

penetration

tolerance

time

Example 1

26504

0.5mm 0.5mm

0mm 0.5mm

47.42s 90.32s

Example 2

245638

5mm 5mm

0mm 5mm

149.39s 414.43s

VI. C ONCLUSION Both examples above have been submitted by Renault as difficult problems. In the windscreen wiper example, there is no solution path with a clearance greater than 1mm (the operator never found any solution with the current available marketed CAD tools). This threshold is 10mm for the sit example (a solution was found ’by hand’ in two days).

Our algorithm can be implemented within any CAD software as soon as its collision checker provides a static tolerance. Then the only parameter the operator has to enter is the desired effective clearance he wants. The solution path found by our algorithm is guaranteed to be exactly safe with respect to this clearance. Finally our approach inherits from the probabilistic

(a) Mounted Position

Fig. 7.

(b) Dismounted Position

Example 1: path disassembling a wiper motor from the car body.

(a) Mounted Position

Fig. 8.

(b) Dismounted Position

Example 2: path disassembling a seat from the car body.

completeness of the general paradigms it comes from. ACKNOWLEDGMENT The authors would like to thank Renault for permitting the publishing of the use-cases. This work has been partially funded by MOVIE, a project from the Information Society Technologies (IST) programme from the European Community under contract number: IST-2001-39250. R EFERENCES [1] M. Akinc, K. Bekris, B. Chen, A. Ladd, E. Plakue, L. Kavraki. Probabilistic Roadmaps of Trees for Parallel Computation of Multiple Query Roadmaps. Proceedings of Eleventh International Symposium on Robotics Research (Siena, Italy), October 2003.

[2] N.M. Amato, O. Bayazit, L. Dale, C. Jones, D. Vallejo. OBPRM: an Obstacle-Based PRM for 3D Workspaces. In Robotics: The Algorithmic Perspective (WAFR98), P. Agarwal et al. (Eds). AK Peters, 1998. [3] J. Barraquand, J.C. Latombe. Robot Motion Planning: a distributed representation approach. International Journal of Robotics Research, 10 (6), 1991. [4] J. Barraquand, P. Ferbach. A Penalty Function Method for Constrained Motion Planning. In IEEE Int. Conf. on Robotics and Automation, pages 1341–1348, 1994. [5] P. Bessière, J.M. Ahuactzin, E.-G. Talbi, E. Mazer. The ”Ariadne’s Clew” Algorithm: Global Planning with Local Methods. IEEE Internationnal Conference on Intelligent Robots and Systems (IROS), vol. 2, pages 1373–1380, 1993. [6] R. Bohlin, L.E. Kavraki. Path Planning using Lazy PRM. Proc. IEEE International Conference on Robotics and Automation, pages 521–528, 2000.

[7] V. Boor, M.H. Overmars, A.F. van der Stappen. The Gaussian Sampling Strategy for Probabibilistic Roadmap Planners. Proc. IEEE International Conference on Robotics and Automation, pages 1018–1023, 1999. [8] P. Cheng, S.M. LaValle. Resolution Complete Rapidly-Exploring Random Trees. IEEE International Conference on Robotics and Automation, 2002. [9] D. Hsu, J.C. Latombe and R. Motwani. Path Planning in Expansive Configuration Spaces. Int. J. Computer. Geom. and Appli., 4:495-512, 1999. [10] D. Hsu, J.-C. Latombe, R. Motwani. Path Planning in Expansive Configuration Spaces. IEEE International Conference on Robotics and Automation, vol. 3, pages 2719–2726, 1997. [11] D. Hsu, L.Kavraki, J.-C. Latombe, R. Motwani, S. Sorkin. On Finding Narrow Passage with Probabilistic Roadmap Planner. In Robotics: The Algorithmic Perspective (WAFR98), P. Agarwal, al (Eds). AK Peters, 1998. [12] L. Kavraki, J.-C. Latombe, R. Motwani, P. Raghavan. Randomized Query Processing in Robot Motion Planning. ACM Symposium on Theory of Computing, pages 353–362, 1995. [13] L. Kavraki, P. Svestka, J.-C. Latombe, M. Overmars. Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces. IEEE Trans. on Robotics and Automation, 12 (4), 1996. [14] J. J. Kuffner and S. M. LaValle. RRT-connect: An efficient approach to single-query path planning. Proc. IEEE Int’l Conf. on Robotics and Automation , pages 995-1001, 2000. [15] J.-P. Laumond, T. Siméon. Notes on Visibility Roadmaps and Path Planning. Workshop on the Algorithmic Foundations of robotics.(WAFR’00), Hannover (USA), pages 67–77, March 2000. [16] S.M. LaValle, J.J. Kuffner. Rapidly-Exploring Random Trees: Progress and Prospects. Workshop on the Algorithmic Foundations of Robotics, 2000. [17] S.M. LaValle, M.S. Branicky. On the relationship between classical grid search and probabilistic roadmaps. Workshop on the Algorithmic Foundations of Robotics, December 2002. [18] M. Lin, D. Manocha, J. Cohen, S. Gottschalk. Collision detection: algorithms and applications. in Algorithms for Robotic Motion and Manipulation (WAFR’96), J.-P. Laumond and M. Overmars Eds, A.K. Peters, 1996. [19] G. Sanchez and J.C. Latombe. A Single-Query bidirectionnal Probabilistic Roadmap Planner with Lazy Collision Checking. Int. Symposium on Robotics Research (ISRR’01), Lorne, Victoria, Australia,2001. [20] F. Schwarzer, M. Saha and J. C. Latombe. Exact Collision Checking of Robots Paths. Workshop on Algorithmic Foundations of Robotics, Nice, France, 2002 (to be published by Springer Verlag). [21] T. Siméon, J.-P. Laumond, C. Nissoux. Visibility Based Probabilistic Roadmaps for Motion Planning. Advanced Robotics Journal, 14 (6), 2000. [22] P. Jimenez, F. Thomas, C. torras. 3D collision detection: a survey. Computers and Graphics, 25 (2), 2001. [23] C. Van Geem, T. Siméon. KCD: a collision detector for path planning in factory models. Rapport technique, LAAS-CNRS N¡01073, Mars 2001. [24] S.A. Wilmarth, N.M. Amato, P.F. Stiller. Motion Planning for a Rigid Body Using Random Networks on the Medial Axes of the Free Space. ACM Symposium on Computational Geometry, pages 173–180, 1999.