Path Planning of 5-DOF Manipulator Based on Maximum Mobility

Transcription

1 INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 15, No. 1, pp JANUARY 2014 / 45 DOI: /s Path Planning of 5-DOF Manipulator Based on Maximum Mobility Hu Chen 1 and Jangmyung Lee 1,# 1 School of Electrical Engineering, Pusan National University, Jangjeon2-dong, Geumjeong-gu, Busan, South Korea, # Corresponding Author / jmlee@pusan.ac.kr, TEL: , FAX: KEYWORDS: 5-DOF humanoid arm, End effector mobility, Inverse kinematics solver, Path planning In this paper, a new path-planning algorithm is proposed based on inverse kinematics (IK) and end effector maximum mobility direction. As a fundamental tool, the IK is used in many fields, forexample, path planning. In order to find the manipulator configuration, first, a 5-DOF humanoid manipulator IK solver is obtained and verified. According to Yoshikawa s manipulability definition, the mobility of the end effector in a desired direction can be measured. If the end effector moves along the maximum mobility direction, the motion generation efficiency in the Cartesian space is optimal. However, the maximum mobility direction is defined locally for the current configuration of the manipulator. If the start configuration and the traveling time are pre-determined, the path can be determined uniquely, and the end effector may not reach the precise goal position. Therefore, a unit direction vector is used for revising the direction and making the end effector move close to the goal position. The simulation results demonstrate the effectiveness of this path-planning algorithm. Manuscript received: December 7, 2012 / Accepted: November 11, 2013 NOMENCLATURE i = frame number i-1 T i = transformation matrix, from frame i-1 to frame i O ij = element of orientation of end effector p x,y,z = position of end effector θ i = joint variable φ = roll angle γ = pitch angle ψ = yaw angle J = Jacobian matrix in Cartesian space P n = current configuration position P n1 = next configuration position with U 1 P n2 = next configuration position with U 1 and U 2 P g = goal configuration position U 1 = direction vector of the maximum mobility U 2 = direction vector toward the goal position from P n1 w 1, w 2 = weight values l m = straight-line distance from P n1 to goal position l n = straight-line distance from P n to goal position O 0 = start configuration orientation O n = next configuration orientation O g = goal configuration orientation s i = sin(θ i ) c i = cos(θ i ) 1. Introduction Multi DOF manipulators are required in many fields. Robot manipulators werefirst used in industrial manufacturing and have developed quickly since the late 1950s. Because they are required to have a highlevel of intelligence and automation, service robot manipulators have been the focus of research in recent years. There are several topics related to robot manipulators, such as path planning and motion and cooperative control, and many research studies have been carried out in the USA and Japan in these areas. As an important aspect of robot engineering, path-planning problems have been studied for many years. The A* search, 1 genetic algorithm (GA) search, 2-4 and co-evolutionary GA (CGA) are compared in. 5 The authors present a path-planning approach using CGA for finding the KSPE and Springer 2014

2 46 / JANUARY 2014 INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 15, No. 1 minimum distance and collision-free path of two cooperative construction manipulators. A newly developed path planning algorithm, the backtrack-free path-planning algorithm (BFA) is explained and compared with the probabilistic roadmap method (PRM) in, 6-8 and used for the path planning of cooperating multi manipulators. The success of PRM planning depends mainly and critically on favorable visibility conditions in free space. In, 9 the authors present an algorithm named lazy PRM. This algorithm is very useful in high dimensional, relatively uncluttered configuration spaces. In, 10,11 the rapidlyexploring random tree (RRT) is introduced. The advantage of RRT is that it can be applied directly to nonholonomicand kinodynamicplanning. The authors present a transpose directed rapidly exploring random trees (JT-RRTs) in; 12,13 this algorithm does not need inverse kinematics (IK). A workspace goal heuristic function and RRTs are used for path planning based on an IK solver in. 14 A disjunctive program is introduced in 15 and the authors present an algorithm based on obstacle avoidance and kinematic and dynamic constraints. The potential field method is introduced in 16,17 and improved using PRM in. 18 The PRM can generate a collision-free path and the potential field method can determine the configuration of the manipulator. Moreover, the adapted randomized gradient descent (RGD) method and the alternate task-space and configuration-space exploration (ATACE) method are compared in. 19 These algorithms are used for finding the minimum distance and collision-free path when obstacles exist. However, they do not consider the mobility of the end effector or the maximum mobility direction of the manipulators themselves. The mobility and mobility direction of the end effector can be measured according to Yoshikawa s manipulability definition in. 20 After that, a new algorithm for measuring and optimizing the manipulability measure using SVD method is proposed in 21,22 and the authors gave some case studies. In this paper, the path-planning problem of moving the end effector from the start to the goal configuration in an environment without any obstacles has been addressed, considering the maximum mobility direction of the end effector, based on an IK solver. The remainder of this paper is organized as follows. A kinematic model is built in Section 2.1. The IK solver is generated in Section 2.2. The maximum mobility direction vector is obtained according to a singular value decomposition of a Jacobian matrix in Section 3.1. The next configuration position and orientation are designed in Sections 3.2 and 3.3. The path-planning algorithm is defined in Section 3.4, and finally, the path-planning algorithm, verified through MATLAB simulations, is shown in Section 4. Conclusions are presented in Section 5. Fig. 1 Dual arm manipulator Fig. 2 5-DOF manipulator model Table 1 DH parameters i θ (degree) d (mm) a (mm) α (degree) Joint limit (degree) 1 θ ~ θ ~ θ ~ θ ~ θ ~ Kinematic Model 2.1 Forward kinematics (FK) For given joint variables, the position and orientation of the manipulator end effector can be obtained by coordinate transformation. As shown in Fig. 1, there are five DOF in each arm of a dual arm manipulator. Only the left arm of the dual arm manipulator is modeled in this study. Fig. 2 illustrates the 5-DOF manipulator model. The frame of each joint is assigned to satisfy the Denavit-Hartenberg convention. The DH parameters are shown in Table 1. The relation between two consecutive frames i-1 and i can be described using the homogeneous transformation matrix i 1 i T = c θ i c α i s θi s αi s θi ac θ i s θi c α i c θi s α i c θi as θi 0 s α i c α i d i where c and s correspond to cosine and sine, respectively, and θ, d, a, and α are the modified DH parameters for the model given in Table (1)

3 INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 15, No. 1 JANUARY 2014 / Therefore, the transformation matrix from the base frame (Frame 0) to the end effector (Frame 5) is given by 0 T 5 = 0 T 1 1 T 2 2 T 3 3 T 4 4 T 5 (2) where the matrices i-1 T i are defined according to Eq. (1). Solving Eq. (2), the transformation matrix 0 T 5 can be written as O 11 O 12 O 13 p x 0 T 5 = O 21 O 22 O 23 p y O 31 O 32 O 33 p z where O ij is the orientation matrix and p is the position vector. O ij and p can be written as O 11 = s 5 ( s 1 c 3 + c 1 c 2 s 3 ) c 5 ( c 4 ( s 1 s 3 c 1 c 2 c 3 ) + c 1 s 2 s 4 ) O 12 = s 5 ( c 4 ( s 1 s 3 c 1 c 2 c 3 ) + c 1 s 2 s 4 ) c 5 ( s 1 c 3 + c 1 c 2 s 3 ) O 13 = s 4 ( s 1 s 3 c 1 c 2 c 3 ) + c 1 s 2 c 4 O 21 = s 5 ( c 1 c 3 s 1 c 2 s 3 ) + c 5 ( c 4 ( c 1 s 3 + s 1 c 2 c 3 ) s 1 s 2 s 4 ) O 22 = c 5 ( c 1 c 3 s 1 c 2 s 3 ) s 5 ( c 4 ( c 1 s 3 + s 1 c 2 s 3 ) s 1 s 2 s 4 ) O 23 = s 4 ( c 1 s 3 + s 1 c 2 s 3 ) + s 1 s 2 c 4 (3) (4) (5) (6) (7) (8) (9) Fig. 3 Roll, pitch, yaw rotation angles roll, pitch, and yaw are shown in Fig. 3, and denoted by φ, γ, and ψ. The order of rotation is specified as x-y-z; the successive rotations are relative to the fixed frame. The orientation matrix can be rewritten as n x o x a x n y o y a y = R z, φ R y, γ R x, ψ (17) n z o z a z Therefore, the relation between the joint and the Cartesian space can be described using ( θ 1, θ 2, θ 3, θ 4, θ 5 ) = f( φθψp,,, x, p y, p z ) (18) The transformation matrix from the base frame (Frame 0) to the elbow (Frame 3) is given as O 31 = s 2 s 3 s 5 c 5 ( c 2 s 4 + s 2 c 3 c 4 ) (10) 0 T3 = 0 T 5 5 T 3 (19) O 32 = s 5 ( c 2 s 4 + s 2 c 3 c 4 ) + s 2 s 3 c 5 O 33 = c 2 c 4 s 2 c 3 s 4 p x = d 5 ( s 4 ( s 1 s 3 c 1 c 2 c 3 ) c 1 s 2 c 4 ) + d 3 c 1 s 2 p y = d 5 ( s 4 ( c 1 s 3 + s 1 c 2 c 3 ) + s 1 s 2 c 4 ) + d 3 s 1 s 2 p z = d 1 + d 3 c 2 + d 5 ( c 2 c 4 s 2 c 3 s 4 ) (11) (12) (13) (14) (15) 2.2 Inverse kinematics IK is used for finding the joint variables in terms of the position and orientation of the end effector. In general, IK is more difficult to calculate than forward kinematics. However, the IK solver is a fundamental tool that can be used to solve many problems, for example, path planning. IK transforms the path planning in Cartesian space into a joint actuator pathin joint space for the manipulator. However, there may be multiple solutions. Using the following equations, one solution can be obtained. Eq. (3) can be rewritten as n x o x a x p x 0 T5 = n y o y a y p y (16) n z o z a z p z The orientation of the end effector can also be described as a product of successive rotations taken in a specific order. These rotation angles, Equating the fourth column of both sides of Eq. (19), q 1 and q 2 can be obtained. where (20) (21) (22) (23) The transformation matrix from the shoulder (Frame 2) to the end effector (Frame 5) is given as (24) Equating the third column of both sides of Eq. (24), q 3 can be obtained. where θ 1 = arctan( p y 200a y, p x 200a x ) θ 2 = arctan( s 2, c 2 ) 260+ p c z 200a z 2 = s 2 = 1 c 2 2 T5 = 2 T 0 0 T 5 θ 3 = arctan( N 1, M 1 ) + π N 1 = s 1 a x + c 1 a y (25) (26)

4 48 / JANUARY 2014 INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 15, No. 1 M 1 = c 1 c 2 a x + s 1 c 2 a y s 2 a z (27) P = J( θ)θ (36) Equating the third row of both sides of Eq. (24), q 5 can be obtained. θ 5 = arctan( N 2, M 2 ) (28) where where J(θ) is a Jacobian matrix. This equation describes the relation between the end effector velocity in Cartesian space and each joint angular velocity in joint space. According to the kinematics equations obtained in Section 2.1, the Jacobian matrix in Cartesian space can be obtained by N 2 = c 1 s 2 o x + s 1 s 2 o y + c 2 o z (29) P P x x P x P x P x θ 1 θ 2 θ 3 θ 4 θ 5 M 2 = ( c 1 s 2 n x + s 1 s 2 n y + c 2 n z ) (30) J( θ) = P P y y P y P y P y θ 1 θ 2 θ 3 θ 4 θ 5 (37) The transformation matrix from the elbow (Frame 3) to the end effector (Frame 5) is given as 3 T 5 = 3 T 0 0 T 5 (31) Equating the third column of both sides of Eq. (31), q 4 can be obtained. P P z z P z P z P z θ 1 θ 2 θ 3 θ 4 θ 5 Manipulability measure represents the total mobility in all directions of current configuration of the manipulator. Yoshikawa defined the manipulability measure [19] as MM = det( J( θ)j( θ) T ) (38) where θ 4 = arctan( N 3, M 3 ) N 3 = ( s 1 s 3 c 1 c 2 c 3 )a x + ( c 1 s 3 + s 1 c 2 c 3 )a y s 2 c 3 a z M 3 = c 1 s 2 a x + s 1 s 2 a y + c 2 a z (32) (33) (34) where J(θ) is a non-square matrix. If J(θ) is a square matrix, Eq. (38) can be rewritten as MM = det( J( θ) ) (39) Let the singular value decomposition (SVD) of Jacobian matrix as J = USV T (40) 3. Path Planning In this section, a path-planning algorithm based on IK and the maximum mobility direction of the end effector is defined. According to Yoshikawa s manipulability definition, the mobility of end effector of the manipulator can be measured in all directions. However, the manipulator can move only along a specified direction at each movement which called a task direction. Therefore, only the direction of maximum mobility is necessary at each configuration regardless of the total mobility. Ideally, the end effector of the manipulator moves along the maximum mobility direction. The end effector mobility always changes with the configuration of the manipulator. However, the end effector may not move close to the goal position because the direction of its maximum mobility may not be the same as that toward the goal position. Therefore, a unit direction vector is used for revising the moving direction and making the end effector move close to the goal position. 3.1 Maximum mobility direction The relation between the Cartesian and the joint space is given by P = f( θ) (35) where P is end effector position and θ are joint variables. The differential equation of Eq. (35) is given by where U and V are orthogonal matrices and S is a diagonal matrix. Scan be written as with S = σ σ σ m σ 1 σ 2 σ m 0 (41) (42) Then the total mobility can beexpressed as the product of the singular values MM = σ 1 σ 2 σ m (43) Yoshikawa defined the manipulability ellipsoid 19 as an ellipsoid with principal axes σ 1 U 1, σ 2 U 2,, σ m U m, where U i is the i th column vector of U and direction of σ i. From this definition, the total mobility is equal to the product of the axes of mobility ellipsoid. The mobility ellipsoid describes the total mobility in all directions. Notice that the major axis is the longest straight line from center to the curve, therefore length of the major axis represents the maximum mobility. That is, the direction of major axis is the direction of maximum mobility. From Eq. (41) and (42), σ 1 is the major axis, therefore σ 1 is the maximum mobility and its direction U 1

5 INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 15, No. 1 JANUARY 2014 / 49 manipulator, not only the configuration position obtained in Section 3.2 but also the posture when the end effector reaches the desired position is required. Therefore, a configuration orientation design is also required. The configuration orientation is denoted by O = [ roll pitch yaw] (51) The next configuration orientation satisfies l --- n l m O g O = n O g O 0 (52) Fig. 4 Next configuration position design where O g is the goal configuration orientation, O 0 is the start configuration orientation, and O n is the next configuration orientation. This equation can be rewritten as is the direction of maximum mobility. O n O g ( O g O 0 ) l n = --- l m (53) 3.2 The next configuration position design The end effector moves w 1 along U 1 as shown in Fig. 4, its position can be obtained by (44) where P n is the current configuration position and P n1 is the next configuration position. Then, the direction vector toward the goal position from P n1 can be obtained by (45) where P g is the goal configuration position and l n is the length between P g and P n1. l n is given by l m is the length between P g and P n, given by (46) (47) Therefore, the next configuration position revised by U 2 can be written as with p gx p n1x U 2 = P n1 = P n + w 1 U 1 l n p gy p n1y l n p gz p T n1z l n l n = ( p gz p n1z ) 2 + ( p gy p n1y ) 2 +( p gx p n1x ) 2 l m = ( p gz p nz ) 2 + ( p gy p ny ) 2 +( p gx p nx ) 2 P n2 = P n1 + w 2 U 2 w 1 + w 2 = 1 w 1, w 2 0 (48) (49) (50) w 1 and w 2 are a set of numbers in {0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9,1}. 3.3 The next configuration orientation design In order to find the manipulator configurations, an IK solver is required and obtained in Section 2.2. However, for a non-planar 3.4 Path planning algorithm The moving path is made up of a certain number of points which are obtained in Section 3.2. The moving path of end effector changes with the weight values w 1 and w 2. When w 1 is 1 and w 2 is 0, the end effector always moves along the direction vector of maximum mobility, and when w 1 is 0 and w 2 is 1, the end effector moves the shortest length to satisfy the desired configuration orientation, from the start to the goal position. This algorithm is based on the IK solver, maximum mobility direction of the end effector, and a revise direction vector. The initial values of w 1 and w 2 were given as [0.5, 0.5], which satisfies Eq. (49) and (50). An initial path was generated according to initial values of w 1 and w 2. Subsequently, a new path will be generated by increasing w 1 and decreasing w 2, and the average maximum mobility of the new path will be calculated. The generation of the new path will be repeated until the path cannot reach the goal position. The last path which can reach the goal position will have the largest average maximum mobility, this path would be chosen as the optimal path and the parameters (w 1 and w 2 ) to generate this path are going to be determined as the optimal values. An overview of the path-planning algorithm is as follows: Step 1: Calculate the start and goal configurations using IK solver. Step 2: Calculate U 1 using Eq. (40). Step 3: Calculate U 2 using Eq. (45). Step 4: Calculate P n2 using Eq. (48). Step 5: Calculate O n using Eq. (53). Step 6: Calculate the next configuration using the results of Steps 4 and 5 and the IK solver. Step 7: Check the results of Step 6 using FK. Step 8: Repeat Steps 2 to 7 until l n < 2 mm, and then move the end effector to the goal configuration. Step 9: Check the reach ability and change the weight values until the optimal path is found. 4. Simulation and Experiments In this section, the path-planning algorithm is verified using

6 50 / JANUARY 2014 INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 15, No. 1 Table 2 Start and goal configurations Variables (unit) Start Goal (Task 1) Goal (Task 2) Roll (degree) Pitch (degree) Yaw (degree) X (mm) Y (mm) Z (mm) Table 3 Path planning with different weight values for task 1 [w 1,w 2 ] Reachability Number of discrete configurations Length of path (mm) Average maximum mobility (10-7 m 3 ) [1, 0] unreachable [0.9, 0.1] unreachable [0.8, 0.2] unreachable [0.7, 0.3] unreachable [0.6, 0.4] reachable [0.5, 0.5] reachable [0.4, 0.6] reachable [0.3, 0.7] reachable [0.2, 0.8] reachable [0.1, 0.9] reachable [0, 1] reachable Table 4 Path planning with different weight values for task 2 [w 1,w 2 ] Reachability Number of discrete configurations Length of path (mm) Average maximum mobility (10-7 m 3 ) [1, 0] unreachable [0.9, 0.1] unreachable [0.8, 0.2] unreachable [0.7, 0.3] unreachable [0.6, 0.4] unreachable [0.5, 0.5] reachable [0.4, 0.6] reachable [0.3, 0.7] reachable [0.2, 0.8] reachable [0.1, 0.9] reachable [0, 1] reachable Fig. 5 Moving path with weight values [0, 1] for task 1 Fig. 6 Moving path with weight values [0.7, 0.3] for task 1 MATLAB and a dual arm manipulator. For the experiments, the start and goal positions and orientations are given in Table 2. There are two goal configurations with same start configuration. We then determine the weight values w 1 and w 2 to find the optimal path. The moving path changes with these two values and the reach ability, number of discrete configurations, length of path and average maximum mobility are shown in Table 3 and Table 4. Table 3 shows the all possible solutions with different weight values for task 1. When weight values are [0, 1], the path is a straight-line from start to goal positions as shown in Fig. 5 and the length of path is the shortest. When w 1 is bigger than 0.6, the end effector can t reach the goal position as shown in Fig 6. The endeffector moves close to the goal positionfirst and then moves away. There are 7 paths which the Fig. 7 Optimal moving path with weight values [0.6, 0.4] for task 1 end effector can reach the goal position. The number of discrete configurations decreased by decreasingw 1 and the length of path and average maximum mobility are decreased too. In this task, the optimal path is found with weight values are [0.6, 0.4] as shown in Fig. 7 and the joint variables are tracked and shown in Fig. 8. The experiments were performed to validate the algorithm with the manipulator shown in Fig. 1, and the results were compared with the simulation results as shown in Fig. 9. Comparing two simulation result tables, the optimal weight values are different for different tasks. As shown in Table 4, there are 6 paths which the end effector can reach the goal position for task 2 and the optimal path is found with weight values [0.5, 0.5].

7 INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 15, No. 1 JANUARY 2014 / Conclusions In this paper, a path-planning algorithm based on the direction of maximum mobility is proposed for a 5DOF humanoid manipulator. The feasibility of the algorithm was verified by simulations and experiments. The proposed approach for selecting optimal path has been applied to a 5DOF manipulator based on the open loop kinematics. This approach may not be suitable for closed loop kinematics. This approach can be applied for a more complex arm. However, in this research, it is focused on the humanoid-type manipulators. The algorithm can be applied further for planning the path of a mobile manipulator because it always considers the maximum mobility and mobility direction of the endeffector. This algorithm will be improved further and used in an environment with obstacles in the future. Fig. 8 Joint variables with optimal moving path ACKNOWLEDGEMENT This research was supported by Basic Science Research Program through the National Research Foundation of Korea (NRF) funded by the Ministry of Education, Science and Technology ( ). This work was supported by the National Research Foundation of Korea (NRF) Grant funded by the Korean Government (MSIP) (NRF2013R1A1A ). REFERENCES 1. Huang, B., Sun, Y., Sun, Y. M., and Zhao, C. X., A Hybrid Heuristic Search Algorithm for Scheduling FMS Based on Petri Net Model, The International Journal of Advanced Manufacturing Technology, Vol. 48, No. 9-12, pp , Rao, N. M., Synthesis of a Spatial 3-RPS Parallel Manipulator Based on Physical Constraints using Hybrid GA Simplex Method, The International Journal of Advanced Manufacturing Technology, Vol. 52, No. 5-8, pp , Sibalija, T. V. and Majstorovic, V. D., An Integrated Simulated Annealing-Based Method for Robust Multiresponse Process Optimisation, The International Journal of Advanced Manufacturing Technology, Vol. 59, No. 9-12, pp , Lin, C. W., Simultaneous Optimal Design of Parameters and Tolerance of Bearing Locations for High-Speed Machine Tools using a Genetic Algorithm and Monte Carlo Simulation Method, Int. J. Precis. Eng. Manuf., Vol. 13, No. 11, pp , Ali, M., Babu, N., and Varghese, K., Offline Path Planning of Cooperative Manipulators using Co-evolutionary Genetic Algorithm, Proc. of International Symposium on Automation and Robotics, Vol. 989, No. 3, pp Tamura, S., Murata, T., Islam, M. N., Yanase, T., and Taniguchi, S., A Path Planning Algorithm for Multi Manipulators, Proc. of IEEE International Conference on Industrial Technology, pp. 1-6, Fig. 9 Experimental results compared with Fig Tamura, S., Yanase, T., Islam, M. N., Ito, T., and Miyashita, H., A

8 52 / JANUARY 2014 INTERNATIONAL JOURNAL OF PRECISION ENGINEERING AND MANUFACTURING Vol. 15, No. 1 New Path Planning Algorithm for Manipulators, Proc. of IEEE International Conference on Systems, Man and Cybernetics, Vol. 3, pp , Hsu, D., Latombe, J. C., Kurniawati, H., On the Probabilistic Foundations of Probabilistic Roadmap Planning, The International Journal of Roborics Research, Vol. 25, No. 7, pp , Bohlin, R. and Kavraki, E. E., Path Planning using Lazy PRM, Proc. of IEEE International Conference on Robotics and Automation, Vol. 1, pp , Mechanisms, Proc. of IEEE International Conference on Robotics and Automation, Vol. 2, pp , Elkady, A., Mohammed, M., and Sobh, T., A New Algorithm for Measuring and Optimizing the Manipulability Index, Journal of Intelligent and Robotic Systems, Vol. 59, No. 1, pp , Magdy, M., ElKady, A., and Sobh, T., New Concept In Optimizing Manipulability Index of Serial Manipulators Using SVD Method, Novel Algorithms and Techniques In Telecommunications, Automation and Industrial Electronics, pp , Kim, J. and Ostrowski, J., Motion Planning of Aerial Robots using Rapidly-Exploring Random Trees with Dynamics Constraints, Proc. of IEEE International Conference on Robotics and Automation, Vol. 2, pp , Aguinaga, I., Borro, D., and Matey, L., Parallel RRT-Based Path Planning for Selective Disassembly Planning, The International Journal of Advanced Manufacturing Technology, Vol. 36, No , pp , Berenson, D., Srinivasa, S. S., Ferguson, D., Collet, A., and Kuffner, J. J., Manipulation Planning with Workspace Goal Regions, Proc. of the IEEE international conference on Robotics and Automation, pp , Vande Weghe, M., Ferguson, D., and Srinivasa, S. S., Randomized Path Planning for Redundant Manipulators without Inverse Kinematics, Proc. of the IEEE-RAS International Conference on Humanoid Robots, pp , Betram, D., Kuffner, J., Dillmann, R., and Asfour T., An Integrated Approach to Inverse Kinematics and Path Planning for Redundant Manipulators, Proc. of the IEEE International Conference on Robotics and Automation, pp , Blackmore, L. and Williams, B., Optimal Manipulator Path Planning with Obstacles using Disjunctive Programming, Proc. of American Control Conference, pp , Chen, F., Di, P., Huang, J., Sasaki, H., and Fukuda, T., Evolutionary Artificial Potential Field Method Based Manipulator Path Planning for Safe Robotic Assembly, Proc. of International Symposium on Micro-NanoMechatronics and Human Science, pp , Volpe, R. and Khosla, P., Manipulator Control with Superquadric Artificial Potential Functions: Theory and Experiments, Proc. of the IEEE International Conference on Systems, Man and Cybernetics, Vol. 20, No. 6, pp , Park, J. J., Kim. H. S., and Song, J. B., Collision-Free Path Planning for a Redundant Manipulator Based on PRM and Potential Field Methods, Journal of Institute of Control, Robotics and Systems, Vol. 17, No. 2, pp , Yao, Z. and Gupta, K., Path Planning with General End-Effector Constraints, Robotics and Autonomous Systems, Vol. 55, No. 4, pp , Yoshikawa, T., Manipulability and Redundancy Control of Robotic