MoboTSP: Solving the Task Sequencing Problem for Mobile Manipulators
MMoboTSP: Solving the Task Sequencing Problem forMobile Manipulators
Nicholas Adrian and Quang-Cuong Pham
Abstract — We introduce a new approach to tackle the mobilemanipulator task sequencing problem. We leverage computa-tional geometry, graph theory and combinatorial optimizationto yield a principled method to segment the task-space targetsinto reachable clusters, analytically determine base pose foreach cluster, and find task sequences that minimize the numberof base movements and robot execution time. By clusteringtargets first and by doing so from first principles, our solutionis more general and computationally efficient when comparedto existing methods.
I. I
NTRODUCTION
A mobile manipulator consists of a manipulator (e.g. arobotic arm) mounted unto a mobile base. The combinedsystem extends the workspace of a fixed-based manipulator[1] [2] [3].A frequent task for a mobile manipulator is to visit aset of ordered or unordered targets scattered around theworkspace: think for example of drilling multiple holeson aircraft fuselages or on curved housing walls (Fig. 1).Another example is motivated by mobile 3D-printing of largeworkpieces [4].For fixed-based manipulators, finding the optimal se-quence of targets and the corresponding Inverse Kinematics(IK) solutions is known as the Robotic Task SequencingProblem (RTSP) see [5] for a recent review. Compared tofixed-base manipulators, solving RTSP for mobile manipula-tors comes with extra complexities due to the distribution oftask space targets that spans beyond the reachable workspaceof a fixed-base manipulator.One solution may consist of segmenting a priori thetargets into what we call workspace clusters . Each workspacecluster would contain workspace targets that can be reachedby a corresponding workspace base pose . However, whenthere is no evident of a priori spatial clusters, this hierarchi-cal approach will likely result in suboptimal solutions.Here we introduce a new approach to tackle mobile manip-ulator task sequencing problem. We leverage computationalgeometry, graph theory and combinatorial optimization toyield a principled method to segment the task point targetsinto reachable clusters, analytically determine base pose foreach cluster, and find task sequences that minimize thenumber of base movements and robot execution time.Note that the minimization of the number of base move-ments has the highest priority, as such movements are thelargest source of localization errors (as compared to thekinematic errors of a fixed-base manipulator).
The authors are with the HP-NTU Digital Manufacturing CorporateLab and School of Mechanical and Aerospace Engineering, NanyangTechnological University, Singapore. Email: [email protected]
Fig. 1: Example of a mobile manipulator consisting of aDenso VS60 robot arm mounted on a a Clearpath Ridgebackmobile base. The task is to perform drilling on a set of targetson a curved surface. There is no a priori constraint on thetarget cluster or sequence.Our contributions are as follows: • A method to find a set of balls within the manipulator’sworkspace, such that all the points contained in the ballsare reachable from all the directions of a given cone.This is useful in both fixed-base and mobile manipulatorcases; • A method to cluster targets by covering by balls and toobtain the respective base pose in each cluster; • A task sequencing algorithm for mobile manipulator,minimizing the number of base movements and the ma-nipulator trajectory time within and between workspaceclusters .The remainder of the paper is organized as follows. InSection II, we review related works in mobile manipulatortask sequencing. In Section III, we introduce the overallpipeline. In Section IV, we present the detailed calculationsof the set of balls. In Section V, we introduce our methodto cluster the targets into separate workspaces, followed bytask sequencing algorithm in Section VI. We present thesimulation results in Section VII. Finally, in Section VIII,we conclude and sketch directions for future work.II. L
ITERATURE R EVIEW
To start with, we look at some works on finding robotplacement. In [6], reachability map is introduced as a 3Dworkspace representation that represents the reachabilityprobability of points in task space. The reachability map is a r X i v : . [ c s . R O ] N ov sed in [7] to find robot placement for a constrained lineartrajectory. The work is extended in [8] to 3D trajectory.In [9], extended manipulability measure as quality indexis used as precomputed workspace representation insteadof reachability. Robot placement can also be found usingInverse Reachability Distribution [10].Several works on RTSP have been attempted but they aremostly limited to fixed-base manipulator. In [11], GeneticAlgorithm (GA) is employed on 3-DOF and 6-DOF robotswith multiple IK solutions per target. They reported 1800seconds CPU time for 50 task points. This work was ex-tended in [12] to include base placement of the robot. Nocomputational details were provided for varying number oftask points or base permutations per task point. In [13],the author considered 50 targets with five configurationseach and the solution was found in 9621.64 seconds. Thefastest solution for fixed-based manipulator RTSP, to ourbest understanding, is found in [5]. A fast near-optimalsolution was found using a three-step algorithm exploitingboth task and configuration space. In their experiment, theyvisited 245 targets with an average of 28.5 configurationsper target and the solution was found in 10 seconds with 60seconds execution time. For the case of mobile manipulator,a search on 10-DOF configuration space is done as part ofGenetic Algorithm-based optimization to minimize overallconfigurations displacement in [1].In [2], a relatively similar problem of part-supply pick andplace is presented where the robot had to pick parts frommultiple tray locations. Given m number of trays containing n number of objects with o number of possible graspingposes each, m × n × o IK queries would be required. Objectsare also pre-clustered into trays which reduce the numberof base regions to consider. However, such target clusteringmight not be available or easily defined in other cases suchas when continuous target distribution is involved.In [3], the mobile manipulator’s end-effector has to fol-low a continuous path trajectory. To deal with inaccuratebase locomotion, the author used manipulability performancefunction to find minimum base stopping positions that willallow the robot to follow discretized points on the pathtrajectory. However this method requires the discretizedtarget points to be pre-sequenced.Compared to other methods, our approach exploits the of-ten overlooked strategy of clustering targets first. This allowsus to have a fast and computationally efficient solution. InSection VII, we show that we clustered 183 targets in 0.104seconds. To our best understanding, no other solutions havebeen able to do so in a systematic way while also providingan analytical solution for finding base poses that can reachall the targets in each cluster.III. P
IPELINE
We assume that the targets’ positions x k =[ x kx , x ky , x kz ] , k ∈ [1 , n ] of X ∈ R n × and their respectivedrilling direction vectors R ∈ R n × are provided for all n targets. 1) X is segmented into c clusters of X i for i ∈ [1 , c ] . X i is guaranteed to be geometrically covered by a ball ofcenter position c i and diameter d . Refer to Figure 2a.2) Each ball centered at c i is matched with B ( c i ) ⊆ B where B is a pre-calculated set of balls with the samediameter d relative to the robot’s frame. Through thismatching process, we immediately obtain the robot’sbase pose T ibase to visit the target cluster X i . Thedetails on how B is obtained is included in SectionIV. Refer to Figure 2b.3) Base tour that minimizes base pose distance in task-space between X i is calculated.4) Manipulator tour is calculated that respects the basetour.5) Collision-free trajectory is calculated based on ob-tained base and manipulator tour.IV. R EACHABLE S ET OF B ALLS
Note that c i ’s center coordinate can vary in x,y,z axes.Thus, we need to find a set of balls B of varying centerheights to account for the variation during matching. Varia-tion in c i ’s coordinate in xy-plane can be easily handled bythe base locomotion.We pre-calculate a set of balls B with diameter d relativeto the robot’s frame such that the balls’ centers form acontinuous straight line in 3D, close-bounded by the highestand lowest balls.Let P ∈ R be the continuous set of all points within aclosed ball of center coordinate b and diameter d : P ( b ) = { p : (cid:107) p − b (cid:107) ≤ ( d/ } As mentioned in Section III, each X i ball cluster centeredat c i is matched with a ball B ( c i ) ⊆ B . Any distributionof targets in X i matched with B ( c i ) is guaranteed to bereachable from any directions in R if all points in P ( B ( c i )) is reachable from all directions in R . Since it is impossibleto check for every point in the continuous space of P ( B ( c i )) and R , we apply an approximation condition: Every pointin the discretized P ( B ( c i )) must be reachable from thedirections bounding R .To do so, we construct a focused kinematic reachabil-ity (fkr) database which involves discretizing the Cartesianworkspace into 3D voxels. For each voxel position, weperform Inverse Kinematics (IK) to obtain a set of IKsolutions for each direction in R ext ⊆ R where R ext contains the bounding directions for R . All voxels containingIK solutions for all directions in R ext are then marked andsaved as V fkr .Given that V fkr represents the discretized volume thatis reachable from the directions bounding R , we can nowproceed to find the set of balls B within the V fkr thatfulfills the condition above. To make the problem tractable,we can solve this with simple linear programming if wecan obtain a maximal convex representation of V fkr . Find-ing the largest convex subset is often known as findingthe Maximal Area/Surface Convex Subset (MACS), potato-peeling or convex skull problem. More specifically, we are X d (a) base T cbase T b b bottom b top b c (b) Fig. 2: (a) Illustration of how X is segmented into c = 3 clusters of X , X , and X with equal diameter d . Thered cross represents the center coordinate for X cluster. (b)Left: The set of balls B is defined by the center coordinates b top and b bottom shown in orange dots. The outer shapesof the top and bottom balls of diameter d are displayed indashed orange circle. Right: Target cluster X i is covered by aball centered at c i with diameter d . Matching: Ball centeredat c i can be matched with the respective ball B ( c i ) ⊆ B centered at b i such that base T b i = base T c i .dealing with the digital [14] [15] variation given V fkr is ininteger coordinates. In this paper, we implemented a heuristicfor 3D digital potato-peeling from [14] to obtain a MACSapproximation M fkr . Refer to Figure 3 for an example ofour MACS finding algorithm implementation.Finally, we find the the ball set B that lies within M fkr via linear programming [16]. By convexity, we can de-fine B by simply finding two balls on the extreme endswith center coordinates b top = [ b tx , b ty , b tz ] and b bottom =[ b bx , b by , b bz ] respectively where b tz = max(height( x i )) and b bz = min(height( x i )) . The linear program has to fulfill thefollowing inequalities and equalities constraints which are visualized in Figure 4: min x e T a such that A mfkr a ≤ b mfkr [ − , , , , , , T a ≤ x offset [0 , , , − , , , T a ≤ x offset [0 , , − , , , , T a ≤ z offset [0 , , , , , − , T a ≤ z offset [0 , , , , , , T a = min(height( x i ))[0 , , , , , , T a = max(height( x i )) where e T = [0 , , , , , , − a = [ b bx , b by , b bz , b tx , b ty , b tz , d/ A mfkr = M fkr plane coefficients b mfkr = M fkr plane offsets x offset = X-axis collision plane z offset = Z-axis collision planeV. W
ORKSPACE C LUSTERING
The goal here is to segment X into X i clusters and obtainthe base pose T ibase that can reach all the targets in X i .We do this by clustering X in 3D space and ensuringthat each X i is contained within a ball centered at c i ofdiameter d which is then matched with the correspondingball B ( c i ) B ⊆ B .We formulate the problem of 3D points covering withballs as partitioning graph by cliques or clique covering .We form the undirected graph G by connecting two nodes(targets) if the Euclidean distance between them is less orequal to δ such that the clique can be covered by ball ofdiameter d . Clustering by clique in G is similar to coveringby independent set in the complement graph ¯ G which is oftenreferred to as the graph-coloring problem. Graph-coloring problem is NP-complete but several heuristics exist [17][18] [19]. In this paper we implemented greedy coloring ofconnected sequential breadth-first search variant [17] as itresults in minimum average number of clusters based on ourexperiment.The calculation for δ is presented here. Let e be the centerof a circle with radius δ and f is a point on the circle’scircumference as illustrated in Figure 5. Points e and f are ofdistance δ and hence would be connected by an edge if theyare nodes in the Graph G . Point g is the intersection pointbetween two circles centered at e and f of similar radius δ and the three points form a clique. Circle O is smallestcircle that circumscribes points e , f , and g . It is clear thatany maximal clique that contains the three points will becontained inside O with diameter d . As this theorem extendsto 3D, we can then find the relation between ball diameter d and maximum edge length δ by solving for the circumradiusig. 3: V fkr , shown in green, marks all the voxels on the top-front quarter of the robotic workspace that are reachable fromdirection base R ext = R = [1 , , . The maximal convexsubset approximation M fkr is shown in red.of the equilateral triangle (cid:52) ef g . The solution is providedbelow as: δ = √ d To obtain T ibase , we match the ball centered at c i with theball B ( c i ) ⊆ B centered at b i : r = height( w c i ) − w b bzw b tz − w b bz b i = (1 − r ) · b bottom + r · b top Fig. 4: To find the set B , we find the intersection ofhalf-spaces subject to two equality constraints using linearprogramming. The half-spaces consist of the half-spaces of M fkr (red), two collision inequality planes (blue) with vectorsshowing the normals and two equality planes (green) to coverthe minimum and maximum target height respectively. Theresulting two balls centered at b top and b bottom are shownin orange. By convexity, any ball centered along the linebetween the two balls are inside the M fkr half-spaces andhence included in the set B . e fgδd Fig. 5: Diagram containing the minimum circle O withdiameter d that covers a maximal clique with maximum edgelength δ satisfying: w T ibase · base T b i = w T c i base T w · w T c i = base T b i base T b i = base T c i where w is the world’s frame.The robot’s base transformations w T ibase can then becalculated for each c i by simple transformation: w T ibase = w T c i · c i T base = w T c i · ( base T c i ) − = w T c i · ( base T b i ) − I. T
ASK S EQUENCING
Let seq( g ( k )) be a sequence of k configurations in thecoordinate space spanned by g . The length of the sequenceis defined by length(seq( g ( k ))) = (cid:80) k − n =1 (cid:107) g ( k ) n +1 − g ( k ) n (cid:107) .When g ( k ) = g ( k ) k , seq( g ( k )) forms a tour.Let t = [ t x , t y ] be the base positions. The algorithmconsists in:1) Finding a near-optimal base transforms tour seq( t ( c +2)) in task-space that minimizes length(seq( t ( c +2))) .2) Finding the near-optimal target sequence seq( x ( n +2)) in task-space given seq ( t ( c + 2)) .3) Finding the optimal manipulator q ∗ = [ q , ..., q n +2 ] configuration for each target in seq( x ( n + 2)) suchthat length(seq( x ( c + 2))) is minimum. Collisions areignored at this stage.4) Computing fast collision-free configuration space tra-jectories on 9-DOF combined base and manipulatorconfigurations based on seq( t ( c + 2)) and seq( x ( n +2)) .Our task sequencing algorithm is inspired by RoboTSP, afast RTSP solution for fixed-base manipulator [5].In Step 1, the tour seq( t ( c +2)) is calculated which starts atpredetermined t start , covers all t i for all c clusters and endsat t end where t start = t end . The base station tour is foundthrough finding the approximated solution to the TravellingSalesman Problem (TSP) using the algorithm [20].In Step 2, the tour seq( x ( n + 2)) is obtained which startsat predetermined x start , covers all n number of x i and endsat x end . Unlike in previous step, the target tour finding isformulated as a minimum-length Hamiltonian Path probleminstead.This is achieved through virtually translating x start , x ,..., x n , x end along an axis with distance h between them toform a stack-of-clusters as seen in Figure 6. We denote x (cid:48) for the translated x . In our implementation, we define h tobe: h = h scale · max ≤ i ≤ c { dist( a, b ) : a, b ∈ X i , a (cid:54) = b } where h scale ≥ The minimum-length Hamiltonian Path is calculated on alltargets on the stack-of-clusters starting from x (cid:48) start and end-ing at x (cid:48) end . The resulting seq( x (cid:48) ( n + 2)) is translated backto obtain seq( x ( n + 2)) , which is a target tour sequence thatrespects the base tour sequence seq( t ( c + 2)) and minimizesmanipulator trajectory distance between base movement.Similar to [5], in Step 3 we firstly construct an undirectedgraph of n + 2 layers following the order in seq( x ( n + 2)) .Each layer i , where i ∈ [1 , n +2] contains v i nodes represent-ing the number of IK solutions of the 6-DOF manipulator fortarget i , resulting in a total of (cid:80) n +2 i =1 v i nodes. Note that thefirst and last nodes are “Start” and “Goal” nodes respectivelysuch that v = v n +2 = 1 . Next for i ∈ [1 , n + 1] , we add anedge between each nodes of layer i and layer i + 1 , resultingin a total of (cid:80) n +1 i =1 v i v i +1 edges. Lastly, we find the shortestpath between the “Start” and “Goal” nodes using a graphsearch algorithm to find the optimal sequence of IK solutions. Fig. 6: An illustration of stack-of-clusters . The clusteredtargets (top) are “translated” and stacked back-to-back be-tween a Start and End end-effector position to form x (cid:48) start , x (cid:48) , x (cid:48) , x (cid:48) , x (cid:48) , x (cid:48) and x (cid:48) end (bottom). Minimum-lengthHamiltonian Path is calculated on the “translated” clustersto obtain seq( x (cid:48) (7)) , a target tour that respects the base toursequence seq( t (7)) and minimizes manipulator trajectorydistance between base movement.VII. E XPERIMENT
For our experiment we generated the fkr dataset for thetop-front quarter grid of the robot’s workspace similar toin Figure 3. The distance between two workspace voxels isset at 0.04 m . For R ext , we consider four direction vectorsresembling a pyramid that makes θ = 10 degree from theaxis that runs along the pyramid’s height. The four directionvectors are: • [cos( θ ) , , sin( θ )] • [cos( θ ) , , − sin( θ )] • [cos( θ ) , sin( θ ) , • [cos( θ ) , − sin( θ ) , The drill target directions in simulation are then pro-grammed such that they cover the vector range bounded byvalues in R ext .Below we provides tables that list the time breakdown ofour proposed method. In Table I, we show the time requiredto perform sampling for V fkr and subsequently obtaining themaximal digital convex subset M fkr . We consider these asoffline processes as they can be pre-generated and reusedon other target distributions with similar robot and targetdirections requirement. Following that, in Table II and TableIII we provide the time breakdown for different number oftargets for target clustering and target sequencing respec-tively.TABLE I: Time breakdown for offline processes time (seconds) V fkr M fkr ig. 7: Simulation in OpenRAVE where the mobile manipu-lator has to visit 183 non-clustered and unsequenced targets.The algorithm found five workspace clusters to visit them.The orientations of the targets cover the range within thebounding directions R ext used in building the set of B . TABLE II: Time breakdown for online target clustering No of targets Target clustering in seconds ( ± std)183 0.104 ( ± ± ± TABLE III: Time breakdown for online target sequencing
No of targets Base and hole tour finding in seconds ( ± std)183 0.919 ( ± ± VIII. C
ONCLUSION
The problem of visiting multiple task points with a mo-bile manipulator has always been challenging. Comparedto its fixed-base counterpart, there are more complexitiesto deal with when the task space targets span beyond thereachable immediate workspace. Most approaches rely oncomputationally expensive base poses sampling. To makethe problem tractable, they often require the targets to bearbitrarily pre-clustered or pre-sequenced which might notalways be possible such as in drilling operations.In this paper, we introduced a novel approach based onclustering the task point targets first. We presented a a fastand principled way to segment the task point targets intoreachable clusters, analytically determine base pose for eachcluster and find task sequence to minimize robot’s trajectory.The method here can be easily extended to the continuouscase, such as in 3D-printing. However, one needs to enforcethe continuity of the manipulator motion, which excludes
IK-switching . Integrating such a constraint into MoboTSP is theobjective of our current research.A
CKNOWLEDGMENT
This research was conducted in collaboration with HPInc. and supported by National Research Foundation (NRF)Singapore and the Singapore Government through the Indus-try Alignment Fund-Industry Collaboration Projects Grant(I1801E0028). R
EFERENCES[1] S. Vafadar, A. Olabi, and M. S. Panahi, “Optimal motion planning ofmobile manipulators with minimum number of platform movements,”in , pp. 262–267, IEEE, 2018.[2] J. Xu, K. Harada, W. Wan, T. Ueshiba, and Y. Domae, “Planning anefficient and robust base sequence for a mobile manipulator performingmultiple pick-and-place tasks,” in , pp. 11018–11024, 2020.[3] D. H. Shin, B. S. Hamner, S. Singh, and M. Hwangbo, “Motionplanning for a mobile manipulator with imprecise locomotion,” in
Proceedings 2003 IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS 2003)(Cat. No. 03CH37453) , vol. 1,pp. 847–853, IEEE, 2003.[4] M. E. Tiryaki, X. Zhang, and Q. Pham, “Printing-while-moving: anew paradigm for large-scale robotic 3d printing,” in ,pp. 2286–2291, 2019.5] F. Su´arez-Ruiz, T. S. Lembono, and Q.-C. Pham, “Robotsp–a fastsolution to the robotic task sequencing problem,” in , pp. 1611–1616, IEEE, 2018.[6] F. Zacharias, C. Borst, and G. Hirzinger, “Capturing robot workspacestructure: representing robot capabilities,” in , pp. 3229–3236,Ieee, 2007.[7] F. Zacharias, C. Borst, M. Beetz, and G. Hirzinger, “Positioningmobile manipulators to perform constrained linear trajectories,” in , pp. 2578–2584, IEEE, 2008.[8] F. Zacharias, W. Sepp, C. Borst, and G. Hirzinger, “Using a modelof the reachable workspace to position mobile manipulators for 3-d trajectories,” in , pp. 55–61, IEEE, 2009.[9] N. Vahrenkamp, T. Asfour, G. Metta, G. Sandini, and R. Dillmann,“Manipulability analysis,” in , pp. 568–573, IEEE,2012.[10] N. Vahrenkamp, T. Asfour, and R. Dillmann, “Robot placement basedon reachability inversion,” in , pp. 1970–1975, IEEE, 2013.[11] P. T. Zacharia and N. Aspragathos, “Optimal robot task schedulingbased on genetic algorithms,”
Robotics and Computer-Integrated Man-ufacturing , vol. 21, no. 1, pp. 67–79, 2005. [12] K. Baizid, R. Chellali, A. Yousnadj, A. Meddahi, and T. Bentaleb,“Genetic algorithms based method for time optimization in robotizedsite,” in , pp. 1359–1364, IEEE, 2010.[13] M. Saha, T. Roughgarden, J.-C. Latombe, and G. S´anchez-Ante, “Plan-ning tours of robotic arms among partitioned goals,”
The InternationalJournal of Robotics Research , vol. 25, no. 3, pp. 207–223, 2006.[14] G. Borgefors and R. Strand, “An approximation of the maximalinscribed convex set of a digital object,” in
International Conferenceon Image Analysis and Processing , pp. 438–445, Springer, 2005.[15] L. Crombez, G. D. da Fonseca, and Y. G´erard, “Peeling digitalpotatoes,” arXiv preprint arXiv:1812.05410 , 2018.[16] G. B. Dantzig,
Linear programming and extensions , vol. 48. Princetonuniversity press, 1998.[17] A. Kosowski and K. Manuszewski, “Classical coloring of graphs,”
Contemporary Mathematics , vol. 352, pp. 1–20, 2004.[18] D. W. Matula and L. L. Beck, “Smallest-last ordering and clusteringand graph coloring algorithms,”
Journal of the ACM (JACM) , vol. 30,no. 3, pp. 417–427, 1983.[19] N. Deo, J. S. Kowalik, et al. , Discrete optimization algorithms: withPascal programs . Courier Corporation, 2006.[20] D. L. Applegate, R. E. Bixby, V. Chvatal, and W. J. Cook,