University of Southern California Dissertations and Theses

Planning for mobile manipulation

(USC Thesis Other)

Planning for mobile manipulation

PDF

Download a page range

Download transcript

Copy asset link

Request this asset

Transcript (if available)

Content Planning for Mobile Manipulation by Shantanu Thakar A Dissertation Presented to the FACULTY OF THE GRADUATE SCHOOL UNIVERSITY OF SOUTHERN CALIFORNIA In Partial Fulllment of the Requirements for the Degree DOCTOR OF PHILOSOPHY (Mechanical Engineering) August 2021 Copyright 2021 Shantanu Thakar Acknowledgements First and foremost, I would like to thank Professor Satyandra K. Gupta for being my doctoral advisor. His guidance, patience and constructive critique has been very instrumental in shaping my research philosophy and my approach to solving new problems. I am very fortunate to have an understanding and caring advisor who trusted me and had faith in my decisions. Throughout my Ph.D he strived hard to make sure I had all the required facilities and infrastructure to have a successful dissertation. There are numerous valuable lessons I have learned from him that have had a strong in uence on me and will continue to guide me through my career. I would like to thank the members of my committee, Professor Assad Oberai and Professor Heather Culbertson, for their valuable insights, feedback, and suggestions for this work. I would also like to thank the National Science Foundation (NSF grants 1634431, 1925084, 1935712) for their generous nancial support. I would like to thank the professors who taught me and guided me at Indian Institute of Technology Bombay and Indian Institute of Science Bangalore; specically Professor Ashwini Ratnoo at IISc. I would like to thank all my labmates for their pleasant company and numerous stimulat- ing discussions throughout the past few years. Specically, I would like to thank Ariyan Kabir, Pradeep Rajendran, Brual Shah, Alec Kanyuck, Shaurya Shriyam, Sarah Al-Hussaini, Rishi Mal- han, Prahar Bhatt, Yeo Jung Yoon, Jason Gregory, Aniruddha Shembekar, Hyeojung Kim, Liwei Fang, Neel Dhanaraj, Nithyananda Kumbla and Omey Manyar. During this journey I was lucky to have the company of so many wonderful friends by my side. I would like to thank Parthe Pandit, Arpit Agarwal, Niladri Chatterji, Saurabh Mogre and Vighnesh ii Vatsal for their humour and wit that kept me going. I would also like to thank Nitesh Singh, Puneet Arora, Sahil Agarwal, Shanay Shah, Ankit Agrawal, Davis Matthew, Anmol Goyal, Janaki Seth, Omkar Thakoor, Kartik Lakhotia and Anup Kanale for all support and encouragement over the years. I would specically like to thank Mayank Bendarkar, my oldest friend for so many stimulating discussions and great times we've had over the years. I want to thank Sunipa for her constant love and support throughout this journey, for moti- vating me to aspire more, and for bearing with me during tough times. This would not have been possible without you. I am grateful to my brother Shreyas and sister-in-law Ketki for taking care of me and making me comfortable when I newly arrived in this country. Finally, I am eternally grateful to my parents back in India who instilled a drive in me and provided me with a platform to excel, without which I would not have done this. Your encouragement and unwavering belief in me. Without you, I would not be the person I am today. iii Table of Contents Acknowledgements ii List Of Tables viii List Of Figures ix Abstract xv Chapter 1: Introduction 1 1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.2 Goal and Scope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 1.3 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 Chapter 2: Literature Review 9 2.1 Point-to-Point Planning for High DOF Robotic Systems . . . . . . . . . . . . . . . 9 2.1.1 Sampling-Based Planning Methods . . . . . . . . . . . . . . . . . . . . . . . 9 2.1.2 Discrete Search-Based Planning Methods . . . . . . . . . . . . . . . . . . . 13 2.1.3 Optimization-Based Planning Methods . . . . . . . . . . . . . . . . . . . . . 15 2.2 Path-Constrained Motion Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.3 Area Coverage Path Planning for Manipulators . . . . . . . . . . . . . . . . . . . . 21 2.4 Robotic Grasping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 2.5 Task Assignment and Motion Planning . . . . . . . . . . . . . . . . . . . . . . . . . 24 2.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 Chapter 3: Mobile Base Trajectory Planning for Part Pick-up 27 3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 3.2 Background and Prelimnaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30 3.2.1 Grasping Strategy and Grasping Area . . . . . . . . . . . . . . . . . . . . . 30 3.3 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31 3.3.1 Candidate Approaches . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 3.4 Planning Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34 3.4.1 Graph Representation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 3.4.2 Cost Function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 3.4.3 Heuristic . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 3.4.4 The Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 3.4.5 Manipulator Planning and Grasping Delay . . . . . . . . . . . . . . . . . . 39 3.5 Results and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43 3.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45 iv Chapter 4: Manipulator Planning on given Mobile Base Path for Part Pick-up 46 4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 4.2 Background and Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 4.3 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 4.4 Planning Framework . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 4.4.1 Mobile Base Motion Smoothing . . . . . . . . . . . . . . . . . . . . . . . . . 51 4.4.2 Manipulator Motion Planning . . . . . . . . . . . . . . . . . . . . . . . . . . 53 4.4.3 Constrained Trajectory for Grasping . . . . . . . . . . . . . . . . . . . . . . 58 4.5 Methods to speed-up computations . . . . . . . . . . . . . . . . . . . . . . . . . . . 58 4.5.1 Focused Sampling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59 4.5.1.1 WS Sampling to Bias Search . . . . . . . . . . . . . . . . . . . . . 59 4.5.1.2 WS based Tree Propagation . . . . . . . . . . . . . . . . . . . . . 60 4.5.2 Conguration Space Patch . . . . . . . . . . . . . . . . . . . . . . . . . . . 62 4.5.3 Selection of Goal Tree . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 4.6 Incorporating Speed Up Techniques in Planning Algorithm . . . . . . . . . . . . . 66 4.7 Results and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72 4.7.1 Scenarios . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73 4.7.2 Experimental Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73 4.7.3 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75 4.7.4 Changing Sampling Ratio . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78 4.8 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79 Chapter 5: Active Learning for Picking-Up Objects with a High-Speed Gripper 82 5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82 5.2 Background and Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84 5.3 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85 5.4 A Meta Model for Estimating Part Grasping Success Probability . . . . . . . . . . 86 5.4.1 Active Learning for Generating Classication Model . . . . . . . . . . . . . 88 5.4.2 Model Generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88 5.4.3 Constructing Success Probability Meta-Model . . . . . . . . . . . . . . . . . 90 5.5 Trajectory Generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91 5.5.1 Denition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91 5.5.2 Successive Renement Procedure . . . . . . . . . . . . . . . . . . . . . . . . 94 5.6 Results and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95 5.7 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99 Chapter 6: Accelerating Bi-Directional Sampling-Based Search for Motion Plan- ning of Non-Holonomic Mobile Manipulators 101 6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101 6.2 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104 6.3 Determining Focus Regions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105 6.3.1 Focus Region for the mobile base (MB) . . . . . . . . . . . . . . . . . . . . 105 6.3.2 Focus Region for Manipulator EE . . . . . . . . . . . . . . . . . . . . . . . 106 6.4 Constructing Trees . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106 6.4.1 Finding Nearest Neighbors . . . . . . . . . . . . . . . . . . . . . . . . . . . 107 6.4.2 Extend towards the Random Sample . . . . . . . . . . . . . . . . . . . . . . 108 6.4.3 Heuristics for Connecting Trees . . . . . . . . . . . . . . . . . . . . . . . . . 110 6.5 The HS-Bi-RRT Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111 6.6 Results and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113 6.6.1 Experimental Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113 6.6.2 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114 v 6.7 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 117 Chapter 7: Area Coverage Planning for Spray-Based Surface Disinfection with a Mobile Manipulator 119 7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 7.2 Background and Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123 7.2.1 Literature Review on Spray and Spray Gun Trajectory Modelling . . . . . . 125 7.3 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 7.3.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 7.4 The Disinfectant Spray Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127 7.5 Overview of Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132 7.6 Spray Path Generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133 7.6.1 Planar Polygon Generation . . . . . . . . . . . . . . . . . . . . . . . . . . . 133 7.6.2 The Spray Radius . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135 7.6.3 The Spray Path Generation Algorithm . . . . . . . . . . . . . . . . . . . . . 136 7.6.4 Two Zig-Zag Paths for Same Spray Segments . . . . . . . . . . . . . . . . . 143 7.6.5 Spray Path Selection Criterion . . . . . . . . . . . . . . . . . . . . . . . . . 143 7.7 Motion Planning for Mobile Manipulator . . . . . . . . . . . . . . . . . . . . . . . . 145 7.7.1 Nozzle Pose Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145 7.7.2 Robot Motion Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147 7.7.3 Motion Planning Problem Formulation . . . . . . . . . . . . . . . . . . . . . 148 7.7.4 Spline Representation and Seed Generation . . . . . . . . . . . . . . . . . . 150 7.7.5 Successive Constraint Renement for Solving Multi-Stage Optimization . . 153 7.8 Computation of Time Intervals between Waypoints . . . . . . . . . . . . . . . . . . 154 7.9 Results and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158 7.9.1 Test Cases . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158 7.9.2 Discussion on Generated Spray Paths . . . . . . . . . . . . . . . . . . . . . 159 7.9.3 Bench-marking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164 7.9.4 Robot Motion Generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167 7.9.5 Physical Experiment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 169 7.10 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 171 Chapter 8: Task-Assignment and Motion Planning for Bi-Manual Mobile Manip- ulation 174 8.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174 8.2 Background and Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 176 8.3 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177 8.3.1 Task Network Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177 8.3.2 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 179 8.4 Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 180 8.4.1 Task-Agent Assignment Layer . . . . . . . . . . . . . . . . . . . . . . . . . . 180 8.4.1.1 Task Constraint Heuristics . . . . . . . . . . . . . . . . . . . . . . 181 8.4.1.2 Spatial Constraint Heuristic . . . . . . . . . . . . . . . . . . . . . 182 8.4.2 Motion Planning Layer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182 8.4.3 Caching Scheme . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183 8.4.4 The Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184 8.4.5 Spatial Constraint Checking . . . . . . . . . . . . . . . . . . . . . . . . . . . 188 8.4.6 Motion Generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 189 8.5 Results and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 189 8.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192 vi Chapter 9: Conclusions 194 9.1 Intellectual Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 194 9.2 Summary of Lessons Learnt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 198 9.3 Anticipated Benets . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 200 9.4 Future Directions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201 Reference List 204 vii List Of Tables 4.1 The span times, computation times, number of collision checks and percentage success for the planner with successively adding features mentioned in the approach section. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76 5.1 This table shows the impact of the uncertainty in part pose on the velocities of the mobile base and the gripper . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97 5.2 This table shows the impact of the uncertainty in part pose on the velocities of the mobile base and the gripper . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97 5.3 This table shows the impact of the uncertainty in part pose on the velocities of the mobile base and the gripper . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98 5.4 This table shows the impact of the uncertainty in part pose on the velocities of the mobile base and the gripper . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99 6.1 The average computation time and average path length of the EE for he 5 test cases. The average is over 20 runs. Computation time includes the time required for generating free-space balls. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116 7.1 The table describing the performance of the Spray Path Generation algorithm on the 5 test cases from Fig. 7.19. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159 7.2 The performance of the optimization-based motion planner for the mobile manip- ulator for the ve test cases. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164 7.3 Comparison between the trajectory execution time before and after determining appropriate waypoint intervals and the corresponding percentage of point cloud points that did not receive enough disinfectant. . . . . . . . . . . . . . . . . . . . . 168 8.1 Impact of having spatial constraint checking and caching in task-agent assignment and motion planning on computation time . . . . . . . . . . . . . . . . . . . . . . . 193 viii List Of Figures 1.1 (a) Mobile manipulator carrying a long rod [223] (b) Mobile manipulator picking up objects while moving [222]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.2 (a) Correct assignment of tasks to the manipulators (b) Incorrect task assign- ment. [220] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 1.3 The overview of the inherent tasks for mobile manipulators in this dissertation . . 5 3.1 An example scene in a factory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 3.2 A part and the three associated grasping strategies . . . . . . . . . . . . . . . . . . 31 3.3 Grasping area for Grasping strategy b . . . . . . . . . . . . . . . . . . . . . . . . . 32 3.4 Specic case in which two searches will result in sub optimal solution . . . . . . . . 35 3.5 Color Scheme in the search . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 3.6 Description of heuristics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 3.7 Segment of the path where grasping happens . . . . . . . . . . . . . . . . . . . . . 39 3.8 Computation time, number of expansions and Path cost vs the percentage of free space in dierent layout dimensions . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 3.9 Two paths for the given scene with one obstacle placed dierently and 70% free area 43 3.10 The time delay for the path in Fig. 3.9(b) vs the maximum joint rate for the manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44 4.1 An example warehouse scene where the mobile manipulator has to pick up a part and transport it to the goal location. . . . . . . . . . . . . . . . . . . . . . . . . . . 47 4.2 The grasping area for a grasping pose . . . . . . . . . . . . . . . . . . . . . . . . . 50 4.3 The mobile base trajectory generated by the planner (red) and after smoothing (black) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 ix 4.4 u I is the location on P mb where the manipulator motion starts. . . . . . . . . . . 55 4.5 A single Jacobian step to map workspace samples to conguration space . . . . . . 61 4.6 An illustration of congurations trapped behind a narrow passage in 2D. It can be seen that only a few congurations are able to easily expand out of the narrow passage for the goal treeT g1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62 4.7 Example of two competing goal treesT g1 andT g2 for dierent grasping poses. It can be seen thatT g2 has a higher chance of connecting toT s as it has expanded more towards the start node. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 4.8 An illustration of free space ballsB i and the two goal trees growing inWS (for the rst stage only). The workspace projection of a conguration space patch is shown. It is essentially the 3D volume containing the EE position at all the manipulator congurations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65 4.9 An illustration of how a wrong homotopy class of free space balls is generated using Algo. 10, that will result in infeasible motion for the arm. Assume that the obstacle here is tall. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70 4.10 4 representative test cases are shown here. It can be seen that the manipulator has to avoid obstacles in order to reach the part or move away from the part after grasping. This is where using multiple trees has an advantage. The tree which can reach the part easily is connected automatically. . . . . . . . . . . . . . . . . . . . 72 4.11 The plot shows the span time ratio observed for all 20 scenarios for each of the 4 planners. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74 4.12 The plot shows the average computation time in seconds for all the 20 scenarios for each of the 4 planners. The y-axis has a logarithmic scale. . . . . . . . . . . . . 75 4.13 The plot shows the average computation times for a range of sampling ratio for the scenes 3, 7 and 10. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79 4.14 The plot shows the average span times for a range of sampling ratio for the scenes 3, 7 and 10. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80 5.1 Two scenarios (a) and (b) with dierent gripper speeds are shown. In (a), S fast refers to the fast gripper speed and in (b), S slow refers to the slow gripper speed. S c refers to the gripper closing speed which is constant in both scenarios. The nominal pose of the part is in blue, the actual part pose is in red. The grasping time is dictated by the gripper closing time, which remains constant in both cases. Hence, in (a), the gripper starts closing further away from the part as compared to in (b). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83 5.2 The owchart of the oine computation for the Grasping success probability model for one pair of S g and S c . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86 x 5.3 These plots show the Probability of success vs the gripper speed and the closing speed for a part with (a) low uncertainty in part pose ( r = 2mm in x & y, = 5 o (in orientation)), (b) high uncertainty r = 12mm, = 15 o . The light green bars show the probability using extensive sampling of the part pose with the given uncertainty for the pairs of S g and S c . The dark green bars show the probability measured with active learning for the same S g and S c . . . . . . . . . . . . . . . . 89 5.4 Shows the number of discrepancies in prediction vs the normalized distance from the GSB for (S g , S c ) as (1, 0.15) . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92 5.5 Probability vs Number of samples . . . . . . . . . . . . . . . . . . . . . . . . . . . 93 5.6 The description of the test cases, the parts and the grasping strategies . . . . . . . 96 5.7 The speeds of the mobile base and the gripper for high (3 rad=sec) and low limits( 2 rad=sec) on joint rates, but same maximum for the mobile base speed limit are shown for Case 1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100 6.1 (a) A nonholonomic mobile manipulator carrying a long rod in a congested factory environment (start: red, goal: blue) (b) Workspace focusing with green spheres for the manipulator and with white disks for the mobile base . . . . . . . . . . . . . . 102 6.2 A sequence of free space spheres (green) for the EE from the start to the end along the MB free space disks (blue) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107 6.3 q near where, q near :q b = (x n ;y n ; n ) (blue) is a candidate nearest neighbor congu- ration for the randomly sampled conguration q rand where, q rand :q b = (x r ;y r ; r ) (red). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107 6.4 Extending from q near :q b = (x n ;y n ; n ) (blue) to q rand :q b = (x r ;y r ; r ) (red), lands up at q e :q b = (x e ;y e ; e ) (green) at a distance d e from (x n ;y n ) . . . . . . . . . . . 109 6.5 The 5 of the 20 test scenes for which the numerical results are presented in Tab. 6.1. Green: Start, Blue: Goal congurations . . . . . . . . . . . . . . . . . . . . . . . . 115 7.1 The spray nozzle attached to the end-eector of the UR5 robot of ADAMMS-SD. . 121 7.2 A simulation scene in CoppeliaSim [194] of ADAMMS-SD spraying liquid disinfec- tant in a washroom. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124 7.3 The spray cone showing the spray angle and the distancesD f andD n that form a frustum of the cone. The rate of deposition is modeled as a Gaussian distribution for the circular cross-section at a distance D n . . . . . . . . . . . . . . . . . . . . . . 128 7.4 The spray deposition rate modeled as a Gaussian changes as the distance from the nozzle tip. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 129 7.5 A pointP W is sprayed on, and a point P with its coordinates in the world coor- dinates are (x w ;y w ;z w ). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131 xi 7.6 The framework for robotic disinfection describing the sequence of operations to determine the nal trajectory of the mobile manipulator for disinfection of the surface represented by the point cloud. . . . . . . . . . . . . . . . . . . . . . . . . . 132 7.7 (a) The point cloud to be disinfected is shown (b) The projection of the point cloud on a plane perpendicular to the dominant normal of the point cloud is shown along with the polygon containing all the projected points. . . . . . . . . . . . . . . . . 134 7.8 The minimum radius of the spray circle on the point cloud is R n . However, when the point cloud is projected on a planar surface, the radius of the spray circle also needs to be projected. There are several projected radii, which are called R si for dierent angles of projection of the plane. . . . . . . . . . . . . . . . . . . . . . . . 136 7.9 A single spray segment. We assume that when the spray starts, the spray starts from the boundary of the polygon to avoid a corner to be left unsprayed. We will discuss in Sec. 8.5 how we modify the spray paths to spray the unsprayed corners. 138 7.10 The spray algorithm. The red area has not yet been disinfected; the blue area is the disinfected area. The polygon with a blue boundary is the oset polygon of the polygon to be sprayed. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138 7.11 The remaining polygon which has no oset polygon. . . . . . . . . . . . . . . . . . 139 7.12 Due to a narrow section of the polygon, the oset polygon is discontinuous. . . . . 141 7.13 The two dierent parallel paths from the same spray segments. . . . . . . . . . . . 143 7.14 The mobile manipulator and the capability map of the manipulator at the corre- sponding to the pose of the mobile base . . . . . . . . . . . . . . . . . . . . . . . . 144 7.15 (a) The planar path generated by the spray path generation algorithm and (b) corresponding path on the point cloud . . . . . . . . . . . . . . . . . . . . . . . . . 145 7.16 The spray cone axis should intersect the spray path waypoint inside the frustum of the cone. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146 7.17 The limits on the angle the spray waypoint transformation matrix z-axis makes with the line joining the waypoint position and the nozzle position. The dotted red line represent the instantaneous plane at the spray point position. . . . . . . . . . 147 7.18 The spray deposition rate for point P k as the spray passes through it. c 0 to c 8 are the circles (or cross-sections of the spray cone frustum with the point cloud) as the nozzle moves from left to right. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 156 7.19 The 5 test cases and the corresponding point clouds on which we tested our al- gorithm. (a) A wash basin sink (b) A workplace table and computer (c) Chairs representing a movie theatre or waiting area (d) Passage doors (e) A hospital bed . 157 xii 7.20 The polygons for the 5 test cases and the corresponding spray paths. The solid parts represent segments from the Algo. 12, and the dashed parts represent the connecting segments. The areas of the polygon not sprayed are highlighted for each test case. The \s" corresponds to the dashed starting segment of the spray path and \t" stands for the solid terminal segment of the spray path. . . . . . . . . 159 7.21 The spray path for the 5 test cases plotted on the corresponding point clouds. . . . 160 7.22 (a) This shows the spray eciency across the polygon for the test case (e). It can be seen that at the corners and at the ends of narrow extensions, the spray eciency is low as compared to the other parts of the polygon. (b) Bar plot showing the percentage of area that does not have spray eciency of 1 for all the 5 test cases. . 161 7.23 Percentage area not sprayed vs. the number of spray path iterations for all the 5 test case scenes. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162 7.24 For the test case (a): (a) The longest convex edge zig-zag path (b) principle com- ponent zig-zag Path and (c) Spiral path . . . . . . . . . . . . . . . . . . . . . . . . 165 7.25 Table comparing the longest convex edge zig-zag path to the principle component zig-zag path for all the 5 test cases . . . . . . . . . . . . . . . . . . . . . . . . . . . 165 7.26 Table comparing the Spiral path to the path generated by the spray path generation algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 166 7.27 The spray model for the physical experiment. . . . . . . . . . . . . . . . . . . . . . 169 7.28 The sequence of nozzle locations at 10 dierent locations of the nozzle tip path. . 170 7.29 (a) The scene for physical experiment (b) The spray path on the polygon (c) The point cloud and the overlayed spray path . . . . . . . . . . . . . . . . . . . . . . . 170 8.1 A Bi-manual Mobile Manipulator (BMM) has to attach a part (blue board) to the wall using a drilling machine. The BMM has the agents (M l ;M r ;B) (right arm, left arm, mobile base). If it is assigned M r for holding the part and M l for drilling as shown in (b), it will result in an infeasible and in-collision pose for doing the task. Whereas the opposite assignment will result in a feasible pose without collisions for the task. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 176 8.2 Task network for the example shown in Fig. 8.1. . . . . . . . . . . . . . . . . . . . 178 8.3 The layers described in our approach . . . . . . . . . . . . . . . . . . . . . . . . . . 181 8.4 Example of completely expanded search trees at dierent temporal windows of the three layers for the example in Fig. 8.2 . . . . . . . . . . . . . . . . . . . . . . . . . 184 8.5 Task No. 3 : The BMM has to pick up a box (green) and place it on the table, open it and pick up a part (blue) and place it outside. Then it has to pick up a small part (red) and assemble it with the blue part using a hammer . . . . . . . . 190 xiii 8.6 Task No. 1 : The BMM has to pick up a long part and attach it to the wall using a drilling machine (blue). After it drills on the right end, it has to move and drill in the center while holding the part. It has to nally drill at the left end to complete the task. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 191 8.7 Task No. 2 : The BMM has to take a long rod (red) out of a box (brown) and transfer it to a dierent location. It pulls out the part using the right arm, then holds it using both the arms and pulls it out completely. Thereafter, it transfers the part to a dierent location . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192 xiv Abstract Mobile manipulators are essentially robotic arms that can move around and hence provide ex- ibility in the kinds of tasks they can do. This signicantly reduces down-time of such robots as compared to xed manipulators. Moreover, moving the manipulator and mobile base concurrently can make mobile manipulators more ecient as compared to moving them one at a time as done traditionally. In certain scenarios, it may be even be necessary to move the mobile base and the manipulator concurrently. Motion planning for mobile manipulators with concurrent motions of the mobile base and the manipulator especially with a nonholonomic mobile base is a very challenging problem. In this dissertation, specialized techniques for motion planning of mobile manipulators are presented for three inherently important tasks. These tasks are namely object pick-up, transportation, and area coverage. In the rst task, the goal is to pick-up objects while the mobile base is in motion. The dissertation individually focuses on mobile base, manipulator and gripper motion planning under object pose uncertainty. If a mobile manipulator is tasked to pick-up an object from a table while the base is still moving, there are several decisions the robot must make. It must gure out how to move the mobile base such that it reaches close to the object since the intermediary point where the manipulator picks up the object is unknown. Moreover, it must determine when on the mobile base path should the manipulator start moving. Furthermore, the robot must gure out how to grasp the object such that it is the easiest to execute the motions. And nally, if there is uncertainty in the pose of the object, how should the gripper move so that the probability of grasping is high. These challenges are addressed in this dissertation by having: (a) a discrete xv search-based planner for the mobile base with specialized heuristics to guide it towards the object and a cost function for the time-delay caused by the manipulator motion; (b) three specialized techniques for determining manipulator motion on a moving mobile base and the grasping strategy; (c) an active learning-based method for determining the gripper speed and gripper closing speed for successfully picking up objects with pose uncertainty. In the next task, the objective is to carry large objects through a cluttered environment with a nonholonomic mobile manipulator. The goal is to nd a good quality solution for this challenging planning problem quickly. A bi-directional sampling-based method for growing rapidly exploring random trees with focused sampling for the mobile base and the manipulator has been developed for this. The focusing accelerates the trees towards the desired congurations in the workspace of the robot. Moreover connecting the two trees is challenging since the mobile base is nonholonomic. Hence, a heuristic for choosing how to connect the two trees has been developed that focuses only on attempting connections between appropriate kind of tree nodes only. The nal task is developing an area coverage planner for a mobile manipulator for spray-based disinfection of surfaces. In this regard, a framework to disinfect surfaces represented by their point clouds and execute motions with a mobile manipulator has been developed. The rst step is to project the point cloud on a plane and generate a polygon. On the polygon multiple spray paths are then generated using a branch and bound-based tree search algorithm such that each spray path covers the entire area of the polygon. Mobile manipulator trajectory to follow one of the spray path is then generated using a successive renement-based parametric optimization. Once the trajectory is generated, there is a need to make sure that the joint velocities of the mobile manipulator are regulated appropriately such that each point on the surface receives enough disinfectant dose. To this end, the time intervals between the robot path waypoints are computed such that enough disinfectant liquid is sprayed on all points of the point cloud that results in thorough disinfection of the surface. xvi The dissertation then focuses on executing tasks with a bimanual mobile manipulator. If tasks are to be executed with such a robot, assigning appropriate task to the appropriate manipulator is important. If the tasks are not assigned correctly, the notication of failure will be received only after expensive motion planners fail to nd a solution. This can be highly time-consuming. In this regard, a two-layered architecture for task-agent assignment and motion planning of bimanual mobile manipulators for executing complex tasks has been developed. Search trees are used in temporal windows to determine feasible task assignments of agents using task and spatial constraint-based heuristics. A caching scheme has been introduced for moving between dierent trees so as to avoid re-planning of those portions of the robot motion that were successful. This greatly reduces the number of calls to the motion planner as compared to direct motion planning after task-agent assignment. xvii Chapter 1 Introduction 1.1 Motivation Manipulators provide exibility in several tasks like picking up and transporting objects in facto- ries, painting automotive parts, assembly tasks, tending to machines, etc. This exibility is limited since the manipulator is xed to the ground. Making manipulators mobile can have several advan- tages over xed manipulators, like the ability to perform multiple types of tasks as compared to the single assigned task for a xed manipulator. This provides additional exibility and makes the manipulator signicantly more useful by reducing its downtime. Hence, mobile manipulators are becoming more and more popular for operations in factories, warehouses, shop oors as well as in homes. They can be used for a variety of applications. Some of these applications include trans- portation of parts for assembly and tending machines by extracting machined or 3D printed parts and transporting it to dierent cells for post-processing, spray and UV disinfection of objects by having a spray nozzle at the end-eector, etc. They can also be used for inspecting large parts for irregularities by having sensors like ultrasound attached to the end-eector. At homes, they can be used for assisting elderly people with daily chores. Hence, it is important to study applications for mobile manipulators and provide algorithmic foundations for their successful implementation. 1 Single and dual-arm mobile manipulators are complex robotic systems due to the high degrees of freedom they possess. To perform any task eciently with such complex systems, we need to study their motion planning. The motion planning for these systems is challenging because of the dierent set of constraints on the two dierent kinds of robots involved, the mobile robotic base and the manipulator. This is true, especially for nonholonomic mobile bases. Mobile manipulators provide exibility due to the mobility and dexterity they oer. However, in most applications, they are currently used by decoupling the motions for the mobile base and the manipulators. This can be detrimental when it comes to the time-related performance of the system. It is necessary to explore the coupling of the two in order to complete tasks in the least amount of time. Examples of tasks that can be performed eciently if the mobile base and the manipulator move concurrently are shown in Fig. 1.1. Fig. 1.1 (a) shows a mobile manipulator that needs to carry a long rod through a narrow door. It may be able to achieve this by moving the mobile base and the manipulator one at a time, but this is not ecient. Moving the two in coordination will make the process ecient and save time. Similarly, Fig. 1.1 (b) shows a mobile manipulator that needs to pick up an object from the table. Picking the object up while the mobile base moves is more ecient and may save a signicant amount of time in critical operations. Figure 1.1: (a) Mobile manipulator carrying a long rod [223] (b) Mobile manipulator picking up objects while moving [222]. Bi-manual mobile manipulators are even more complex systems. Motion planning of such systems cannot be dealt with without discussing task planning as well. It is important to specify 2 what motion should be executed by which manipulator, and determining this during planning time is mandatory for building an intelligent system. An example for this is shown in Fig. 1.2, where if we assign the task of holding the blue board to the right and the task of drilling with the red drill-gun to the left arm, it will result in a collision and failure to complete the task as shown in Fig. 1.2 (b). Figure 1.2: (a) Correct assignment of tasks to the manipulators (b) Incorrect task assign- ment. [220] For single and dual-arm mobile manipulators, the mobile base and the manipulator(s) should move concurrently. We need such robots to be able to program themselves i.e., make them autonomous for using them in complex tasks. They need to be able to plan their own trajectories in real-time based on high-level task descriptions. They also need to safely learn and suciently estimate process models, with minimal use of resources, to satisfy task requirements. Therefore, methods that can signicantly reduce the time, cost, and eort for programming robots for new tasks or dierent instances of the same task have the potential to improve industrial eciency and open up other opportunities for autonomous robots, thereby creating signicant additional value for the society at large. As mentioned earlier, the problem of concurrent motion for single and dual-arm mobile ma- nipulators is challenging due to the high degrees of freedom and motion constraints of the robots. 3 Moreover, if the objects such robots manipulate have uncertainty in their localization or if they're large, this makes the robot planning problem signicantly challenging. In this dissertation, we try to solve challenging problems pertaining to motion planning of mobile manipulators for grasping objects on the go with and without pose uncertainty, transportation of large objects in narrow passages, area coverage planning with applications to spray-based disinfection and a task-agent assignment methodology for dual-arm robots. 1.2 Goal and Scope There are certain inherent tasks that a mobile manipulator must perform in an ecient manner. Many other tasks can be performed if the robot is capable of performing these inherent tasks. Some of the inherent tasks for mobile manipulators are pick-up & transportation of objects, point- to-point motion planning and area coverage motion planning. In this dissertation, the focus is on solving these tasks with a mobile manipulator eciently by having concurrent motions of the mobile base and the manipulator. Moreover, we also study the motion planning for bi-manual mobile manipulators. For a bimanual mobile manipulator, the focus in this dissertation is on combined task-assignment and motion planning for generic tasks. Fig. 7.6 shows the dierent tasks studied in this dissertation. The goal of this dissertation is to develop computational foundations for motion planning of mobile manipulators to perform the mentioned inherent tasks in an ecient manner. This disser- tation presents traditionally diverse techniques- discrete state-space search, non-linear parametric optimization and exploration exploitation-based sampling to solve dierent aspects of motion planning for mobile manipulators. It presents novel contributions in each of these techniques to overcome the limitations of existing methods and improve computational eciency. The work presents a discrete-search-based motion planner for the mobile base with specialized heuristics so that the manipulator can pick up objects without the mobile base stopping. The 4 Figure 1.3: The overview of the inherent tasks for mobile manipulators in this dissertation work also presents a sampling-based methodology for motion planning of the manipulator on a moving mobile base. Thereafter, the work is focused on an active learning-based method to gener- ate grasping constraints for picking up objects under pose uncertainty with a moving gripper. The work is then focused on an exploration-exploitation-based sampling algorithm for point-to-point motion planning of mobile manipulators. The work then moves on to solving the area coverage problem for mobile manipulators using a projected polygon edge-based search methodology. Fi- nally, for the planning of bi-manual mobile manipulator motions, the work presents a tree-search with branch guiding and pruning heuristics. The main research issues addressed in this work are summarized below: • Pick-Up and Transportation of Objects: The motion planning for mobile manipulation for pick-up of objects/parts and their transportation is one of the problems studied here. It is a challenging problem as there is a complex interaction between three distinct entities, namely the mobile base, the manipulator, and the robotic gripper. This problem has been approached in the following sequence: (a) mobile base planning by taking into account the grasping constraints (b) Manipulator planning for grasping on the above mobile base path (c) high-speed grasping of objects with pose uncertainty with the robotic gripper. The 5 mobile base planning is done using discrete state-space search with manipulator motion constraints used as an added cost. The manipulator planning on a moving mobile base is done by introducing three accelerating techniques applied to sampling-based methods. • Point-to-Point Motion Planning: If large objects need to be transported in cluttered using mobile manipulators, sampling based methods can be used for the robot motion planning. However, this problem is inherently challenging since the conguration space of the robot may have narrow tunnels and the mobile base has nonholonomic constraints. The work presents a method to quickly nd good quality feasible trajectories for the mobile manip- ulator using focused and exploration-exploitation-based sampling for the mobile base and the manipulator. • Area Coverage Motion Planning: For area coverage, the focus in the dissertation is on spray- based disinfection of surfaces. For this, the point cloud of the surface to be disinfected is projected on a plane to obtain a polygon. It is challenging to determine a spray path that covers the entire area of the polygon. A branch and bound-based search is implemented on the polygon to nd a spray path that covers the entire area. Once the mobile manipulator trajectory is obtained, it constraints the spray nozzle to follow the generated spray path, an interval determination methodology is implemented to ensure thorough disinfection of each point of the surface. • Task Assignment and Motion Planning for Bi-manual Mobile Manipulation: For a bimanual mobile manipulator, it is necessary to assign appropriate task to the correct manipulator. If the assignment is wrong, it may result in wastage of computation time since the robot won't know of the wrong assignment until notication from the motion planner comes back. Hence, there is a need for a method that assigns tasks in an ecient manner. The work presents a tree search with branch pruning and guiding heuristics for ecient task-assignment and motion planning of such complex robotic systems. 6 1.3 Overview This section brie y summarizes the chapters of this dissertation. Chapter 2 presents a literature review on dierent kinds of generic motion planning algorithms for high degree of freedom systems and ones specialized for mobile manipulators, path constrained trajectory generation, robotic grasping & optimization-based algorithms, task assignment & mo- tion planning methods and area coverage algorithms. Chapter 3 presents a mobile base discrete search-based planning algorithm that uses state- space search with motion primitives and specialized heuristics. The optimal resolution planning is done by taking into account the time slow down of the mobile base caused by the grasping of the object by the robotic gripper attached to the manipulator. The above study considers a simplistic approach to manipulator planning for its use as a heuristic in the mobile base planning. In chapter 4, the manipulator motion planning with the mobile base moving along the path generated in the chapter 3 is studied. Here, the work describes how dierent grasping strategies can be integrated into a sampling-based manipulation planning framework. Three dierent speed-up techniques are studied to generate high-quality paths for a mobile manipulator grasping objects while in motion. Till now, the assumption is that the grasping happens when the gripper is stationary with respect to the object. However, for reducing execution time, the grasping of objects with a gripper moving at high speeds is studied. This is particularly important when there is uncertainty in the pose of the object. In chapter 5, the work uses active learning to study the interactions of this uncertainty with gripper forward and closing speeds. In Chapter 6, the point-to-point motion planning for nonholonomic mobile manipulators carrying large objects is discussed. Here, nding a good quality initial solution is the priority since for such robots, that can be a challenge due to the environmental clutter and nonholonomic constraints of the robot. A sampling-based method is used where there is sampling of the mobile 7 base and the manipulator end-eector congurations are focused in certain regions to decrease the computation time. Chapter 7 discusses area coverage with a mobile manipulator with a focus on spray-based disinfection of surfaces. For this, a projection of the surface on a plane is taken and a polygon for the same is constructed. A branch and bound-based method is implemented for each edge of the polygon as a node to determine the best spray path. For this spray path, a successive renement-based mobile manipulator motion planning approach is implemented. The trajectory determined by this is corrected to ensure thorough disinfection of every point on the surface. Chapter 8 introduces a study on bi-manual mobile manipulators as a natural extension of the previous chapters. This is an especially complex system to plan motions for, as any motion plan- ning problem has to be integrated with task planning. The complexity arises because the system has two manipulators interacting with each other via the external world as well as through the common mobile base. In this chapter, the work presents a search-based algorithm and heuristics to assign appropriate tasks to the manipulators in order to reduce the computational complexities in motion planning. The dissertation discusses the intellectual contributions of the work in chapter 9. It discusses the potential impact of the research and also identies avenues for future research. 8 Chapter 2 Literature Review Motion planning for autonomous systems like mobile robots and high degree of freedom (DOF) systems like manipulators and mobile manipulators are an extensively studied problem. This chapter reviews motion planning methods for all such robots for xed obstacles. Task and motion planning methods in literature, along with recent work in robotic grasping, are also discussed. 2.1 Point-to-Point Planning for High DOF Robotic Systems Point-to-point motion involves the robot going from a starting conguration to a goal conguration in a safe manner by avoiding obstacles. All these planners can be applied to high DOF systems like manipulators & mobile manipulators as well as mobile robots that typically have at most three degrees of freedom. Such planners are typically divided into four categories, sampling- based, discrete search-based, and optimization-based methods. We rst discuss the sampling- based methods. 2.1.1 Sampling-Based Planning Methods For high DOF systems, the conguration space of the robot is high dimensional. Such high di- mensional spaces are observed in many elds where there a large number of variables in the state space [65]. In robotics, to plan for motions for such high DOF systems, a feasible path must be 9 determined in the conguration space. Sampling-based methods have an advantage when plan- ning in such a high-dimensional state space since the sampling is dimension invariant. Popular sampling-based planners for this application include PRM [122] and RRT [142, 132] and their variations [8, 141, 132, 38]. These planners are typically computationally fast as compared to dis- crete search-based planners. These algorithms are extended to anytime algorithms. For example, Karaman et. al. proposed RRT* [118], which approaches optimality in an anytime fashion, i.e., given enough time, these algorithms rewire the current tree to generate asymptotically optimal solutions. Since this is an anytime algorithm, reducing the computation time is a challenge. How- ever, several modications have been suggested to RRT* like [105] which help in the convergence of the algorithm. However, the main challenge with these algorithms is that if the initial path generated by random sampling is distant from the actual optimal path, the quality of the solutions by RRT* based approaches would be bad unless given enough time. This is mainly due to the rewiring happens only locally and for that to extend globally to change, for example, the homotopy class requires extensive time. This is especially true in high DOF. Planners like [81, 83, 223, 222] perform focused sampling in the conguration space for determining the optimal solution faster. Similar techniques are used in [38], which describes a specialized planner for constrained mobile manipulation. In these planners, sampling is done in the conguration space where there is less intuition on where to sample; hence after an initial feasible solution is found, the focus is on de- termining the optimal solution at a faster rate. However, in certain cases determining the initial feasible solution may not be straightforward. One technique to address this is to sample in the workspace of the robot, as we have a good knowledge of the robot surroundings. We can then map the samples back to the conguration space. Sampling can be focused by incorporating articial potential eld (APF) [123, 217] into methods into RRT like in [183, 216]. Here random samples are moved towards the goal by performing gradient descent on the potential eld. The resultant conguration is then used as the random sample for RRT*. Such methods can be used for a high 10 DOF system like mobile manipulator in cluttered environments; however, dening the articial potential eld appropriately can be challenging. Planners like in [193, 149, 223, 185, 186, 184, 222], use the information of the workspace to sample in appropriate areas to accelerate the planning of high degree of freedom systems. In [193, 223], focused sampling towards the goal pose of the end-eector is done to produce high-quality solutions for xed and mobile manipulators. An exploration and exploitation-based technique is used so as to nd solutions in cases where the free space balls are arranged in the incorrect homotopy class in the 3D workspace. However, only workspace-based sampling may bias the congurations that may not result in the desired robot conguration at the goal location. In sampling-based methods, there have been several works on multi-tree approaches [138, 230, 237, 53, 231, 232, 66, 180]. Multi-goal path planning has been studied in [230, 180] where multiple trees grow from each of the multiple goals and connect to give a feasible solution to the traveling salesman problem. However, in our case, we have multiple goal trees, out of which only one needs to connect to the start tree. Tree switching for connection has been studied in [91], where multiple-goal trees are grown for a single start tree. The goal tree to connect to is decided based on distance-based simple heuristics. In [138], multiple disjointed trees grow in the conguration space for exploring and increasing local connectivity. This, along with [53, 231, 232] are single query planners where multiple trees grow, but there is a single start and goal conguration. To extend these planners to nonholonomic robotic systems, either random control signals are applied to the robot actuators or motion primitives are dened based on where the mobile robot conguration is sampled. Non-holonomic robots include car-like robots, dierential drive robots [70], UAVs [225, 224] and even spherical robots [69]. In [44], space exploration and heuris- tic search are combined for motion planning of nonholonomic motion planning. Actuator control commands are sampled such that the robot ends up in focus regions in the mobile robot con- guration space. [119] presents a computational geometry-based method for extending RRT* algorithm to handle a large class of nonholonomic systems with the use of Dubins' car dynamics. 11 In [170] a novel RRT extend function for ecient and smooth mobile robot motion planning is presented. For every pose of the robot, several trajectories based on the steering and forward actuator commands are generated. After sampling, the closest resulting robot pose is selected as the new node. All the high-DOF planners can be applied to holonomic and nonholonomic mobile manipula- tors. Work has also been done on task-constrained mobile manipulation planning [38], which is an extension of Informed-RRT* [82]. Since the mobile base in these is holonomic, determining the connection between start and goal trees is straightforward. These, along with BIT* [83, 51] can be extended to the planning of nonholonomic mobile manipulators. In these methods, the focus region is rened, as informed by each successive solution. As progressively better solutions lead to shrinking focus regions, optimal paths are found quickly. These methods perform exceptionally well for problems with the following two (not limited to) features: (a) determining the initial feasible path is easy and quick and (b) the length of the optimal path is relatively long i.e., there is no maze-like structure in the conguration space. Firstly, since the focusing is done based on a feasible path, it is necessary to determine a feasible initial path quickly. Hence, the performance of these algorithms is limited by the technique used to determine the initial path. Secondly, the focus region is based on the length of a feasible path. If the length of the optimal path is large (typically when there are narrow tunnels in the conguration space), the focus region will be comparable to the entire conguration space, resulting in a minimal focusing eect. The planning for nonholonomic mobile manipulator carrying large objects in cluttered environments does not satisfy either of these conditions and hence using these methods may not be ideal. Multi-modal and hierarchical planners like [177, 178] are very eective at generating feasible motions for mobile manipulators in complex environments under uncertainty. These planners plan for motions of the mobile base and manipulators separately based on the uncertainty in the pose of the mobile base. Planners like TGGS [36] use separate roadmaps for the base and the arm for planning. Such methods cannot be directly used when the manipulator and the mobile base need 12 to coordinate their motions. Sampling can be focused by incorporating articial potential eld (APF) [123, 217] into RRT based methods like in [182, 216]. Here random samples are moved towards the goal by performing gradient descent on the potential eld. The resultant conguration is then used as the random sample for RRT*. Such methods can be used for a high DOF system like a mobile manipulator in cluttered environments; however, dening the APF appropriately is challenging. Sampling-based motion planning for nonholonomic mobile manipulators has been studied in [168], where the robot has to track the end-eector trajectory. However, end-eector trajectories may not be available. 2.1.2 Discrete Search-Based Planning Methods Lattice-based or discrete search-based planners [136, 179, 148] are used with graph search algo- rithms are promising for motion planning of high-DOF robotic systems like mobile manipulators. These approaches compute the globally optimal paths but take signicant computational time. Hence, using these approaches requires the use of search space reduction method. Using these approaches independently for the mobile base and the manipulator for nding optimal paths for each, may not result in the globally optimal path. Hence, it is necessary to plan coordinated base-manipulator motions, which respect the joint limits, avoidance of self-collisions as well as collisions with obstacles in the environment. For nonholonomic mobile robot motion planning, sampling-based approaches and lattice-based planners are the most popular as they provide a way to have motion primitives which satisfy the constraint [140, 121]. Primitive-based approaches [162, 54, 88] are used for motion planning of redundant manipu- lators. Here, the adaptive primitives are executed in the search based on the distance from the target conguration. There may be perfectly viable paths of the mobile base where neither of the primitives of the manipulator is able to grasp the part. Hence, designing primitives for the appli- cation mentioned here becomes critical. Graph search-based algorithms can be used for nding solutions for high-DOF systems. These solutions are typically resolution optimal. An anytime A* 13 algorithm with informative heuristics and motion primitives to plan manipulator trajectory was used in []. Their work has been extended to [55], which presents an approach that uses adaptive motion primitives for manipulator trajectory planning. This method dynamically constructs the graph using static, inverse kinematics (IK)-based, and orientation solver-based motion primitives. The authors considered a weighted sum of metrics in position and orientation space as the heuris- tic for the search. The position metric was calculated using Dijkstra search in xyz space, which essentially gives the least cost path from any xyz point in the workspace to the goal xyz. The metric in orientation was the angle of rotation about a xed axis between the orientation of a state and the goal orientation. An algorithm to speed up high dimensional trajectory planning by adaptively searching in lower-dimensional state spaces was proposed in []. The authors ex- tended their work to develop a motion planning algorithm for manipulators with independent wrist joints [88]. They divided the high-dimensional planning problem into two lower-dimensional problems. The algorithm focuses on lower-dimensional planning to bring the wrist closer to the goal region rst. Then the algorithm uses IK-based tracking to reach the goal pose using high dimensional planning. The heuristics are often problem-specic and carefully crafted in search-based algorithms. Researchers have focused on using multiple heuristics simultaneously to reduce the eort on heuristic design and search convergence time. A Multi-Heuristic A* (MHA*) [7] algorithm was developed that combines the advantage of each of the heuristics. It iterates through multiple searches in a structured way using separate heuristic and maintains a separate priority queue for each search. [101] developed a method to generate heuristics for MHA* search dynamically. It monitors the progress of all the baseline heuristics and dynamically generates a new heuristic when the baseline heuristics are stuck in local minima. [102] developed a bounded, suboptimal, bidirectional search (A*-connect). This heuristic search algorithm behaves like the RRT-connect as it simultaneously searches from both start and goal nodes. The algorithm uses an approximate front-to-front heuristic to lead the forward and backward searches towards each other. The authors 14 used improved MHA* [165] with one guiding heuristic towards the opposite root and one towards the opposite frontier. They used a "connect" heuristic, which guides towards the most promising last expanded state of the opposite search, instead of scanning through the complete frontier. In [110] a discrete search-based for manipulator planning in conned workspaces was developed. The algorithm monitors the search progress and uses dierent search strategies in dierent parts of the search space. It seeks input from the human user when search heuristics are unable to provide guidance like in [185]. 2.1.3 Optimization-Based Planning Methods Researchers have also studied optimization-based methods for point-to-point trajectory planning. The major methods developed for manipulator trajectory planning are CHOMP [248], STOMP [114] and TrajOpt [201]. CHOMP improves the seed trajectory by using analytical gradients based on the scenarios in the conguration space. STOMP uses stochastic sampling around the initial seed to improve the solution using an estimated gradient based on the sampling. Like CHOMP, TrajOpt algorithm can be used to nd collision-free trajectories from nave, straight- line initializations that might be in collision. At the core of the TrajOpt algorithm is a sequential convex optimization procedure, which penalizes collisions with a hinge loss and increases the penalty coecients in an outer loop as necessary. Moreover, It also includes an ecient formulation of the no-collisions constraint that directly considers continuous-time safety. These methods provide eective and smooth motions for several kinds of robots. Any robot motion constraint can be integrated with such methods as optimization constraints. The main drawback of such methods is that they can get stuck in a local minimum due to the fact that collision are severely penalized and once a local minimum is reached, it is challenging to get out of it and nd a better solution. These methods work well to nd a feasible solution but may not be eective in determining optimal solutions. 15 2.2 Path-Constrained Motion Planning Constrained motions are motions where the robot is constrained to move in a certain ways. For example, in robotic 3D printing [2, 204, 19, 3, 4, 24, 21, 25, 22, 18, 23, 26], the nozzle needs to move along a constrained path. The area coverage paths or any end-eector paths need to be followed by a manipulator require the use of constraint motion planning algorithms. Researchers have explored sampling-based approaches to generate constrained point-to-point trajectories for high-DOF robotic systems like mobile manipulators [127, 128, 129]. Dalibard et al. [61] proposed an approach where task constrained Jacobian-based local motions were generated to connect nodes in the bi-directional RRTs for generating whole-body motions in humanoids. Berenson et al. [16, 15, 17] presented sampling-based point-to-point planners to generate robot trajectories that satisfy certain kind of constraints. The sampling for new nodes and possible targets was done by projecting random samples on constraint manifolds in conguration space. Hence, all the possible nodes of the trees satisfy the constraints of the task. Task-constrained mobile manipula- tion planning has been done by Burget et al. [38], which is an extension of Informed-RRT* [82]. Tangent Space-RRT [227] and Tangent Bundle-RRT [125] are another class of methods for con- strained sampling-based motion planning. Here lazy projection is implemented where samples are generated in the tangent space of the constrain manifold. They are projected on the manifold only when the tangent space approximation of the manifold is worse than a certain threshold to save computation time. In Atlas-RRT [103, 104], this is further extended for precomputing the tangent space approximations of the entire manifold at several points. Since there is no fur- ther need for projection on the manifold during the entire time of planning, this further saves computation time. Oriolo et al. [168] developed a motion planner for mobile manipulators to follow given end-eector paths. Padri et al. [172] presented a sampling-based approach for task constraint motion planning for mobile manipulators with nonholonomic constraints. They sam- ple points on the point cloud, which is the constrained manifold in the workspace, and use the 16 null-space of the constrained Jacobian to integrate the nonholonomic constraints, similar to the work done by Thakar et al. [223]. Cefalo et al. [43] presented a sampling-based planner that produces cyclic, collision-free paths in conguration space and guarantees continuous satisfaction of the task constraints. Cyclic motions require the robot's end-eector to continue tracing the same curve in cycles. The proposed algorithm relied on bidirectional search and loop closure in the task-constrained conguration space. The tness function was designed as a combination of pose error and eort. Tarokh et al. [218] also approached the problem using a genetic algorithm. The population diversity and tness were used to adjust the probabilities of the operators for the next generation. It can be very hard to identify a feasible region where all the constraints are satised using sampling in high dimensional parameter space. The genetic algorithm-based approaches can be computationally expensive and may fail to nd a feasible solution for complex motion synchro- nization problems with a high-DOF system and con icting constraints. Huang et al. [99] used B-Spline representation of the joints to achieve various optimization objectives. Shi et al. [206] used a NURBS (Non-uniform rational B-spline) representation of the joints for similar objectives. A genetic algorithm is employed to minimize cost functions with joint limits in both methods. Menasri et al. [161], discretized the trajectory and, at each step, performed a search for a new position of the end eector in the workspace to reach the nal position. Because of the redundancy of the robot, any end-eector position can be achieved by innitely many robot congurations. This enables them to nd the best conguration that allows for avoiding obstacles and the singularities of the robot. The genetic algorithm-based approaches can be computationally expensive and may fail to nd a feasible trajectory for complex motion generation problems like for mobile manipulators system since many of the constraints are con icting in nature. The path of the end-eector is often given in curve tracing applications. Conkur [57] presented an approach for a dierent kind of path following problem for hyper-redundant manipulators. For a given path, represented using B-Spline curve, the problem was to guide the links of the robot along the path 17 such that eventually, the robot takes the shape of the specied path or curve. The work presented a numerical approach to keep the links of the manipulator tangent to the curve and within a tight tolerance around the curve as they maneuvered along the path. However, this approach did not consider time-varying path constraints. Constrained Quadratic Programming (CQP) [74], Quadratic Programming (QP) [30] and non- linear programming [189, 190], have been used to generate a time-optimal trajectory for robots like mobile manipulators by minimizing pose error which is approximated using the Jacobian. Stilman et al. [214, 213] proposed an approach for high degree of freedom robot motion genera- tion by combining sampling-based and Jacobian-based methods. Shankar et al. [203] presented a QP-based approach to solving local motion planning for whole-body manipulation for mobile manipulators. Their formulation can take collision avoidance as constraint of QP or as part of the objective function. Escande et al. [73] presented an approach for trajectory planning of humanoids using hierarchical quadratic programming. The main idea of their approach is to solve the QP for one constraint at a time. The solutions for the latter constraints are found in the null space of the prior constraints. Giftthaler et al. [86] approached the trajectory planning for mobile manipula- tors using sequential linear-quadratic optimal control. It is often challenging to represent many constraints (e.g., mesh-to-mesh collision) in a quadratic format. Moreover, Jacobin-control-based approaches may not converge without proper initialization [80]. Also, Jacobian approximations may be accurate enough in certain areas of the conguration space and may result in trajectories that do not satisfy the constraints adequately. Toussaint et al. [226] discussed the use of optimization for robotic applications. Kieer et al. [124] addressed the path and trajectory generation in the robot conguration space from a given end-eector path in the robot workspace. They solved the path-nding problem in cong- uration space by representing each joint motion as a parametric curve and then converted it into the trajectory. Martin et al. [157] considered a parametric representation of each joint motion using B-Splines and formulated the minimum-eort trajectory generation problem as an SQP. 18 Their objective was to minimize torque over the course of the trajectory. The work presented a recursive algorithm for computing the objective function and the gradients of objective and con- straint functions. Halladay et al. [95, 94] presented an approach for generating conguration space trajectories for a given workspace path in the presence of obstacles. They use dierent types of distance metrics to measure the dierence between the desired path and the generated path after path constrained optimization. The performance of optimization-based approaches with paramet- ric representation for trajectory generation is highly dependent on the initial seed. Generating the seed trajectories or seed parameters for a given problem can be challenging. Finding an ap- propriate parametric representation and a good optimization seed can signicantly accelerate the optimization process as demonstrated by Kabir et al. [108, 112, 221]. Here an initial seed is deter- mined by computing inverse kinematic (IK) solutions at a high resolution. Multiple B-splines are used to represent dierent areas of the workspace paths using the path curvatures. The control points of the B-splines are used as the optimization variables, with the objective function being the trajectory execution time. The collision and several other constraints are applied. A succes- sive renement-based method is used to solve the highly non-linear and non-convex optimization problem. This ensures that multiple constraints do not contradict each other resulting in quick computation of the solution as compared to single-stage optimization where all constraints are applied together. This method works well when the workspace path is complex as it can break down the path into multiple simpler segments. Researchers have developed methods [29, 72] to optimize pre-dened trajectories or generate trajectories from pre-dened conguration space paths by minimizing time, jerk, and eort under joint position, velocity, and torque constraints. The solution for each joint angle for the complete trajectory is found either as a functional using optimal control [79] or as a parametric curve using discrete parameter optimization [175, 59]. Pfeier et al. [175] developed a dynamic programming- based procedure to solve the problem in minimum time by representing the curve in the robot's workspace using arc-length parameterization. Constantinescu et al. [59] solved the parameter 19 optimization problem using exible tolerance method. Piazzi et al. [176] represented the motion of each joint using cubic spline and used a minimax algorithm and interval analysis to solve minimum jerk trajectory generation problem. Chettibi et al. [47] solved the problem with cubic spline representation using sequential quadratic programming (SQP). Gasparetto et al. [85] combined jerk and time in their objective function and used B-splines [64] for approximating the trajectories. However, these approaches can only generate trajectories from a given initial conguration space path. To generate path-constrained trajectories, we can generate conguration paths from given workspace paths and then convert it to conguration space trajectories. Kieer [124] addressed the problem by generating path and trajectory in the robot's joint space from a given end- eector's path in the robot's workspace. The work solves the path nding problem in joint-space by representing each joint motion as a parametric curve and then converts it into the trajectory. Ordinary singularities of manipulators were dened as the dead points on the curve where the end-eector needs to pause, but the manipulator joints can keep on moving. The algorithm is based on the predictor-corrector method and can generate trajectories to pass through ordinary singularities by nding alternate joint congurations at the point of singularity. Search-based algorithms can be used for path-constrained trajectory generation. We can nd the joint conguration for manipulators at the waypoints on the workspace path by solving inverse kinematics (IK). There can be multiple valid joint congurations for each waypoint or end-eector's pose. In that case, a graph is constructed considering all the IK solutions for each waypoint. A graph search algorithm can then be used to nd the conguration space path as a sequence of joint congurations to move the end-eector through the waypoints by minimizing time or eort. Descartes [1] is an open-source library developed by ROS-Industrial Consortium that uses graph search on inverse kinematics (IK) solutions at sampled points on the workspace path for path constrained trajectory generation. 20 Search-based approaches can be computationally expensive as the branching factor in the graph increases exponentially with the number of joints of the manipulator, and the graph depth increases linearly with the number of waypoints used to approximate the workspace path. 2.3 Area Coverage Path Planning for Manipulators The problem we are trying to solve in this dissertation involves spraying the disinfection on the entire surface of the point cloud, which is a coverage path planning problem for the nozzle attached to the end-eector of the robot. Such problems have been studied extensively in literature in the survey paper by Galceran and Carreras [78]. Area coverage is required for several robotic applications like lawn mowing [246], agricultural eld plowing [166], bush trimming [116], spray painting [208], CNC machining [239], cleaning [109], nishing [20], composite sheet layup [155], inspection [27] etc. These problems involve coverage planning for mobile robots; however, we are interested in solving the coverage path planning for the end-eector of a manipulator. One of the eective methods for solving the coverage path planning problem is to formulate it as a traveling salesman problem (TSP) [233]. Kaljaca et al. [116] formulated the bush trimming problem with a manipulator mounted with a trimming tool at the end-eector as a TSP by using the mesh of the desired shape of the bush. Each triangle on the mesh was considered as a vertex for the TSP. Such an approach is eective only when there are not a large number of vertices. A framework for 3D surface coverage by redundant manipulator was implemented by Hess et al. [93]. Here dierent inverse kinematic solutions for the robot are treated as individual nodes in a graph which is modeled as a generalized traveling salesman problem (GTSP). GTSP is where the nodes of a graph are subdivided into clusters, and at least one node in each cluster needs to be visited. This method can work for mobile manipulators; however, the nonholonomic nature of the mobile base can make it challenging to nd feasible edges for the graph. When a point cloud with thousands of points is available, like in our case, solving TSP directly on it is infeasible. Leidner 21 et al. [144] presented a method to perform cleaning and wiping chores with a redundant robot Rollin' Justin. The medium of the wiping task is described by a particle distribution, whereas the motion plan is generated, splitting the task into a high-level planning module and a specic control for the required cleaning action. The objective was to optimize the Cartesian path length. Paus et al. [173] present a combined approach for robot placement and coverage planning for mobile manipulators. The approach takes constraints like collision and stability into account to determine appropriate base placements and solve the TSP for the end-eector. Yang et al. [238] solve the problem of non-revisiting coverage task with minimal discontinuities for non-redundant manipulators. In our method, the mobile base and the manipulator move in a coordinated fashion to cover the entire area. For industrial manipulator, coverage path planning for determining eddy currents in aeronautical parts is discussed by Olivieri et al. [167] using a zig-zag pattern. Glorieux et al. [87] implemented a non-random targetted viewpoint sampling strategy for coverage planning to cover the entire area for camera-based inspection of large parts. Coverage planning is one of the important aspects in CNC machining that has been optimized by Yao and Gupta [239]. In CNC, the most common cutting paths are either zig-zag or inward spiral. These paths provide advantages since they are easy to generate; however, they may not be the shortest paths. The shortest path in such cases can be a combination of zig-zag and spiral paths. They present an approach for generating such combinations using a search-based method that chooses dierent combinations of the edges of the polygon and eliminating such combinations based on certain pruning heuristics. For generating paths for spraying on the surface, similar approaches can be used. The dierence is that the cutting tool must remain perpendicular to the surface, whereas the spray can be at an angle to the normal to the surface. Kalburgi et al. [115] use a spanning tree-based coverage path planning algorithm based on approximate cellular decomposition for CNC machine. The cutter tool needs to pass through all the region that is required to be removed without any gaps. In our work, we implement an approach based on a branch and bound depth-rst tree search-based approach for area coverage of a 2D polygon 22 generated by projection of the given point cloud of the surface. This method gives us the exibility to choose one of the several paths it generates based on the ease of the robot the follow the path when projected on the 3D point cloud. 2.4 Robotic Grasping Signicant work has been done to study the eects of uncertainties in shape, part pose, contact, physics, dimensions, environment, etc. for grasping of parts [147, 68, 245, 52, 37, 126, 247, 163, 89, 133, 134, 154]. However, these studies deal with grasping when the gripper is stationary with respect to the part. Learning-based techniques have been used for grasping of parts using dierent types of grippers [145, 197, 35, 146, 50, 169, 34, 98, 76, 32, 195, 139]. Deep learning algorithms have been used for learning robotic grasps with vision-based techniques [106, 117, 135, 229, 187]. In the research on Dex-Net 1,2,3 & 4 [152, 153, 150, 151] the authors have learned grasping objects represented by point clouds with robot parallel-jaw and suction grippers. They have dened metrics of grasp robustness based on physics for thousands of 3D object models to train machine learning-based methods to plan robot grasps. In these studies, the gripper grasps an object that is stationary. The grasping methodology presented in Chapter 5, we assume that the grasping pose is given to us and we determine the feasible gripper speed and closing speed that will make it a successful grasp. In [212], a Dex-Net-based method for low precision mobile manipulator is presented. The idea is that the onboard cameras for such a mobile manipulator would result in low-quality depth images that need to be used to testing of the learned model. Moreover, the arm actuators are low precision as well; hence, the resulting grasps should take that into account as well when suggesting places to grasp any objects. Another method for grasping objects with a mobile manipulator is presented in [243]. The R-CNN-based method is used for mobile manipulators to not only grasp objects appropriately but also place them in the right 23 orientation at the right location. A review of several such grasping methodologies has been done in a survey paper [41]. Classication of grasps based on the degrees of freedom of the object and the grasp parameters using techniques such as Support Vector Machines (SVM) [174, 14, 143, 62] and AdaBoost [198] have been used. Moreover, vision-based active learning for robotic grasping was demonstrated in [196, 164, 158]. The focus in these works has been how to grasp the part. However, in our work, we assume that how the part is being grasped is given. Trajectory generation for grasping using physics-based simulations has been demonstrated in [130, 67]. Combined grasp and manipulation planning using trajectory optimization has been implemented in [97]. The grasping discussed in this paper is with a moving gripper. The interactions with the parts are dierent as compared to a stationary gripper. The methods in the literature do not discuss situations where the gripper is moving when the gripper's ngers are being closed. In most learning-based methods in the literature, grasp quality can be modeled based on pseudo-static physics. Hence, for data-set theoretical grasp quality is available. However, for a moving gripper, uncertainty and friction play an important role in whether the grasp is successful or not. Friction, when object has pose uncertainty, is very challenging to model and hence it is dicult to replicate such methods for high-speed grasping. Moreover, due to uncertainty, we can only give a probability of successful grasping. 2.5 Task Assignment and Motion Planning Robots like Herb 2.0 [210], HoLLiE [92], Rollin Justin [33] have been developed and studied for bi- manual mobile manipulation. In [241] parallelized planning and execution of tasks is implemented using Rollin Justin. Independent tasks are planned using parallel path planners and dependent tasks which need both the arms are planned using a single motion planner. Signicant work has been done in the combined task and motion planning for such robotic systems. An interface 24 between the task planner and the motion planner is developed in [211], which generates errors giving feedback to the task planner in terms of logical predicates when motion plans fail. O- the-shelf tasks and motion planners are used along with geometric information, making them ecient. Many specialized search-based approaches for solving similar tasks and motion planning problems have been implemented [113, 63, 188]. However, in these methods, failure of the motion plan is the only notication for an incorrect task assignment. In [28] geometric backtracking is implemented for combined task and motion planning. This enables the robot to backtrack into the task planning domain if motions are not feasible. Various heuristics have been suggested for combined task and motion planning methods [48, 84, 234]. Heuristics guide the search towards feasible motions, thus reducing the number of motion plan attempts. In our approach, We assume that a task network is already given to us. We focus on the task-agent assignment and motion generation for a bimanual mobile manipulator for the given task network such that motions are feasible and the task objectives are achieved. In systems like bi-manual mobile manipulators, the motions of the mobile base and each of the arms are coupled. Task and agent assignment and scheduling for robotic systems have been studied in detail in [10]. However, in these methods, multiple robots with shared workspace do not move at the same time. Geometric conditions of manipulators are integrated with a symbolic description for task and manipulator motion planning in [42]. This enables performing manipulation tasks with several robots and objects. Combined task and motion planning is studied for a bi-manual humanoid setup in [120] focusing on perception and plan execution. Mobile manipulator task and motion planning have been implemented in [236]. Here, a hierarchical planner is presented where kinematic solutions are determined for task-level problems to determine optimal solutions for abstracted problems. Mobile base positioning is especially important in bi-manual setups, where the two arms can perform independent tasks if the mobile base is located at the appropriate position [156]. 25 2.6 Summary The methods from the literature discussed in this chapter provide potential avenues to solve the problems discussed in this dissertation. In fact, the methods developed in the coming chapters use methodologies discussed in the literature as a basis over which specialized techniques are applied to modify them for mobile manipulator-related applications. This is true especially for motion planners, where we have developed mobile manipulator-specic techniques over generic motion planners to solve for challenging planning instances. 26 Chapter 3 Mobile Base Trajectory Planning for Part Pick-up 3.1 Introduction Many material handling and inspection tasks require manipulating parts and tools over large distances. Representative examples include moving a part from one machine to another machine in a job shop, moving an ultrasound sensor over a large structure to perform inspection, or moving of products in warehouses. Mobile manipulators that integrate a robotic manipulator and a mobile base in a single platform can be useful in such tasks[159]. Recent works have shown usefulness of mobile manipulators in a variety of applications [11, 131, 49, 200, 181]. A potential way to utilize mobile manipulators is to decouple mobility and manipulation modes like in [96]. This means that the mobile manipulator rst uses mobility mode to position itself at the task location. Then the manipulation mode is used to carry out the pick-up task. This decoupling of mobility and manipulation modes signicantly simplies the planning problem. The motion planning for the mobile base and the robot manipulator can then be solved independently, by using their respective existing methods. However, this approach has signicant performance limitations as it does not guarantee the shortest time operation for the combined system. For example, for the scene in Fig. 3.1, the mobile manipulator takes about 60 seconds to reach the goal position from the current location without picking up the part. If the mobile base stops and 27 Figure 3.1: An example scene in a factory then picks up the part with maximum joint rates of 15 degrees/second it takes up adds a delay of about 14 seconds to the path. Whereas picking up the part while moving adds only about 2 seconds delay to the path (Fig. 3.10). In most factory and warehouse scenarios like in Fig. 3.1, increasing the number of deliveries made by a single mobile manipulator has signicant performance benets. Independent planning of the mobile base and the manipulator increases the execution time leading to reduction in throughput. This may increase the necessity to have more mobile manipulators resulting in congestion and further degrading the throughput. Therefore, minimization of the task execution time is a desired objective when performing pick-and-transport tasks. This requires the mobile manipulator to use its mobility and manipulation modes simultaneously (i.e the manipulator picks the object while the mobile base is moving). Concurrent utilization of mobility and manipulation creates a need to solve a complex planning problem for the mobile manipulator. In this work, we present a pick-and-transport operation that requires the mobile manipulator to travel from an initial location, pick up a part with the manipulator while the mobile base is still in motion, and nally move towards a goal location. In 28 other words, objective here is to nd time-optimal trajectories for the mobile manipulator system that picks a part with a known pose and drops it o at a goal location. We assume that the pose of the part that needs to be transported is known. Instead of stopping the mobile base, the manipulator picks up the part while the mobile base is still moving. The uncertainty in the pose of the part will make this process less robust as compared to grasping when the mobile base is stationary. However, by carefully choosing the grasping strategy (i.e., the way the part is to be picked up) we can make the process robust while saving on the time. In this chapter, we focus on the motion planning of the mobile base from an initial pose to a goal pose while passing through an intermediary location. The grasping of the part with the manipulator should happen around this intermediary location. The main challenge here is that the intermediary location which will give the fastest path is initially unknown. Hence, it is not possible to dene an intermediary goal point for this location before the planning begins. However, we know an area surrounding the part within which if the mobile base is located, successful grasping of the part is possible. Hence, it is necessary to bias the search towards this area, grasp the part with the manipulator while the mobile base is still in the area and subsequently move towards the goal location. In most cases shortest mobile base paths lead to fastest mobile base trajectories. However, if the manipulator joint rates are limited, then the mobile base may have to slow down to ensure that the manipulator has sucient time to pick up the part. Hence, longer paths with no slow down may result in faster mobile base trajectories. On the other hand, there may be cases where stopping the mobile base to pick up the part is necessary in a time-optimal trajectory. The main contribution of this work is an algorithm that generates such time-optimal trajectories for the mobile manipulator that result in successful grasping of the part. 29 3.2 Background and Prelimnaries 3.2.1 Grasping Strategy and Grasping Area For a part, there can be dierent ways in which it can be picked up with a two ngered gripper. Each of these ways is called a grasping strategy. Fig. 3.2 shows a part and the three grasping strategies associated with it. It can be observed that all the three grasping strategies are robust to uncertainty in the pose of the part as they have a large overlap with a wide two ngered gripper. Hence, we consider only these grasping strategies. A grasping strategy consists of insertion, grasping, and post-grasping (or retract) poses of the end eector for any particular part pose, given by W T G (t). The grasping strategy to be used is given before the planning begins. Although grasping strategies shown in Fig. 3.2(b) and Fig. 3.2(c) seem to be mirroring each other, they depend on the placement of the part with respect to the table and may dier signicantly in terms of collisions and inverse kinematics (IK). For a part, these grasping strategies can be generated by performing grasp planning [107]. For a given pose of the part, the grasping strategy decides the pose of the gripper called the grasp-pose with which it is supposed to approach the part. For a given grasping strategy W T G (t), the pose of the part W T P , and the table orientation W T B , we can compute a region consisting of valid mobile base poses from which the entire grasping strategy is executable with respect to IK, and without collision between the table and mobile manipulator. We call this region the Grasping-AreaA g ()A G . Where,A g () is the grasping area dependent on the orientation of the mobile base . A G is the grasping area for the entire range of i.e., (;]. For each grasping strategy and the location of the part and its table, there is a grasping area. A Ga is the grasping area for the entire range of for the grasping strategy a. It can be observed from Fig. 3.3, that the grasping area is dependent on the pose of the part, the table and the orientation of the base. 30 The grasping area for a corresponding grasping strategy is computed by sampling collision free points for the pose of the mobile base. Further, checking if a valid and collision free inverse kinematic solution is available for the manipulator. This generates exhaustive grasping area for each orientation (i.e., yaw) of the mobile base. Figure 3.2: A part and the three associated grasping strategies 3.3 Problem Formulation The planner needs to compute a time-optimal, collision-free trajectory for the mobile manipulator. It should be feasible with respect to joint rates of the manipulator and the forward velocity and steering rate of the mobile base, between the start and the goal states of the Mobile Manipulator. 31 Figure 3.3: Grasping area for Grasping strategy b The grasping strategy given initially should be executed for picking up a part with a known pose. More formally, given: 1. The continuous state space = consisting states x = [ T ; T ], where = [x;y;] T 2 2 S 1 is the pose of the mobile base and = [ 1 ; 2 ; 3 ; 4 ; 5 ; 6 ] T 2 S 6 is the conguration of the 6 DoF robotic manipulator xed on the mobile base. 2. The start state is x M;S and the goal state is x M;G of the mobile manipulator. 3. lies within the range [ min ; max ] which for the manipulator(UR5) considered is (;]. 4. The continuous, state-dependent control action spaceU(x M )S 1 of the mobile base with each control action primitive m c = [V; d ] T consists of a forward speedV , and a heading d . All control action primitives take a constant time T p . V is constant for the entire planning process. Forward speed constraintV <V max . The continuous, state dependent action space for the manipulator is _ = [ _ 1 ; _ 2 ; _ 3 ; _ 4 ; _ 5 ; _ 6 ] T . The joint rate constraint _ _ max 32 5. W : The world frame of reference;P : Part frame;G : Gripper frame;R : Mobile base frame;M : Manipulator base frame;B : Table frame; i T j : Frame j with respect to frame i. 6. W e T P is the estimated pose of the part in the world frame. P T G is the gripper pre- grasping(insert) pose for a part poseP for a particular grasping strategyG s ( W e T P ). 7. The geometric regionO s = S K k=1 o s;k 2 , occupied by static known obstacles which may hinder the motion of the mobile base as well as the manipulator.O T 2O s is the table as an obstacle. A representative scenario is shown in Fig. 3.1. Compute: A time-optimal, collision free trajectory : [0;T ]! , satisfying velocity and joint rate constraints, such that (0) = x M;S , (T ) = x M;G and T is minimized, and the grasping strategyG s ( W T P ) is executed at time T g , where T g 2 [0;T ]. Each state x M (t) along belongs to the free space free =n obs =fx M (t)jM( M (t); M (t))\O s =;g fort2 [0;T ], where M( M (t); M (t)) 3 is the region in 3D space occupied by the mobile manipulator. 3.3.1 Candidate Approaches Planning of the mobile base is an important component of this work. We begin with a discussion of the possible issues with adopting existing approaches to generate the motion of the mobile base starting from an initial pose to the nal pose via some point in the grasping area. 1) One approach to this problem is to select a point on the boundary of the grasping area as an intermediary goal, which is minimizes the total length of the path from the start point to the goal point through this point. This may turn out to be a greedy method and as the search proceeds this intermediary point may not result in admissible heuristics. Hence the paths generated may be sub-optimal. Therefore this approach is not viable. 2) Another strategy to solve this problem can be to have two independent search problems, one for the mobile base to go towards the part and the second from the location of grasping to the 33 goal point. The termination condition of the rst one can be reaching the grasping area. However, for the part and the table poses and the obstacles as illustrated in Fig. 3.4, this approach gives a path with a60% longer path compared to a dierent solution. 3) The third possible strategy that can overcome the issues with the above two approaches can be to sample multiple intermediary points in the grasping area. Then implementing two searches like in the second approach for each of the sampled intermediary goal points. Paths through dierent intermediary goal points can be compared and the best path can be selected. In order to approach the optimal solution, this approach will require sampling the grasping area densely for the intermediary points. Each intermediary point will require running two dierent searches (one from start pose & one to goal pose) to compute the path. As we have mentioned before, the grasping action may need the mobile base to slow down. Therefore, the standard A* algorithm with costs based on distances cannot be used to compute the trajectory. Hence, a trajectory selection algorithm will become necessary, which will require modications to account for the mobile base slow down during manipulation. Using this approach will basically require running the modied A* algorithm a very large number of times to ensure that we have found a good quality solution. This can be computationally expensive and non desirable. The approach presented in Section 3.4 overcomes the limitations associated with the third approach. It does not explicitly searches through a set of prescribed intermediary goal points and therefore avoid unnecessary searches. The search process automatically selects the right intermediary goal point and computes trajectory through that point. 3.4 Planning Algorithm The planning of the mobile base is based on the lattice-based planning for dynamically feasible trajectories [148]. The lattice-based representation results in the discretization of the conguration space into a set of states and connections between these states. 34 Figure 3.4: Specic case in which two searches will result in sub optimal solution 3.4.1 Graph Representation Let G = (S;E) represent the lattice-based graph, where S is the set of all discrete states and E represents the transitions between any two states. Each state is the discretized 3 DoF state of the mobile base represented as a tuple (x;y;). And, each edge E is the from the set of predened feasible paths or motion primitives from one such state to another. For each state, there is a set of n motion primitives to chose from to move to the successive state. The mobile base is a dierentially drive robot and the primitives considered for its motion are constant time primitives, i.e the time to go from a state to any of its successor states is constant(T ). The turning time and the forward motion time will be dierent for each primitive such that the total primitive time is T . The following are the rules for marking colors to nodes during graph construction as shown in Fig. 3.5. They are formalized in algorithm 3 in lines 14 to 28. 35 Figure 3.5: Color Scheme in the search • The start node and the consecutive nodes are marked red if they are not in the Grasping AreaA g ()( is the mobile base orientation at the particular node). • Once a node is expanded inA g (), it will be marked as yellow. All children of a yellow node are blue. • All children of a blue or a green node are marked green. 3.4.2 Cost Function Since the objective is to minimize the time taken to go from the initial conguration to the target conguration, the cost function is in units of time. The cost function is F (S) = G(S) +H(S). Where G(S) is the cost to reach the node S from the start node and H(s) is the cost to go from node S to the goal. As mentioned in the Sec. 3.4.1 the time taken to traverse between any two 36 neighboring states(action primitive) is a constant T . The G cost of any state S 0 whose parent is S is G(S 0 ) =G(S) +T . The H cost is explained in the next section. 3.4.3 Heuristic We are interested in constructing a computationally ecient and admissible heuristic. In this case, the mobile base must reach the Grasping-areaA G before moving towards the goal point. Initially, the heuristic should guide the search towards the grasping area. Once, the search reaches the grasping area, the heuristic should then guide it towards the nal goal point. Fig. 3.6 explains the two dierent heuristics. The red nodes are the ones where the path hasn't reached the grasping area. Hence for them we use the heuristics isH 1 as described in Fig. 3.6a. For nodes which are in the grasping area(i.e yellow) or have an ancestor in the grasping area(i.e blue or green nodes), the heuristic is H 2 as described in Fig. 3.6b. The algorithm 1 describes how these heuristics are generated. The Fast- Marching method (FMM) [202] is used and generates an array with shortest distance values for each discrete point in the workspace to the point about which it is calculated. It must be noted that this is a pre-computation operation and takes about 3-5 seconds in MATLAB to compute for a given scene and part and table poses for all points on the boundary of the grasping areaA G with the discretization of space. Here usingA G , the union of all grasping areas makes sure that the heuristic is admissible. Algorithm 1 Compute Heuristic 1: function COMPUTE HEURISTIC(T;AG;Os)) 2: Consider all n points (P1;P2:::Pn) on the boundary ofAG 3: for Pi2 (P1;P2:::Pn) do 4: Compute FMMP i givenOs 5: end for 6: Compute FMMT for the Goal givenOs 7: for every (X,Y) location in the workspace do 8: H1(X;Y ) =min(FMMP i (X;Y ) + FMMT (XP i ;YP i ))=V 9: end for 10: return H = [H1;H2] 11: end function 37 (a) Heuristic before the search reaches the grasping areaAG (b) Heuristic after the search reaches the grasping areaAG Figure 3.6: Description of heuristics Algorithm 2 HEURISTIC 1: function HEURISTIC(S;T (Goal);H) 2: X;Y; =S:state 3: if S:color is RED then 4: return H1(X;Y ) 5: else 6: return H2(X;Y ) 7: end if 8: end function 3.4.4 The Algorithm The algorithm 3 describes the search procedure for the motion planning of the mobile base. The dierences from the traditional A search are the color scheme and a delayed evaluation of the path when particular types of nodes are expanded. Every successive node of the current node being expanded has a prescribed color based on the rules mentioned in algorithm 3. A typical search tree is as shown in Fig. 3.5. Any prospective path consists red nodes, followed by one yellow node and one blue node, and then green nodes. If the current node being expanded is a blue node, the grasping delay function is called. This function computes the delay if any caused due the grasping of the part with the given grasping strategy. Further, this delay is added to the g cost of the blue node. 38 The termination condition for the algorithm is that a green is chosen from the open list as the current node and should satisfy the termination condition. This makes sure that the path passes through the grasping area and has an evaluation of the delay caused due to grasping. Figure 3.7: Segment of the path where grasping happens 3.4.5 Manipulator Planning and Grasping Delay For the planning of the manipulator, we have used a simple inverse kinematics solver to provide a collision free motion from the home position( s ) to the insert pose. For picking up of the part, another inverse kinematics based solver is used which determines if the mobile base needs to reduce its velocity for grasping the part the given the maximum joint rates of the manipulator. The grasping delay function returns this delay. The grasping delay function is called when any blue node is expanded. The parent of a blue node is a yellow node which lies inside the grasping area denoted byA g () (orientation at the yellow node is ) as shown in Fig. 3.7. The orientation associated with the blue node is . The 39 Algorithm 3 Search algorithm for mobile base motion 1: function MobileBaseMotionPlanner(Tg;AG) 2: H Compute Heuristic(Tg;AG;Os) 3: g(Sstart) 0; Sstart:color RED 4: OPEN =;; CLOSED =; 5: insert Sstart into OPEN with 6: f(Sstart) g(Sstart) +Heuristic(Sstart;Tg;H) 7: while OPEN6=; do 8: pop S with the smallest f value from OPEN 9: if S:color =GREEN and S within goal tolerance then 10: return PATH 11: end if 12: if S:color is BLUE then 13: g(S) g(S) +GraspingDelay(S;S:parent; W T P ;AG) 14: end if 15: for each successor S 0 of S do 16: g(S 0 ) g(S) +T 17: if S:color is RED and S 0 = 2AG then 18: S 0 :color S:color =RED 19: else if S:color is RED and S 0 2AG then 20: S 0 :color YELLOW 21: else if S:color is YELLOW then 22: S 0 :color BLUE 23: else if S:color is BLUE then 24: S 0 :color GREEN 25: else 26: S 0 :color GREEN 27: end if 28: f(S 0 ) g(s 0 ) +Heuristic(S 0 ;Tg;H) 29: if a node with the same state and color asS 0 2OPEN and has lowerf value thanS 0 then 30: continue 31: elseif a node with the same state and color asS 0 2 CLOSED and has lowerf value than S 0 then 32: continue 33: else 34: Insert S 0 into OPEN 35: end if 36: end for 37: Add S to CLOSED 38: end while 39: end function 40 mobile base arrives at the yellow node with orientation , rotates by an angle at the yellow node, and then moves towards the blue node with an orientation . The grasping strategy as described in section 3.2.1 is implemented over the segment from points 1 to 2. Point 2 is at the edge ofA g ( ) along the line segment from the yellow node to the blue node. When the mobile base is at point 1(yellow node) with orientation , the manipulator is in the insert pose. And at point 2 it is in the post-grasping(retract) pose. Point 2 is the last point on the edge from yellow to blue nodes, where the post-grasping pose is feasible. The line segment from 1 to 2 is divided into m points. As the mobile base moves from 1 to 2, the manipulator goes from the insert pose to the grasping pose and then to the post-grasping pose. These motions are determined by interpolating between the IK of the manipulator over the m points. It may be possible that an intermediate IK(at one of the m points) of the manipulator may not be achievable if the maximum joint rate(s) is low and/or the velocity (translational and rotational) of the mobile base is high. In such a case, reduction of the velocity of the mobile base is the only solution for the intermediate conguration to be achievable. This results in a delay in the original trajectory of the mobile base. Such delays along the m intermediate points are added to give the total delay due to slowing down of the mobile base. It should be noted that having the post-grasping pose at the point 2 makes sure that the delay due to the limited joint rates of the manipulator is minimized. Also, since in the grasping strategies discussed before there are no large joint angle dierences, we assume that the congurations of the manipulator when interpolating between the IKs are valid. There may be cases where point 1 lies at the edge of the grasping areaA g () and point 2 coincides with it. In such cases, the grasping of the part will happen when the mobile base is rotating instead of translating. The same principles are used to calculate the delay in motion in such cases as well. After the post-grasping pose is reached, the planning to the g is done again using the inverse kinematic solver. 41 With the given joint rate constraints and the velocity of the base, using inverse kinematics of the mobile manipulator, it is computed whether the end eector grasping poses are achievable. If they are, then it results in no slowing down of the mobile base. If the end eector grasping poses are found to be impossible to achieve, the check is computed with a slower velocity(V s ) of the mobile base. If at any slower velocity of the mobile base, the poses are achievable the corresponding delay (VV s )4t=V is added to the g cost of the blue node. It must be noted that there will be cases when the mobile base has to completely stop in order to achieve grasping, either due to the placement of obstacles or due to the limited joint rates of the manipulator. This planner will successfully generate such trajectories. Figure 3.8: Computation time, number of expansions and Path cost vs the percentage of free space in dierent layout dimensions 42 Figure 3.9: Two paths for the given scene with one obstacle placed dierently and 70% free area 3.5 Results and Discussion We ran the planner with 11 motion primitives for the mobile base, with each primitive of a constant time of 1 second. The turning velocity of the mobile base was taken to be 1 radians/sec and the forward velocity to be 1 m/s. The maximum joint rates of the for the manipulator (UR5) was taken to be 15 degrees/second. Experiments were carried out on varying scene layouts ranging from 10 m10 m to 50 m50 m. The discretization of 20 cm is considered in each case. The percentage free space (i.e., obstacle density) is varied and the computation time, the number of expansions and the path cost are recorded. To decrease the percentage free area, obstacles are progressively added to an initially random scene. As the size of the scene increases, the obstacle size is also proportionally increased for the same scene. The results presented in Fig. 3.8 are obtained for 75 trials. The pre-computation of heuristics is included in the computation time of the search. The grasping strategy a (from Fig. 3.2) is used in each case with the corresponding grasping area. This area is marked in magenta in Fig. 3.9. For the following experiments, planning was performed o-board on a single core of a standard desktop CPU (Intel Xeon, 3.5 GHz) in MATLAB. The maximum computation time is about 110 seconds. This can be reduced considerably by having a multi-core implementation and parallelization in C++. 43 Figure 3.10: The time delay for the path in Fig. 3.9(b) vs the maximum joint rate for the manipulator For each scene size, it can be observed that the computation time and the number of expansions are similar in values from high to low percentage free area. However it is lower for lower percentage of free area. The reason for this is that the heuristic as obtained from the algorithm 1, guides the search through narrow corridors eectively as a result of FMM. This results in lesser node expansions due to lesser control actions feasible at each pose and hence less computation time. It can also be observed that the number of expansions and the computation time increases for a value of percentage free area as the scene size increases. Fig. 3.9 shows paths for the composite search from a starting location to the goal location via the grasping area for a dierent placement of a single obstacle. It can be observed that the path changes and passes through the grasping area on the other side. For the trajectory in Fig. 3.9(a), the grasping of the part happens when the mobile base is rotating. Whereas for Fig. 3.9(b), it happens when the mobile base is translating. Fig. 3.10 shows the time delay added to the path 44 in Fig. 3.9(b) as the maximum joint rates of the manipulator increases. For very high joint rates the delay drops to zero. The time delays if the same path is taken and the grasping happens when the mobile base has completely stopped at the node inside the grasping area is shown in blue. Even for very high joint rates, there is a considerable time delay in that case. 3.6 Summary We have developed a planner which generate time-optimal trajectories for the selected action discretization level. The resulting trajectory are able to simultaneously move the mobile base and the manipulator. It takes into account the joint constraints for the manipulator. This results in grasping of the part more naturally without stopping the mobile base motion, giving time-optimal trajectories with respect to discretization. The focus of this work has been the planning of the mobile base while the manipulator con- strains its motion. The joint limits of the manipulator add to the cost of the path of the mobile base. There are a few limitations of the planner presented in this chapter. The manipulator plan- ner for moving into the grasping area and implementing grasping is simplistic and purely based on kinematics. Furthermore, there may be a time delay due to the motion of the manipulator from the initial pose to the grasping pose at the grasping node, if the scene near the part is cluttered. Also, it may happen that the joint congurations of the manipulator for some grasp strategies for a part may not be valid when interpolating between IK solutions. Another limitation is that the manipulator starts moving at a particular node expanded before the grasping area. The delay caused to the mobile base can be reduced in certain situations when the manipulator starts mov- ing before. This may reduce the time taken to execute the path further. Moreover, the grasping strategy for grasping is given to the planner. If there are several grasping strategies optional, the mobile base motion time may be further reduced by using the appropriate grasping strategy. 45 Chapter 4 Manipulator Planning on given Mobile Base Path for Part Pick-up 4.1 Introduction Mobile manipulators integrate a mobile base with a robotic manipulator and are becoming more and more popular for the dexterity they oer. They can be used for inspection of large parts and machine tending jobs [13, 9]. Their utility and applications are shown in some of the recent works [11, 131, 49, 200, 181, 220, 111]. They are expected to be useful in warehouses and shop oors for pick-up and transportation of parts. Mobile manipulator motion can be achieved by decoupling its mobility and manipulation modes [96]. This implies that for pick-up operations like in the scenario shown in Fig. 4.1, the mobile manipulator rst stations itself near the part, and then the manipulator moves to pick up the part. However, this approach can lead to a high total motion execution time. In most shop oor and warehouse scenarios, like in Fig. 4.1, increasing the number of deliveries made by a single mobile manipulator has signicant performance benets. The interleaved motion of the mobile base and the manipulator increases the motion execution time leading to a reduction in throughput. Having more mobile manipulators is may not be a viable option as it may lead to congestion and, thus, a reduced throughput. Minimization of the task execution time is a desired 46 Figure 4.1: An example warehouse scene where the mobile manipulator has to pick up a part and transport it to the goal location. objective when performing pick-and-transport tasks. This requires the mobile manipulator to use its mobility and manipulation modes simultaneously (i.e., the manipulator picks the object while the mobile base is moving). In the previous chapter 3, we presented a search-based planner to optimize the time taken for the mobile base to go from a starting location towards the goal location while picking up a part/object on the way. A specialized heuristic guides the mobile base towards the part and through the grasping area (an area around the part within which grasping is feasible Sec. 4.2) and spend sucient time inside it so that grasping while moving is possible. The planner takes into account the constraints associated with the motion of the manipulator for grasping. In that planner, we x the nodes in the search where the manipulator starts and ends its motion. Moreover, we had presented a simplistic linear interpolation based planner for the manipulator, as the focus was on the planning for the mobile base. In this chapter, the focus is on the planning for the manipulator motions on a predetermined mobile base path using the method in [219] in a cluttered environment where such interpolation will result in self and external collisions. 47 The planning of the manipulator for grasping, on a moving mobile base is very dierent from the standard point-to-point planning problem for a ground-xed manipulator. A number of intermediate sub-goal congurations need to be determined, like the congurations where the manipulator motion and grasping begins and ends respectively. There are multiple end-eector poses with which a manipulator can grasp a part. We call each of these possible end-eector poses a feasible grasping pose. There are multiple mobile base locations where grasping can take place, and for each of those mobile base locations, there are multiple feasible grasping poses for the manipulator. The grasping pose to be used, the location of the mobile base on its predetermined path where grasping happens, and the associated manipulator conguration are all unknown and have to be determined for a given planning query. Sampling-based methods like RRT [132] provide the most promising and computationally vi- able approach for solving such motion planning problems since the state-space is high dimensional and the scene is complex. We can grow search trees by sampling randomly in the high-dimensional conguration space of the robot. If the sampling in the conguration space for growing these trees is done at random, especially in complex environments, it may result in longer computation times. Hence, there is a need to focus the sampling in appropriate regions of the conguration space to speed up the growth of the trees. Moreover, since we have multiple grasping poses, we grow a search tree from each of the congurations corresponding to a grasping pose and mobile base location, with a single tree growing from the start conguration. A decision needs to be made regarding which of the goal trees the start tree should connect to. In this chapter, we present three techniques addressing the above-mentioned issues. The rst technique is to have focused sampling for accelerating the growth of the start and the goal trees in appropriate regions. This is done by having a collision-free workspace region between the start and the goal end-eector positions and sampling inside it for the end-eector poses and mapping these to the conguration space. The second technique is biasing the search towards those manipulator congurations of a grasping pose, which are practically feasible to achieve. 48 This will help in determining the mobile base location where grasping should happen. The third technique is having a heuristic for choosing the appropriate tree to connect to. This will help in determining the appropriate grasping pose to be used based on the ease of achieving that particular grasping conguration. 4.2 Background and Preliminaries There can be dierent ways in which a part can be grasped with a two-ngered gripper. Each of these ways is called a grasping pose. Formally, a grasping pose is dened as the pose (homogeneous transformation matrix) of the gripper during grasping. For a given grasping pose and the pose of the part, we can compute a region consisting of valid mobile base poses from which the grasping pose is executable with respect to collision-free inverse kinematics of the manipulator. This region is called the Grasping-AreaA g ()A G . WhereA g () is the grasping area dependent on the orientation of the mobile base yaw (). A G is the grasping area for the entire range of i.e., (;]. For each grasping pose and the location of the part, there is a grasping area. A Ga is the union of all the grasping areasA g () for the grasping pose `a'. The grasping area for a grasping pose is computed by sampling a large number of mobile base locations near the part, with the same orientation and then computing whether a collision-free inverse kinematic solution for the manipulator is feasible. We can do this for a discrete set of 2 (;] and take a union of all the areas to generate the overall grasping area A G for a grasping pose. 4.3 Problem Formulation Let the workspace of the robot be denoted asWSR 3 . Let =fx;y;; 1 ; 2 ;:::; n g be the conguration of mobile manipulator with a manipulator with n joints. (x;y;) is the pose of the mobile base, i.e the location and the orientation. Let the geometry of the mobile manipulator at 49 Figure 4.2: The grasping area for a grasping pose a conguration be represented as a set of rigid bodiesM()W. LetOW be the set of workspace obstacles. LetCS be the conguration space of the mobile manipulator containing all the valid congurations of the robot. The set of congurations that lead to collision is denoted byCS obs =f2 CS : M()\O6=;g. The set of congurations that are collision-free is CS free =fq2CSnCS obs g. Let T be the homogeneous transformation matrix representing the pose of the End-Eector (EE) inW. For a given joint conguration , we can nd T by applying Forward Kinematics (FK). We use the dot notation to extract quantities i.e T:p denotes the position vector and T:R denotes the rotation matrix in T. For a given pose of the end-eector, we can nd a set of corresponding joint congurations by computing Inverse Kinematics (IK). A tree node n consists of a conguration , the EE transform frame T(). lies within the range [ min ; max ] which is (;] for the manipulator joints ( i ) & and (X min ;X max ) for x & y. 50 Let IK(T ee ; T M b ; ) be the inverse kinematics of the manipulator, that gives the conguration closest to when the mobile base pose is T M b and the EE pose is T ee . LetJ be the analytical Jacobian of the mobile manipulator which maps the joint velocities to the velocities of the EE i.e _ x =J _ , where x = (x;y;z;;; ) and , & are the zyx Euler angles. :q b denotes the pose of the 3-D mobile base and :q m denotes then-D conguration of the manipulator from the conguration . The objective is to plan the trajectory for a non-holonomic mobile manipulator to move from a starting conguration to a feasible grasping conguration and from that grasping conguration to the nal goal conguration. Given the geometric and kinematic robot model, workspace obstacles O, the start and the end congurations i ; f , the objective is to nd a collision-free path made of points Q =f k g K k=0 where 0 = i ; K = f and the non-holonomic constraint is satised between every two consecutive k :q b . Moreover, we want congurations g1 & gm whereg 1 ;g m 2 [0;K] andg 1 <g m such that FK( gi )= T G , fori2 [1;m]. T G being the EE pose for one of the grasping poses. This essentially implies that grasping happens when the gripper pose is constant (T G ) for a time T g which is the gripper closing time, implying that the time taken to go from g1 to gm is T g . 4.4 Planning Framework 4.4.1 Mobile Base Motion Smoothing The motion planning for the mobile manipulator is done in a partially coupled manner. We plan the motion of the mobile base rst, considering a simplistic motion for the manipulator. This is done in order to generate a trajectory for the mobile base through the grasping area with a sucient amount of time spent inside it so that grasping by the manipulator is feasible. The mobile base planning is done using the search-based technique described in our previous work [219]. This method gives the resolution optimal trajectory through the obstacles for the mobile base via the 51 grasping area, with a simplistic manipulator motion. We use this trajectory of the mobile base to plan for the manipulator motion. In this chapter, we focus on the motion planning of the manipulator when the mobile base moves on a given path. The goal is to determine where on the mobile base path, the manipulator motion should begin and end, as well as to determine which is the appropriate grasping pose to be used to grasp the part. Moreover, we also determine the appropriate velocities for the mobile base to achieve the above objectives. The mobile base trajectory generated ((t)) by the above planner is resolution optimal and looks like theP mb (planner) in Fig 4.3. We smooth this path by solving the following optimization problem, as described in [108]. P mb (t) = arg min P mb (t) R t f ti dt (4.1) s:t: P mb P mb (t)P mb (4.2) _ P mb _ P mb (t) _ P mb (4.3) g curve (P mb (t));(t)) = 0 (4.4) g coll (P mb (t)) 0 (4.5) g nhc (P mb (t)) = 0 (4.6) Here,P mb (t) is the mobile base trajectory consisting of thex;y; coordinates as a function of time. Eq. 4.2 and Eq. 4.3 represent the constraints on the pose of the robot and the corresponding rates respectively. The constraint on the mobile base trajectory P mb (t) to be close to the trajectory (t) found by the planner at each time instance is represented in the Eq. 4.4. The motion constraint for the non-holonomic motion of the mobile base is specied in Eq. 4.6 and the constraint 52 corresponding to obstacle avoidance is specied in Eq. 4.5. We use a parametric representation for each of the three degrees of freedom (x;y;) as follows. P mbj (u;w j ) = Ncp X i=1 R i;k (u)w j i ;u2 [0; 1] (4.7) The map between the arc-length parameter (u) and time (t) is the following,u = 0 whent =t i and u = 1 when t =t f . N cp is the number of control points for the one dimensional spline curve representingP mbj ,w j is the vector of control points for P mbj , andR i;k (u) are the basis functions parametrized with arc-length parameter u. Given the spline representation, the optimization problem is to nd the optimal set of control points (W ) whereW is a vector created by vertically stacking the vectorsw j ;8j. After smoothing we get a function P mb (u), where u2 [0; 1]. As u goes from 0 to 1, the mobile base moves from the start location to the goal location. We ensure that the mobile base moves with a constant forward velocity of V along the path P mb (u) using the constraint in Eq. 4.3. For the pick-up and transportation task to be completed in the least amount of time, the mobile base should ideally move at its maximum possible velocity i.e., V . However, if the manipulator joint rates are not high enough, it may result in failure to grasp. Therefore, we attempt to move the mobile base at a constant and its maximum velocityV , and slow down only when the motions of the manipulator are not kinematically feasible due to this high speed. 4.4.2 Manipulator Motion Planning With the parameterization described in Sec. 4.4.1, the mobile manipulator hereon is taken as a n + 1 DoF system [u; 1 ;::: n ]. u2 [0; 1] species the pose of the mobile base on the trajectory P mb (u). The manipulator trajectory is planned in 3 stages. The rst stage is to go from the start conguration (home) to a grasping conguration. The second stage is to complete the grasping 53 Figure 4.3: The mobile base trajectory generated by the planner (red) and after smoothing (black) (i.e., gripper closing), and the third stage is going from the nal grasping conguration to the home conguration. All these stages happen while the mobile base moves along P mb . It is desired that the manipulator motion starts as late as possible on the mobile base path. Fig. 4.4 shows the location on the mobile base path, where the motion of the manipulator starts is u I . How to determine u I is discussed later in this section. The manipulator does not have any motion as the mobile base moves from u = 0 to u = u I . Fig. 4.4 also shows the path of the mobile base inside the grasping area. The time for grasping the part (gripper to close) is T g . The time taken for the robot to traverse inside the grasping area (i.e from point A to D)A g is T Ag . In Fig.4.4, T Ag > T g . If the grasping starts at point A, it will end at point C. Point B is the last point on the path such that the grasping ends while the robot is inside the grasping area i.e., at point D. These points are chosen for when the mobile base moves at a constant (and it's 54 Figure 4.4: u I is the location on P mb where the manipulator motion starts. maximum) speed V . Hence, for successfully grasping the part, the robot should start grasping when the mobile base is at a point inside the segment AB. The grasping will then end at the appropriate point inside segment CD after time T g . The parameter u at points A and B is u a and u b respectively. And similarly at C and D it is u c and u d respectively. The points A, B, C, and D are dierent for dierent grasping poses as the grasping areaA g is dierent them. The rst part of the motion from the initial conguration to a grasping conguration is ex- plained here. Planning of the manipulator motion as the mobile base moves fromu I to the grasping area along P mb can be done using a generic bi-directional sampling-based approach [132]. The start tree has its root node at (u I ; I ). A goal tree starts at a random u2 [u a ;u b ] with the manipulator conguration to be an IK solution satisfying EE pose for that grasping pose. If there aren g grasping poses, there will ben g goal trees. Algo. 11 presents the algorithm for a multi-tree bidirectional RRT. Here there are multiple goal trees, one for each grasping pose growing with random sampling in the conguration space (u; ). The extend and the connect steps are made in conguration space like in [132]. The only dierence is that in these steps, kinematic feasibility is considered. Kinematic feasibility is checked based on the motion of the mobile base. The time 55 taken by the mobile base for the step can be computed using its maximum velocity. For feasibility, we check if each of the manipulator's joints can achieve its step in the above time by moving with its maximum joint velocity. If all the joints can achieve the step, we proceed with it. If one of the joints needs to move faster than what its maximum joint velocity allows, we move that joint by the max velocity in that step. This way, we end up moving towards +4, making sure that each step is collision-free and kinematically feasible. When the number of nodes in the goal tree in consideration and the start tree are equal, we attempt a connection between those trees with a random node in the start tree trying to connect to its nearest node in the goal tree. If the connection fails, we choose one of the other goal trees at random and repeat the process. This is done until we nd a successful connection. Algorithm 4 MT-BiRRT Algorithm 1: function MT-BiRRT(I;fn1g ) 2: T I fnode(i;)g, 3: T g 1 fnode(11;)g, 4: T g 2 fnode(21;)g; ::: 5: while t<tmax do 6: S rand RandomSample() 7: nnear NearestNode(T c;S rand ) 8: new Extend(nnear;S rand ) 9: if TotalNodes(T c)>TotalNodes(T o) then 10: Swap(T c;T o) 11: else 12: (fconnectsg;flag) Connect(T c;T o) 13: if flag is Failure then 14: T o SelectGoalTree(T g i ) 15: end if 16: end if 17: end while 18: return Path(T c;T o;fconnectsg) 19: end function u I , which is the point on the P mb where the manipulator starts its motion, can be anything between 0 to u b . If it is 0, it implies that the mobile base and the manipulator start moving at the starting conguration. However, this may not be safe as it may result in collisions with the environment, which may not be accurately mapped. Therefore, it may be desired that the 56 manipulator moves only when the mobile base is close to the part to be grasped. Hence, having u I as further away from 0 as possible is desirable. In Fig. 4.4, point A is the rst point where grasping can start. We determine the amount of time needed for the manipulator to go from the initial conguration to the grasping conguration at point A using maximum joint rates _ max . This can be done for the point A corresponding to all the grasping poses. We then nd the corresponding u I on the mobile base path such that the time for reaching u A from u I is the same for the mobile base as that for the manipulator. We choose the least of the u I s, for the dierent grasping poses to be the starting point of the manipulator on P mb . The idea is that the mobile base should move at its maximum possible speed V along P mb , start the manipulator motion as late as possible and slow down only if it is absolutely needed. The need for slow down emerges from the fact that the manipulator joint speeds are limited due to safety reasons. During Algo. 11, if we do not nd a feasible trajectory within a computation time t max , there can be two major reasons for it. The rst reason is that the u I is too close to the grasping location and it needs to be moved back so that the manipulator gets enough time to move. Reducing u I u I 4u will result in the manipulator to be in motion for a longer stretch onP mb . The other reason may be that the mobile base speed is high and the manipulator maximum joint rates are not high enough for its motions to be possible. In that case we reduce V f 1 V and _ f 2 _ . Here f 1 & f 2 are fractions and taken to be 1 1:1 . This results in a slow down of the mobile base so that the manipulator can successfully grasp the part. We can use either of these methods to start a new iteration. It is the decision of the user to whether iterate with u I or decrease the speeds. In the algorithm here, we use them alternatively. For u I u I 4u, instead of starting to grow all the trees again, we just grow a start tree from the new u I till it reaches the number of nodes as one of the goal trees and then attempt a connection. 57 4.4.3 Constrained Trajectory for Grasping The second stage of the planning after the manipulator has reached a grasping conguration for a grasping pose is discussed here. When the mobile manipulator EE has reached the grasping location, the gripper starts closing while the mobile base moves along P mb . To determine the manipulator trajectory, we perform IK computations using the previous grasping conguration as seed using the techniques described in Sec. 4.5.2. We also compute the time taken by the mobile base, and as soon as it exceedsT g , we terminate grasping. With this, we have the rst two stages of the planning, resulting in a motion from the starting conguration of the mobile manipulator to a conguration where the grasping of the part has been completed by the gripper (called post grasping conguration). We follow the same algorithm (Algo. 11) for moving the manipulator from the grasping con- guration to the initial (home) conguration, i.e. the third stage of the manipulator motion. The only dierence is that we have only two trees, one from the post grasping conguration and the other form the goal conguration (u F ; f ). Here,u F is the location on the mobile base where the motion of the manipulator terminates and only the mobile base moves till u = 1. We compute u F similar to the way we compute u I . 4.5 Methods to speed-up computations The above framework generates the trajectories for the mobile manipulator for part pick-up op- erations while the mobile base traverses alongP mb . However, the computation times can be large with the vanilla implementation described in Sec. 4.4. In this section, we describe three methods to address three specic drawbacks observed in the vanilla implementation. These methods speed the rst and the third stages of the trajectory, i.e. from the initial conguration to a grasping conguration and from the post grasping conguration to the nal conguration. 58 4.5.1 Focused Sampling A cluttered scenario for the mobile manipulator may result in narrow passages in the n + 1 dimensional conguration space of the robot. This can result in higher computation time for generating a feasible path. Hence, there is a need for specialized a technique to speed up motion planning in such cases. One such technique is to focus the sampling in appropriate regions of the conguration space. To achieve this focusing, we search in regions of the conguration space which are relevant to the problem and avoid searching in regions that are not. This focused sampling helps in moving the trees towards their goals faster, hence reducing computation time. Focused sampling in this work is achieved by generating free-space volumes in the workspace and sampling for the EE inside them. Here, such free-space volume in the workspace through which the EE passes is heuristically known as it is the volume surrounding the mobile base path. We use this to sample random EE poses in that free space and move the trees towards their goals. 4.5.1.1 WS Sampling to Bias Search We dene a free space ballB to be a sphere inWS, which is completely collision-free inside. We nd a sequence of connected free space ballsfB i g from the start EE position (T I :p) to the grasping EE position (T G :p), along the mobile base pathP mb . The algorithm is shown in Algo. 10. The rst free space ballB 1 is centered at the EE position at (u I ; s ). To compute the radius, we use the Euclidean Distance Transform (EDT) [75] to determine the distance to the nearest obstacle from the EE position voxel. Then, we uniformly sample n s points on the surface ofB 1 . We add these points to a priority queue based on their distances to T G :p. We then pop the m closest points and select the one closest to P mb in the xy plane. This ensures that we nd the free space balls along P mb . At the chosen point, we make the second free space ball B 2 . We continue this until the T G :p is within a sphere. Also, to make sure that T G :p is not very close to the surface of the nal sphere; hence we add a term r in the termination condition. This gives a sequence of free space balls along P mb like shown in Fig. 4.8 59 Algorithm 5 Generate workspace collision-free balls 1: function Ws Bals(T G ;Ti) 2: V , E , PQ 3: ri DistanceToObstacle(Ti:p) 4: Q1 CreateSphere(Ti:p;ri; ) 5: H(Q1) jjT G :pTi:pjj (rir ) 6: INSERT(PQ;H(Q1);Q1) 7: while PQ6= do 8: Qs POP(Q;m) 9: Q (T;r;Qparent) ClosestToMBPath(Qs) 10: V V[Q 11: E E[ (Qparent;Q) 12: ifjjT G :pT:pjj<r then 13: return (V;E) 14: end if 15: P U(Q;ns) 16: for pi2P do 17: if pi is outside all other spheres then 18: r 0 DistanceToObstacle(pi) 19: Q 0 CreateSphere(pi;r 0 ;Q) 20: INSERT(PQ;jT G :ppijjr 0 ;Q 0 ) 21: end if 22: end for 23: end while 24: end function 4.5.1.2 WS based Tree Propagation Here, we will explain the propagation of a conguration space tree using random sampling in WS. At every step, we sample a 3D position in an appropriate free space ball fromfB i g. We randomly chose an orientation to generate a T rand homogeneous matrix for the EE. We then drive the mobile manipulator at the current conguration (u i ; i ) alongP mb to as close to T rand as possible using Jacobian pseudo-inverse method [39]. This is described in Fig. 4.5. We use the analytical Jacobian J as a function of the robot conguration (u; ). We denote (u; ) by from the following equations to be the conguration space of the mobile manipulator along P mb . We determine the geometric Jacobian from the analytical Jacobian. J g =M( )J (4.8) Here, is the Z Euler angle at EE. Letp = T rand :pT i :p, ando =R i (Quat(R T i R rand ):xyz) where Quat(R) is the quaternion representation of R. Let (v;) == max(kvk 1 ;). 60 x = [(p; p )p ;(o; o )o] (4.9) = (J T g J g +I) 1 J T g x (4.10) =(; q ) (4.11) i+1 i + (4.12) u i u i1 m Sampled in 1 i i1 rand with random orientation j1 acobian inverse control Figure 4.5: A single Jacobian step to map workspace samples to conguration space We check for collisions along i to i+1 and stop if we encounter a collision. This is described in detail in Algo. 8. The expansion of the start tree T I and choosing the appropriate ball is explained in Algo. 6. We can do a similar propagation for a tree from a goal conguration towards the start conguration for bidirectional tree expansion. This focused sampling for the manipulator within the workspace balls accelerates the growth of the trees in the conguration space. 61 4.5.2 Conguration Space Patch In the framework explained in Sec. 4.4, for the goal conguration, we chose a random u between u a and u b and the corresponding IK solution at the EE conguration of the specic grasping pose, as the manipulator conguration. However, it is observed that in some scenes, there are obstacles hindering the motion of the manipulator at certain u2 [u a ;u b ]. The trees that have such congurations as the root nodes do not grow much. Hence, if such a u is randomly selected, it may result in a wastage of planning time. Since the manipulator congurations for u2 [u a ;u b ] for a grasping pose are similar, they can be grouped together, as shown in Fig. 4.6. It can be seen that only a few congurations can easily expand into free space. Figure 4.6: An illustration of congurations trapped behind a narrow passage in 2D. It can be seen that only a few congurations are able to easily expand out of the narrow passage for the goal treeT g1 . Hence, there is a need to bias the selection of u2 [u a ;u b ] for root nodes. For any grasping pose, we can nd IK solutions for the manipulator ( 1 ;::: n ) such that u2 [u a ;u b ]. For 6 DoF manipulators like UR5, we get analytical solutions and choose the one which is collision-free and the most similar to the starting conguration of the manipulator. For redundant manipulators with ( 7) DoFs, we use the method described in [108], with the starting conguration of the manipulator as the seed for the optimization. 62 Let the conguration of the mobile manipulator at pointA be (u a ; a ) such that the grasping pose is satised (FK(u a ; a ) = T G ). a is computed using the IK methodology described above. As the mobile manipulator moves from A to B, i.e from u a to u b , the manipulator conguration (IK solution) moves from a to b . We dene a conguration spaceH patch centered at (u m ; m ) where u m = (u a +u b )=2 and m = IK(T G ; a ) such that the dierence between Jacobians of the manipulator at a (u k ; k ) and (u m ; m ) where u k 2 [u a ;u b ] is bounded. Corr(J( k );J( m ))4 (4.13) where Corr() is the correlation function between the two input matrices [45]. It is used to determine how similar the two matrices are. The correlation coecientr (i.e Corr(A;B)) for two matrices A and B is computed as follows: r = P m P n (A mn A)(B mn B) q ( P m P n (A mn A) 2 )( P m P n (B mn B) 2 ) (4.14) where, A is the mean of all elements of A. When Corr is closer to 1 the input matrices are close to being equal. Also, FK( m ) = FK( k ) = T G H is a functional vector of the same dimension as .H 1 (s) represents the mobile manipulator conguration from a to b .H 1 (0) = a ,H 1 (1) = b andH 1 (0:5) = m . For the discussion, we assume that the condition of being a conguration space patch i.e Eq. 4.13 is satised. However, if Eqs. 4.13 are not satised, we can divide the length between u a andu b equally to create multiple patches that satisfy the conditions. To propagate a conguration space patch as a tree, we sample multiple congurations on the patch i.e. s2 [0; 1], and create a convex hull using those. The vertices of the convex hull are extended in the conguration space using a single random sample. The convex hull represents those congurations that are at the edge of the patch as shown in Fig. 4.6. If these points are able 63 2 s Figure 4.7: Example of two competing goal treesT g1 andT g2 for dierent grasping poses. It can be seen thatT g2 has a higher chance of connecting toT s as it has expanded more towards the start node. to extend towards the random samples, it implies that all the internal congurations are also able to extend. Once these points are extended, Eq. 4.13 is used to verify if there is a need to create an extra patch. We do this by using binary search alongs and check for the condition in Eq. 4.13 to verify if an extra patch is needed. This ensures that only those parts of the patches which have congurations that will extend more into the free space are propagated. In cases where there is clutter in the workspace near the part being picked up and the manipulator motion is obstructed, patch propagation helps in expanding only those grasping congurations foru2 [u a ;u b ] which can easily expand. When we try to connect a patch node from a goal tree to a node in the start tree, we attempt it at a few dierent s. Patches can also be propagated by sampling in conguration space as well as workspace. 4.5.3 Selection of Goal Tree The rst stage of the trajectory has multiple goal trees. A goal tree from the set (fT gi g) is to be selected when a connection attempt to one of the trees fails. One way to do so, as explained in Sec. 4.4.2, is to switch to a random tree amongst the other goal trees from the set (fT gi g). 64 2 3 4 5 6 tree T EE position of a node 7 8 Volume containing of the conguration workspace projection space patch mb tree T g2 H EE position of the center of a Patch cong space connection tree T g Figure 4.8: An illustration of free space ballsB i and the two goal trees growing inWS (for the rst stage only). The workspace projection of a conguration space patch is shown. It is essentially the 3D volume containing the EE position at all the manipulator congurations This way, connection attempts to dierent goal trees are equally likely, even if dierent goal trees have dierent structures. However, if the goal tree for a certain grasping pose is conned to a small region due to obstacles and the goal tree for a dierent grasping pose has grown extensively towards the start node inWS, both will have the same likelihood to be selected for a connection attempt. This can be seen in Fig. 4.7. On the other hand, if we make more number of attempts to the goal tree that has grown more towards the start node, we are more likely to nd a feasible connection. Hence, we need to bias the selection of the goal tree towards the tree(s) that have grown more towards the start node. As a measure of this growth, we use the mean distance of the leaf nodes of each goal tree to the start node inWS. Let there be n (> 1) goal trees, with the mean distance to the EE location of starting node (of the start tree) from the EE locations of the leaf nodes of the i th tree be d i . Then the probability of choosing the i th tree is given by the heuristic in Eq. 4.15. 65 p i = n P j=1;j6=i d j (n 1) n P j=1 d j (4.15) This ensures that if the mean distance of the leaf nodes to the start node (d i ) of the i th goal tree small as compared to the other tree(s), it will have a higher chance of getting selected and vice versa. We call this strategy to switch between goal trees to be intelligent tree switching (ITS). 4.6 Incorporating Speed Up Techniques in Planning Algorithm In this section, we present the overall algorithm (Algo. 6) by modifying the Algo. 11 to incorporate the ideas discussed in the above sections. The output of the Algo. 10 i.e. the workspace balls will be used as one of the inputs to the Algo. 6. Fig. 4.8 shows the schematic for the Bi-directional Rapidly exploring Patch Tree algorithm (BiRPT). The rst step is to get collision-free workspace regions along the mobile base path from the starting EE position to the EE goal position i.e. the grasping pose (T G ). The next step is to grow multiple trees. There is one tree growing from the u I , and the same number of trees as the number of feasible grasping poses from the EE goal position. Algo. 6 explains the details of the algorithm. The inputs to the algorithm are the EE poses at all the n g grasping congurations, the conguration space patches at those congurations the EE pose at the starting conguration, the sequence of free space balls, and the sampling ratio. We can sample in both the workspace and the conguration space of the robot. The sampling ratio is the ratio ofWS sampling toCS sampling and is set by the user. Nodes are created with the conguration (patch) of the robot, the EE pose, the parent node, and the free space ball the EE lies inside. There are multiple goal trees growing out of the grasping conguration in the conguration space for each grasping of then g grasping poses. We assign the balls to the trees in 66 the direction we want the trees to grow. Hence, for the goal trees, the balls are reversed in order. T gi is the goal tree corresponding to the ith grasping pose. In line 10 of Algo. 6 we sample either inWS orCS depending on the sampling ratioR s . When a random coin toss is greater thanR s , we sample inWS else inCS. When sampling in WS, we sample a random location in the current ball of the current tree-like in Algo. 7. Sampling is done with normal distribution around the center of the sphere with the specied for the sphere. The direction of sampling in the sphere is random. This can generate a sample inside as well as outside the sphere. When sampling inCS, random sampling is done over the entire space. HavingWS sampling helps in accelerating towards the goal. However, due to Jacobian control, the congurations can be biased without enough diversity. Moreover, in cases where the free space balls nd the wrong homotopy class like in Fig. 4.9, having conguration space samples helps in nding the solution quickly. Having onlyCS samples results in bad quality of solutions. The output of the function NextRandSample returns the random sample either inWS orCS and the corresponding label l. The NearestNode function in line 11 nds the node with the nearest EE position in the current tree if the label denotesWS. Otherwise, it nds the nearest node inCS. If the current tree is one of the goal trees, then we nd the node with the nearest EE position/conguration for the congurations corresponding to the patch centers. The Extend function used in line 12 is explained in Algo. 8. If the analytical Jacobian at the conguration of the nearest node is singular, then we do not proceed with extending the tree. Otherwise, if the label l suggests it is a WS sample, we move towards the sampled EE pose by computing4 using Eq. 4.9. If the label suggestsCS sample, then we compute4 in the conguration space and wrap the joint angles between (;] taking into account the joint limits. In the function ExtFeasible in lines 7 & 12 in Algo. 8, the move towards +4 is achieved by taking steps of =d. At every step, check for collision is made as well as a check for kinematic 67 Algorithm 6 MT-BiRPT Algorithm 1: function BiRPT(TI;fTgng; I;fHn1g;fBng;Rs ) 2: T I fnode(Ti; i;;B 1)g, 3: T g 1 fnode(Tg 1 ;H 11;;Bn)g, 4: T g 2 fnode(Tg 2 ;H 21;;Bn)g; ::: .8 grasping poses 5: T I:Balls fBng,T gi:Balls ReversefBng 6: T c T I ,T o RandomlySelect(fT g i g) 7: T c:current ball T I:Ballsf1g, 8: T o:current ball T g:Ballsf1g 9: while t<tmax do 10: (S rand ;l) NextRandSample(T c:current ball;Tg;Rs) 11: nnear NearestNode(T c;S rand ;l) 12: (Tnew; new) Extend(nnear;S rand ;l) 13: if new6= then 14: nnew node(Tnew; new;nnear) 15: nnew:Ball GetLastMatchingBall(T c:Balls) 16: T c UpdateTree(nnew) 17: T c:current ball: = (1)T c:current ball: 18: else 19: T c:current ball: = (1 +)T c:current ball: 20: end if 21: if T c:current ball:> 0 then 22: T c:current ball T c:current ball:parent 23: T c:current ball: = 24: end if 25: if T c:current ball:< 00 then 26: T c:current ball T c:current ball:child 27: T c:current ball: = 28: end if 29: if TotalNodes(T c)>TotalNodes(T o) then 30: Swap(T c;T o) 31: else 32: (fconnectsg;flag) Connect(T c;T o) 33: if flag is Failure then 34: T o SelectGoalTree(T g i ) 35: end if 36: end if 37: end while 38: return Path(T c;T o;fconnectsg) 39: end function Algorithm 7 NextRandSample 1: function NextRandSample(Bc;Tg;Rs) 2: if Rand()>Rs then 3: T rand :p N(Bc:;Bc:radius) 4: T rand :R U() 5: S rand ; l T rand ; WS 6: else 7: rand U() . uniform sampling inCS 8: S rand ; l rand ; CS 9: end if 10: return 11: end function 68 Algorithm 8 Extend 1: function Extend(nnear;S rand ;l) 2: if nnear: is Singular then 3: return 4: else 5: if nnear is patch then 6: Hnew Hnear +4 .4 depends on S rand and l 7: if ExtFeasibile(Hnear toHnew) at s = 0 & 1 then 8: return (FK(Hnew(0:5));Hnew) 9: end if 10: else 11: new near +4 .4 depends on S rand and l 12: if ExtFeasibile(near to new) then 13: return (FK(new(0:5)); new) 14: end if 15: end if 16: end if 17: end function Algorithm 9 Connect 1: function Connect(T c;T o) 2: T 1 GetConfigTree(T c;T o) 3: T 2 GetPatchTree(T c;T o) 4: nc1 RandomNode(T 1) 5: fnng kNearestNodeConfigSpace(T 2) 6: for ni2fnng do 7: if ConFeasibile(nc1: to ni:H(s)) at any s2 [0; 1] then 8: fconnectsg Interpolate(nc1:;ni:H(s)ni:H(so)) 9: return (fconnectsg;Success) 10: else 11: return (;Failure) 12: end if 13: end for 14: end function 69 feasibility, as explained in Sec. 4.4.2. Extend for a conguration space patch is done the same way as that for a normal conguration node, but at those s that are on the convex hull of the patch. With the center of the patch described by Eq. 4.13, we check for Jacobian condition and split the patch into multiple n new s if needed. If we nd a feasible conguration, a new node is created, and the last matching ball from the set of balls of the current tree is assigned to it. Lines 17 to 28 are used to change the standard deviation of sampling inside a sphere. If the tree is able to extend to nodes within the sphere, then reducing the of that sphere ensures focused sampling near its center. However, if the is too low (< 00 ) we initialize it again to . Moreover, if the tree is not able to extend to a new node, we increase the . This will result in samples outside the sphere with a higher probability. It helps in exploring other collision-free volumes of the workspace. This is particularly important if the homotopy class found by Algo. 10 results in infeasible motions as shown in Fig. 4.9. Such a situation often arises even though the free space balls are along P mb because every new sphere is generated by randomly sampling its centers on the surface of the previous sphere. It is likely that the point that is chosen as the center of the next sphere results in a situation like in Fig. 4.9. Here, samples in the correct homotopy class can help in nding a feasible path for the arm. These operations are done only when the sampling is done inWS. Figure 4.9: An illustration of how a wrong homotopy class of free space balls is generated using Algo. 10, that will result in infeasible motion for the arm. Assume that the obstacle here is tall. 70 We make sure that both the trees are growing equally by swapping them at every iteration. When the total number of nodes are equal, we attempt connecting the two trees. The connections happen in conguration space, as shown in Fig. 4.8. This is explained in Algo. 9. We rst choose a node randomly in treeT I . Then we nd the nearest neighbor from the other tree in the conguration space. Since the other tree nodes have patches, we choose the nearest node based on patch centers. We then try to determine if a feasible connection is possible with the nearest neighbor. We try to nd the connection feasibility for a few random s2 [0; 1] in this nearest node patch. The connection feasibility is checked similar to the extend step in the function ConFeasible. Collision feasibility is checked for every step, followed by kinematic feasibility. If, after a few attempts with dierent s, we are unable to nd a feasible connection, a failure ag is returned. We then move on to selecting a dierent goal tree in line 34 by calling the function SelectGoalTree using the intelligent tree switching heuristic as described in Sec. 4.5.3. We repeat the procedure until we nd a connection within the time out. If there is a timeout, and there is no feasible trajectory, we iterate u I and V as described in Sec. 4.4.2, till we nd a trajectory. This methodology generates a trajectory for the mobile manipulator from the start conguration to a grasping conguration i.e the rst stage. We need to follow the method described in Sec. 4.4.3 to determine the trajectories for stage 2 and 3. The stage 3 is similar to stage 1, with the dierences being that there is a single goal tree and no conguration space patch. It must be noted that, whenever we reduce V after a timeout and connection failure, there is a delay added to the motion of the mobile manipulator. It must also be noted that due to sampling in the entire conguration space, the three stages are probabilistic complete. The trajectory of the mobile manipulator generated by our algorithm is smoothed and we use this smoothed trajectory to check for path quality. Our smoothing method is based on the method by [90] with several modications. The trajectory generated by our method consists of several way points for the mobile base pose and the manipulator conguration. To smooth the path, 71 Figure 4.10: 4 representative test cases are shown here. It can be seen that the manipulator has to avoid obstacles in order to reach the part or move away from the part after grasping. This is where using multiple trees has an advantage. The tree which can reach the part easily is connected automatically. we take the rst waypoint and try to connect it to the last waypoint using the connect method described in Algo. 9. The connect method takes into account the kinematic feasibility and collision avoidance between the two points. If it fails to connect those two points, we attempt to connect the rst waypoint to the previous to the last waypoint. We continue this connection attempt to the farthest waypoint till a feasible connection is determined. Once a feasible connection for the rst waypoint is found to a dierent waypoint, we attempt a connection between that waypoint to the very last waypoint. This continues till we have a smooth trajectory from the start to the goal conguration. 4.7 Results and Discussion We dene span time to be the time the manipulator is in motion. For safety measures, ideally, we would want the manipulator to be in motion for the least amount of time. We evaluate the performance of the proposed planner for the span time and the computation time without some of the features discussed above so as to discuss the importance of each feature. Here we only evaluate the performance of the manipulator planner with the mobile base moving on a pre-computed path P mb (u). 72 4.7.1 Scenarios We have conducted experiments on 20 diverse scenarios to observe the overall performance of the algorithm. We have shown four representative scenarios with varying obstacles near the part to be grasped in Fig. 6.5. The following three scene attributes aect the complexity of the required trajectory to pick up the part: (1) presence of obstacles near the target part that aects the manipulator motion during the positioning of gripper at the grasping location, (2) desired grasp- ing pose and part shape that aects the manipulator motion during grasping, and (3) presence of obstacles near the target part that aects the manipulator motion during retraction of the gripper after the grasping. Obstacles and grasping pose make the computation of manipulator trajectory easy or complex depending upon how it performs collision-free prepositioning, grasp- ing, and retraction motions. We have carefully selected twenty scenarios that include adequate diversity and provide problem instances that have varying degrees of complexity in the required manipulator trajectories to perform prepositioning, grasping, and retraction motions. Amongst the 10 scenarios discussed in Tab. 6.1, scenarios 3-6 and 9 share similar characteristics in terms of obstacle placement that one of the grasping poses will result in goal trees less likely to get out in the free space towards the starting EE location. In scenarios 9 and 10 the object can be grasped by three grasping poses. 4.7.2 Experimental Setup We implemented our method in MATLAB on a Dell workstation with Intel Xeon 3.5 GHz CPU and 32 GB RAM. For each scenario, we pre-compute the Euclidean Distance Transform (EDT). The collision checks between the robot, the part and the environment are done using a spherical approximation of the robot and the part. The parameters q , p , u and are taken from [186] and4 is taken from [156]. The parameters , , 0 and 00 are determined by studying their performance like in [193] are as follows: : 0.2, q : 0.1, p : 0.1, o : 0.1, : 0.05,4 : 0.95, 73 : 1, 0 : 0.75, 00 : 3. We use spherical approximation of the robot with EDT [75] for collision detection. Figure 4.11: The plot shows the span time ratio observed for all 20 scenarios for each of the 4 planners. We have compared the proposed method with versions of the method without some of the features to check for the span times and computation times. The rst method is Multi-Tree Bi- directional RRT (MT-BiRRT), where in the rst stage, we use conguration space sampling with multiple trees growing for dierent grasping poses. The location of the mobile between u a and u b (Fig. 4.4) for the dierent grasping poses are chosen at random and corresponding collision- free IK solutions are taken as the manipulator congurations. The second method is multi- tree bi-directional rapidly exploring random patch trees with only conguration space sampling (MT-BiRPT-CS). The third method is similar to the second but with a sampling ratio of 0.5 for workspace sampling. In all the three methods, we use the alternating goal tree connection strategy, as explained in Sec 4.5.3, we label this as MT-BiRPT-WCS. The fourth method is the third method with the intelligent tree switching and labeled as MT-BiRPT-WCS-ITS. We compare only with bi-directional methods as they have superior performances as compared to single directional versions of the methods [132, 186]. The results for the average span time, 74 Figure 4.12: The plot shows the average computation time in seconds for all the 20 scenarios for each of the 4 planners. The y-axis has a logarithmic scale. average computation time, the collision checks, and percentage success for 10 scenarios are shown in Tab. 6.1. All results are averaged over 20 trials with a time limit t max to be 10 seconds. If the algorithm does not nd a solution in that time, then we proceed as explained in 4.4.2. The algorithm is given 300 seconds to compute a solution in each trial run. If it fails to compute a solution within that time, it is deemed as a failure, and its span time, computation time and the number of collision checks are not included in computing the mean in Tab. 6.1. Span time ratio is the ratio of the average span time for a scenario with the best-observed span time for all the algorithms for all the scenes in all the trial runs. The span time ratios for all the 4 planners are shown in Fig. 4.11. Similarly, the computation time ratio is the ratio of the average computation time for a scene to the best-observed computation time for all the algorithms in all the runs for that scene. The computation time ratio is shown for all the algorithms in Fig. 4.12. 4.7.3 Discussion We rst discuss the dierence between MT-BiRRT and MT-BiRPT-CS. In MT-BiRPT-CS con- guration space patches are used for the goal trees instead of single congurations. The purpose 75 Table 4.1: The span times, computation times, number of collision checks and percentage success for the planner with successively adding features mentioned in the approach section. of having patches is that a randomly sampled goal pose (u) for the mobile base between u a and u b may not have a manipulator conguration, which can extend onto the free space. For example, in Fig. 6.5, the obstacle surrounding the parts may hinder the motion of the manipulator at the randomly selected u. This will reduce the chance of connection with the other tree without a need to slow down. Moreover, when using a conguration patch, we attempt connecting to multi- ple congurations, making a connection without a slowdown more likely. Hence, we consistently observe that the span time is lower for MT-BiRPT-CS as compared to MT-BiRRT in Tab 6.1. However, the computation time for MT-BiRPT-CS is higher than that for MT-BiRRT mainly because of the overhead associated with extending multiple congurations instead of a single one as in MT-BiRRT. When we compare the above two methods with MT-BiRPT-WCS, however, we see a signicant decrease in computation time. This decrease is attributed to focused sampling towards the goal EE location. The free space balls inWS provide a collision-free route to the EE with Jacobian control, speeding up the growth of the tree towards the goal EE location. The goal patch trees also grow with samples in the free space balls towards the start EE location. This signicantly speeds up the growth of the trees towards each other in theWS. The connection between the two trees is carried out in theCS. Since both the trees are extended using Jacobian control, the congurations in both the trees may be very dierent and result in failures in connections. Hence, by performing sampling 76 in CS along with WS provides the required diversity in congurations to achieve successful connections. This also helps in getting manipulators out of cluttered environments where the homotopy class of the free space balls may not be right. Hence we observe a signicant decrease in computation time. Moreover, with the higher chance of connections, there is a decrease in the span time. With MT-BiRPT-WCS-ITS, we add intelligent tree swapping to MT-BiRPT-WCS. This is specically benecial in cases that have the conguration for one of the grasping poses to be near- collision or entrapped between obstacles. Such a situation can arise in cases 2 to 6, and 9. Here, one of the goal trees gets entrapped in obstacles and is unable to grow; however, the other tree with a dierent grasping pose can grow fast towards start EE location. Giving more preference to connect to the tree that grows more increases the chances of successful connections, and this signicantly decreases the computation time. The span time also decreases due to higher chances of connections. The minimum span times for each case are also shown in Tab. 6.1. It is the minimum computed over all the trials for all the variations of the algorithm. It can be seen that MT-BiRPT-WCS- ITS provides the mean span time closest to the minimum span time. This can be observed in Fig. 4.11, where we show the average span time ratios for all the four planners for 20 scenarios. It is the ratio of the average span time to the best-observed span time. It can be seen that the ratio for MT-BiRPT-WCS-ITS is closest to 1. This shows that MT-BiRPT-WCS-ITS consistently outperforms the other three planners. One of the important reasons for this observation is that aftert max , we reduce the velocity of the mobile base or decrease u I . This increases the span time as well as the computation time, which can also be observed in Fig. 4.12. It can be observed that the trend for the span time and computation time is similar from the Figs. 4.11 and 4.12. Moreover, the computation times for MT-BiRRT and MT-BiRPT-CS are similar even though the span times for the later are lower. This can be attributed to the fact that patch computation and the corresponding collision counts are higher for MT-BiRPT-CS as explained before. 77 The video for a dierential drive mobile manipulator executing the motions be found here: https://youtu.be/Du-2Hn2j1Y8. We have smoothed the trajectories generated by our algorithm as described in the last paragraph of Sec. 6.5. 4.7.4 Changing Sampling Ratio To observe the eect of sampling ratio on the span time and the computation, we have conducted experiments on three dierent scenarios. With dierent sampling ratios, we ran 20 trials for each scene for the MT-BiRPT-ITS algorithm. We measure the computation time and the span time for each trial. The results for the computation times are shown in Fig. 4.13 and for span times are shown in Fig. 4.14. It can be observed that the computation time drops when the sampling ratioR s goes from 0:01 to 0:1. At 0:01, there is 99% workspace sampling and 1% conguration space sampling. The high computation time and the corresponding high span time forR s = 0:01 can be attributed to the fact that the Jacobian method for extending congurations results in a less diverse set of congurations in the tree. Due to this, it becomes challenging to determine feasible connections between the two trees in the conguration space. Adding conguration space sampling provides the randomness needed to nd feasible connections. However, having only conguration space sampling without any workspace sampling results in unbiased growth in all directions and not towards the goal as desired. Hence, with 100% conguration space sampling results in higher computation and span times. It can also be observed that varying the sampling ratio from 0:1 to 0:9 does not cause too much variation in the computation and span times. Having the sampling ratio as 0 resulted in much higher computation and span times and hence was not the plots. 78 Figure 4.13: The plot shows the average computation times for a range of sampling ratio for the scenes 3, 7 and 10. 4.8 Summary We have presented an algorithm for the planning of mobile manipulators for pick-up and trans- portation operations. Firstly we plan for the mobile base trajectory taking a simplistic interpolation- based approach for the manipulator. This gives us the mobile base trajectory that passes through the grasping area, spending enough time inside it so that grasping while moving is possible. Fur- ther, we introduce a multi-tree bi-directional rapidly exploring patch tree (MT-BiRPT) algorithm for the planning of the mobile manipulator on the above mobile base trajectory. Here, the cong- uration space patch structure enables to expand multiple similar congurations at the same time. This, in turn, helps in reducing the span time and the computation time. We also introduce mixed workspace and conguration space sampling to bias the search to- wards the goal using the free space balls as the guiding heuristics. We make sure that we grow their sizes appropriately to ensure the wrong homotopy classes do not negatively aect the search. Furthermore, we introduce the intelligent tree selecting approach to make sure the goal tree, which has expanded towards the start tree, further gets priority during connections. This helps in signif- icantly reducing the computation time in cases where one of the goal trees is unable to expand due 79 Figure 4.14: The plot shows the average span times for a range of sampling ratio for the scenes 3, 7 and 10. to constrained grasping conguration. It should be noted that this algorithm can be extended into having a rewire function for RRT* type asymptotically optimal planning. Determining a better rst solution quickly will save time when extended to nd the optimal solution. Although there has been an attempt in coupling the mobile base and the manipulator motions during planning, we have kept them separate. This can limit the determination of time-optimal trajectories. In certain cases, the mobile base path computed initially may not be in the correct homotopy class for the true manipulator motion. Even if it is the correct homotopy class, there may be a need for perturbation so that the manipulator motion is achieved easily. In the future, we plan to integrate this perturbation into the planner based on feedback from the manipulator motion. We also plan to have a feedback mechanism where if there is a failure to compute the motion plan from the post-grasping conguration to the nal conguration with the grasping pose, it recomputes the rst motion to the grasping conguration from the starting conguration with a dierent grasping pose. Along with that, we plan to integrate continuous grasping poses into the planning framework in the future. To execute the oine trajectories generated by our algorithms on a real mobile manipulator, there is a need for an appropriate real-time controller. 80 We have explored Model predictive control in our previous work [56] which provides a promising avenue for such execution. In the planning framework presented in Sec. 4.4, one of the limitations is that the planning from the post grasping conguration to the goal conguration is attempted after the planning from the starting to the grasping conguration is completed. It assumes that the corresponding grasping pose will result in a post grasping conguration, which will always result in a successful plan to the goal conguration. This may not be true if the scene has clutter such that the tree from the post grasping conguration is not able to grow enough or to connect the two trees is challenging. Another limitation of the algorithm is having discrete grasping poses. There might be situa- tions where any of the selected grasping poses may not be viable. In such cases, there may be a need to search amongst a continuous set of grasping poses to determine the appropriate poses for growing the goal trees from. Moreover, another limitation of this work is that there is no option to perturb the motion of the mobile base in order to make sure that grasping can be feasible and appropriate grasping strategy can actually be used. 81 Chapter 5 Active Learning for Picking-Up Objects with a High-Speed Gripper 5.1 Introduction In chapters 3 and 4, the focus for mobile manipulator part pick-up and transportation was on mobile base and manipulator motion planning respectively. The object there was known to have no uncertainty in its pose and the gripper grasping it was stationary with respect to the object. However, this is not time-optimal as parts can be grasped while the mobile base and the gripper are in motion. This behavior is routinely demonstrated by humans who can pick up objects with their arms while walking or running. Therefore, we are interested in moving the mobile base and the manipulator simultaneously during the pick-up operation. We have demonstrated the feasibility of this idea when there is no uncertainty in part pose estimates [219]. In most cases, there is some uncertainty in part pose estimates when the mobile manipulator attempts to pick up the part. This uncertainty aects the speed of the pick-up operation. For example, Fig. 5.1 illustrates a case where the gripper is not aligned well with the part during the pick-up operation due to the uncertainty in the part pose estimate. If the gripper moves at a fast speed as shown in 5.1(a), the gripper nger will collide with the part resulting in a failure to grasp. On the other hand, as shown in 5.1(b) if it moves at a slower speed, the gripper ngers will align 82 with the part when they close resulting in a successful grasp. However, a slower gripper speed may require the mobile manipulator to slow down resulting in increased operation time. This illustrates the need for adjusting the gripper speed based on the part pose estimate uncertainty. fast slow d d t = t = t = 2 t = 2 Actual Part Pose Nominal Part Pose a) b) Collision Gap TOP VIEW Gripper Fingers Figure 5.1: Two scenarios (a) and (b) with dierent gripper speeds are shown. In (a), S fast refers to the fast gripper speed and in (b), S slow refers to the slow gripper speed. S c refers to the gripper closing speed which is constant in both scenarios. The nominal pose of the part is in blue, the actual part pose is in red. The grasping time is dictated by the gripper closing time, which remains constant in both cases. Hence, in (a), the gripper starts closing further away from the part as compared to in (b). The rst main problem investigated in this chapter is the eect of part pose estimation un- certainties on gripper velocities. We are interested in understanding and characterizing how the probability of successful grasping depends upon gripper speed and part pose estimation uncer- tainties. This characterization helps us in selecting the appropriate gripper speed based on the expected uncertainties in the part pose estimates. Characterizing the probability of successful grasps through exhaustive simulation of various combinations of underlying operation parameters is computationally not viable. Moreover, the contact physics between the gripper and the part with a moving gripper is complex and hence it is dicult to model the grasp success probability 83 analytically. Therefore, we will use an active learning method to characterize the probability of successful pick-up operation using simulations. Given the desired success probability, we are interested in moving the mobile base and the manipulator such that the pick-up operation time is minimized. This is done by accounting for the part pose estimation uncertainties and its eect on gripper speed. The second problem investigated in here is the problem of picking up the part using the manipulator while moving the mobile base from a given initial location to the goal location. The method will be designed for problems where the initial and goal locations are near the part location. We assume that a state space search will be performed to identify the mobile base's initial location and the goal location when we need to solve a large distance part transport problem [219]. The state space search is not the focus in this chapter. Mobile manipulator trajectories computed using the method described in this chapter can be used as motion primitives in the state space search to solve large distance part transport problems like the ones presented in Chapter 3. 5.2 Background and Preliminaries We dene four frames of reference. W is the world frame of reference. The mobile manipulator poses and velocities are with respect toW. P is the frame of reference attached to the part. B is a frame of reference on the table on which the part is kept.G is the frame of reference attached to the gripper. A pose is dened by the homogeneous transformation matrix T. The poses of the part, the mobile base and the gripper in any frameF are denoted by F T p , F T mb and F T g respectively. The frameB is such that the location of its origin is the same as the origin ofP, but the axes are aligned withW. The origin ofB changes with a change in the nominal pose of the part. The mobile manipulator is an n + 3 degrees of freedom (DOF) system with conguration variables as (x;y;; 1 ;:::; n ). (x;y;) are w.r.t. W. The kinematic model includes the 84 forward kinematics (FK) which maps the mobile manipulator DOFs to the gripper (attached to the end-eector) pose W T g and the Jacobian J, which maps the joint rates of the manipulator and the velocities of the mobile base ( _ ) to the gripper velocity W V g (6 1). The approach vector of the gripper towards the part in the frame B is ^ n g . The gripper orientation does not change while it is approaching the part. The pose of the gripper with respect to the frameP is G T P . A grasping strategy g is dened by the pair (^ n g , G T P (t 0 )), where t 0 is the time instance at which grasping ends. In essence, it gives the direction from which the gripper will approach the part, and the orientation of the gripper. The gripper velocity is denoted by V g inW. The gripper velocity in the frameW will be same as inB. The gripper speed is denoted by S g (this includes only the translation velocity magnitude). As the gripper orientation does not change during grasping, the angular velocity components of V g are zero. The gripper closing speed is denoted by S c . The standard deviation in the estimated part pose uncertainty is denoted by and the mean is zero. We dene as the grasping success probability threshold. In other words, during grasping, the probability of success should be greater than . 5.3 Problem Formulation Given initial , goal , W T p , g , and , the local trajectory generation can be represented as an optimal control problem. Our formulation is one that transforms the basic optimal control problem into one of nonlinear programming using direct transcription. minimize (t) T subject to C((t)) 0; 0tT Where,T is the time required to traverse a trajectory. C((t)) is a vector representation of the constraints on the mobile manipulator at time t which include, the end-eector or gripper pose 85 Figure 5.2: The owchart of the oine computation for the Grasping success probability model for one pair of S g and S c constraint while picking up the part, the Jacobian constraints for end-eector (or gripper) velocity, the mobile base non-holonomic constraints, grasping success probability threshold constraint, joint limit, joint rate, velocity constraints and also the self and external collisions constraints. To express the grasping success probability threshold constraint, we develop a meta-model that estimates the success probability as a function of S g and S c for a given (see Sec. 5.4 for details). 5.4 A Meta Model for Estimating Part Grasping Success Probability Grasping of a part while the gripper is in motion is a task sensitive to the pose of the part. Even slight uncertainty in part pose will result in a failure to grasp. Hence, it is necessary to model the success of the grasp with a particular grasping strategy as a function of the gripper speed, the gripper closing speed and the uncertainty in the part pose. One method is to sample extensively over a range of part poses, gripper speeds and closing speeds and have a grasping success check. However, to generate such a representation of success probability with a high accuracy, it is 86 necessary to sample extensively in this space. This approach requires large number of simulation runs and computation time. Hence, we develop an active learning based approach which gives a classication boundary between a successful and a failed grasp for a part and a particular grasping strategy. Contact physics between the gripper and the part when the gripper is moving at a speed is complex and it is dicult to model the motion dynamics analytically even for parts with simple surfaces. Moreover, when the ngers of a gripper close, instead of having a force and a form closure for grasping, it may result in collision with the part. This adds to the complexity of the problem. To build a meta-model for estimating the grasping success probability, we use a real-time physics engine (Bullet Physics in V-REP) for simulating the grasping of parts with a moving gripper. We dene grasping success conservatively, by measuring the gripper overlap on the part and checking whether the part is held inside the gripper while it is moving for a predened period of time. Also, the distance between a xed point on the gripper and a xed point on the part is measured for any changes throughout the gripper motion. If the part moves more than a threshold amount after being grasped, we label that as a failure. Using this notion of grasping success, we use the simulation data to build a classication model. Physical experiments with a gripper mounted on a manipulator for various gripper speeds and closing speeds have been conducted to verify the accuracy of the physics engine used for simulation. By placing three parts in dierent poses, we performed 90 physical experiments and estimated an accuracy of about 94% of the simulations in terms of success and failure of grasp. The success of a grasp is dependent on the part pose relative to the gripper, the gripper speed and the gripper closing speed. Given these inputs, determining success of a grasp can be viewed as a classication problem. The training example vectors can be 5 dimensional with a 3 1 part pose relative to the gripper (x;y;; this denition of part pose will be used in this section), the gripper speed (S g ) and the gripper closing speed (S c ) (Fig. 5.2). Some of these variables have predictable eect on the success of grasp and we can reduce the dimension of the input vector for 87 the classier. For example, for a certain pair of S g and S c , if the grasp is successful for a S c , it is also successful for a higher S c for the same S g and the part pose. Instead of using 5D input vectors, we use training example vectors consisting of only the 3 1 part pose relative to the gripper. So instead of having one classication model in 5D we havek models in 3D, and perform interpolation between them. We take inspiration from [171] where active learning was used to determine the contact sur- face for collision detection in high dimensional conguration space. We proceed with a physics simulator based grasp success evaluation function. 5.4.1 Active Learning for Generating Classication Model As discussed previously, it is dicult to analytically model the success of grasping for a moving gripper for a particular part pose, gripper velocity and a gripper closing speed. We use an active learning based approach to determine the classication surface in the part pose conguration space for pairs of gripper velocity and gripper closing speed. Our goal is to understand how much deviation from the nominal part pose will start causing grasping failures for a pair of S g and S c . We denote the classication surface as Grasping Success Boundary (GSB). For a given (S g ,Sc), the Grasping Success Boundary is GSB Sg;Sc . The GSB is dependent on the physics engine, we use Bullet Physics for out simulations. However, there is a margin of error for every physics engine, and this margin creeps into the computation of GSB as well. 5.4.2 Model Generation For a given pair of (S g ,S c ) and a pose of the part call to the grasp evaluator initiates a physics- based simulation instance which places the part at the sampled pose and the gripper moves with S g and S c using the desired grasping strategy. The approach for generating a nonlinear classier based on SVM [60] is illustrated in Fig. 5.2. The description henceforth is for a pair of S g and 88 Sc. The grasping for the nominal pose of the part is such that, the gripper grasps at a predened point on the surface of the part, with negligible slipping of the part after force closure. We start with a initial classier surface (GSB 0 ) with few part pose samples and rene it using active learning. Our goal is to actively select pose samples so that a better approximation of the grasping success boundary GSB 1 can be obtained subsequently. We start with equal weight on exploration and exploitation. During exploration after GSB i , we bias random sampling of new poses in areas that were not explored before. This renes the GSB in places which initially had only a few samples. If the prediction accuracy after exploration increases, we increase the bias towards exploitation. At every step, new pose samples are added and the GSB is updated. This procedure is repeated until the prediction accuracy of the generated model is greater than a predened threshold or when the total number of pose samples generated is larger than a given threshold. For rening the generated GSB, we sample near the boundary by choosing a pair of support vectors of opposite labels and nding their mid point in the feature space. This midpoint lies close to the boundary as stated by the maximal margin property of SVM [60]. This results in local renement of the boundary. Figure 5.3: These plots show the Probability of success vs the gripper speed and the closing speed for a part with (a) low uncertainty in part pose ( r = 2mm in x & y, = 5 o (in orientation)), (b) high uncertainty r = 12mm, = 15 o . The light green bars show the probability using extensive sampling of the part pose with the given uncertainty for the pairs of S g and S c . The dark green bars show the probability measured with active learning for the same S g and S c . 89 5.4.3 Constructing Success Probability Meta-Model Given a part pose uncertainty, our goal is to build a model that will predict success probability as a function of S g ;S c using the classier from the previous section. We dene Success Depth (SD) as the distance from the GSB inside the success region. It is approximated as, SD(p 0 ; GSB) = min p2GSB dist(p 0 ;p). We dene Failure Threshold Distance (D FT ) to be the distance from GSB into the success region, such that any pointp 0 withSD(p 0 ; GSB)D FT is always a success. At the GSB, there is high uncertainty in the success classication because of the physics engine approximations. Hence, we set aD FT to overcome this uncertainty resulting in a conservative denition of grasping success. Once aGSB Sg;Sc is generated satisfying the termination conditions, we query a large number of poses generated by the standard deviation in the part pose. They are labeled by taking into account their SD. The ratio of the number of successful grasps and the total number of query points is the probability of success for the tuple (,S g ,S c ). We do this for pairs of feasibleS g and S c for a particular. Furthermore, there is a need to interpolate to get the probability of success for a S g , S c pair for which this probability was not computed. For this, we t a surface and use its lower bound approximation to generate a conservative probability of success for any S g and S c pair. This surface is an analytical function ofS g andS c denoted by (S g ;S c ). Fig. 5.3 shows an example of the generated (S g ;S c ) for two values of . This (S g ;S c ) will be used in the grasping success probability constraint in Sec. 5.5. The Fig. 5.3 also shows the results for this method and compares it with the probability computations using extensive sampling for two dierent levels of uncertainty in part pose. Ex- tensive sampling required about 2000 samples to converge to a probability value for every pair of S g andS c . It can be observed that in all cases, the probability computed using active learning is lower than the probability computed using extensive sampling. The extent to which it is lower is dependent on . Because for a higher , the number of samples close to the GSB is higher and 90 samples with SD<D FT will be labeled as failure. It can also be seen from Fig. 5.3 that for lower uncertainty levels, the dierence between the active learning method and the extensive sampling method is less. The reason for this is that because of low, almost all the samples are away from the GSB, hence their SD >D FT resulting in an accurate prediction of success. For high in the worst case, the dierence between the active learning and the extensive sampling is about 10%. The uncertainty in the part pose typically depends on the vision system used for part detection. Generating the above GSB Sg;Sc is a one-time process for a particular part, grasping strategy, a grasping speed and a gripping closing speed. It can be queried with any . However, the method of extensive sampling for probability generation requires resampling with every new . Each simulation run for a sample is about 1.3 seconds and hence it is infeasible to simulate such a large number of poses. To train the model using active learning with exploration and exploitation, the number of samples that we need on an average is about 350 to 500. This greatly reduces the computation time. This can be explained by the D FT . Higher D FT results in a higher dierence in the proba- bilities from the two methods as a larger number of actual success poses are labeled as failure. However, this also implies that the sample is deep inside the success region and hence a higher condence. This can be observed in Fig. 5.4b. Larger is the distance from the GSB, lesser is the discrepancy from the actual simulation result. 5.5 Trajectory Generation 5.5.1 Denition The trajectory planning problem is formulated as a non-linear optimization problem. Given, initial , goal , W T p , g , and 91 Figure 5.4: Shows the number of discrepancies in prediction vs the normalized distance from the GSB for (S g , S c ) as (1, 0.15) minimize T s:t: (5.1) C mbpath (; _ ) 0; C grasping (; _ ) 0 (5.2) C uncertainty 0; C collision () 0 (5.3) C initial (; _ ) 0; C final (; _ ) 0 (5.4) C joint () 0;C jointrates ( _ ) 0 (5.5) Each of the constraints is explained as follows. Path Constraint for the Mobile Base: This includes the non-holonomic constraint ( _ x sin _ y cos = 0) as well as any other path constraint. We also have a constraint that during grasping, the mobile base should lie inside the grasping areaA g . Grasping area is an area around the part 92 Figure 5.5: Probability vs Number of samples within which when the mobile base is located, the end-eector can reach the part and grasp it with a specic grasping strategy [219]. These constraints are represented as C mbpath . Grasping Constraints: LetT 1 be the time at which grasping starts (gripper starts closing) and T 2 be the time at which grasping is completed (gripper is at the grasping location and closed). Therefore, 0 T 1 T 2 T . This constraint is specic to a grasping strategy g i.e (^ n g , G T P (T 2 )). During the time interval T 2 T 1 , the process of grasping is executed, i.e., this is the time required for the gripper to close. The pose of the gripper should follow the velocity constraint on conguration variables (J((t)) _ (t) = V g ) and pose constraint during grasping (FK((T 2 ) = G T P (T 2 )). Grasping Success Probability Constraints: S g and S c determine the probability of success as described in Sec. 5.4. Hence, in order to make sure that we are moving the gripper such that even in the presence of uncertainty the resulting grasping is successful, we use the function (S g ;S c ) generated in Sec. 5.4 for an uncertainty level to get additional constraints 93 onS g andS c . TheC uncertainty can be written as (S g ;S c ) 0. Where, is a given threshold of success to grasp. Other Constraints: Eqn. 5.4 represents the constraints on initial and nal conguration of the mobile-base. Eqn. 5.5 represents the joint position and velocity constraints of the manipulator. 5.5.2 Successive Renement Procedure We represent each DOF of the mobile manipulator as a polynomial in time ( i = P m k=0 a i;k t k ). The degree (k) of this polynomial depends on the expected motion. Our exploratory investigation demonstrated that a cubic polynomial is sucient to represent motions for the mobile base and the wrist joints and a quintic polynomial is needed for the base and shoulder joints of the manipulator. Let, q be the vector of optimization variables. It includes a 19;k (parameters of the poly- nomials), T , T 1 , T 2 , and S g . We have developed a successive renement approach to solve the optimization problem. In this approach, the initial solution for the next optimization step is generated by the previous step. The problem is set up as before. We discretize the time in the following manner for evaluating constraints. Time intervals [0;T 1 ], [T 1 ;T 2 ], and [T 2 ;T ] are uniformly sampled for m time instances in each interval. The constraints mentioned above are satised at each time instance. q 0 is the initial value (seed) for the optimization variable witha 1;1 =x i ,a 2;1 =y i anda 3;1 = i (x i ;y i ; i is initial pose of mobile-base). T 1 , T 2 , and T are initialized such that 0T 1 <T 2 T . Other elements of q 0 are assigned randomly. The solveNLP function takes in the seed, the objective function and the constraints and uses non-linear programming to determine a locally optimal solution. The following steps describe our approach. 1. q 1 solveNLP (q 0 ;ObjFunc;Constraints) where, Objfunc =T ; Constraints : C mbpath , C i;f , C joints . This gives a feasible trajectory for the mobile base. 94 2. q 2 solveNLP (q 1 ;ObjFunc;Constraints) where, Objfunc =T ; Constraints : C mbpath , C i;f , C joints , C gT2 . This step results in a trajectory such that the end-eector is at the grasping pose at time T 2 . 3. q 3 solveNLP (q 2 ;ObjFunc;Constraints) where, Objfunc =T ; Constraints : C mbpath , C i;f , C joints , C gT2 , C jacobian . This step results in a trajectory such that the end-eector follows the Jacobian constraints for t2 [T 1 ;T 2 ] and it is at the grasping pose at time T 2 . 4. q 4 solveNLP (q 3 ;ObjFunc;Constraints) where, Objfunc =T ; Constraints : C mbpath , C i;f ,C joints ,C gT2 ,C jacobian ,C uncertainty . This step ensures that the gripper speed is such that the probability of grasping success threshold is met for a given and P p . 5. q 5 solveNLP (q 4 ;ObjFunc;Constraints) where, Objfunc =T ; Constraints : C mbpath , C i;f ,C joints ,C gT2 ,C jacobian ,C uncertainty ,C coll . Finally we add the collision constraints for generating a feasible trajectory. The non-linear programming in each step terminates when the improvement in the objective function or the step size falls below a critical tolerance. is the nal trajectory generated from q 5 . Since the parametric equations in time are poly- nomials and the constraints on the manipulator are essentially between T 1 andT 2 , it may exhibit undesirable motions between 0 to T 1 and between T 2 to T . Therefore, we rene the manipulator trajectory in those intervals separately. We use STOMP [114], ensuring that the joint and velocity constraints at these points are met. 5.6 Results and Discussion We have considered a 9 DOF mobile manipulator with a dierentially driven mobile base (In- spectorBot), a UR5 manipulator, and a Robotiq 2-ngered gripper. We tested the planner on 6 dierent test cases, some of which are shown in Fig. 5.6. The three parts shown require three 95 Figure 5.6: The description of the test cases, the parts and the grasping strategies dierent gripper orientations for grasping. The test cases include dierent grasping strategies, which result in dierent directions of motion for the gripper and the mobile base. We can extend the study to any gripper and part orientation. In each test case, we vary the desired ^ n g while the mobile base follows similar paths. The initial location of the mobile base in each case is (0m; 0m) and its orientation is=6rad and the nal location and orientation is (8m; 0m) and 0rad respec- tively. We allow a tolerance (0.5 m in position and =4 rad in orientation) at the goal location. The nominal part location was (3m; 4m) for all test cases and the nominal part orientations were 0;=4;=2; 0;=4, and =2 for the 6 cases respectively. The grasping direction ^ n g is determined according to the nominal part orientations. The maximum linear velocity of the mobile robot is 2 m=s and the maximum joint velocity for the manipulator is rad/sec. We consider the same uncertainty levels as mentioned in Sec. 5.4. The algorithm was implemented using MATLAB 96 on a computer with an Intel Xeon 3.50GHz processor and 32GB of RAM. Interior point method from matlab's fmincon library was used as the optimization algorithm. For SVM, we have used Thundersvm [235] for fast computation in C++. The physics based simulations were conducted in VREP [71]. Table 5.1: This table shows the impact of the uncertainty in part pose on the velocities of the mobile base and the gripper Table 5.2: This table shows the impact of the uncertainty in part pose on the velocities of the mobile base and the gripper We have presented the error comparison of our successive renement based approach by bench- marking it against the non-linear programming with all the constraints combined together with randomly selected initial seeds satisfying 0T 1 <T 2 T . The results in tables 5.1, 5.2, 5.3 are for medium level uncertainty in part pose given by r = 7mm, = 10 o . is 0.96. We observe from table 5.1 the mobile base speed is higher in our approach. Since the trajectory time depends on the mobile base motion, a high speed plays a signicant role in reducing overall 97 Table 5.3: This table shows the impact of the uncertainty in part pose on the velocities of the mobile base and the gripper time. In table 5.2, we observe that the pose errors during grasping are signicantly lower for our approach. Moreover, the non-holonomic constraints for the mobile base motion are also lower. From table 5.3, it can also be observed that that the trajectory execution time is lower with our approach. The probability of success values from the model are similar for both the approaches. However, because of the high errors in the end-eector poses, actual successful grasping of the part may not be possible. We observe that the computation time for our method is comparable to the non-linear programming with all the constraints combined together. The use of successive renement method leads to use of signicantly improved seeds for successive stages of the optimization. This reduces the chance of the procedure from returning a poor local minima as the nal solution. It also helps reduce the computation time. Now we will discuss only our approach. We observe from table 5.1 that the gripper velocity in cases 1 and 4 is less than that of the mobile base. However, since the motions of both are in the same directions, the manipulator compensates for the motion of gripper so as to enable the mobile base to move faster resulting in a decrease in the time taken to reach the goal. For the cases 2 and 3, the mobile base velocity decreases because the manipulator has to compensate not only along the motion of mobile base but also perpendicular to it. Cases 4 and 5 can be explained similarly. In table 5.4 the results of low and high uncertainties for the cases 1, 4, and 6 are presented. It can be observed that the velocity of the gripper is higher for the case of low uncertainty and 98 lower in the case of high uncertainty. Subsequently, the mobile base velocities in the two cases are higher and lower respectively as well. Also, the probability of grasping success is higher for low uncertainty and lower for high uncertainty. Fig. 5.7 shows the capabilities of the planner when we change the constraints. As it can be seen, the mobile base velocity is high for high joint rates limits, while the gripper velocity is low. However, when we reduce the joint rate limits on the manipulator, the mobile base velocity also decreases as it has to slow down for the gripper to move with the desired velocity.. Table 5.4: This table shows the impact of the uncertainty in part pose on the velocities of the mobile base and the gripper 5.7 Summary In this chapter, we present an approach for solving two challenging problems encountered during part pick-up with a moving gripper. The rst one addresses the eect of the uncertainty in the part pose on the gripper speed. We present an approach for constructing a meta-model for estimating the success probability of grasping a part as a function of the gripper speed and gripper closing speed. The second problem addressed in the chapter is generating a trajectory for the mobile manipulator to satisfy the previously generated success probability of grasping constraints. We use a sequential renement method for doing optimization to generate trajectories. We show that this method performs better compared to a one-step optimization in terms of errors in part poses, the nal trajectory execution time. It also results in the mobile base moving at higher speeds with the manipulator compensating for the motion of the gripper. 99 Figure 5.7: The speeds of the mobile base and the gripper for high (3 rad=sec) and low limits( 2 rad=sec) on joint rates, but same maximum for the mobile base speed limit are shown for Case 1. The limitations of this work are that the grasping strategy needs to be given in order for the gripper to determine the probability of success. Moreover, the model has to be learned for a specic pair of the gripper speed and gripper closing speed. These limit the exibility of the method since now it has to be repeated for several pairs of the gripper speed and gripper closing speeds. Another limitation of this work is that during simulation, the behavior may not be similar to the behavior of the physical system, due to the approximation of the physics in simulation. This may for certain scenarios limit the accuracy of the method and the corresponding probability of success constraint. 100 Chapter 6 Accelerating Bi-Directional Sampling-Based Search for Motion Planning of Non-Holonomic Mobile Manipulators 6.1 Introduction Mobile manipulators are useful for transporting and delivering objects [219, 108, 31, 220, 111, 221, 13]. In the previous three chapters, we focused on pick-up tasks with a mobile manipulator. In this chapter, the focus is on transportation of large objects that have already been picked-up. In such transportation tasks, the manipulator can be at a \home" position on the mobile base (MB) and the need for its planning arises only when the MB arrives at the target location. However, in cases where large objects are to be transported, there may be a need to simultaneously move the manipulator and the MB, especially in cases where there is a narrow door or a passage. An example can be seen in Fig. 6.1(a), where the mobile manipulator needs to transport a long rod while avoiding contact with the machinery through a door. Motion planning for nonholonomic mobile manipulators is challenging due to the high number of degrees of freedom (DOF) and the motion constraints on the MB. Sampling-based meth- ods [142, 207] provide an eective way for motion planning of such robots. Bidirectional RRT like methods [132] provide a promising approach for motion planning of such systems. However, when the robot is carrying objects through congested environments, as shown in Fig. 6.1(a), narrow 101 Figure 6.1: (a) A nonholonomic mobile manipulator carrying a long rod in a congested factory en- vironment (start: red, goal: blue) (b) Workspace focusing with green spheres for the manipulator and with white disks for the mobile base 102 passages may form in the high dimensional conguration space of the robot, and it may take a long time to compute a feasible path. In factory settings with time-critical operations, this can be detrimental to the productivity. Hence, there is a need for specialized techniques to speed up motion planning for such systems. One such technique is to focus the sampling in appropriate regions of the conguration space. To achieve this focusing, we should search in regions of the conguration space which are relevant to the problem and avoid searching in regions that are not. To select relevant regions in the conguration space, we use the information provided by the description of the workspace, which is the environment surrounding the robot. The focusing technique considered in this chapter for the motion planning of mobile manip- ulators is shown in Fig. 6.1(b). Here, there are disks in the workspace along the oor from the start to the goal location of the MB. Also, there are spheres in the workspace from the starting end-eector (EE) location of the robot till the goal EE location. Focused sampling in the disks for the MB can be used to accelerate the search towards goal MB location. Similarly, sampling for the EE poses in the spheres and mapping the samples to the conguration space can also be used to accelerate the search. However, the focused sampling for the MB and the manipulator must be coordinated appropriately, so that together they are eective. We perform search by growing two rapidly exploring random trees [132] in the high dimensional conguration space of the mobile manipulator, one from the start and the other from the goal conguration, with samples being generated inside the focus regions. These two trees need to be connected in order to nd a feasible path from the start to the goal. However, connecting the two trees can be challenging due to the boundary value problem that needs to be solved for a feasible path between the connecting congurations of the respective trees [142]. For randomly selected nodes (one from each tree), this boundary value problem in most cases will not have a feasible solution. Moreover, attempting to solve a boundary value problem is computationally expensive and hence should be done between pairs of nodes that seem promising. Furthermore, even if there is a successful connection between the MB congurations of two nodes, the manipulator 103 congurations of those two nodes may not connect because those two congurations may be very dierent. Hence, there is a need for a heuristic, which helps in determining the nodes of the two trees, between which connection attempts should be made. This can help in reducing the number of unsuccessful connections attempts and hence reduce computation time. In this chapter, we present a method for motion planning of nonholonomic mobile manipulators carrying large objects. Our work makes the following contributions: (a) a method to coordinate the focused sampling for the MB and the manipulator congurations, and (b) a heuristic to deter- mine which nodes from the corresponding trees are to be selected for a connection attempt due to the nonholonomy of the MB. Finding a feasible solution quickly is challenging and of paramount importance in the context of mobile manipulation in congested environments. Therefore, our objective is to determine feasible solutions quickly. 6.2 Problem Formulation Let the workspace of the robot be denoted asW R 3 . Let q =fx;y;; 1 ; 2 ;:::; n g be the conguration of mobile manipulator with a manipulator with n joints. (x;y;) is the pose of the MB (MB), i.e the location and the orientation. Let the geometry of the mobile manipulator at a conguration q be represented as a set of rigid bodies M(q)W. LetOW be the set of workspace obstacles. LetC be the conguration space of the mobile manipulator containing all the valid congurations of the robot. The set of joint congurations that lead to collision is denoted byC obs =fq2C :M(q)\O6=;g. The set of joint congurations that are collision-free isC free =fq2CnC obs g. Let T be the homogeneous transformation matrix representing the pose of the End-Eector (EE) inW. For a given joint conguration q, we can nd T by applying Forward Kinematics (FK). We use the dot notation to extract quantities (e.g. T:p denotes the position component and T:R the rotation matrix). A tree node n consists of a conguration q, the EE transform frame T(q). The terms nodes and congurations are used interchangeably 104 and we make the distinction where appropriate. q:q b denotes the pose of the 3-D MB and q:q m denotes the n-D conguration of the manipulator from the node q. T s denotes the tree of nodes rooted at the start node andT g for the tree rooted at the goal node. We are interested in the path planning problem for a non-holonomic mobile manipulator. Given the geometric and kinematic robot model, workspace obstaclesO, the start and the end congurations q s ; q g , the objective is to nd a collision-free path made of congurations Q = fq k g K k=0 where q 0 = q s ; q K = q g and the non-holonomic constraint is satised between every two consecutive q k :q b . 6.3 Determining Focus Regions In this section, we explain the procedure to determine the focus regions for the MB and the EE, within which sampling happens during run-time for the bi-directional RRT [132]. 6.3.1 Focus Region for the mobile base (MB) The focus region for sampling MB locations is determined by generating disks on the xy plane from the start to the goal MB locations. The rst disk is generated with the start location of the MB (q s :q b :x; q s :q b :y) as the center. The distance to the nearest obstacle minus the incircle radius of the MB is taken as the radius. k points are sampled randomly on the circumference for the rst disk. The point closest to the goal location is chosen, and a disk is created with that point as the center and radius is determined as before. This procedure is followed until the goal location (q g :q b :x; q g :q b :y) is inside a disk. This results in a sequence of free-space disksfD i g from the start to the goal MB locations. This helps in focusing the sampling on being within those passages that are wide enough for the mobile manipulator to pass. Since this strategy to generate these free-area disks is greedy, we may not generate them along the shortest possible path. However, the greedy approach is fast and aligns well with our objective to determine a feasible path quickly. 105 6.3.2 Focus Region for Manipulator EE To focus the sampling for the EE, we determine a collision-free focus region in the workspace from the starting EE position to the goal EE position. We dene a free space sphereS to be a sphere inW with no obstacle inside. We nd a sequence of connected free space spheresfS i g from the start EE position (T s :p) to the goal EE position (T g :p). The rst free space sphereS 1 is centered at (T s :p). We compute the radius by nding the distance to the nearest obstacle using [75]. Then, we sample k points on the surface ofS 1 . We add these points to a priority queue based on their distances to T g :p. We then pop the m closest points to the goal EE location, and of those chose closest to one of the free space disk centers in (x;y). Then we make the next free space sphereS 2 . We continue this until the T g :p is within a sphere. This gives a sequence of free space spheres for the EE through the workspacefS i g. The sequence of free-space disks and spheres can be seen in Fig. 6.2. It can be seen that if we do not consider the disks to determine the EE free space spheres, they may pass through the narrow gap, resulting in inconsistency in the MB and the EE paths. The procedures for generating fD i g andfS i g is almost identical and is described in Algo. 10, (forfD i g m = 1 and exclude line 9). In the algorithm, we use ball to refer to a disk (2-ball) or a sphere (3-ball) where P is 2D or 3D location of the center, respectively. 6.4 Constructing Trees Construction of trees for bi-directional RRT has a few standard steps like nding the nearest neighbor, extend, connect, and swapping trees. In this section, we explain how each of these for a nonholonomic mobile manipulator is implemented to construct search trees. The random sampling is done inside either of the focus regions. A random sample is either the complete (3+n) dimension conguration q rand (i.e inC) where the MB position is inside one of the disksfD i g and its orientation and the n dimensions of the manipulator are uniformly random. Or, the random 106 Figure 6.2: A sequence of free space spheres (green) for the EE from the start to the end along the MB free space disks (blue) sample is a homogeneous transformation matrix T rand , with T rand :p being the EE position inside one of the spheresfS i g and T rand :R being a random rotation matrix. By abuse of notation, this sampling is said to be inW. 6.4.1 Finding Nearest Neighbors Figure 6.3: q near where, q near :q b = (x n ;y n ; n ) (blue) is a candidate nearest neighbor congu- ration for the randomly sampled conguration q rand where, q rand :q b = (x r ;y r ; r ) (red). The procedure to determine the nearest neighbor when sampling is done inC is dierent as compared to when sampling is done inW. To determine the nearest neighbor inC, we need to determine the nearest neighbor for the MB conguration rst. Due to the non-holonomic nature 107 Algorithm 10 Generate free-space balls 1: function FS Balls(Ps;Pg) 2: V , E , PQ 3: r1 DistanceToObstacle(Ps) 4: Q1 CreateBall(Ps;r1; ) 5: H(Q1) jjPgPsjjr1 6: INSERT(PQ;H(Q1);Q1) 7: while PQ6= do 8: Qs POP(Q;m) 9: Q (P;r;Qparent) ClosestToFreeAreaDisks(Qs) 10: V V[Q 11: E E[ (Qparent;Q) 12: ifjjPgPjj<r then 13: return (V;E) 14: end if 15: P U(Q;k) 16: for pi2P do 17: if pi is outside all other balls then 18: r 0 DistanceToObstacle(pi) 19: Q 0 CreateBall(pi;r 0 ;Q) 20: INSERT(PQ;jjPgpijjr 0 ;Q 0 ) 21: end if 22: end for 23: end while 24: end function of the MB, the `nearness' of manipulator congurations does not matter unless the MB poses are near. The Fig. 6.3(a) shows the criterion for choosing the nearest poses for the MB. There is a need to take the orientation of the MB of the sampled conguration (q rand ) into account. The criterion is that (x r ;y r ) should lie within the gray arc (within d) described by. Further, sgn( ) = sgn() andj j, where =j r n j is the dierence in the orientations of the MB at q rand and q near . We determine K nearest nodes only based on the above criterion for nearness and of those, we choose the one with the least Euclidean distance between the manipulator congurations q near :q m and q rand :q m . Here, ,, ,K and d are parameters that need to be set. The nearest neighbor, when sampling is done inW is the node in the tree with the closest EE location to T rand :p. 6.4.2 Extend towards the Random Sample Once the q near is chosen according the the conditions in Sec. 6.4.1, we extend the MB conguration as shown in Fig. 6.4. We rotate the MB at (x n ;y n ) to point towards (x r ;y r ) and move along the line joining (x n ;y n ) and (x r ;y r ) by a distance d e to reach the point (x e ;y e ). The extended 108 Figure 6.4: Extending from q near :q b = (x n ;y n ; n ) (blue) to q rand :q b = (x r ;y r ; r ) (red), lands up at q e :q b = (x e ;y e ; e ) (green) at a distance d e from (x n ;y n ) MB conguration is q e :q b = (x e ;y e ; e ), where e = arctan(y r y n ;x r x n ). Further, we interpolate the manipulator conguration towards q rand :q m from q near :q m and check for collision at intermediate steps. The extended conguration is q e . When the random sampling is inW, we drive the mobile manipulator at the near conguration q near towards T rand using Jacobian pseudo-inverse method [39]. We use the analytical Jacobian J of the EE as a function of the robot conguration q as J _ q = _ x, where x is the 6 1 EE pose. As the mobile manipulator is a 3 +n DOF robot, J is a 6 (3 +n) matrix. Hence, we can write _ q = J + _ x + N _ Where, J + is the Moore-Penrose inverse, J + = J T (JJ T ) 1 . N is a matrix representing the projection into the null space of J and _ is an arbitrary (3 +n)-dimensional velocity vector. Here, N projects the velocity _ q into the null-space of J and the corresponding motion does not aect the EE tracking task. The second term in the RHS can be used to obtain dierent joint velocities _ q with the same EE velocity _ x. This can be exploited to make sure that the non-holonomic constraints of the MB are satised. In this regard, we represent the non- holonomic constraints of the MB i.e _ x sin _ y cos = 0 as J h _ q = _ x h , where J h is a 1 (3+n) 109 vector with all the terms zero except the rst and the second terms which are sin and cos respectively. _ x h is 0. Combining these, we get _ = (J h N) + (_ x h J h J + _ x) (6.1) _ q = J + _ x + N(J h N) + (_ x h J h J + _ x) (6.2) q J + x N(J h N) + J h J + x (6.3) We determine the Jacobian J at q near . x is the dierence between the EE pose at q rand and q near . We perform collision checks along q from q near in steps to reach q e which in the best case is q near + q. 6.4.3 Heuristics for Connecting Trees The start and goal trees grow in the conguration space with sampling either in conguration space or workspace. We attempt a connection between the two trees when they have equal number of nodes. We select the newest node q new (or q e ) of one of the trees and use the strategy described in Sec. 6.4.1 to determine its nearest neighbor (q o ) in the other tree. We then attempt a connection between the two nodes using a strategy similar to the one described in Sec. 6.4.2. We rotate the MB at its place at (q o :q b :x; q o :q b :y) to point towards (q e :q b :x; q e :q b :y) and then drive towards it. After reaching (q e :q b :x; q e :q b :y), we rotate it at its place to make its orientation q e :q b :. We then interpolate the manipulator conguration from q o :q m to q e :q m along this MB path. A connection is successful if this connecting path of the mobile manipulator is collision-free and satises the joint limits. The heuristic refers to the strategy used to determine candidate nearest neighbors in the other tree or the same tree. Here we ignore those nodes which do not satisfy the conditions described in Sec. 6.4.1 when determining the nearest nodes. 110 6.5 The HS-Bi-RRT Algorithm In this section, we formalize the algorithm based on focused sampling for the MB and EE. The over-all algorithm, Hybrid Sampling-based Bi-directional RRT (HS-Bi-RRT), is shown in Algo 11. We initialize the two trees (T s ;T g ) with the start and the goal congurations as well as the sphere and disk sequences (lines 2-5). These are reversed for the goal tree. The current tree is initialized as the start tree and the other tree is initialized as the goal tree. For each tree, there is a current sphere and a current disk in which sampling will be done (lines 7-10). Both these are represented by `ball' in the Algo. 11 from lines 12 onward, as we will perform similar operations on both. R s is a sampling ratio which tells the probability of sampling in the conguration space as compared to the workspace. In the function NextRandSample we sample in both with equal probability, i.eR s is 0.5. When the sampling is to be done inC, we generate a random sample in the current disk via a normal distribution with a mean at the center and standard deviation for the MB location. The MB orientation and the manipulator conguration are sampled at random. Further, 10% of the sampling inC is done in the entire conguration space with no focusing for MB, to maintain the completeness of the algorithm. When sampling is to be done inW, we similarly sample the EE location in the current sphere with a normal distribution with a mean at the center and standard deviation . This standard deviation is specic for the current ball (i.e the current sphere and the current disk) and the current tree. The idea behind using the normal distribution for sampling is that there is relatively more free space for the robot to move near the center. l is the label for whether the sampling is done inC orW. Depending on l we nd the nearest nodes and extend as explained in Secs. 6.4.1 & 6.4.2. When we have a new node q new (or q e ) after sampling within a sphere or disk with the corresponding , it suggests that this sphere or disk is promising for further sampling. We then reduce the of this disk or sphere so as to exploit it by further focusing the sampling in it, like in line 17. However, if there is a failure in generating a q new , it conveys that we need to explore more 111 Algorithm 11 Hybrid Sampling based Bi-directional RRT 1: function HS-Bi-RRT(qs;qg;fDn 1 g;fSn 2 g;Rs ) 2: T s fnode(Ts;qs;S 1;D 1)g, 3: T g fnode(Tg;qg;Sn 2 ;Dn 1 )g, 4: T s:spheres fSn 2 g,T g:spheres ReversefSn 2 g 5: T s:disk fDn 1 g,T g:disk ReversefDn 1 g 6: T c T s,T o T g 7: T c:current sphere T s:spheresf1g 8: T o:current sphere T g:spheresf1g 9: T c:current disk T s:disksf1g 10: T o:current disk T g:disksf1g 11: while t<tmax do 12: (S rand ;l) NextRandSample(T c:current ball;Tg;Rs) 13: qnear NearestNode(T c;S rand ;l) 14: (Tnew;qnew) Extend(qnear;S rand ;l) 15: if qnew6= then 16: T c UpdateTree(qnew) 17: T c:current ball: = (1)T c:current ball: 18: else 19: T c:current ball: = (1 +)T c:current ball: 20: end if 21: if T c:current ball:< 00 then 22: T c:current ball: = 23: T c:current ball T c:current ball:child 24: end if 25: if T c:current ball:> 0 then 26: T c:current ball: = 27: T c:current ball T c:current ball:parent 28: end if 29: if TotalNodes(T c)>TotalNodes(T o) then 30: Swap(T c;T o) 31: else 32: (fconnectsg;flag) Connect(T c;T o) 33: if flag is Success then 34: return Path(T c;T o;fconnectsg) 35: end if 36: end if 37: end while 38: end function 112 in order to grow the tree instead of focusing more. Hence, we increase the standard deviation of that disk or sphere for sampling (line 19). The increase and decrease of the standard deviation are parameterized by (lines 17 & 19). This is dierent for a sphere and a disk and needs to be tuned appropriately. There has to be a limit on the exploration and exploitation being done. If we exploit till is too small, then we may end up in very similar locations for the random samples. Once exploitation is suciently done, i.e the reduction of the of the current sphere or the current disk less than a set value ( 0 ), we reset the to the original value and move ahead to the next sphere or disk in the sequence. (lines 21-24). Further, if is too large, we may end up not focusing at all. That also suggests that there have been repeated failures in generating a new conguration. Hence, there is a need to retract and go to the previous sphere or disk in the sequence. Further, the standard deviation needs to be reset for the sphere or disk (lines 25-28). If the current ball is the rst or the last, we reset the and do not move to a neighboring sphere or disk. To make sure that both the start tree and the goal tree grow equally, we swap them when the current tree has a greater number of nodes [186] (lines 29-30). Further, when they have an equal number of nodes, we make a connection attempt using the procedure explained in Sec. 6.4.3 (line 32). The algorithm returns a feasible path once a successful connection is found (line 34) within the timeout. 6.6 Results and Discussion 6.6.1 Experimental Setup We implemented the planner HS-Bi-RRT in MATLAB on a Dell workstation with Intel Xeon 3.5 GHz and 32 GB RAM. We benchmarked our method with existing algorithms and variations of our method. WD-Bi-RRT refers to whenR s is zero, and the sampling for the MB happens inside the workspace disk only, whereas the sampling for the manipulator is done at random in 113 its conguration space. WS-Bi-RRT is a variant of our algorithm whenR s is 1, i.e. sampling happens only in the workspace spheres for the EE location. All of these use the heuristic for connecting the two trees. Bi-RRT refers to the standard RRT-connect [132] with sampling in the entire conguration space along. Whereas, Bi-RRT+ is Bi-RRT with the heuristic for connection. We measure the time and the average path cost, which is the distance traveled by the object being carried. We have tested our method on a diverse set of 20 scenarios with a failure time-out of 1000 seconds. The diversity in scenarios is introduced by having a dierential drive mobile manipulator to carry dierent large objects, which are planar (like a broom, rod, plate, wheel) and 3D (trash bin, chair) in dierent types of challenging scenarios. We have also extended the algorithm to a bimanual mobile manipulator carrying two objects in its two arms. The 20 scenarios are a combination of the robots carrying these objects in various congested environments. A representative subset of the test scenarios is shown in Fig. 6.5. These scenarios contain easy and challenging cases, to show the relative performances of the competing methods. The objects being carried are large, and it can be observed that we cannot have the manipulator in a \home" position and carry them without coordinating the motions of the MB and the manipulator. The parameters are as follows: = 8 ,jj = 8 , = 0:2, = 0:05, 0 = 0:75, 00 = 3. These were determined empirically by testing in numerous scenarios. 6.6.2 Discussion In Tab. 6.1, we show the computation time and path cost for the ve cases. The path cost signies how good the initial solution is. A better and faster initial solution, when coupled with an any-time algorithm like BIT* may produce the optimal solution quicker. When tested over 20 scenarios, we observe that HS-Bi-RRT is, on average 3 times faster than the next best method for that scene. We use the heuristic in all the competing methods. If we select nodes for connection based on direct Euclidean distance inC like in [38] instead of using the heuristic, there is on average about 73% failure rate for HS-Bi-RRT, 81% for WD-Bi-RRT, 71% for WS-Bi-RRT 114 Figure 6.5: The 5 of the 20 test scenes for which the numerical results are presented in Tab. 6.1. Green: Start, Blue: Goal congurations and 90% for Bi-RRT. Further, when there is a success, the computation time is 9 times more on average. This shows the importance of using the heuristic for connections. In scene S04 the robot carries a long broom from a box to the room via a narrow passage. In S07 the robot transports a chair from one room to another. In both, there is a single narrow passage to be traversed to reach the goal for the MB. It can be observed in Tab. 6.1 that for these cases, HS-Bi-RRT, which has focusing for the MB as well as the manipulator, produces a path signicantly faster than the other three. WD-Bi-RRT, which has focusing for the MB, produces a solution faster than Bi-RRT+. It was observed that the time taken by WS-Bi-RRT is high due to the infeasibility to determine successful connections between the two trees. This happens 115 Table 6.1: The average computation time and average path length of the EE for he 5 test cases. The average is over 20 runs. Computation time includes the time required for generating free-space balls. due to biasing towards very dierent congurations for the manipulator, which do not connect in such narrow passages. However, WS-Bi-RRT produces the best quality solution. This can be attributed to the greedy sampling for the EE pose towards the goal. In these scenes, each method was successful in producing a solution for all the 20 attempts. In scene S12, the robot has to transport a large metallic sheet from one machine to another while avoiding a pillar on the way. The pillar may result in the workspace spheres taking a dierent homotopy class as compared to the workspace disks. The MB cannot pass between the pillar and the machine as the gap is too small. This can result in congurations in the two trees that may never connect due to collision with the pillar. This is where the advantage of sampling for the manipulator in the entire conguration space is evident. However, it should also be observed in Tab. 6.1 that HS-Bi-RRT still produces a better solution quicker than WD-Bi-RRT on average. This is due to the fact that focusing for the manipulator, even if misguided, helps when coupled with complete conguration space sampling. Here, WS-Bi-RRT is successful in producing a solution within 1000 seconds only 60% of the times. However, when WS-Bi-RRT does produce a solution, it does so faster than WD-Bi-RRT and Bi-RRT+ and with a shorter path length. Scene S13 shows the cases when there are multiple homotopy classes for the MB. Here, the time taken by HS-Bi-RRT is lower yet comparable to that of WD-Bi-RRT and Bi-RRT+. This 116 is due to the fact that there is enough free space for them to grow and nd connections. In fact, Bi-RRT+ is faster than WD-Bi-RRT as it can nd a longer path in the free space quickly. In such cases with multiple homotopy classes, the quality of solution by HS-Bi-RRT is highly dependent on the initial sequence of workspace disks. It may not nd the correct homotopy class in each run. We can easily extend our algorithm to manually select a homotopy class to bias the sampling of the MB congurations. In S17, we show that our algorithm can be extended to a nonholonomic bi-manual mobile manipulator for transporting two dierent objects in the two arms and passing through a narrow door. The algorithm is modied to have two dierent workspace sphere sequences for the two EEs. Here, it can be observed that results follow the same trends as in the S04 and S07. Moreover, WS-Bi-RRT fails to produce a result in 7 out of 20 runs. In this example, determining a feasible connection between the two trees is even more challenging due to the presence of 2 manipulators. The video of simulations of the robot trajectory and physical experiments can be found in the following link: https://youtu.be/UfxfBq3bjHw 6.7 Summary In this chapter, we have addressed the motion planning for nonholonomic mobile manipulators using a sampling-based method described by our algorithm HS-Bi-RRT. In such problems, deter- mining a feasible solution is a challenge due to narrow passages in the conguration space and the nonholonomic constraints of the MB. Focusing the sampling in dierent regions of the congura- tion space can signicantly reduce the computation time. For a mobile manipulator, this focusing must be done for the MB and the manipulator in appropriate regions to accelerate the growth of the trees towards each other. The primary contribution in this chapter is a method to coordinate the focusing for the MB and the manipulator in the workspace and grow the trees in appropriate directions. Moreover, we have presented a heuristic for determining connections between the MB 117 rst and then nd connections for the manipulator. It was observed that without this heuristic, each of the algorithms have a high failure rate to even compute a path. Furthermore, we also sample in the entire conguration space to make sure that the algorithm is probabilistic com- plete. We have shown that these together signicantly reduce the computation time as compared to existing methods in diverse scenarios. One of the limitations of this work is that when the workspace disks are misguided, for example, having a short door so that the mobile manipulator cannot pass, but the mobile base alone can pass under. Since our method is probabilistically complete, it will nd a path in this case as well, but the computation time will be signicantly higher. Another limitation of this work is that end-eector orientation is sampled uniformly. However, when the robot is carrying a large object, a lot of such samples will result in the collision for the object. Hence, those samples will be rejected and this can be a time consuming process since collision detection can be computationally expensive. Hence, there is a need to create some classication model that can immediately reject such samples without collision detection. And the nal limitation of this work is that the mobile base stops, rotates and then moves, such motions happen due to the way extend step has been dened in this work. But such an extend step can take much longer than moving on an arc or a curved path. 118 Chapter 7 Area Coverage Planning for Spray-Based Surface Disinfection with a Mobile Manipulator 7.1 Introduction A once-in-a-century pandemic [209] has resulted from the spread of the novel coronavirus disease 2019 (COVID-19), caused by severe acute respiratory syndrome coronavirus 2 (SARS-CoV-2). This has resulted in several million people losing their lives and many more having serious health consequences. There have been other dangerous viruses like Ebola, MERS, etc., in recent years that have resulted in severe health hazards. In the future, pandemics from in uenza-type viruses like SARS-CoV-2 may become more frequent due to the increased intimate contact with certain wild life [77]. One of the biggest challenges with Covid-19 and similar viruses is that they are highly contagious. Studies have shown that coronaviruses can remain infectious on metal, glass, wood, fabrics, and plastic surfaces for several hours to several days [215]. When a coronavirus-infected person coughs or sneezes, the infected droplets land on the surrounding surfaces. When these surfaces are touched by another person (even after several hours) who then touches their mouth, eyes, or nose, they can contract the virus [191]. Locations where there are a constant in ux and out- ux of people like washrooms, train stations, public transport vehicles, oces, restaurants, grocery stores, gyms, etc., are places where the virus can easily spread through surfaces. Hence, 119 it is important to disinfect regularly touched surfaces in such environments to reduce the spread of viruses. Surface disinfection can play an important role in preventing the spread of viruses. Small surfaces like doorknobs, switches, taps, touch screens, etc., can be disinfected manually. However, manually disinfecting large areas regularly can be extremely risky, time-consuming, labor inten- sive, and mundane. There is potential for a person disinfecting to miss or inadequately disinfect surfaces due to fatigue. Moreover, it can be expensive due to the disposable protective gear re- quired to execute the job. Alternatively, autonomous robots can be used for performing this task in an ecient and eective manner. The main advantage of autonomous robots for disinfection is that they can operate 24/7 without putting humans at risk. They can work all times performing disinfection, auto-recharging while liberating time for healthcare workers to focus on other critical activities, such as interacting with patients. They can even self disinfect. Hence, it is benecial to automate this repetitive task by using disinfection robots [205]. Robots for disinfection can be divided into two distinctive branches, UV light-based, and spray- based disinfection robots. These robots can be mobile robots, manipulators, mobile manipulators, or even drones, each operating autonomously or semi-autonomously with human-in-loop [100]. One of the ways to disinfect spaces with UV light is with mobile robotic systems that mount a large UV source. Such a robot can be used to disinfect large rooms by moving around to dierent locations and switching on the UV light source. These robots can be driven and operated manually or autonomously and have been used commercially in hospitals, food banks, airplanes etc [12, 6]. However, these robots are used for general disinfection and not deep disinfection of specic objects or surfaces like handles, mugs, medical equipment, etc., that may be exposed to the virus. For disinfection of such objects, mobile manipulators like ADAMMS-UV [100] that manipulate and disinfect occluded objects using a UV light wand can be used. Although UV light provides an eective solution for disinfection, especially of electronics and sensitive equipment, it may not be useful for disinfection of fabrics, and rough surfaces as UV light is not highly penetrating. Even 120 slight occlusion can result in a shadow being cast on the infected surface rendering the UV light ineective. Moreover, for eective disinfection, the intensity of the UV light needs to be high, which may not always be possible. Furthermore, it is challenging to recognize if a surface has indeed been disinfected with UV light as there is no residue left to detect. Figure 7.1: The spray nozzle attached to the end-eector of the UR5 robot of ADAMMS-SD. Liquid spray-based disinfection robots spray disinfectant liquid on surfaces and can be used on fabrics, rough surfaces, and places where UV light may not reach. With enough dosage, surfaces can be very eectively disinfected. The main advantage of liquid spray-based disinfection is that the disinfectant spray droplets can be detected, and we can measure how eectively surfaces are sterilized. Several mobile robots have been developed recently that spray liquid disinfectants as they move around. They are eective in disinfection of corridors by performing uniform spraying and do not focus on any particular equipment or surface. To increase the eectiveness of spray disinfection, mobile manipulators XDBOT [5] (eXtreme Disinfection roBOT, developed at NTU) and ADAMMS-SD (Agile Dexterous Autonomous Mobile Manipulation System for Surface Dis- infection, developed at our lab) are used to spray the liquid in particular directions and disinfect desired surfaces thoroughly. The nozzle for the spray is attached to the end-eector of the UR5 121 manipulator of ADAMMS-SD is shown in Fig. 7.1. The manipulator gives the robot the ability to focus the spray of the liquid on desired surfaces due to its wide reachability in distance and orientation. The focus of this chapter is to develop a novel algorithmic framework for area coverage planning for eective and ecient liquid spray-based disinfection of surfaces and large objects, like shown in Fig. 7.2 with a spray nozzle attached to the end-eector of a mobile manipulator. The goal is to compute conguration space (or joint) trajectories of the mobile manipulator such that the entire surface of the object is sprayed with enough disinfectant to guarantee thorough disinfection. The robot trajectory should be such that the nozzle at the end-eector follows a path that that covers the entire area with the spray from the nozzle. We can potentially spray in a zig-zag fashion on the surface to cover the area; however, the resulting path may not cover the entire area, and if it does, it may end up wasting disinfectant liquid by going on areas of the surface multiple times. Hence, to make the spraying process ecient and eective, we have developed a spray path generation algorithm. For this, we record a point cloud of the surface to be sprayed and project it onto a plane. The 2D polygon generated by the edge of the projected point cloud is used as the area to be covered. We have developed a branch and bound-based approach for generating a spray path that covers the entire area of the polygon using the sides of the polygon. This gives us spray paths that, if followed, will result in covering the entire area of the polygon. Moreover, the spray remains inside the polygon, resulting in no wastage of the disinfectant uid, making the process ecient. We choose a spray path using a capability map-based selection criterion for the mobile manipulator. We then map the chosen spray path on the original point cloud and determine the nozzle tip workspace path based on the spray model that we have used. In our disinfection framework, once the spray path is available, one can use any constrained trajectory generation algorithm. In this work, we use a sequential constraint renement-based non-linear parameter optimization-based approach for trajectory generation of the robot. The trajectory that this method or any other method develops is then rened to a time-optimal trajectory for 122 the specic congurations that result in complete disinfection of the surface being sprayed. For this, we present a linear programming-based method that determines the amount of time the robot should take between every two consecutive waypoints such that the robot sprays a quantity of disinfectant that is greater than a threshold for each point on the surface. This, in turn, gives us the rates for each degree of freedom such that the appropriate time intervals between successive waypoints are executed. Hence, the main contributions of this study are: 1. an algorithm for generating polygon area covering spray paths 2. a selection criterion for selection of appropriate spray path for the mobile manipulator 3. spline-based problem formulation for constrained optimization and a sequential successive renement based method for solving the highly non-linear optimization problem for robot motion planning 4. a linear programming based methodology for determining the time intervals between con- secutive waypoints that ensures enough disinfectant dosage for each point on the surface We have tested our disinfection framework on ve distinct test cases in simulation and one experimental demonstration. 7.2 Background and Preliminaries Let the workspace of the robot be denoted asW R 3 . Let q =fx;y;; 1 ; 2 ;:::; n g be the conguration of mobile manipulator with a manipulator with n joints. (x;y;) is the pose of the mobile base, i.e the location and the orientation. The maximum forward velocity of the mobile base isV max , and its maximum turning velocity is! max . The maximum joint velocity of each joint is _ max . Let the geometry of the mobile manipulator at a conguration q be represented as a set of rigid bodiesM(q)W. LetOW be the set of workspace obstacles. LetC be the conguration space of the mobile manipulator containing all the valid congurations of the robot. The set of 123 Figure 7.2: A simulation scene in CoppeliaSim [194] of ADAMMS-SD spraying liquid disinfectant in a washroom. joint congurations that lead to collision is denoted byC obs =fq2C :M(q)\O6=;g. The set of joint congurations that are collision-free isC free =fq2CnC obs g. Let T be the homogeneous transformation matrix representing the pose of the end-eector (EE) inW. TSE(3) (Special Euclidean Group). For a given joint conguration q, we can nd T in the world frame of reference by applying Forward Kinematics (FK). We use the dot notation to extract quantities (e.g. T:p denotes the position component and T:R the rotation matrix). q:q b denotes the pose of the MB and q:q m denotes the n-D conguration of the manipulator. The nozzle and a depth camera are attached to the end-eector of the robot, and the corre- sponding homogeneous transformation matrices are T N and T C . T N and T C can be determined using FK and their transformation matrices from the end-eector. The robot captures the point cloud of the surface to disinfect with the depth camera in the camera frame of reference, which is then transformed to the world frame of reference using T C . We denote the point cloud in the world frame of reference asP W . The point cloudP W contains N p number of points. 124 7.2.1 Literature Review on Spray and Spray Gun Trajectory Modelling Spray modeling has been studied in the literature in regards to robotic spray painting. The common theme in literature is to use a specic spray deposition model on the specic surface and plan paths and velocities such that there is uniform deposition along the path that covers the surface. Bo et al. fastbo2015 presented a fast planning framework for robotic spray painting on planar, cylindrical, conical, and spherical surfaces. They considered an elliptical double-beta static coating growth rate model as a reference model for the spray trajectory generation. Based on the thickness requirements, they determined the spray gun velocity and stroke distance. Arikan and Balkan sahir2000process modeled the single painting stroke paint thickness distribution as a beta distribution that varies as the paint gun moves along a line. A trajectory planning tool for automotive painting was developed by Conner et al. conner2005paint. They present an analytic deposition model for electrostatic rotating bell atomizers where the deposition thickness prole is taken to be a 1D Gaussian revolved about the z-axis to make it into an asymmetric volcano model. This deposition model is used to determine the velocities of the spray gun as it trav- els over objects that have changing surface curvatures so as to have uniform spray coating. A similar deposition model is presented by Chen et al. chen2017trajectory, where they consider a rectangular model for spatial paths for spray painting. Chen and Zhao chen2013path developed a deposition rate function on a plane according to experimental data, which they then used for multi-objective constraint optimization to determine the tool trajectory based on the requirement that the deposition is uniform. In the deposition model by Duncan et al. duncan2005frequency that is used by Kout and Muller kout2009parameter, the particles y on straight rays sharing a starting point inside the gun, and the deposited amount is taken to be the fraction absorbed by the surface like the refraction of light. They use this model to nd a nite set of spray gun congurations that cover the surface and minimize the error between the target coating and the coating achieved by those congurations. Gleeson et al. gleeson2020robot presented an approach 125 for spray paint optimization that can minimize paint thickness deviations from a target thickness by tting a spline function to the experimental data to get the applicator footprint prole. This prole is then projected onto the geometry, and the deposition model on curved surfaces is gener- ated. This is used in an optimization routine to get the desired paint thickness. There are several such methods in literature well documented in a survey paper by Chen et al. chen2017paint. The paper discusses deposition and spray models as well as CFD-based methods to generate deposition layers. A Gaussian model of the deposition rate of the spray during the cold spray process was discussed by Zhang et al. [40]. We have incorporated this model for our disinfectant spray and determined the parameters accordingly. The problem of spray disinfection is inherently dierent from the problem of robotic spray painting. For spray painting, the requirement is that throughout the surface, the paint thickness must be uniform. This requires rigorous modeling of the spray deposition. Moreover, the nozzle tip does not have much exibility on the orientation as compared to the normal to the surface due to the uniformity requirements. On the other hand, spray disinfectant deposition requires that the entire surface be covered in at least a threshold amount of disinfectant. This may result in certain areas of the surface receiving just enough dosage, whereas other areas may receive a signicantly large dosage. Even though this may not be desired, such a situation is acceptable. This relaxes the constraints on the nozzle orientation with respect to the normal to the surface. Moreover, since there is no visual feedback, uncertainties in the surface are tolerated as compared to spray painting. 7.3 Problem Formulation 7.3.1 Problem Statement A mobile manipulator is given with 3 +n degrees of freedom and the corresponding maximum forward, turning, and joint velocities. A nozzle attached at the end-eector (Fig. 7.1) of the 126 mobile manipulator sprays disinfectant liquid with a spray modelS(;D n ;D f ;d o ;) (Fig. 7.3, described in detail in Sec. 7.4). Also, given is a continuous point cloud P W with N p number of points of the surface to be disinfected. The objective is to determine a collision-free, time- optimal conguration space trajectory of the mobile manipulator such that each of the N p points of the point cloud receives at least a minimum threshold dosage (H o ) of disinfectant as the mobile manipulator moves. 7.4 The Disinfectant Spray Model The spray from the nozzle is assumed to be conical with a cone angle that we call the spray angle (). The eectiveness of the spray decreases with the distance from the nozzle tip. We dene a distance D f to be the cut-o distance from the nozzle tip, after which the eectiveness of the spray is minimal. We also dene a distance D n to be a cut-o distance from the nozzle tip, such that if the distance of a surface from the nozzle tip is less than D n , the spray liquid starts owing due to the high concentration. This may not be desirable when spraying sensitive equipment or when there is no option to wipe it with a cloth. Hence, the spray from the nozzle is considered eective if the surface being sprayed is within the frustum of the cone formed by the cone angle, and the distances D f and D n from the nozzle tip. This frustum of the cone is shown in Fig. 7.3. The rate of deposition of the spray liquid on a surface perpendicular to the direction of the spray at a distance D n can be modeled as a Gaussian distribution with a standard deviation and zero mean. [58, 46, 40, 242, 244] The deposition rate at the center of the circular cross-section is d o . The parameters , d o , D n and D f can be determined experimentally. This spray model is denoted byS(;D n ;D f ;d o ;). The spray model brie y described Fig. 7.3 gives the liquid disinfectant deposition rate (or the disinfectant height build-up rate) if a surface was to intersect the spray cone at a distance D n perpendicular to the nozzle axis. As mentioned, the frustum of the cone formed between D n and 127 Figure 7.3: The spray cone showing the spray angle and the distances D f and D n that form a frustum of the cone. The rate of deposition is modeled as a Gaussian distribution for the circular cross-section at a distance D n . D f is where spray disinfection is actually eective. Hence, it is necessary to determine the rates of liquid disinfectant deposition at any point within the volume of frustum of the cone between D n and D f . The liquid disinfectant deposition rate at a distance D n as shown in Fig. 7.3 is given as follows [40]: d R (D n ;r) =d o e 1 2 r 2 2 (7.1) where, r is the radial distance from the spray cone axis. Based on the continuity equation, we can derive the the deposition rate d R (z;r) where z2 [D n ;D f ] and r2 [D n tan;D f tan]. The deposition rate at a distance z from the nozzle tip is given as follows: d R (z;r) =d o D 2 n z 2 e 1 2 r 2 2 D 2 n z 2 (7.2) 128 We derive the spray deposition rate mentioned in Eq. 7.2. Given that at a distance D n from the nozzle tip, the deposition rate is as dened from Eq. 7.1(Eq. 7.4), we want to nd what is the deposition rate at a any distance z from the nozzle. We assume that gravity does not impact the spray close to the nozzle. d R (D n ;r) =d o e 1 2 r 2 2 (7.3) To derive this, essentially we need to determine the constants d q and q of the following equation: d R (z;r) =d q e 1 2 r 2 2 q (7.4) D n D z d o R n R Figure 7.4: The spray deposition rate modeled as a Gaussian changes as the distance from the nozzle tip. The volume of the spray per second remains constant at any cross-section perpendicular to the spray direction. Hence, the volume at D n per second is equal to the volume at a distance z. dV =d R (D n ;r)dA (7.5) dA = 2rdr (7.6) dV =d o 2re 1 2 r 2 2 dr (7.7) 129 here, dA is the area of a ring centered at the cone axis. Integrating, we get the volume at D n to be V Rn = 2d o 2 (1e R 2 n 2 2 ) (7.8) By applying conservation of volume, the volume rate at D n is equal to the volume rate at z, 2d o 2 (1e R 2 n 2 2 ) = 2d q 2 q (1e R 2 q 2 2 q ) (7.9) R q =R z Dn (7.10) Now assuming that the spray rate distribution standard deviation changes linearly with distance we get, q = z Dn (7.11) d q =d o D 2 n z 2 (7.12) hence, the deposition rate at any distance z and a radial distance r is given by: d R (z;r) =d o D 2 n z 2 e 1 2 r 2 2 D 2 n z 2 (7.13) Using the Eq. 7.2, we can determine the deposition rate at any point within the frustum of the spray cone. Hence, for any point belonging to the point cloudP W , we can determine the deposition rate normal to the local plane at that point. As shown in Fig. 7.5, the distance of point P on the point cloudP W from the nozzle tip isD P , and the angle that the vector from the nozzle tip to the pointP makes with the spray axis is. This givesz =D P cos andr =D P sin. The normal to the point cloud at point P is ^ n P . The angle that ^ n p makes with the vector from point P to the nozzle tip is . Hence, the rate of deposition at point P normal to the surface of the 130 Figure 7.5: A pointP W is sprayed on, and a pointP with its coordinates in the world coordinates are (x w ;y w ;z w ). point cloud can be approximated by the following equation, assuming that the disinfection uid deposited normal to the surface does not ow after being sprayed. P:d R =d o D 2 n D 2 P sec 2 e 1 2 D 2 n tan 2 2 cos (7.14) We represent the deposition rate for point P using dot notation as P:d R . The direction of the normal at point P , ^ n p can be determined by planar segmentation of the point cloud P W at point P . Hence, using Eq. 7.14, we can evaluate the deposition at any point of the point cloud as long as it lies inside the frustum of the spray cone. It should be noted that the Eq. 7.14 is an approximation and can change for dierent type of disinfection uid. Our method is agnostic to the spray model chosen, however, we show the results using the deposition rate modeled as Eq. 7.14. Also, we have neglected the impact of gravity in the derivation of Eq. 7.2. 131 7.5 Overview of Approach In order to thoroughly disinfect a surface given by the point cloudP W , the mobile manipulator has to move the nozzle attached to its end-eector such that all the points of the continuous point cloud are within the frustum of the spray cone at some point during the robot motion. Hence, it is necessary to determine a path consisting of waypoints with position and orientation for the nozzle tip such that as this path is followed in sequence, points of the point cloud pass through and out of the frustum of the spray cone. By the end of the path, all points should have passed through the frustum i.e. the frustum should have passed through all the points. This path is such that each waypoint of this path2 SE(3). And to determine the workspace path of the nozzle tip, a sequence of points of the point cloud must be determined, which will be called the spray path henceforth. The framework and an overview of the approach for determining the mobile manipulator trajectory for spray disinfection is shown in Fig. 7.6 Figure 7.6: The framework for robotic disinfection describing the sequence of operations to de- termine the nal trajectory of the mobile manipulator for disinfection of the surface represented by the point cloud. The rst step is to determine the sequence of points on the point cloud, which we call the spray path. A spray path waypoint is a point where the spray cone axis intersects the point cloud. This point should be on or inside the eective frustum of the spray cone. There can be several spray paths that can be eective in disinfecting the entire point cloud; we select one path based on a 132 selection criterion. This selection criterion is based on the reachability of the mobile manipulator and is explained in detail in Sec. 7.6.5. Once we have chosen a spray path, based on the constraints of the spray cone that each point of the point cloud must lie inside the frustum, and the robot motion constraints, we determine the congurations of the mobile manipulator for each waypoint on the spray path using sequential constraint-based optimization. This is explained in details in Sec. 7.7. Once we have the mobile manipulator and hence the nozzle path, we determine the velocities of each of the mobile manipulator congurations so that each point of the point cloud receives enough dosage of the disinfectant to sterilize it. This is done by determining how much time the robot should take between every two consecutive waypoints. Details of this are explained in Sec. 7.8. 7.6 Spray Path Generation As mentioned earlier, a spray path is a sequence of points of the point cloud, where each point is the intersection of the axis of the spray cone and the point cloud. This sequence should be such that each point of the point cloud should at least once be inside the eective frustum of the spray cone. This can be formulated as an area coverage problem, where the spray should cover the entire area of the surface. 7.6.1 Planar Polygon Generation To solve this problem, the rst step is to get a planar version of the point cloud. We can get the planar version by either projecting the point cloud on a plane or unwrapping the point cloud into a plane [160]. In this chapter, we project the point cloud onto a plane such that we get its area projection which may or may not be its maximum area projection. The projection operation is shown in Fig. 7.7. It starts with uniformly sampling a large number of points on the point cloud, and at each of those points, we determine the normal direction to the surface locally using planar 133 Side-View Dominant Normal Projection Plane Projected Point Cloud Polygon generated from projected Point Cloud a) b) Figure 7.7: (a) The point cloud to be disinfected is shown (b) The projection of the point cloud on a plane perpendicular to the dominant normal of the point cloud is shown along with the polygon containing all the projected points. segmentation. We then align all the unit normals to one of the sides of the point cloud, i.e., above or below, or upwards or downwards. Then we take a vector sum of the normals to determine the dominant normal direction as shown in Fig. 7.7(b). We then project each point of the point cloud to the plane perpendicular to the dominant normal, as shown. We then determine the boundary points of the planar projected set of points. The goal is to form a bounding polygon with these boundary points. This polygon cannot be a convex hull of all the points due to the concavity that may result from the projection operation. Moreover, we cannot form a polygon with all the boundary points as vertices since that will result in a large number of very small polygon edges, which is not desirable. Hence we must choose a 134 subset of those boundary points as the vertices of a polygon. Here, (P 1 ;P 2 ;:::P m ) represent the m boundary points in a sequence. The pointP 1 is such that the angle formed by the two segments joiningP 1 to its two adjacent points (for P 1 these points areP 2 andP m ) is the smallest amongst all m points. We choose P 1 to be one of the vertices of the desired polygon. From P 1 we go to P 2 and sequentially to P m , and nd the rst point P i such that the distance between P 1 and P i is greater than a length l o . If all points between P 1 and P i lie within a lateral oset distance of o from the line joining P 1 and P i , then the next vertex of the polygon adjacent to P 1 is P i . If such a P i does not exist, we choose the furthest point from P 1 such that all points between that point and P 1 are within o distance laterally from the line joining that point and P 1 , as the next vertex of the polygon. This point will be at a distance <l o fromP 1 . Once the adjacent vertex to P 1 is decided (call that P b ), we repeat this process to determine the adjacent vertex to P b . This process gives a polygon, but the maximum length of an edge would be about l o . Hence, even a possible straight edge much longer thanl o will be divided into multiple segments. We remove the intermediary points of such a long straight edge by checking for the angle of the polygon at each point. If the angle is close to 180 o o , we remove the polygon vertex. o is a small angle that can be tuned. Similarly, the parameters l o , l 0 o and o can be chosen appropriately. This methodology gives us a polygon, an example of which is shown in Fig. 7.7(b). This polygon is denoted asG o . 7.6.2 The Spray Radius The algorithm for computation of the spray paths on the polygonG o generated by the methodology described in Sec. 7.6.1 is explained in Algo. 12. Before going into the detail of the spray path generation algorithm, it is important to discuss the spray radius or radii that will be used for the algorithm. As can be seen in Fig. 7.5, when the point cloud is sprayed, the spray radius is at least R n . If the spray cone axis is not orthogonal to the point cloud, the projection will be an ellipse. The minimum length of the ellipse axes would be R n . Hence, we assume that we spray with a radius ofR n , which is the minimum radius or axis length of the ellipse. As can be seen in Fig. 7.8, 135 when the point cloud is projected on the plane, the radius of the spray circle also decreases on the projection plane. We call the radius of spray on the plane to be the spray radius R s . The spray radii vary for dierent regions of the point cloud, as can be seen in Fig. 7.8. We sample several points on the point cloud and compute the spray radius on the plane on those points and take the minimum of those radii to be the spray radius R s . However, if the minimum of those radii is very small as compared toR n , it will result in the spray segments very close to each other at a place where the projected radius is not as small. Hence in such a case, we have dierent R s for dierent regions of the projected point cloud or polygon. For the purpose of explaining the algorithm, we assume there is a single R s for the entire projected polygon. s s2 s3 Spray ones Projection Plane Point loud Figure 7.8: The minimum radius of the spray circle on the point cloud is R n . However, when the point cloud is projected on a planar surface, the radius of the spray circle also needs to be projected. There are several projected radii, which are calledR si for dierent angles of projection of the plane. 7.6.3 The Spray Path Generation Algorithm The spray path algorithm is shown in detail in Algo. 12 and illustrated in Fig. 7.10. We are given the projected polygonG o of the point cloud P W , and the spray radius R s and the goal is to 136 determine a sequence of segments on the polygonG o such that the entire polygon and the point cloud are covered with disinfectant. We call this sequence of segments the spray path. Once we have the spray path on the polygon, we will project those on the point cloud and generate mobile manipulator end-eector waypoints for its motion planning. The rst step for the generation of the spray paths is to generate a polygonG o that is oset inside the given polygonG o by a distance equal to the spray radius R s . If we spray along the segments of the polygonG o , the boundary of the polygonG o will be sprayed. A simple spray segment is shown in Fig. 7.9. We get the oset polygon and add all the individual segments of the oset polygon to a stack (lines 6-7 of Algo. 12, column 1. of Fig. 7.10). We then choose the latest segment in the stack (line 10) and expand it (line 11). Expanding a segment is basically spraying the segment and getting the polygon remaining after spraying that line segment. This is shown in Fig. 7.10, column 2. The polygon sprayed is in blue, which is essentially a trapezium with a height equal to twice the spray radius R s . It can be pointed out that due to the circle, the corner of the polygon (the bottom right-hand corner in black in Fig. 7.9) will not be sprayed. However, since the spray radius is the minimum possible radius, it is expected that the corner will be sprayed when the actual larger circle (or ellipse) of the frustum of the spray cone is used. In the case that the corner angle is very small, a large area of the corner would remain unsprayed. In such a case, spraying this segment will automatically be eliminated, as will be explained in the next steps of the algorithm. Also, when the segment is sprayed, the checked area in Fig. 7.9 will remain unsprayed. However, as we move to the next segment, this part will be sprayed. Hence we assume that the checked portion is also sprayed, and hence the sprayed polygon in blue is a trapezium. Also, it must be noted that for the very rst segment being expanded, we start the spray from the edge of the polygon as shown by the red dotted line in Fig. 7.9. As can be seen in Fig 7.10, this trapezium is shown in column 2. for s 1 and s 7 since those segments are convex segments of the polygon. A convex segment is where all the vertices of the polygon lie on the same side of the polygon. On the other hand, segment s 2 is a non-convex segment where at least one vertex lies on the opposite side of 137 the segment as compared to others. For a non-convex segment, the sprayed polygon is terminated at the end of the segment, as can be seen in the second polygon for column 2. of Fig. 7.10. Sprayed rea Figure 7.9: A single spray segment. We assume that when the spray starts, the spray starts from the boundary of the polygon to avoid a corner to be left unsprayed. We will discuss in Sec. 8.5 how we modify the spray paths to spray the unsprayed corners. R s 7 6 5 4 3 2 7 6 5 4 3 2 7 6 5 4 3 2 R s Connecting segment 7 1 2 Disinfected rea b c 2 2b 7b 7c 7 ... ... b ... c ... ... ... ... ... ... oset oset 3 4 2 2 oset 3 3 oset 2 ... 5 ... Figure 7.10: The spray algorithm. The red area has not yet been disinfected; the blue area is the disinfected area. The polygon with a blue boundary is the oset polygon of the polygon to be sprayed. The remaining polygon after spraying a segment is the next polygonG next (line 11). We then nd the oset polygon ofG next as shown in column 3 (line 15). Then we go on to determine the next segments, which would be the children of the current segment. For a convex segment being expanded, there are three children, the two adjacent segments (denoted by s 1b & s 1c and s 7b & s 7c ) and one parallel segment (s 1a and s 7a ). For a non-convex segment, there are only the two adjacent segments as shown for s 2 as s 2a and s 2b . All the children segments are derived from 138 the oset polygon of the next polygonG next . These segments are generated and pushed onto the stack (lines 46-50). The next polygonG next then becomes the current polygon (line 51), and we continue the process by popping the most recent addition to the stack till all the segments have been expanded. Hence, this is a depth-rst search with a branching factor of 3. Here the root node is a virtual node with the number of segments of the rst oset polygon as the number of children. The condition that the leaf node has reached for a branch is that the oset polygon of the current polygon does not exist. This is possible only when the current polygon is small, as shown in Fig. 7.11. Once the leaf node is generated, the path for that branch of the tree from the root to the leaf is generated by connecting the spray segments sequentially with connecting segments as shown in Fig. 7.10 column 3. The nal spray segment is generated for the polygon with no oset polygon by drawing a line through the end of the previous segment in the direction of the principal axis of the polygon. The dashed line in the polygon with no oset polygon shown in Fig. 7.11 is such a nal segment. This terminates a single spray path for that particular branch of the search tree. Next, we go into the details of the termination conditions for generating the spray path and the branch pruning heuristics used to accelerate the optimal path generation. Sprayed Area Spray path Polygon with no oset polygon Figure 7.11: The remaining polygon which has no oset polygon. The rst branch pruning condition is that if the next polygonG next (line 11) and its oset polygonG o (line 15) are not continuous, then we do not expand the current segment since. An 139 Algorithm 12 Generate Spray Paths 1: function Generate Spray Paths(Go, Rs) 2: S Stack() 3: BestPathLength 1 4: BestAreaCoveringPaths 5: G curr Go 6: G o G curr:Get Offset Polygon(Rs) 7: S:Push(G o :Segments) 8: LeastNofPtsNotSprayed Total Grid Points(G curr) 9: while S6= do 10: scurr S:Top(), S:Pop() 11: G next G curr:Get unsprayed polygon(scurr) 12: ifG next is NOT Continuous then 13: Continue 14: end if 15: G o G next:Get Offset Polygon(Rs) 16: ifG o 6= then 17: ifG o is NOT Continuous then 18: Continue 19: end if 20: SprayPath Get Path(scurr) 21: sprayPts get sprayed pts(SprayPath;G next) 22: spraypolygonpts get polygon pts(SprayPath) 23: ptsNotsprayed spraypolygonptssprayPts 24: if ptsNotsprayed>LeastNofPtsNotSprayed then 25: Continue 26: end if 27: else 28: FinalPath Get Final Path(scurr) 29: sprayPts get sprayed pts(FinalPath;G next) 30: spraypolygonpts get polygon pts(FinalPath) 31: ptsNotsprayed spraypolygonptssprayPts 32: if ptsNotsprayed<LeastNofPtsNotSprayed then 33: LeastNofPtsNotSprayed ptsNotsprayed 34: BestPathLength finalpathlength 35: BestAreaCoveringPaths 36: BestAreaCoveringPaths:Append(FinalPath) 37: end if 38: if ptsNotsprayed ==LeastNofPtsNotSprayed then 39: LeastNofPtsNotSprayed ptsNotsprayed 40: if finalpathlength<BestPathLength then 41: BestPathLength finalpathlength 42: BestAreaCoveringPaths:Append(FinalPath) 43: end if 44: end if 45: Continue 46: end if 47: S children scurr:Get Children(G o ) 48: for si2S children do 49: si:Parent scurr 50: S:Insert(si) 51: end for 52: G curr G next 53: end while 54: return BestAreaCoveringPaths 55: end function 140 Discontinuous O set Polygon Figure 7.12: Due to a narrow section of the polygon, the oset polygon is discontinuous. example of discontinuous oset polygon is shown in Fig. 7.12. The discontinuity will require the spray path to join the two discontinuous regions, making the spray path longer. We prune the branches of the search tree in order to avoid such a situation. The pruning of such polygons happens in lines 12-14 and 17-19 of Algo. 12. If the oset polygon of the next polygon does exist and is continuous, then we have the next pruning heuristic. For this pruning heuristic, we measure the area sprayed by the current incomplete path as compared to the polygon the spray is supposed to spray on. For the purpose of the algorithm, for example, in Fig. 7.10 column 2, after we spray on the red segment, we consider the blue trapezium as the sprayed polygon. However, as can be seen in Fig. 7.9, the actual sprayed area will be the blue plus the checked area, and the bottom right corner (black area) will remain unsprayed. This unsprayed area can be signicant when the angles of the polygon are small. Hence, we need to determine the dierence between the polygon we expect to spray (blue) and the area it actually sprays. Although we mentioned earlier that the larger spray radius would result in the corner getting sprayed on, that is not always true. Hence, we prune branches of the search tree that result in paths that have large unsprayed regions. We measure this unsprayed area by having a dense grid of points in the polygon and count the points inside the sprayed polygon vs. the actual sprayed area. The dierence between the two numbers is the number of grid points not sprayed and can be used as a measure of the area not sprayed. In Algo. 12, we generate the incomplete spray path in line 20 and get the number of grid points this incomplete path actually sprayed on in line 21. We determine the number of grid points that are inside 141 the trapeziums of every path segment and take their dierence to get the number of grid points not sprayed on by the incomplete path. This number of grid points not sprayed resembles the measure of the area that the incomplete path should have sprayed but did not. If this number of grid points not sprayed is greater than the least number of points not sprayed documented till now, we prune this branch (line 24-26). It must be noted that the measure of the best path is not the length of the path but the smallest number of grid points it ends up not spraying on. If the oset polygon of the next polygon does not exist (like in Fig. 7.11), this means that we have reached the leaf node. We compute the nal path as described earlier. This is shown in line 28 of Algo. 12. We similarly get the grid points the nal path actually sprays on and the grid points it should spray on in lines 29 and 30, respectively. If the number of grid points the nal spray path does not spray on is less than the current least number of grid points not sprayed on, we nd the length of this nal spray path and make it the best path length till now and record the least number of grid points not sprayed on, in lines (34 and 33 respectively). We store this is as one of the best area covering path. If the number of grid points the nal spray path does not spray on is equal to the current least number of grid points not sprayed on, we check if the length of the current path is less than the best path length recorded till now, we make this the best path length and store this path as one of the best area covering path till now (lines 41 and 42). We continue to the next iteration after that in line 45. The algorithm presented is an any-time algorithm, where it keeps on nding shorted paths that cover more and more of the area. We can terminate the algorithm once it has found a path. If the number of grid points not sprayed is zero and the lengths of the progressive paths computed do not change by more than a certain percentage, we terminate the algorithm. The resulting paths can be zig-zag, spiral, or a combination of the two, as shown in column 5 of Fig. 7.10. We record all the paths that have the least amount of grid points not sprayed on since a longer path may be better than a shorter one to spray on with a mobile manipulator. 142 7.6.4 Two Zig-Zag Paths for Same Spray Segments As shown in Fig. 7.10 column 4 rst polygon, there are two parallel spray segments. There are two ways to connect the two segments, one from either end of the segment. Hence, there can be two ways a number of parallel segments can be connected. The two ways are shown in Fig. 7.13. When a path is to be determined in lines 20 or 28, in such a scenario, we determine both parallel paths and keep the one or both if they satisfy the conditions of being spraying on equal to or more of the grid points as compared to the previous best spray paths. Figure 7.13: The two dierent parallel paths from the same spray segments. 7.6.5 Spray Path Selection Criterion As the spray path generation algorithm terminates, it returns several planar spray paths, all of which cover the area of the polygon. In this section, we explain the selection criterion for the spray path from the multiple paths generated. The mobile manipulator has a nonholonomic mobile base and a robotic arm mounted on it. For every pose, i.e., the position and orientation of the mobile base, the manipulator can reach dierent locations in the workspace. The capability map [240] of the manipulator shown in Fig. 7.14. It shows scaled reachability, which is the ease with which each point in the workspace is reachable, i.e., the reachability of the robot for each point in the workspace. The capability map of a manipulator is computed by sampling a large number of its 143 Figure 7.14: The mobile manipulator and the capability map of the manipulator at the corre- sponding to the pose of the mobile base joint congurations and measuring the number of times each 3D voxel in the workspace is reached by the end-eector. For the endpoints of each segment of the spray path as shown in Fig. 7.15(b), we generate a pose for the nozzle tip of the robot by moving by a distance D n from Fig. 7.3, in the direction normal to the point cloud at that point. We do this for all the segment endpoints (blue points in Fig. 7.15(b)). The spray path segments are thus transformed into nozzle tip segments. For each spray path, we check if every nozzle tip segment lies inside the capability map, i.e., if any point on the segment has the scaled reachability close to zero. This implies that in order to spray on that segment, the mobile base will have to move. We want to make sure that the mobile base moves as little as possible when the robot is performing disinfection. Hence, we choose whichever spray path has the least number of segments going outside the capability map at the initial conguration of the mobile manipulator as the spray path to execute. 144 7.7 Motion Planning for Mobile Manipulator Once the spray paths on the projected polygon are generated, we move on to the motion planning of the mobile manipulator such that the nozzle at the end-eector of the robot sprays along the spray paths projected back on to the point cloud as shown in Fig. 7.15. Before we plan the motions, we need to select an appropriate spray path from the multiple paths generated by the algorithm. Figure 7.15: (a) The planar path generated by the spray path generation algorithm and (b) corresponding path on the point cloud 7.7.1 Nozzle Pose Constraints For a spray path on the point cloud, like in Fig. 7.15(b), we create nozzle tip waypoints by choosing points at regular intervals along the path. We make sure that the corner points for the spray path are included in the nozzle tip waypoints. We add a frame at each point with the z-axis along the normal direction to the plane at that point. Each waypoint is a 4 4 transformation matrix 2 SE(3). We denote this sequence of 4 4 transformation matrices along the spray path as nozzle tip waypoints. There areN W spray path waypoints. The sequence of nozzle tip waypoints represents a workspace curveW p (s) =fW p (i) : i = 1; 2;:::;N W g s2 [0; 1]. Where s is a spray path length parameter; s = 0 implies i = 1 and s = 1 implies i = N W . One of the nozzle tip waypoints (T = W p (5)) is shown in Fig. 7.16. For each of these nozzle tip waypoints, there are constraints on the feasible nozzle congurations. The spray cone axis must intersect the 3D 145 position of the spray path waypoint, T:p. Moreover, T:p must lie within D n and D f from the spray path position, i.e., within the frustum of the spray cone. Spray Cone xis Spray Path Waypoint Sprayed Points =W5) Figure 7.16: The spray cone axis should intersect the spray path waypoint inside the frustum of the cone. When a plane intersects the frustum of the spray cone, the intersection is a circle or an ellipse, as can be seen in Fig. 7.16. However, if the angle of intersection is skewed, the intersection can be a truncated ellipse or a parabola. This is not desirable since we want to make sure that the entire spray is eective. Hence, it is necessary to make sure that the nozzle tip is at a relative orientation from the spray path waypoint such that the intersection of the frustum of the spray cone and the plane to the point cloud at the waypoint is an ellipse or a circle and not a truncated one. In order to make sure of that, there are limits to the angle between the vector from the waypoint to the nozzle tip and the waypoint Z-axis (i.e., the normal to the point cloud at the waypoint). As mentioned earlier in the section, the spray cone axis intersects the nozzle tip waypoint position. In Fig. 7.17 the red dotted line is the plane to the point cloud at the nozzle tip waypoint 146 D 2 2 D 2 2 < 2 2 < Spray waypoint position a) b) Figure 7.17: The limits on the angle the spray waypoint transformation matrix z-axis makes with the line joining the waypoint position and the nozzle position. The dotted red line represent the instantaneous plane at the spray point position. positions and the dotted arrow is the normal to the plane at the point.. D z1 and D z2 are the distances of the waypoint to the nozzle tip. In Fig. 7.17(a), it can be seen that D z1 is such that, D n < D z1 < D f and angle 1 < 2 . In order to make sure that the intersection ellipse are not truncated, the angle between the normal to the plane and the spray cone axis should be such that 1 < < 1 . In Fig. 7.17(b) D z1 is such that, D n < D z1 < D f and angle 2 < 1 , hence here the angle between the normal to the plane and the spray cone axis should be such that 2 < < 2 . Hence, the constraint on the angle is lim < < lim where lim = min( 1 ; 2 ) with 1 = arctan( D f Dz R f ) and 2 = arctan( DzDn Rn ). 7.7.2 Robot Motion Constraints For each nozzle tip waypoint, the robot conguration q should be such that the pose of the noz- zle T N = FK(q) satisfy the constraints described till now. Due to the nonholonomic nature of the mobile base motion, there are constraints on the mobile base congurations. This nonholo- nomic constraint must be satised between every two consecutive mobile base congurations q:q b . 147 Moreover, the constraints for the joint limits, joint velocities, the mobile base forward and turning velocities should be satised. And nally, the robot's motion should be free of self-collision and external collisions. There can be three kinds of collision- robot-robot collision, robot self-collision, and robot-object collision. In this work, we approximated each rigid body as a collection of spheres [199] and considered the signed distance among sphere pairs for scoring collisions. If the distance between two spheres is less than the sum of the radii of the two spheres, those two spheres are in collision. We can use this sphere representation for collision between the manipulator and the mobile base and for external collision with the point cloud. 7.7.3 Motion Planning Problem Formulation The goal is to nd a sequenceQ of congurations q such that the nozzle sprays along the spray path and the mobile manipulator moves from the starting nozzle tip waypoint to the nal nozzle tip waypoint in the least amount of time. This ensures that the disinfection of the surface represented by the point cloud happens in a time-optimal manner. Moreover, we also want to make sure that the mobile base moves as little as possible so as to mitigate the uncertainties associated with the mobile base localization. With this objective, the motion planning problem is formulated as a non-linear optimization problem. Given the N W nozzle tip waypoints W p , the robot model and its sphere representation and the constraints due to the spray modelS, the objective is to nd the conguration space trajectory for the mobile manipulatorQ. The problem statement is as follows: 148 minimize Q w 1 T +w 2 P N W 1 i=1 dist(q i :q b ; q i+1 :q b ) s:t: (7.15) C nozzle pose (q) 0 (7.16) C nonholonomic (q; _ q) = 0 (7.17) C collision (q) 0 (7.18) C joint limits (q) 0; C velocity limits ( _ q) 0 (7.19) where, T is the motion execution time for the robot. We have considered a surrogate function to estimate the execution time. The idea is that we will be moving each joint with maximum displacement between two waypoints at its maximum velocity and regulate the velocities of other joints such that they all complete motion at the same time. Hence,4t is the time the slowest conguration variable (i.e., degree of freedom) takes as the robot goes from the ith to the i + 1st waypoint. T = P i=N W 1 i=1 4t i (7.20) 4t i = max((q j i+1 q j i )= _ q max ) (7.21) Here dist(q i :q b ; q i+1 :q b ) is the distance covered by the mobile base between two consecutive waypoints. w 1 andw 2 are the weights corresponding to each objective. The nozzle pose constraints C nozzle pose (q have been described in Sec. 7.7.1. We can represent the nozzle pose constraints as nozzle pose error by converting it into an exponential error function [108]. Nozzle Pose Error = exp (w a (D n z N )) + exp (w a (z N D f )) + exp (w b q x 2 N +y 2 N ) + exp (w c ( lim )) + exp (w c ( lim )) (7.22) 149 where, (x N ;y N ;z N ) is the position of the nozzle with respect to a spray path waypoint computed using the robot FK and the homogeneous matrix associated with the spray path waypoint. Com- putation of is as described in Sec. 7.7.1. w a ; w b ; & w c are the weights associated with each error. Similarly, the constraints due to the nonholonomic constraints of the mobile base can be converted into the nonholonomy error. Nonholonomy Error = exp (w h abs((x i+1 x i ) sin i (y i+1 y i ) cos i )) (7.23) where (x i ;y i ; i ) and (x i+1 ;y i+1 ; i+1 ) are the mobile base poses q i :q b and q i+1 :q b for two con- secutive robot congurations. Hence we can change the objective function to include the nozzle pose and the nonholonomy errors. The objective function is as follows: w 1 Total Time +w 2 Total MB Distance +Nozzle Pose Error + Nonholonomy Error (7.24) with the collision, joint limits, and joint velocity limit constraints as is. Here Total Time is T and Total MB Distance is the distance travelled by the mobile base, dist(q i :q b ; q i+1 :q b ) from Eq. 7.15. We solve this motion planning problem with non-linear optimization with spline representation for the joint congurations. 7.7.4 Spline Representation and Seed Generation The trajectoryQ of the robot that needs to be determined is a sequence of robot congurations q for each of theN W nozzle tip waypoints. For a mobile manipulator, the number of degrees of free- dom isn + 3, wheren is the degrees of freedom of the manipulator. Generating path-constrained trajectories is computationally challenging for such high-DOF robotic systems. This is due to a large number of highly non-linear constraints and a large number of congurations (N W (n+3)) 150 that are to be solved for. Hence, we convert the trajectory generation problem into a discrete pa- rameter optimization problem. In this chapter, we approximate each of the conguration variable trajectories as a third-order B-Spline. Representation of the j th conguration variable, j , will be, j (s;x j ) = Ncp P i=1 R i;k (s)x j i ;s2 [0; 1] (7.25) R i;k+1 (s) = s i i+k i R i;k (s) + i+k+1 s i+1 R i+1;k (s) (7.26) R i;1 (s) = 8 > > > < > > > : 1; if i s< i+1 0; otherwise (7.27) In equation (7.26) and (7.27), i are the knots. The knots of a B-spline are the points where the piece-wise polynomials of the B-Spline meet. The map between the nozzle tip path length parameter (s) and time (t) is the following, s = 0 when t =t i and s = 1 when t =t f . N cp is the number of control points for the one dimensional spline curve representing j , x j is the vector of control points for j , andR i;k (s) are the basis functions parameterized with nozzle tip path-length parameter s of the workspace curveW p (s). In this work, we will represent the trajectories as splines and the trajectory generation problem as a parameter optimization problem. Each spline will represent the motion of one degree of freedom. Given the spline representation, the optimization problem is to nd the number of control points (N cp ) and the optimal set of control points (X) for the conguration variables. X is a vector created by vertically stacking the vectorsx j ;8j. For a mobile manipulator system with n + 3 degrees of freedom, the length of the vector X will be (n + 3)N cp . We need to determine the control points X such that the robot congurations satisfy the constraints on all N W nozzle tip waypoints. 151 The optimization variable for the problem instance in Eq. 7.15 is now the control points X. For an X, we can determine the robot congurations for any s2 [0; 1] giving the trajectory of the entire mobile manipulator. If we initiate the optimization routine with a random seed (or initial solution), then we may not be able to nd a feasible solution within a reasonable computation time-bound. We may land on an infeasible solution, as well as a highly sub-optimal solution, owing to the numerous local minima in this class of problems. We present a method to generate an approximate initial seed by nding an inverse kinematics (IK) solution for the mobile manipulator at a feasible nozzle pose for every nozzle tip waypoint. We uniformly sample N s = N W =10 nozzle tip waypoints, making sure that endpoints of individual segments are included in N s . With an initial mobile manipulator conguration (q seed ) as the seed, we compute a feasible IK solution for the N s nozzle tip waypoints by solving the following optimization problem. q i = argmin q (Nozzle Pose Error) i :i = 1; 2;:::N s (7.28) Nozzle pose error computation for a conguration q is done according to Eq. 7.22. Once we have the congurations for all the N s nozzle tip waypoints, we need to assign a nozzle tip path- length parameter s2 [0; 1] to each conguration. We assign the parameter s to a nozzle tip waypoint based on the length along the nozzle tip path from the rst waypoint position to the particular waypoint position. If the length along the nozzle tip path to a waypoint position is l, then s = l=L, where L is the nozzle tip path length. We assign the corresponding s to the conguration q obtained from solving the Eq. 7.7.4. We then determine the control points and knot vectors for (3 +n) third-order B-splines, one for each conguration variable (i.e degree of freedom) j like in Eq. 7.25. We stack the control points for each conguration variable to get the seed (X o ) of the optimization problem 7.15. X o is a (n + 3)N cp vector. 152 7.7.5 Successive Constraint Renement for Solving Multi-Stage Optimization Solving the full-blown problem considering all the constraints together can be computationally slow. Each constraint has one or several attraction basins in the optimization parameter space. Having all constraints together may result in opposing attraction canceling out, causing the solu- tion to get stuck in local minima or at an infeasible location [112]. Hence, it is necessary to solve a problem with a few constraints rst and successively add constraints to the previous solutions. Moreover, selecting the right sequence for adding constraint components to the optimization pro- cedure can help to converge quickly to the desired feasible region. We have identied the appropriate sequence after extensive experimentation to obtain a fea- sible solution quickly for a complex optimization problem described in 7.15. In the following methodology, the solveNLOpt function takes in the seed control points, the objective function and the constraints, and uses non-linear programming to determine a locally optimal solution. The following steps describe our approach. 1. X 1 solveNLOpt(X o ; Objective; Constraints) Objective : Nozzle Pose Error, Constraints : 2. X 3 solveNLOpt(X 2 ; Objective; Constraints) Objective : Nozzle Pose Error; Nonholonomy Error, Constraints : 3. X 4 solveNLOpt(X 3 ; Objective; Constraints) Objective : Nozzle Pose Error; Nonholonomy Error; Total Time Constraints : 153 4. X 5 solveNLOpt(X 4 ; Objective; Constraints) Objective : Nozzle Pose Error; Nonholonomy Error; Total Time; Total MB Distance Constraints : 5. X 6 solveNLOpt(X 5 ; Objective; Constraints) Objective : Nozzle Pose Error; Nonholonomy Error; Total Time; Total MB Distance Constraints : Joint Limits; Velocity Limits 6. X 7 solveNLOpt(X 6 ; Objective; Constraints) Objective : Nozzle Pose Error; Nonholonomy Error; Total Time; Total MB Distance Constraints : Joint Limits; Velocity Limits; Collision We do not have any constraints for steps 1-4 as the focus is only on reducing the objective function in each step. We add the joint and velocity limit constraints thereafter and then the collision avoidance constraint in the end. The sequence shown here has been obtained empirically after extensive experimentation. 7.8 Computation of Time Intervals between Waypoints In the previous section, the trajectory generated by the optimization-based motion planner sat- ises the robot motion and nozzle tip path-related pose constraints. Hence, it results in the disinfectant sprayed along the spray path. The surrogate for time in the optimization described in Eq. 7.7.3, measures the total time required to execute the task. It measures the time between two consecutive congurations as the maximum of the times required by each degree of freedom to move between the congurations at the maximum possible speed. Hence, this4t is the least 154 amount of time feasible between two consecutive congurations. Even though this results in the shortest trajectory execution time, it may not be enough to thoroughly disinfect every point on the path. Each point on the path needs enough amount of disinfectant to be thoroughly sanitized. If the robot moves too quickly, it may not spray enough disinfectant on some points for them to be sanitized. Hence, it is necessary to regulate the velocities of the conguration variables or degrees of freedom so that the nozzle moves with a velocity that results in thorough disinfection of every point on the path. We call this regulation of velocities as determination of time intervals between successive spray path waypoints. One way to regulate the velocities is to determine the amount of time between every two consecutive congurations of the trajectory so that the points on the surface that are sprayed receive enough disinfectant. As mentioned in Sec. 7.7.4, the number of nozzle tip waypoints used for seed generation for spline parameters and optimization is 10% of the total number of nozzle tip waypoints. Using the splines generated by the optimization-based motion planner, we generate new dense nozzle tip waypoints by computing the pose of the nozzle using forward kinematics of the congurations. An example of the dense nozzle tip waypoints is shown in Fig. 7.18. Consider a point P k on the point cloud as shown in Fig. 7.18. Each circle in the gure is the intersection of the spray cone frustum with the point cloud. It can be a circle or an ellipse, but here we consider it to be a circle for explanation purposes. The spray moves from left to right, P k is initially just outsidec 0 . It is insidec 1 toc 4 where it moves towards the center of the spray, and hence the spray deposition rate increases according to Fig. 7.3 and Eq. 7.14. This spray deposition rate may or may not be Gaussian. Inc 4 ,P k is the closest to the center as compared to any circle. After that, the spray deposition rate decreases, as shown. The area under the deposition rate curve is the total disinfectant deposited at point P k . This total disinfectant deposited must be greater than or equal to H o as mentioned in Sec. 8.3. Since the nozzle tip waypoints are discrete, we cannot compute the deposition rates of the point when the spray moves from c i to c i+1 . Hence, it is dicult to compute the accurate area under 155 Figure 7.18: The spray deposition rate for point P k as the spray passes through it. c 0 to c 8 are the circles (or cross-sections of the spray cone frustum with the point cloud) as the nozzle moves from left to right. the deposition rate curve. Therefore, we compute a conservative area by measuring the deposition rates at the discrete instances. From c 0 toc 1 ,P k is outside and then inside the circle. Hence, the deposition rate is zero initially and increases as it goes inside the circle. Hence, for c 0 to c 1 , we assume the spray deposition rate to be zero. Once P k is inside the circle, the deposition rates are greater than zero at every discrete waypoint. In this case, we take the conservative approximation of the area to be the minimum of the deposition rates at the two points consecutive waypoints multiplied by the time dierence between the two waypoints (which we are yet to determine). Hence, we get the area under the curve approximated as the bars as shown in Fig. 7.18. This area for the point P k of the form a k1 4t 1 +a k2 4t 2 +a k3 4t 3 +a k4 4t 4 +a k5 4t 5 +a k6 4t 6 . Here, we know the coecients a ki . This area should be less than H o . We sample N r points on the 156 Figure 7.19: The 5 test cases and the corresponding point clouds on which we tested our algorithm. (a) A wash basin sink (b) A workplace table and computer (c) Chairs representing a movie theatre or waiting area (d) Passage doors (e) A hospital bed point cloud and get the equation of the area for each of those points. Hence, we have N w 1 4ts to determine. Moreover, we know that each4t should be greater than or equal to the4t obtained from Eq. 7.7.3 from Sec. 7.7 since that is the least amount of time feasible between any two waypoints. Furthermore, we want to make sure that the total time taken to go from the rst nozzle tip waypoint to the last is minimized. Hence, we have the following linear programming formulation to determine the unknowns4t 1 to4t N W 1 . min cx s.t. AxH 0 o x4t 0 (7.29) where, x is a column vector of size N W 1 of4t i s, c is a row vector of ones of size N W 1. Hence cx is the total trajectory execution time. Ax H 0 o is the equation that encodes all the linear equations for the computation of the area for all N r points. Hence, the size of matrix 157 A is N r N W 1. H 0 o is a N r 1 vector with all entries H o . Moreover, as mentioned, each 4t i should be less than the minimum possible time permitted by the robot between every two consecutive waypoints. This is encoded in thex4t 0 .4t 0 is a column vector with each entry for the minimum feasible time dierence between two consecutive waypoints. This equation can be solved using any linear programming methodology. Since this is a set of linear equations, solving which is computationally very fast, a large number of samples from the point cloud (N r ) can be taken. Spraying at a point constantly so that it receives H o disinfectant is important rather than spraying on it at discrete times. It is possible that one point may lie inside several sets of consecutive circles. We want to make sure that only one consecutive set is used in determining the total spray deposited. Hence, we choose the set where the average deposition rate is the highest. This may help in driving down the corresponding4ts. The4ts obtained from this completes the trajectory of the mobile manipulator that guarantees complete disinfection of the points on the surface represented by the point cloud by spraying enough disinfectant on them. We can now regulate the velocity and acceleration of each degree of freedom or conguration variable so as to reach the next waypoint in at least the corresponding4t. 7.9 Results and Discussion 7.9.1 Test Cases We have tested our methodology on ve diverse test situations where disinfection is more likely needed on a daily basis. Fig. 7.19 shows the 5 test cases and the corresponding point cloud of the area to be disinfected. The test case (a) has a washbasin sink and adjacent area that needs disinfection. The test case (b) has a work area with a computer. We assume that the spray disinfectant evaporates, and hence it is safe to use on electronics. A pair of chairs are shown in the test case (c), representing a waiting area or a movie theatre. Test case (d) represents 158 Figure 7.20: The polygons for the 5 test cases and the corresponding spray paths. The solid parts represent segments from the Algo. 12, and the dashed parts represent the connecting segments. The areas of the polygon not sprayed are highlighted for each test case. The \s" corresponds to the dashed starting segment of the spray path and \t" stands for the solid terminal segment of the spray path. two doors of a busy hallway that needs disinfection due to constant usage. The nal test case (e) shows a hospital bed that needs disinfection. The corresponding scenarios and their point clouds were recorded in the robot simulation software CoppeliaSim [194], and the algorithms in this chapter have been implemented in MATLAB. The depth camera is mounted on the robot end-eector using which we capture several point clouds at dierent positions and orientations and stitch them together to generate the large point clouds shown. Table 7.1: The table describing the performance of the Spray Path Generation algorithm on the 5 test cases from Fig. 7.19. 7.9.2 Discussion on Generated Spray Paths The projected polygon for the point clouds and the corresponding planar spray paths are shown in Fig. 7.20. The spray paths show the maximum polygon area covering path possible for the 159 Figure 7.21: The spray path for the 5 test cases plotted on the corresponding point clouds. given polygon. The spray radius R s is taken to be 8 cm to compute the oset polygons, whereas the actual spray radius is 9 cm. This is done to increase the area covered by the spray path and not corners at acute angles. Of the maximum area covering paths, the ones shown are the ones that have been selected by the mobile manipulator capability map-based selection criterion from Sec. 7.6.5. The polygons and the corresponding spray paths are shown in Fig. 7.21. The solid lines in the path correspond to the segments generated by the spray path generation algorithm. The dashed lines correspond to the connecting segments between two consecutive segments generated by the algorithm. The starting dashed segment denoted by s is the starting spray path segment. This starting segment goes beyond the edge of the polygon so as to not leave an unsprayed region at the start. The terminal segment denoted by t is the nal segment. The direction and the starting point of this segment are determined as mentioned in Sec. 7.6.3. Although these paths should cover the entire area of the polygon, in the case of polygons with sharp angles like in cases (a) and (e) in Fig. 7.21, the corners may remain unsprayed. The unsprayed areas of the polygon have also been highlighted therein. The percentage area not sprayed is observed to decrease or remain the same with each iteration of the spray path generation algorithm, as shown in Fig. 7.23. The performance of the Spray Path Generation algorithm for all the ve test cases is shown in Tab. 7.1. For the test case (a), there are eight dierent spray paths generated, with the best path not spraying on about 0:6% of the area. The area not sprayed on is typically the corner 160 Figure 7.22: (a) This shows the spray eciency across the polygon for the test case (e). It can be seen that at the corners and at the ends of narrow extensions, the spray eciency is low as compared to the other parts of the polygon. (b) Bar plot showing the percentage of area that does not have spray eciency of 1 for all the 5 test cases. points when the angles are really small, as shown in Fig. 7.21 (a). During the algorithm, the most expensive step is the expansion of a segment, i.e., computing of its children and the oset polygon. Hence, test cases that have a large number of segments expanded have a higher computation time. Moreover, we observed that when a polygon has concavities, it takes longer to compute its oset polygon. Hence, in the test case (c), where the polygon has concavities, it takes a similar time for spray path generation as for test case (b), which has almost double the number of expanded polygon segments. The case (d) has a convex spray polygon similar to a rectangle. The algorithm computes the multiple spray paths. We terminate the algorithm when the last 5 test paths have zero grid points not sprayed on, and the length of the path does not decrease more than a set factor. Hence, for case (d), no branches are pruned since all attempts to compute the path are successful. The polygon for test case (e) as shown in Fig. 7.21 (e) has several narrow extensions. These are smaller in width as compared to the spray radius; hence the oset polygon does not 161 Figure 7.23: Percentage area not sprayed vs. the number of spray path iterations for all the 5 test case scenes. show any of these narrow extensions. Hence, these areas of the polygon tend to be ignored during the computation of the spray path. However, some of the extensions can be covered due to the starting segment and the nal segment, as can be seen in the gure whereas, other extensions as highlighted are not sprayed. Hence, we get only a single path that covers the maximum possible area, as shown in the gure. We observe a similar situation with respect to narrow extensions not being sprayed in the test case (a) and (c) as well, where there are narrow extensions represented by small acute angles at some of the polygon vertices. Hence, the percentage of the area not sprayed is higher for the test case (a) and (e). 162 To study the polygons that have narrow extensions and/or acute angles at any of the vertices, we dene a term called the spray eciency of any point inside the polygon. We uniformly sample a dense set of points inside the polygon. At each of the sampled points, we draw a circle with that point as the center and the spray radius as the radius. The ratio of the area of that circle that lies inside the polygon to the area of the circle is called the spray eciency. We assign this spray eciency to all the points inside the intersection of that circle and the polygon. We repeat this for every sampled point. Whenever the spray eciency of any particular point is being assigned, we check if the previous spray eciency was greater than, less than, or equal to the current spray eciency. If it is less than the current spray eciency, we make the spray eciency of that point to be the current spray eciency. This computation ensures that any point well inside the polygon has a spray eciency of 1 and points near corners or in narrow extensions have an eciency less than 1. The spray eciency color map is shown in Fig. 7.22 (a). It can be seen that inside the narrow extensions, the spray eciency is signicantly lower than at points well inside the polygon. The points that have a spray eciency less than 1 imply that spraying on those points will waste the spray disinfectant liquid. It can be seen that there is a similarity in the areas of the polygon that have lower spray eciency and the areas that the spray path does not spray on from Fig. 7.21 (e). Since the spray path start and end segments are computed in a dierent way as compared to the rest of the segments, the spray path ends up spraying a larger percentage of the area as compared to the area that Fig. 7.22 (a) would suggest. It should be noted that having a spray eciency less than 1 at a point does not mean that the spray path will not be able to spray on it. The spray eciency color map of a polygon helps us get an understanding of the areas of the polygon that are less likely to be sprayed on by the spray path. We need to make sure that the areas of the polygon that have a spray eciency of less than one are being sprayed by the spray path. Spray eciency not equal to 1 does not mean that the point will not be sprayed on. Hence, when comparing the percentage of the area not sprayed from Tab. 7.1 to the bar plot in Fig. 7.22, the actual percentage is much lower. It is still a nite 163 number for test cases (a), (c), and (d). We need to make sure that the areas marked in red in Fig. 7.21 are sprayed on. We determine the areas in red from Fig. 7.21 by computing the sprayed area and subtracting it from the original polygon. Once we have the areas, we nd the closest spray path waypoint to the centroid of the local unsprayed area (in red) and move towards the centroid from the waypoint till the entire unsprayed area is inside a spray circle. We travel this small segment from the nearest waypoint and back to the waypoint and continue to the next waypoint. The spray paths mapped onto the original point clouds for all the ve test cases are shown in Fig. 7.21. The small extensions to the spray paths are highlighted. This ensures that the entire spray polygon is sprayed on. Table 7.2: The performance of the optimization-based motion planner for the mobile manipulator for the ve test cases. 7.9.3 Bench-marking The spray paths generated by the spray path generation algorithm and the additional segments to cover the narrow extensions cover the entire area of the polygon. The algorithm generates several spray paths, and we choose one from several paths that cover the maximum area and is the shortest. There can be a few simple approaches to generate spray paths without following our algorithm. We compare our method with three potentially competing methodologies for generating spray paths. Zig-zag and spiral paths are typically the most common ways humans intuitively spray on the polygon. These methods may not be optimal in terms of the area coverage and even length of the path. Paths generated by our algorithm are essentially a combination of 164 zig-zag and spiral paths optimized to cover the entire area and not waste spray by staying inside the boundary of the polygon. We describe three competing methods for generating spray paths as follows: Figure 7.24: For the test case (a): (a) The longest convex edge zig-zag path (b) principle compo- nent zig-zag Path and (c) Spiral path Figure 7.25: Table comparing the longest convex edge zig-zag path to the principle component zig-zag path for all the 5 test cases • Longest Convex Edge Zig-Zag Path: For any polygon, we can generate a zig-zag path like in Fig. 7.13 with the parallel lines oriented in any direction. In this method, we take the longest convex edge of the polygon and generate a zig-zag spray path with that edge as the starting segment. The oset for the zig-zag parallel lines is twice the spray radius. The idea behind using the longest convex edge for getting the direction of the parallel segments of the path is that it will result in a greater chance of the spray path remaining inside the polygon. We want to make sure that the spray liquid is not wasted when the path is outside 165 Figure 7.26: Table comparing the Spiral path to the path generated by the spray path generation algorithm the polygon, which can happen when a zig-zag path is being sprayed on. The assumption here is that there is at least one convex edge for the polygon. • Principle Component Zig-Zag Path: This method is similar to the previous one, with the dierence being that the parallel segments of the zig-zag path aligned with the direction of the principle component of the polygon as opposed to the longest convex edge. The idea behind this method is that each spray segment sprays a large area along the segment. This reduces the number of spray segments required. In this and the longest convex edge zig-zag path, we start from a vertex of the polygon and move in a zig-zag fashion in the directions perpendicular to the principal component or the direction to the longest convex edge till there is no polygon left to disinfect. • Spiral Path: A spiral path sprays along the perimeter of the polygon and then goes inside layer by layer. This ensures coverage of the entire area. The eectiveness of a spray path can be studied by measuring quantities like the length of the path, the percentage of the area it covers, the length of the path where the spray disinfectant is wasted, i.e., it is sprayed outside the polygon. We compare the paths generated by the above methodologies with the paths generated by our method with respect to these quantities in order to gauge the performance of our algorithm with human-intuition-based simple approaches. The paths generated for the test scene (a) are shown in Fig. 7.24. The comparison is shown in Figs. 7.25 and 166 7.26. It can be seen that for most test cases, the length of the path generated by the spray path generation algorithm is higher as compared to the paths generated by the other three methods. However, the percentage of the area not sprayed is signicantly higher in each case for the three competing methods. Moreover, the percentage of spray wasted, which is the ratio of the sprayed area by the path outside the spray polygon to the total area sprayed inside the polygon, is also signicantly higher for the three methods. The wastage happens because the spray path no longer stays inside the polygon but can go outside to spray corners like Fig. 7.24(b). The spray path generation algorithm is targeted towards producing paths that cover the entire area and do not go outside the polygon. This is evident from the percentage of the area not sprayed and the percentage of the spray path wasted for all the 5 test cases as seen in Fig. 7.26. Hence, our algorithm ensures that there are no ineciencies during the spraying operations with respect to spray wastage and ensuring the entire area is covered. 7.9.4 Robot Motion Generation The successive constraint renement approach is used for robot motion generation to follow the spray paths from Fig. 7.21. The performance of the successive renement methodology is shown in Tab. 7.2. The number of control points for splines representing each degree of freedom depends on the length of the nozzle tip path and the resolution at which we consider points on the nozzle tip paths. For each point on the nozzle tip path at the considered resolution, we determine a feasible IK conguration of the robot using Eq. 7.7.4. The seed spline which ts all the IK solutions is then successively rened using successive renement methodology. We compare the performance of the successive renement methodology with a single-stage optimization with all the constraints being applied together. From Tab. 7.2, we observe that single-stage optimization generates a solution within a 2000 second time bound only for the test cases (a) and (b). For the next three test cases, single-stage optimization fails to nd a solution within the given time-bound. The algorithm was implemented in MATLAB; hence the computation time was observed to be large. The collision 167 count shows a clear picture in terms of the performance of the two methods. In the single-stage, the collision function is called several times in each iteration. However, for sequential optimization or successive renement, only at the last stage do we include the collision constraints. Hence, the collision count and the computation time for successive renement are signicantly lower than single-stage optimization. The end-eector pose error is taken to be the magnitude of the third term in Eq. 7.22. The maximum values of this error and the non-holonomy error from Eq. 7.23 are lower than 10 6 is each test case for successive renement based approach. Table 7.3: Comparison between the trajectory execution time before and after determining ap- propriate waypoint intervals and the corresponding percentage of point cloud points that did not receive enough disinfectant. For determining time intervals between successive waypoints, we sample points on the point cloud with a resolution of 1 cm. For all the sampled points, we measure the amount of spray disinfectant deposition rate at each waypoint according to Eq. 7.2. From the robot motions, we have the time taken to move between every two consecutive waypoints based on the maximum joint velocities. This is the fastest the robot can physically move between the waypoints, and hence the resulting trajectory is the time-optimal trajectory. However, it may result in not enough disinfectant sprayed on all the sampled points on the point cloud. We solve the linear programming problem in Eq. 7.29 to get the time taken to move between successive waypoints so that each point gets enough disinfectant. This increases the trajectory execution time as compared 168 Figure 7.27: The spray model for the physical experiment. to before appropriate time intervals are applied between waypoints as shown in Tab. 7.3. The trajectory execution times for all test cases before and after determining time intervals between successive waypoints are shown. We also show the percentage of the sampled points that do not receive enough disinfectant dosage before appropriate time intervals between waypoints. For test case (b), more than two-thirds of the points do not receive enough dosage, whereas for (d), only 13:3% of the points do not receive enough dosage. Based on the trajectory execution time before and after determining time intervals between successive waypoints, we can observe the benets in terms of the extra time required. To implement this, we regulate the joint velocities appropriately between the waypoints so as to reach the next waypoint by taking at least the prescribed time dierence from Eq. 7.29. After determining the appropriate time intervals between successive waypoints, each point receives enough dosage. 7.9.5 Physical Experiment We have conducted physical experiments with ADAMMS-SD for spraying on a scene replicating the test case (b). The spray cone parameters for the physical experiments are shown in Fig. 7.27. 169 Figure 7.28: The sequence of nozzle locations at 10 dierent locations of the nozzle tip path. Figure 7.29: (a) The scene for physical experiment (b) The spray path on the polygon (c) The point cloud and the overlayed spray path We used a Lysol disinfectant readily in stores for disinfection. We conducted experiments to determine the distances D n and D f . We take D n to be approximately the distance lower than which the spray liquid starts owing on the surface, andD f as the distance where the spray starts behaving as a projectile when the nozzle is spraying in the horizontal direction. The spray angle was found to be 14 o . By placing a measuring tube at the center of the spray at a distance D n we determined d o , the rate at the center of the spray to be approximately 1:1 mm=s. We use these same parameters for our simulation experiments as well. For eective disinfection, it is important that the treated surface be visibly wet for a certain length of time which is called the contact time. As per CDC guidelines, the contact time can be about 15 seconds for most objects in the home or oce environments. We spray a surface at a distanceD n from the nozzle tip for a certain amount of time and observe if it remains visibly wet for 15 seconds. We record the time and multiply it by d o to get the threshold disinfectant required any point to be H o as mentioned in Sec. 8.3. We 170 obtain this threshold to be approximately 1:8 mm. We use this threshold value for determining time intervals between successive waypoints in Sec. 7.8 and for simulation experiments. For the experiment, we kept the mobile base stationary since the entire scene workspace was reachable for the manipulator. We have regulated the velocities of the end-eector between waypoints according to the data provided by the time interval between successive waypoint de- termination methodology. The entire task is completed in about 17 seconds. 7.10 Summary In this chapter, we present a system and method for spray disinfection using mobile manipulators. The mobile manipulator has a spray nozzle and a depth camera attached to the end-eector. It captures a point cloud of the object or an area to be disinfected. The point cloud is projected and converted into a 2D polygon. We have presented an algorithm for determining a spray path that covers the entire area of the polygon. The algorithm produces non-intuitive paths that cover the entire area of the polygon. We use a branch and bound-based method with pruning heuristics to accelerate the computation of the spray path. The algorithm produces several spray paths, and we choose the one that enables the mobile manipulator to reach the maximum number of segments at the initial pose. We then project the spray path on the original point cloud to get the nozzle tip path. We move on to determining the mobile manipulator motions using a successive renement approach for non-linear optimization to nd the control points of the spline representation of the robot. This gives us the fastest possible path for the robot congurations. This path may not spray enough dosage to disinfect all the points of the area. We then determine time intervals between successive waypoints of the path by adding time delays between them. This results in sucient spraying on every point of a coarsely sampled point cloud. We have shown that the spray path algorithm generates a path that covers the majority of the area. We modify the path to make sure that narrow extensions and corners of the polygons are 171 sprayed suciently. The rst spray path is generated quickly, as shown in Tab. 7.1 and the nal path is generated within a few seconds. The successive renement or multi-stage optimization for motion generation results in signicantly faster computation of the path as compared to single-stage optimization. For the test cases where there is no time out, the successive renement approach computes a solution 30 times faster. Moreover, the errors for the pose of the end-eector and the non-holonomic constraints of the mobile base are of the order 10 6 . The trajectory generated by the optimization algorithm results in a large percentage of points not receiving enough disinfectant. After determining time intervals between successive waypoints, it results in 100% of the coarsely sampled points on the point cloud receiving enough disinfectant. The trajectory execution time increases to between 2-5 times as a result. There are a few limitations on the situations where the spray path generation algorithm may not perform well. Cases where there is a large depth where the nozzle tip may not be able to reach, is one such situation. Moreover, the algorithm only sprays on the area projected as the polygon, and there may be a need to generate a dierent polygon from dierent locations if the object being disinfected is large. Moreover, in cases where the point cloud projection of a large area results in a small area in 2D, it may not be sprayed completely. If the spray path is long, a single spline with a large number of control points may perform worse since the optimization variable size will quickly rise due to the high degrees of freedom of the robot. In such situations, it is necessary to break the splines into a number of splines and have spline connectivity constraints in the optimization. The nal limitation is that when the point cloud is large, even a sparse sampling of points may result in a large number of points for the matrix A of the linear programming problem formulation. It may take a signicant amount of time to create the problem instance in such cases. Moreover, if we make the sampling even sparse to reduce this time, the number of points being disinfected may not be accurate, and there may be areas of the point cloud that do not receive enough disinfectant even after determining time intervals between successive spray path waypoints. 172 It should also be noted that we have used a simplistic model of the disinfectant spray from the nozzle. Since we have neglected gravity, to robot spraying in close to upward directions will result in dierent spray depositions at points than what our model will expect. Having a detailed spray model including gravity and better spray ow model on steep surfaces will make our interval generation method produce accurate results for thorough disinfection. 173 Chapter 8 Task-Assignment and Motion Planning for Bi-Manual Mobile Manipulation 8.1 Introduction In our daily lives, we perform many tasks which require us to use both our arms to manipulate a variety of objects. Many such tasks cannot be completed without coordination between the two arms. Moreover, some tasks also require us to manipulate objects while walking. Bi-manual mobile manipulators are becoming popular because they resemble humans in dexterity and can perform a variety of tasks which a single arm mobile manipulator may not be able to perform. For example, carrying large objects, assembling furniture, opening and holding a CNC machine door while trying to extract the part etc. These tasks can be broken into subtasks. To successfully perform a task, the subtasks need to be assigned to the appropriate (left or right) manipulator. Each arm of a bi-manual mobile manipulator needs to be assigned appropriately to carry out dierent subtasks. Moreover, the mobile base may need to assist either or both the arms in performing those tasks. A symbolic task-agent assignment without taking into account the feasibility of the motions of the agents may lead to failure in task execution. Consider a bi- manual mobile manipulator robot performing the task of attaching a board on the wall using a 174 drill machine as shown in Fig. 8.1. We observe that a wrong assignment of the arms can result in infeasible (here in collision) nal conguration of the robots. Most of the Task and Motion Planning (TAMP) algorithms [137] can generate a task sequence for the agents involved and their corresponding trajectories. These algorithms invoke the motion planning routine for each task-agent assignment to validate the assignment. This approach is computationally viable when the query to the motion planning routine is not computationally intensive. However, in the case of a bi-manual mobile manipulator where for some tasks both the manipulators and the base need to work collaboratively, invoking motion planning for each agent assignment might be computationally expensive due to the high number of Degrees of Freedom (DOF). If the wrong arm is assigned for a task, the notication for this will come only after the motion planner returns an infeasible trajectory. This poses a challenge when we want to execute complex tasks using a bi-manual mobile manipulator. In this work, we have proposed a search based algorithm for determining the correct task-agent assignment for generating feasible motions for a bi-manual mobile manipulator. We assume that the task network is given to us, which is independent of the robot. We propose spatial constraint based heuristics to prune branches of the search tree such that the motion planner is not executed for the wrong task-agent assignment. The developed algorithm selects the correct DOF needed to execute the given task. Moreover, we have a separate tree for motion planning once the task- agent assignment is done. We have introduced a caching scheme to move from motion planning to task-agent assignment if the motion plan fails and assign a dierent set of agents only for the part of the motion that failed. This helps in avoiding repeated calls to expensive motion planners and signicantly reducing computation time. 175 Figure 8.1: A Bi-manual Mobile Manipulator (BMM) has to attach a part (blue board) to the wall using a drilling machine. The BMM has the agents (M l ;M r ;B) (right arm, left arm, mobile base). If it is assignedM r for holding the part andM l for drilling as shown in (b), it will result in an infeasible and in-collision pose for doing the task. Whereas the opposite assignment will result in a feasible pose without collisions for the task. 8.2 Background and Preliminaries Object: Let, O i be an object. In this work, we will assume that O i is a rigid body with a coordinate frame attached to it. We assume that the state of O i is dened as its the pose (p =x;y;z;q x ;q y ;q z ;q w or p =x;y;z;;; ) with respect to the world coordinate frame (W ). Bi-manual mobile manipulator: Let BMM refer to the bi-manual mobile manipulator into consideration. Let M l and M r refer the left and the right arms (serial-link manipulators) re- spectively and let B represent the mobile base of the robot. Here, each of the arms may or not have the same number of DOF. Without loss of generality, we consider two similar arms with n DOF ( 1 ; 2 ::: n ). The mobile base conguration is represented by (x;y;) for its location and orientation. We assume each arm to be equipped with the appropriate end-eector to execute the desired tasks. We denote any of the arms or the base as an agent. Task: LetT k be a task. A task is dened by the state transition of an object. T k , is associated with the pre-conditions (p pre ) and post-condition (p post ) of the state of one object. We assume that the associated object is available at its pre-condition state before the task starts. We assume that the type and number of agents required by the task are specied. One or more agents will 176 carry out the task based on the requirement. The object will be transferred to the post-condition state after the agent/s have completed the task. We assume that the pre/post-conditions are given as sets of poses (i.e., p pre 2 P pre , p post 2 P post ,jP pre j 1,jP post j 1). During task execution, we assume that a xed-joint formation is required between an agent and an object whenever the agent operates on the object. 8.3 Problem Formulation 8.3.1 Task Network Modeling Let T be a task-network. There are one or more connected tasks in T that completes an operation. We assume that any two tasks (T A ;T B ) in T can have one of the following four interdependencies- (1) No interdependency (2) Precedence: IfT A has precedence overT B , thenT B cannot start untilT A has been nished. Based on the availability, we can assign the same or dierent agent for T A and T B . (3) Immediacy: If an immediacy connection comes from T A to T B , then T B has to start immediately after T A . The state of the object at the end of T A will be the state of the object at the start of T B . Based on the object's state transition requirement, it may not be possible to assign dierent agents for T A and T B . (4) Concurrence: We dene container task and embedded task to explain concurrence. Let container task be the task that will have a longer duration and embedded task as the task that is overlapped by the container task. Let, T A and T B two concurrent tasks. Let, T A be the container task task and T B be the embedded task. By denition, T B cannot start until T A has started and T A cannot be nished until T B has nished. Therefore, the same agent cannot be assigned for the embedded task of a container task. 177 Figure 8.2: Task network for the example shown in Fig. 8.1. We consider each of the task given in T as a core task. We assume that there can be supporting tasks required to facilitate the core task. For a BMM we assume that the supporting task will always be done by the mobile baseB. For example, if we assignM r to pick up an object, B will assist when the object is unreachable for M r . There may be cases where B has to assist both M r and M l to perform the task. Let us consider the example shown in Fig. 8.1. The associated task-network for the BMM is illustrated in Fig. 8.2. There are four tasks in this task network. We want to assign agents to carry out these tasks and generate trajectories for the agents. As an example, we will summarize the attributes/requirements of the tasks below. • PickPart: { Associated Object: Part. { Required Agent Type: Manipulator. { Pre-condition: p pre =p part initial . { Post-condition: p post 2P part pickedup . • PickTool: { Associated Object: Tool. { Required Agent Type: Manipulator. 178 { Pre-condition: p pre 2P tool initial { Post-condition: p post 2P tool pickedup • HoldPart: { Associated Object: Part. { Required Agent Type: Manipulator. { Pre-condition: p pre 2p part pickedup { Post-condition: p post =P part on wall • AttachPart: { Associated Object: Tool { Required Agent Type: Manipulator. { Pre-condition: p pre 2P tool pickedup { Post-condition: p post =p tool on wall From our denitions ofprecedence,immediacy, andconcurrence, and the task requirements, we can see thatHoldPart (HP ) has to begin immediately afterPickPart (PP ). AttachPart (AP ) can not start until PT has nished and HP has started. HP cannot nish until AP is nished. 8.3.2 Problem Statement For a given task network, we need to sequence the tasks to satisfy the constraints in the network, assign agents to the tasks, and generate their trajectories to carry out the tasks using a BMM. The trajectories for the agents need to be generated such that the pre and post-conditions of each task inT are satised. Therefore, we need to generate continuous trajectories for the agents such that there is a smooth transition between the end of one task and start of the next. For certain 179 tasks, it might be required to generate synchronous trajectories for the agents to complete the task in a feasible and time-ecient manner. For other tasks, it might be feasible to generate an independent trajectory for an agent without aecting the operation completion time. Formally, given a task networkT and a BMM, our objective is to assign tasks to the agents, sequence the tasks, and generate continuous conguration space trajectories for each agent such that there is no con ict in the assignment and the task is executed successfully. 8.4 Approach In our approach to solving the task-agent assignment and motion planning problems using a BMM, we consider a two-layered architecture as shown in Fig. 8.3. Each layer is divided into temporal windows. The division into temporal windows is based on the task order and constraints. In the rst layer which is the task agent assignment layer, we assign which task is to be done by which arm(s) of the BMM. In the second layer which is the motion planning layer, we execute the motion planners for the task assignments from the rst layer. Based on the feasible congurations and the type of agents assigned for the task, we determine what kind of motion planner to call. If a motion planner fails to nd a feasible solution, we move to the rst layer in the same temporal window. We then assign a dierent agent(s) for the task in the rst layer and attempt motion planning again in the second layer. 8.4.1 Task-Agent Assignment Layer In this layer, we assign appropriate agents for the tasks in each temporal window. We create a search tree from the root node to the leaf node for each temporal window. The root and leaf nodes are virtual nodes. Here, a node means a task-agent assignment. For a BMM, each task can be completed by one of the following 6 sets of agents: M l , M r , (M l ;B), (M l ;B), (M l ;M r ), (M l ;M r ;B). If in a temporal window, there aren tasks, the root node will haven 6 successors, 180 Figure 8.3: The layers described in our approach i.e for each task, there are 6 possible agent combinations to execute the task. For each of those n 6 tasks, we have (n 1) 6 successors for the remaining tasks. The branching factor here is large, however a large number of the nodes will have task-agent assignments that will be infeasible due to the task constraints or spatial constraints. We have developed a set of heuristics to identify such nodes and prune those branches before we run expensive motion planners for executing the tasks. The order of the task-agent node expansion is based on that lesser the number of DOF required to complete the task, the lesser will be the computation overhead for motion planning. For example, if a BMM has to pick up an object from a table and it is reachable for one of the arms, we do not want to plan for all the DOF for the robot. Planning for just that arm is sucient. Hence, for any node the successor task-agent assignments nodes are expanded in the order of increasing DOF as follows: (M l ), (M r ), (M l ;B), (M l ;B), (M l ;M r ), (M l ;M r ;B). Here, left and right can be interchanged, but for the sake of convenience, we x this order. This acts as a branch guiding heuristic for the search. 8.4.1.1 Task Constraint Heuristics The rst task constraint heuristic is based on the concurrency constraint between tasks. For concurrent tasks, if M i is being used for the embedded task, we can prune the branch which has assignment ofM i and (M i ;B) for the container task. The second task constraint heuristic is based on the constraints from the task network provided. If a task T A has been assigned to the arm 181 M i , and there is a task T B such that there is the immediacy condition from T A to T B , then task T B must be assigned to M i , (M i ;B) or (M i ;M j ;B). Here, tasks T A and T B can be in dierent temporal windows. 8.4.1.2 Spatial Constraint Heuristic This heuristic is based on the reachability and existence of collision-free IK for the agents assigned to the task in a node. Tasks in which the object needs only one arm to manipulate and is reachable for arm without the motion of the mobile base can be done using M l orM r or (M l ;B) or (M l ;B), i.e either use the appropriate arm or an arm + base together. For some tasks, it might not be possible to complete the task with a single arm or two arms unless the base is moved. For such tasks, we can only use (M l ;B) or (M r ;B). For tasks which need both the arms and the object is reachable for both, we can use (M l ;M r ) or (M l ;M r ;B). And nally, for tasks which need both arms and for at least one of them the object is not reachable, (M l ;M r ;B) must be used. These combinations are determined by the novel spatial constraint heuristic which checks for the reachability of arms and existence of collision-free IK solutions for arm(s) + base combinations. The check for reachability and existence of feasible collision free IK for high DOF is computationally ecient and helps us to reduces calls to the expensive motion planning query for each task-agent assignment. The details for spatial constraint heuristic implementation is discussed in Sec. 8.4.5. In the task-agent assignment layer, we nd a feasible branch from the root node to the leaf node for each tree in the temporal windows. We connect all these branches and have a solution for the task-agent assignment problem. We use this branch for the motion planning layer. 8.4.2 Motion Planning Layer In this layer, we take in as input, the branches from the root node of the rst temporal window to the leaf node of the last temporal window in the task-agent assignment layer. Here, we adaptively 182 select the motion planners and generate motion for the agents. Each node in each of the search tree in this layer is composed of task-agents assignments. If by sampling the keyframes of the associated object, we can determine the constraints on the object. This, in turn, helps us determine whether a point-to-point (p2p) or constrained motion generation is required for the assigned agents to execute each task. In addition to generating motion of the agents at each node, our method generates motion for the transition between the end of a tree in one window and the beginning of a tree in the next. The details of motion generation for a BMM are described in Sec. 8.4.6. An example of the two layers and temporal windows for the problem in Fig. 8.2 is shown in Fig. 8.4. 8.4.3 Caching Scheme The search trees are labeled ast (i;j) withi being the temporal window index andj being the layer index. There are 2 layers and say m temporal windows. In a layer, if we nd a branch from the root node to the leaf node for a tree t (i;j) , we move on to the tree t (i;j+1) which is in the next temporal window. In the rst layer, if there is no feasible branch from the root to the leaf for t (i;1) , we go to the tree t (i1;1) of the previous temporal window and prune the current expanded branch and continue the search to the next branch. If we reach the leaf node of the tree t (m;1) of the last temporal window in the rst layer, we begin expanding the treet (1;2) of the rst temporal window of the second layer. In the second layer, for trees in each temporal window, we generate motions as described in 8.4.2. However, if the motion planner fails to nd a feasible motion in a treet (i;2) , we directly move to the tree t (i;1) of the same temporal window but in the rst layer. Also, if the motion planner fails to nd a feasible transition between leaf node of t (i1;2) and root node of t (i;2) , we move to the treet (i;1) . In the treet (i;1) , we prune the current branch and explore other branches. If we nd a dierent feasible branch here, we move back to the tree t (i;2) motion planning layer in the same temporal window. If there is any inconsistency between the nodes of tree t (i;1) (current temporal 183 window) and the tree t (i+1;1) (next temporal window) in the rst layer due to dierent agent assignment, we move to the tree t (i+1;1) instead and resolve those inconsistencies but expanding the tree again. This continues till the entire rst layer has a consistent agent assignment based on the task constraint heuristic explained in Sec. 8.4.1.1. Then we move back to the tree t (i;2) which had infeasible motion, to begin with. This caching scheme helps in avoiding re-exploration of the same nodes in trees due to the generation of infeasible motions in the motion planning later. We explain the algorithm using the example in Fig. 8.2. In line 1 of Algo. 13, a container R stores all the combination of agents possible for a BMM. 8.4.4 The Algorithm Figure 8.4: Example of completely expanded search trees at dierent temporal windows of the three layers for the example in Fig. 8.2 The partially expanded search trees for both the layers in the example are shown in Fig. 8.4 . The function AssignTaskAgent in the algorithm 15 shows the pseudo-code of the search for task-agent assignment. Since there are two tasks in the rst temporal window, the root node will have 12 successors. Each of these 12 nodes will have 6 successors for the other task. In Fig. 8.4, we show the rst correct task-agent assignment. Here, we assume that the initial location of the BMM is far away from the part and the tool. Hence, in t (1;1) , the nodes which do not have the baseB as an agent for the task will be pruned using the spatial constrain heuristic (marked red). 184 Algorithm 13 Agent Assignment and Motion Planning 1: function AgentAssignmentAndMotionPlanning(T) 2: R fM l , Mr, (M l ;B), (M l ;B), (M l ;Mr), (M l ;Mr;B)g 3: empty nnGlobal container for Best Solution 4: :cost 1 5: tmax Maximum computation time 6: W IdentifyTemporalWindows(T) 7: T fg nnContainer for Search Trees 8: for i2f1;:::;jWjg do 9: ti;1;ti;2 InitializeTree(i;T;W ) 10: T T[ti;1[ti;2 11: end for 12: AssignTaskAgent(t1;1;T;R;W;T;tmax; ) 13: return 14: end function Algorithm 14 Select Next Tree 1: function SelectNextTree(flag;ti;j;Ln;T ) 2: if flag && j ==Ln && i ==jWj then 3: GenerateSolutionConnectingAllTrees(T ) 4: if .cost < .cost then 5: 6: end if 7: return ti;j1 8: end if 9: ti1;j;ti;j1;ti+1;j;ti;j+1;t1;j+1 GetNextTreeSet(ti;j;T ) 10: if !flag then 11: if j == 1 then 12: nextTree ti1;j 13: else 14: nextTree ti;j1 15: end if 16: end if 17: if !FeasibleTransition(ti;j1;ti;j ) then 18: nextTree ti;j1 19: end if 20: if ti+1;j .available() && FeasibleTransition(ti;j;ti+1;j ) then 21: nextTree ti;j+1 22: end if 23: if !ti+1;j .available() && !FeasibleTransition(ti;j;ti+1;j ) then 24: if i + 1jWj then 25: nextTree ti+1;j 26: else 27: nextTree t1;j+1 28: end if 29: end if 30: return nextTree 31: end function 185 Here, the node GP (M l ;B) is selected for expansion. Once this node is expanded, a collision-free IK solution is generated for the agents combination at the pre and pose conditions of the object in the task and stored in the node. The successor nodes for the PP (M l ;B) node are shown. The nodes with the same agent like PT (M l ) or same pair of agents PT (M l ;B) will be still valid in here even though the same arm is being used for PP and PT . Those nodes will get pruned due to task constraint heuristics in the next temporal window due to the concurrency condition between the hold part (HP ) and the attach part (AP ) tasks. The nodes PT (M r ) may or may not be pruned using the spatial constraint heuristic depending on the nal conguration of the BMM at the parent node. If the arms are not reachable inPT (M l =M r ) and we do not nd a feasible IK conguration for the nodes PT (M l =M r ;B), we prune those branches using (spatial constraint heuristic) and move on to node PT (M r ;M l ;B). At this node, we nd the IK for the whole BMM, to satisfy the post conditions of the task PT . Once we reach the leaf node of the tree in rst temporal window, we move on to the root node of the tree in second temporal window. If the nodePT (M r ;M l ;B) is also infeasible (i.e no feasible IK), we move to the PT (M r ;B), and continue the search in the similar fashion. The function isPruned in line 8 of algorighm 15 is shown in 16. This explains how the pruning is done using the task and spatial constraint heuristics. The SelectNextTree function in line 26 of algorithm 15 is used to select the next tree to move on to. It also encodes the caching scheme explained in Sec. 8.4.3. In the second temporal window, the hold part (HP ) and attach part (AP ) are concurrent tasks. Moreover, HP and PP have immediacy constraints. Hence, we can prune branches that have a dierent arm assigned for HP as compared to GP using the task constraint heuristics described in Sec. 8.4.1.1 to get the resulting tree looking like in Fig. 8.4. After a branch from the root node to the leaf node of the tree in the last temporal window is found for the rst layer, we move to the second layer. 186 Algorithm 15 Assign Task Agent 1: function AssignTaskAgent(ti;j;T;R;W;T;tmax; ) 2: while ElapsedTime()tmax do 3: node ti;j .PQ.pop() 4: if ! node.hasChildren() then 5: break 6: end if 7: S R 8: for s2S do 9: if isPruned(s;T) then 10: continue 11: end if 12: ti;j .PQ.push(child;child.cost) 13: end for 14: end while 15: T:update(ti;j ) 16: flag;ti;j IsBranchComplete(node;ti;j ) 17: if ! flag && i == 1 &&j == 1 then 18: return 19: end if 20: t fail 21: if flag && i ==jWj && j == 2 then 22: flag; ;t fail PlanMotion(T; ) 23: end if 24: if TrajExecutionTime() < TrajExecutionTime( ) then 25: 26: end if 27: nextTree SelectNextTree(flag;ti;j;T;t fail ) 28: AssignTaskAgent(nextTree;T;R;W;T;tmax; ) 29: end function Algorithm 16 is Pruned 1: function isPruned(s;T) 2: flag false 3: parent =s:parent 4: if TaskConstraint(parent:task; s:task; T) is immediacy then 5: if s:arms62parent:arms then 6: flag true 7: end if 8: end if 9: if TaskConstraint(parent:task; s:task; T) is concurrency then 10: if s:arms2parent:arms then 11: flag true 12: end if 13: end if 14: if CheckReachability(s) is false then 15: flag true 16: end if 17: return flag 18: end function 187 8.4.5 Spatial Constraint Checking Nodes which assign a single arm to do a task, like PT (M l ) in Fig. 8.4 are validated using the capability map of the manipulator. A capability map is used to perform collision-free reachability tests for a manipulator [156, 228]. A capability map is a pre-computed voxel grid, where a single query to each voxel returns if the centroid of the voxel is reachable by the robot in position and within a range of orientations. A single query on a pre-computed capability map is signicantly faster than solving IK numerically. We use analytical IK-solver for non-redundant manipulators and numerical IK-solver for re- dundant systems (i.e., mobile-base and arm/s combination) to test the collision-free reachability at key-frames. If a task has to be performed by multiple manipulators, each attached to a dierent mobile-base, then we solve the IK for one manipulator/s and mobile-base combination at a time by keeping the conguration of others constant. The CheckReachability function in line 13 of Algorithm 3 checks these spatial constraints. Finding collision free numerical IK is computationally expensive as we have to include collision costs in the numerical optimization cost function [108]. Hence, we attempt to nd the numerical IK without collision and check for collisions later. This is repeated a few times with dierent seeds for optimization. If there is no feasible solution from this method, we attempt numerical optimization with collision costs once. If this also results in no feasible IK solution, we prune that node. 188 8.4.6 Motion Generation In this chapter, we have used sampling-based planners [132, 192] for the point-to-point motion of high-DOF systems, such as mobile base and arm combinations. We have used an optimization- based method to generate constrained trajectories. We pose the constrained trajectory genera- tion problem as a discrete parameter optimization problem and solve it using successive rene- ment [108]. We set the objective to be minimizing the trajectory execution time and constraints to be (1) maintaining the constraints imposed by the task, (2) satisfying the kinematic and dynamic constraints of the robot, and (3) avoiding collisions. Many tasks require complex coordinated motion among agents, like synchronous motion be- tween both the arms and the base. For high-DOF systems like BMM, we can generate the syn- chronous trajectory for both the arms together using the optimization-based approach. However, it can be computationally ecient to generate motion of agent(s) independently while keeping the previously generated motion of other agent(s) as constraint. For example, we can generate point-to-point motion of one of the arms and the base for a task and use it as a spatiotempo- ral constraint for constrained trajectory generation for the other arm. These motion planning methods are encoded in the PlanMotion function in line 21 of Alg. 15. This takes in the tree container T and the current best solution and nds if the next solution is of a lesser time cost. In this function, we make sure that the transition between the leaf node and the roots nodes of two successive trees in the motion planning layer is smooth. The t fail output argument is to determine which tree of the motion planning layer has infeasible motions. This is useful to execute the caching scheme as explained in Sec. 8.4.3. 8.5 Results and Discussion We have implemented our algorithm for task-agent assignment and motion planning for 3 chal- lenging tasks. The BMM we used has 17 DOF, 3 DOF for the mobile base and 7 for each of 189 Figure 8.5: Task No. 3 : The BMM has to pick up a box (green) and place it on the table, open it and pick up a part (blue) and place it outside. Then it has to pick up a small part (red) and assemble it with the blue part using a hammer the arms. The arms are the KUKA iiwa 7 manipulators and mobile base is the Inspectorbot MegaBot. Here we assume that the mobile base is holonomic. The tasks executed by the BMM are described in Figs. 8.6, 8.7 and 8.5. There were 8, 5 and 12 tasks in the task networks for the three tasks respectively. To demonstrate the eectiveness of the spatial constraint heuristics and the caching scheme used, we show the time taken to nd the rst solution for our algorithm. Further, we compare it with direct motion planning without the spatial constraint heuristic and the caching scheme. The time taken for all the three tasks for both the cases is shown in Tab. 8.1. The algorithm was implemented using MATLAB with an Intel Xeon 3.50GHz processor and 32GB of RAM. We set the total time limit for the algorithm to be 30 mins. For motion planners, we set the time limit to be 30s, 60s, and 90s for motions involving one, two, and three agents respectively. It can be observed from Tab. 8.1 that with spatial checking and caching, on average the computation time for a solution is about 86% lower than without spatial constraint checking and caching. Where there is no spatial constraint checking, the motion planner is called for every task-agent assignment branch resulting in high computation time. Most of the initial branches are infeasible and that is known only after the planner attempts planning. The details of the 190 Figure 8.6: Task No. 1 : The BMM has to pick up a long part and attach it to the wall using a drilling machine (blue). After it drills on the right end, it has to move and drill in the center while holding the part. It has to nally drill at the left end to complete the task. 191 Figure 8.7: Task No. 2 : The BMM has to take a long rod (red) out of a box (brown) and transfer it to a dierent location. It pulls out the part using the right arm, then holds it using both the arms and pulls it out completely. Thereafter, it transfers the part to a dierent location task networks and the generated trajectories are available in the video at the following link https://tinyurl.com/yyku6gmz 8.6 Summary We have presented a two layered architecture for task-agent assignment and motion planning for a bi-manual mobile manipulator with a given task network. We have introduced spatial constraint checking heuristics for pruning branches of search trees in a computationally ecient way. This signicantly reduces the computation time as the motion planner is not called at every task- agent assignment, but only those which are feasible in terms of reachability and collision-free IK solutions. Moreover, we also present a caching scheme in which we move between search trees when motion planner fails to nd a solution for a task, so as to not plan again for the successful portion of the task. These two techniques signicantly reduce the computation time. We have used point to point planners for the BMM and constrained motion generation wherever necessary. 192 Table 8.1: Impact of having spatial constraint checking and caching in task-agent assignment and motion planning on computation time In the future we plan to expand this to a group of co-operating bi-manual mobile manipulators for even more complex tasks. The main limitation of this work is that it is geared towards bi-manual mobile manipulators only. If the number of arms or the number of robots increase, the branching factor for the task agent assignment layer will signicantly increase. This may become intractable since the number of combinations of the robots to be used will increase even further. Moreover, when there are three or more tasks in one of the time windows with multiple robots, this can make it computationally intractable especially when motion planning is taken into account. And nally, for this work we assume that the motion planners are all available, however, for certain motions existing motion planners may not work well. Especially when there are asynchronous motions of the robot required, i.e., when one of the arms starts moving, and before its task is done, the motion of the other arm begins and both their motions end at dierent times. Such complex motions are extremely challenging to execute and are assumed to be available in this work which can be a limiting factor. 193 Chapter 9 Conclusions This dissertation develops computational techniques for computing high quality paths for certain mobile manipulator tasks. It presents novel solutions to eciently complete tasks like part pick- up, transportation, area coverage and day-to-day activities with a bimanual mobile manipulator. Three computational techniques extensively used in this dissertation are discrete search-based planning used for mobile base path and area coverage planning, parametric optimization for trajectory generation for mobile manipulator and exploration-exploitation-based sampling method applied to active learning for high-speed grasping and accelerating sampling based motion planning methods. 9.1 Intellectual Contributions The key contributions from the chapters of this dissertation are summarized below. • Mobile Base trajectory planning for part pick-up and transportation: We have developed a planner which generates time-optimal trajectories for the selected action discretization level. The heuristics are based on the time the mobile base will spend insode the grasping area. The resulting trajectories are able to move the mobile base and the manipulator simultaneously. It takes into account the joint constraints for the manipulator. This results 194 in grasping of the part more naturally without stopping the mobile base motion, giving time-optimal trajectories with respect to the resolution of the discretizated actions. • Manipulator Planning for Part Pick-up on given Mobile Base Trajectory: We have presented an algorithm for planning of mobile manipulators for pick-up and transportation operations. Firstly we plan for the mobile base trajectory taking a simplistic interpolation-based ap- proach for the manipulator. This gives us the mobile base trajectory that passes through the grasping area, spending enough time inside it so that grasping while moving is possible. Further, we introduce a multi-tree bi-directional rapidly exploring patch tree (MT-BiRPT) algorithm with three techniques to speed up the planning of the mobile manipulator on the above mobile base trajectory. The three techniques include a conguration space path for expanding multiple congurations together, biased & hybrid sampling and a tree selec- tion heuristic. With these techniques applied to the algorithm, a trajectory is computed that enables the manipulator to start moving as late as possible on the mobile base path, avoid obstacles and grasp the object with a feasible grasping strategy. The safe trajectories are computed signicantly faster than competing methods, that ensure the gripper remains stationary when grasping happens. • Active Learning for Picking-Up Objects with a High-Speed Gripper: We have presented an approach for solving two challenging problems encountered during part pick-up with a moving gripper. The rst one addresses the eect of the uncertainty in the part pose on the gripper speed. We present an active learning-based approach for constructing a meta-model for estimating the success probability of grasping a part as a function of the gripper speed and gripper closing speed. The second problem addressed here is generating a trajectory for the mobile manipulator to satisfy the previously generated grasping success probability constraints. We use a sequential renement method for optimization to generate trajectories and show that its performance is superior as compared to one without sequencing. The 195 generated trajectories are such that the gripper moves with the required velocity and the gripper starts closing at the appropriate time instance making sure that the object is grasped with the desired probability threshold. • Accelerating Point-to-Point Motion Planning for Mobile Manipulators: In challenging prob- lems like a nonholonomic mobile manipulator carrying a large object, determining a feasible solution is extremely dicult due to narrow passages in the conguration space and the nonholonomic constraints of the mobile base. Focusing the sampling in dierent regions of the conguration space can signicantly reduce the computation time. For a mobile manip- ulator, this focusing must be done for the mobile base and the manipulator in appropriate regions to accelerate the growth of the trees towards each other. The primary contribution in this work is a method to coordinate the focusing for the mobile base and the manipulator samples in the workspace and grow the trees in appropriate directions. Moreover, we also presented a heuristic for determining connections between the mobile base rst and then nd connections for the manipulator. It was observed that without this heuristic, each of the competing algorithms have a high failure rate to even compute a path. We have showed that these together signicantly reduce the computation time as compared to existing methods in diverse scenarios. • Area Coverage for Spray-based Disinfection with a Mobile Manipulator: We have presented a framework for spray-based disinfection using mobile manipulator, where the robot has a spray nozzle and a depth camera attached to the end-eector. It captures a point cloud of the object or an area to be disinfected. The point cloud is projected and converted into a 2D polygon. We have developed an algorithm for determining a spray path that covers the entire area of the polygon. For the algorithm we use a branch and bound-based method with pruning heuristics to accelerate the computation of the spray path. The algorithm produces several non-intuitive spray paths that cover the entire area of the polygon and we 196 choose the one that enables the mobile manipulator to spray on the maximum number of segments at the initial pose. We then project the spray path on the original point cloud to get the nozzle tip path. We then determine the mobile manipulator motions using a successive renement approach for non-linear optimization to nd the control points of the spline representation of the robot. This gives us the fastest possible path for the robot congurations. This path may not spray enough dosage to disinfect all the points of the area. To this end, we determine time intervals between successive waypoints of the path by adding time delays between them. This results a mobile manipulator trajectory that sprays enough disinfectant on each point to ensure thorough disinfection of the surface. • Task-Agent Assignment for Bimanual Mobile Manipulation: Planning motions for a biman- ual mobile manipulator must include assigning appropriate task the to correct manipulator. In this chapter, we have presented a two-layered architecture for task-agent assignment and motion planning for such a robot to complete two independent tasks represented by a task network. We introduced spatial constraint checking heuristics for pruning branches of search trees in a computationally ecient way. This signicantly reduces the computation time as the motion planner is not called at every task-agent assignment, but only those which are feasible in terms of reachability and collision-free IK solutions. Moreover, we also present a caching scheme in which we move between search trees when motion planner fails to nd a solution for a task, so as to not plan again for the successful portion of the task. These two techniques signicantly reduce the computation time as the robot can now avoid planning for paths where the task assignment is wrong. 197 9.2 Summary of Lessons Learnt The previous section went over the major contributions of this dissertation. This section is focused on discussing lessons learned from work in this dissertation and how these can be used by readers for performing coordinated motion planning for various tasks with mobile manipulators. This dissertation explores all three major motion planning techniques for high-degree of free- dom systems like mobile manipulators. The rst one is discrete state-space search for mobile robot motion planning with manipulator constraints. One of the main lesson that can be applied to improve the performance of such algorithms is that collision and heuristic related terms should be pre-computed and stored to be used during run-time. For this purpose the state-space is dis- cretized nely. We use Euclidean Distance Transform (EDT) to compute collision, where each discrete grid point has a corresponding distance to the closest obstacle. Moreover, we use Fast Marching Method (FMM) to compute a heat-map that gives the distance to the goal from any point by avoiding obstacles. This can be used as a very accurate heuristic measure. To make it further more accurate, one can in ate the obstacles taking into account the dimensions of the robot. For parametric optimization for motion planning, it is important to focus on generating a good optimization seed. Generally, the amount of time required to generate a feasible solution from a bad or a random seed is signicantly higher than the amount of time required to nd a good seed and optimizing it to a feasible local minimum. This becomes more and more apparent as the dimension of the optimization variable increases. Moreover, for a complex robot trajectory for which the parametric curve variables required may be large, it is computationally benecial to break complex curve into smaller simpler curves and have constraints to make sure the curves can be merged. This saves computational time since each optimization step is now cheaper since the optimization variable has a smaller dimension in such cases. This philosophy can be extended to sampling-based methods as well. For example, in Chapter 6, our goal is to compute a good 198 quality solution for the challenging planning problem. Most methods in literature focus on nding the optimal solution assuming that a initial solution is readily available. That may not be the case as observed for this particular problem. Here nding the initial solution itself will take up signicant time if specialized techniques are not utilized. Hence, nding a good initial solution can be very important. Moreover, sampling-based methods are generally followed by path smoothing methods to make sure the random nature of the path is ironed out. A good solution will take less computational eort to smoothen as compared to a bad one. Hence, it is important to nd a good initial solution for solving any problem where the goal is to nd an optimized solution. Another lesson that can be learned from the work in this dissertation is that breaking complex problem into smaller ones and solving them in the best possible way to get a good but not the best solution can be signicantly better computationally than solving the whole problem together by putting in immense eort and getting the best possible solution. An example of this is the problem of spray-based disinfection that we addressed in Chapter 7. There we solve the area coverage of the polygon as the step before motion planning of the manipulator to follow this generated path. We could have solved both the problems together, i.e., plan for the robot motion such that it covers the entire area. Although this would have given a possibly optimal solution, it would be extremely challenging to formulate the problem and even more to solve it. The computation time required would also be signicantly higher. The solution we get is by no means the optimal solution but it is a fast and eective solution. Similarly, to ensure that disinfection is thorough we add time intervals after we have computed the trajectory. We could have added this to the optimization constraints, which would have computed a dierent and probably a better solution, but would have taken signicantly higher computational eort. The lesson learned here is that there is a trade-o between solving for the optimal solution and solving for a reasonably good and eective solution in terms of computational eort. A lesson that can be applied for sampling-based motion planning methods was that focus- ing samples in areas of conguration space is an important technique to accelerate the motion 199 planning. However, it is important to understand that focusing of samples is very sensitive, i.e., if due to some reason, we focus the sampling in wrong volumes of the state-space, the resulting computational time will be signicantly worse than with no focusing at all. Hence, focusing tech- niques for speeding up motion planning need to be used in a careful manner. For example, in chapter 6, we need to make sure that the free-space disks for the mobile base and the free-volume balls for the manipulator lie in the same homotopy class when seen from above. If this is not ensured, there may be cases where the manipulator end-eector positions are sampled far away from mobile base positions which is not physically possible to achieve. This will result in the planner failing to nd a solution. This is especially true if one is trying to machine learn focus regions in the conguration space from several planning instances. It should be made sure that the focus region are encoded in a way that mitigates the wrong biases. If the learned focus regions are not appropriate, the resulting planner will perform worse than not having focusing at all. 9.3 Anticipated Benets This dissertation presents methods for mobile manipulation to pick-up and transport object in an ecient manner. This can be useful in warehouses where picking up and transportation of objects is time-critical. It can result in decrease in the number of robots on the shop oors and save costs. The corresponding algorithms can also be used for picking up dangerous garbage on highways without stopping the vehicle or even capturing space debris in the orbit with a robotic arm. The dissertation also presents a method for disinfection of surfaces with an autonomous mobile manipulator. During any pandemic it is important to focus the human-resources to life saving tasks and leave repeatable tasks to autonomous robots. With our area coverage algorithm, thor- ough disinfection is possible by a mobile manipulator stopping the spread of diseases on surfaces. 200 Since disinfection can be a risky task for humans, using mobile manipulators can have an immense impact. Task-assignment for motion planning for bimanual mobile manipulators is also presented in this work. It is anticipated that the research would enable task and motion planners to be used specically for such robots, which are beginning to become mainstream in home automation and warehouses. The methodologies presented in this dissertation can be used not only in a manufacturing setting but also for day-to-day automation at homes. For example, bimanual robots can be used at home where most things are designed for humans like for loading and unloading a dishwasher, loading and unloading washing machines and folding clothes etc. The methodologies presented can form the basis for future development in these areas. 9.4 Future Directions The presented work can be extended in the following directions. • One avenue for extending the work in this dissertation having a learning representation of all the motion planning instances a planner has successfully solved using it in a planner that makes the decisions in tricky scenarios based on past successes. For example, nding paths through narrow passages is important for a planner. Learning how this was done in past planning instances can be utilized in new planning instances to speed-up planning. Another aspect that can be learned from previous planning instances is determining which is a better node from the other tree to connect to the current tree to so as to maximize the change of moving towards the goal. • Another future direction is use experience of previous plans in similar instances to plan for future paths. For example, a stationary manipulator for large portion, will remain in a similar environment and obstacle placement. But currently, a planning instance plans from scratch for every planning query. It is important to make use of previous experiences to 201 plan paths instantly in scenarios where the environment does not change much. Moreover, such experiences should be able to be transferred to dierent robots. • Another area to work on in the future is to minimize grasping time with grasp planner. Our grasp planner has gripper speed and closing speed as the parameters and the grasping strategy is given to us. However, the grasping strategy may not be the best one given that the gripper is in motion. Current deep learning frameworks for grasp learning consider only the locations on the object as grasp parameters, which may not be ideal. One can have a deep learning framework that determines where to grasp the object and the gripper speeds as parameters for learning under pose uncertainty. Such grasp strategies will work not only for mobile manipulators but also for stationary manipulators grasping objects from conveyor belts with not only a two ngered gripper but also suction grippers. • For our point-to-point motion planner for mobile manipulators carrying large objects, we have focus regions only for the end-eector position whereas the orientation is taken to be random. This may not be ideal because the orientation of a large object is an important factor when carrying it through a narrow passage. Hence, in future if one can learn the orientations of the objects that are feasible using an active learning or a rejection sampling method, this may signicantly accelerate the motion planning of the mobile manipulator carrying large objects. • For area coverage with a mobile manipulator, we have used a coverage planner on polygon that is independent on whether the segment can be executed by the mobile manipulator when projected on the point cloud. One can have a cost function that assigns costs to the segment based on how easy or dicult the segment is to be executed. A metric for diculty can be dened and the cost can be a weighted sum of this metric, length of the path and how much area the segment disinfects. This will make it easier to plan for the optimization-based planner to compute robot trajectories. 202 • For our task-assignment and motion planner we use existing planners to plan for robot motions. One of the restrictive factors for this is that there is no planner that can plan for asynchronous motions for a bimanual mobile manipulator. Asynchronous motions implies that for both the arms, the motion starts and ends at dierent times. This problem is especially challenging since the two arms are connected by a mobile base and it is important to determine the time instance at which the second arm begins moving. 203 Reference List [1] Descartes- a ros-industrial project for performing path-planning on under-dened cartesian trajectories. http://wiki.ros.org/descartes. Accessed: 2018-05-21. [2] Trajectory Planning for Conformal 3D Printing Using Non-Planar Layers, volume Volume 1A: 38th Computers and Information in Engineering Conference of International Design Engineering Technical Conferences and Computers and Information in Engineering Con- ference, 08 2018. V01AT02A026. [3] Development of Three-Nozzle Extrusion System for Conformal Multi-Resolution 3D Printing With a Robotic Manipulator, volume Volume 1: 39th Computers and Information in En- gineering Conference of International Design Engineering Technical Conferences and Com- puters and Information in Engineering Conference, 08 2019. V001T02A024. [4] A Robotic Cell for Embedding Prefabricated Components in Extrusion-Based Additive Man- ufacturing, volume Volume 1: Additive Manufacturing; Advanced Materials Manufacturing; Biomanufacturing; Life Cycle Engineering; Manufacturing Equipment and Automation of International Manufacturing Science and Engineering Conference, 09 2020. V001T01A012. [5] Ntu singapore researchers build disinfection robot to aid cleaners in covid-19 outbreak, 2020. Accessed: 2020-04-14. [6] Evan Ackerman. Autonomous robots are helping kill coronavirus in hospitals-ieee spectrum, 2020. Accessed: 2020-03-11. [7] Sandip Aine, Siddharth Swaminathan, Venkatraman Narayanan, Victor Hwang, and Maxim Likhachev. Multi-heuristic a. The International Journal of Robotics Research, 35(1-3):224{ 243, 2016. [8] Baris Akgun and Mike Stilman. Sampling heuristics for optimal motion planning in high dimensions. In International Conference on Intelligent Robots and Systems, pages 2640{ 2645, 2011. [9] Sarah Al-Hussaini, Shantanu Thakar, Hyojeong Kim, Pradeep Rajendran, Brual C Shah, Jeremy A Marvel, and Satyandra K Gupta. Human-supervised semi-autonomous mobile manipulators for safely and eciently executing machine tending tasks. arXiv preprint arXiv:2010.04899, 2020. [10] Sergey Alatartsev, Sebastian Stellmacher, Frank Ortmeier, S Alatartsev, S Stellmacher, and · F Ortmeier. Robotic Task Sequencing Problem: A Survey. Journal of Intelligent Robotic Systems, 80:279{298, 2015. [11] Javier Alonso-Mora, Ross Knepper, Roland Siegwart, and Daniela Rus. Local motion plan- ning for collaborative multi-robot manipulation of deformable objects. In IEEE Interna- tional Conference on Robotics and Automation (ICRA), pages 5495{5502. IEEE, 2015. 204 [12] Mark Anderson. Uv light might keep the world safe from the coronavirus|and whatever comes next-ieee spectrum, 2020. Accessed: 2020-09-28. [13] Vivek Annem, Pradeep Rajendran, Shantanu Thakar, and Satyandra K. Gupta. Towards remote teleoperation of a semi-autonomous mobile manipulator system in machine tending tasks. In ASME Manufacturing Science and Engineering Conference (MSEC), Erie, USA, June 2019. [14] Yasemin Bekiroglu, Janne Laaksonen, Jimmy Alison Jorgensen, Ville Kyrki, and Danica Kragic. Assessing grasp stability based on learning and haptic data. IEEE Transactions on Robotics, 27(3):616{629, 2011. [15] D. Berenson, J. Chestnutt, S. S. Srinivasa, J. J. Kuner, and S. Kagami. Pose-constrained whole-body planning using task space region chains. In 9th IEEE-RAS International Con- ference on Humanoid Robots, pages 181{187, Dec 2009. [16] D. Berenson, S. S. Srinivasa, D. Ferguson, and J. J. Kuner. Manipulation planning on constraint manifolds. In 2009 IEEE International Conference on Robotics and Automation, pages 625{632, May 2009. [17] Dmitry Berenson, Siddhartha Srinivasa, and James Kuner. Task space regions: A frame- work for pose-constrained manipulation planning. The International Journal of Robotics Research, 30(12):1435{1460, 2011. [18] P. M. Bhatt, C. Gong, A. M. Kabir, R. K. Malhan, B. C. Shah, and S. K. Gupta. Incor- porating tool contact considerations in tool-path planning for robotic operations. In ASME Manufacturing Science and Engineering Conference, volume 84256, page V001T05A018, Cincinnati, OH, June 2020. [19] P. M. Bhatt, A. M. Kabir, R. K. Malhan, B. Shah, A. V. Shembekar, Y. J. Yoon, and S. K. Gupta. A robotic cell for multi-resolution additive manufacturing. In 2019 International Conference on Robotics and Automation (ICRA), pages 2800{2807, 2019. [20] P. M. Bhatt, A. M. Kabir, R. K. Malhan, A. V. Shembekar, B. C. Shah, and S. K. Gupta. Concurrent design of tool-paths and impedance controllers for performing area coverage op- erations in manufacturing applications under uncertainty. In 2019 IEEE 15th International Conference on Automation Science and Engineering (CASE), pages 1151{1156, Aug 2019. [21] P. M. Bhatt, A. M. Kabir, M. Peralta, H. A. Bruck, and S. K. Gupta. A robotic cell for performing sheet lamination-based additive manufacturing. Additive Manufacturing, 27:278{289, May 2019. [22] P. M. Bhatt, A. Kulkarni, R. K. Malhan, and S. K. Gupta. Optimizing part placement for improving accuracy of robot-based additive manufacturing. In IEEE International Confer- ence on Robotics and Automation, Xi'an, Canada, May 2021. [23] P. M. Bhatt, R. K. Malhan, and S. K. Gupta. Computational foundations for using three degrees of freedom build platforms to enable supportless extrusion-based additive manu- facturing. In ASME Manufacturing Science and Engineering Conference, Erie, PA, June 2019. [24] P. M. Bhatt, R. K. Malhan, P. Rajendran, and S. K. Gupta. Building free-form thin shell parts using supportless extrusion-based additive manufacturing. Additive Manufacturing, 32:101003, March 2020. 205 [25] P. M. Bhatt, R. K. Malhan, P. Rajendran, A. V. Shembekar, and S. K. Gupta. Trajectory- dependent compensation scheme to reduce manipulator execution errors for manufacturing applications. In ASME Manufacturing Science and Engineering Conference, Virtual, June 2021. [26] P. M. Bhatt, M. Peralta, H. A. Bruck, and S. K. Gupta. Robot assisted additive manufac- turing of thin multifunctional structures. In ASME Manufacturing Science and Engineering Conference, College Station, TX, June 2018. [27] Prahar Bhatt, Rishi K Malhan, Pradeep Rajendran, Brual Shah, Shantanu Thakar, Yeo Jung Yoon, and Satyandra K Gupta. Image-based surface defect detection using deep learning: A review. Journal of Computing and Information Science in Engineering, pages 1{23, 2021. [28] Julien Bidot, Lars Karlsson, Fabien Lagrioul, and Alessandro Saotti. Geometric back- tracking for combined task and motion planning in robotic systems. Articial Intelligence, 247:229{265, jun 2017. [29] J.E. Bobrow, S. Dubowsky, and J.S. Gibson. Time-optimal control of robotic manipulators along specied paths. The International Journal of Robotics Research, 4(3):3{17, 1985. [30] Daniel M Bodily, Thomas F Allen, and Marc D Killpack. Motion planning for mobile robots using inverse kinematics branching. In Robotics and Automation (ICRA), 2017 IEEE International Conference on, pages 5043{5050. IEEE, 2017. [31] Simon Bgh, Mads Hvilshj, Morten Kristiansen, and Ole Madsen. Identifying and evaluat- ing suitable tasks for autonomous industrial mobile manipulators (aimm). The International Journal of Advanced Manufacturing Technology, 61(5-8):713{726, 2012. [32] Jeannette Bohg, Antonio Morales, Tamim Asfour, and Danica Kragic. Data-driven grasp synthesis|a survey. IEEE Transactions on Robotics, 30(2):289{309, 2014. [33] Christoph Borst, Thomas Wimbock, Florian Schmidt, Matthias Fuchs, Bernhard Brunner, Franziska Zacharias, Paolo Robuo Giordano, Rainer Konietschke, Wolfgang Sepp, Stefan Fuchs, et al. Rollin'justin-mobile platform with variable base. In 2009 IEEE International Conference on Robotics and Automation, pages 1597{1598. IEEE, 2009. [34] Abdeslam Boularias, Oliver Kroemer, and Jan Peters. Learning robot grasping from 3-d images with markov random elds. In EEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 1548{1553. IEEE, 2011. [35] Konstantinos Bousmalis, Alex Irpan, Paul Wohlhart, Yunfei Bai, Matthew Kelcey, Mrinal Kalakrishnan, Laura Downs, Julian Ibarz, Peter Pastor, Kurt Konolige, et al. Using simu- lation and domain adaptation to improve eciency of deep robotic grasping. arXiv preprint arXiv:1709.07857, 2017. [36] Chris Bowen and Ron Alterovitz. Accelerating motion planning for learned mobile ma- nipulation tasks using task-guided gibbs sampling. In Robotics Research, pages 251{267. Springer, 2020. [37] Randy C Brost. Planning robot grasping motions in the presence of uncertainty. Carnegie- Mellon University, The Robotics Institute, 1985. [38] Felix Burget, Maren Bennewitz, and Wolfram Burgard. Bi 2 rrt*: An ecient sampling- based path planning framework for task-constrained mobile manipulation. In IEEE/RSJ In- ternational Conference on Intelligent Robots and Systems (IROS), pages 3714{3721. IEEE, 2016. 206 [39] Fabrizio Caccavale and Bruno Siciliano. Quaternion-based kinematic control of redundant spacecraft/manipulator systems. In Proceedings 2001 ICRA. IEEE International Confer- ence on Robotics and Automation (Cat. No. 01CH37164), volume 1, pages 435{440. IEEE, 2001. [40] Zhenhua Cai, Sihao Deng, Hanlin Liao, Chunnian Zeng, and Ghislain Montavon. The eect of spray distance and scanning step on the coating thickness uniformity in cold spray process. Journal of thermal spray technology, 23(3):354{362, 2014. [41] Shehan Caldera, Alexander Rassau, and Douglas Chai. Review of deep learning methods in robotic grasp detection. Multimodal Technologies and Interaction, 2(3):57, 2018. [42] Stephane Cambon, Rachid Alami, and Fabien Gravot. A hybrid approach to intricate motion, manipulation and task planning. The International Journal of Robotics Research, 28(1):104{126, 2009. [43] M. Cefalo, G. Oriolo, and M. Vendittelli. Planning safe cyclic motions under repetitive task constraints. In 2013 IEEE International Conference on Robotics and Automation, pages 3807{3812, May 2013. [44] Chao Chen, Markus Rickert, and Alois Knoll. Combining space exploration and heuris- tic search in online motion planning for nonholonomic vehicles. In 2013 IEEE Intelligent Vehicles Symposium (IV), pages 1307{1312. IEEE, 2013. [45] Peter. Y. Chen and Paula. M. Popovich. Correlation: Parametric and nonparametric mea- sures. Number 137-139. Sage, 2002. [46] Wei Chen, Hao Liu, Yang Tang, and Junjie Liu. Trajectory optimization of electrostatic spray painting robots on curved surface. Coatings, 7(10):155, 2017. [47] T Chettibi, HE Lehtihet, M Haddad, and S Hanchi. Minimum cost trajectory planning for industrial robots. European Journal of Mechanics-A/Solids, 23(4):703{715, 2004. [48] Rohan Chitnis, Dylan Hadeld-Menell, Abhishek Gupta, Siddharth Srivastava, Edward Groshev, Christopher Lin, and Pieter Abbeel. Guided search for task and motion plans using learned heuristics. pages 447{454. [49] Sachin Chitta, Benjamin Cohen, and Maxim Likhachev. Planning for autonomous door opening with a mobile manipulator. In IEEE International Conference on Robotics and Automation (ICRA), pages 1799{1806. IEEE, 2010. [50] Changhyun Choi, Wilko Schwarting, Joseph DelPreto, and Daniela Rus. Learning object grasping for soft robot hands. IEEE Robotics and Automation Letters, 2018. [51] S. Choudhury, J. D. Gammell, T. D. Barfoot, S. S. Srinivasa, and S. Scherer. Regionally accelerated batch informed trees (rabit*): A framework to integrate local information into optimal path planning. In 2016 IEEE International Conference on Robotics and Automation (ICRA), pages 4207{4214, May 2016. [52] Vassilios N Christopoulos and Paul Schrater. Handling shape and contact location uncer- tainty in grasping two-dimensional planar objects. In Intelligent Robots and Systems, 2007. IROS 2007. IEEE/RSJ International Conference on, pages 1557{1563. IEEE, 2007. [53] Matthew Clifton, Gavin Paul, Ngai Kwok, Dikai Liu, and Da-Long Wang. Evaluating performance of multiple rrts. In 2008 IEEE/ASME International Conference on Mechtronic and Embedded Systems and Applications, pages 564{569. IEEE, 2008. 207 [54] Benjamin J Cohen, Sachin Chitta, and Maxim Likhachev. Search-based Planning for Ma- nipulation with Motion Primitives. In IEEE International Conference on Robotics and Automation (ICRA), 2010. [55] Benjamin J Cohen, Gokul Subramanian, Sachin Chitta, and Maxim Likhachev. Planning for Manipulation with Adaptive Motion Primitives. In International Conference on Robotics and Automation, 2011. [56] Raaele Colombo, Francesco Gennari, Vivek Annem, Pradeep Rajendran, Shantanu Thakar, Luca Bascetta, and Satyandra K. Gupta. Parameterized model predictive control of a non- holonomic mobile manipulator: A terminal constraint-free approach. In IEEE International Conference on Automation Science and Engineering (CASE), Vancouver, Canada, August 2019. [57] Erdinc Sahin Conkur. Path following algorithm for highly redundant manipulators. Robotics and Autonomous Systems, 45(1):1 { 22, 2003. [58] David C Conner, Aaron Greeneld, Prasad N Atkar, Alfred A Rizzi, and Howie Choset. Paint deposition modeling for trajectory planning on automotive surfaces. IEEE Transac- tions on Automation Science and Engineering, 2(4):381{392, 2005. [59] Daniela Constantinescu and Elizabeth A Croft. Smooth and time-optimal trajectory plan- ning for industrial manipulators along specied paths. Journal of robotic systems, 17(5):233{ 249, 2000. [60] Corinna Cortes and Vladimir Vapnik. Support-vector networks. Machine learning, 20(3):273{297, 1995. [61] S. Dalibard, A. Nakhaei, F. Lamiraux, and J. P. Laumond. Whole-body task planning for a humanoid robot: a way to integrate collision avoidance. In 2009 9th IEEE-RAS International Conference on Humanoid Robots, pages 355{360, Dec 2009. [62] Hao Dang, Jonathan Weisz, and Peter K Allen. Blind grasping: Stable robotic grasping using tactile feedback and hand kinematics. In Robotics and Automation (ICRA), 2011 IEEE International Conference on, pages 5917{5922. IEEE, 2011. [63] Neil T Dantam, Zachary K Kingston, Swarat Chaudhuri, and Lydia E Kavraki. An in- cremental constraint-based framework for task and motion planning. The International Journal of Robotics Research, 37(10):1134{1151, 2018. [64] Carl De Boor. A practical guide to splines, volume 27. Springer-Verlag New York, 1978. [65] Sunipa Dev and Je Phillips. Attenuating bias in word vectors. arXiv e-prints, pages arXiv{1901, 2019. [66] Didier Devaurs, Thierry Sim eon, and Juan Cort es. A multi-tree extension of the transition- based rrt: Application to ordering-and-pathnding problems in continuous cost spaces. In 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 2991{ 2996. IEEE, 2014. [67] M Dogar, Kaijen Hsiao, Matei Ciocarlie, and Siddhartha Srinivasa. Physics-based grasp planning through clutter. pages 78{85, 2012. [68] Stanimir Dragiev, Marc Toussaint, and Michael Gienger. Uncertainty aware grasping and tactile exploration. In Robotics and Automation (ICRA), 2013 IEEE International Confer- ence on, pages 113{119. IEEE, 2013. 208 [69] Vikranth Dwaracherla, Shantanu Thakar, Leena Vachhani, Abhishek Gupta, Aayush Yadav, and Sahil Modi. Motion planning for point-to-point navigation of spherical robot using position feedback. IEEE/ASME Transactions on Mechatronics, 24(5):2416{2426, 2019. [70] Vikranth Reddy Dwaracherla, Shantanu Thakar, GK Arun Kumar, and Leena Vachhani. Discrete time position feedback based steering control for autonomous homing of a mobile robot. In 2016 12th IEEE International Conference on Control and Automation (ICCA), pages 773{778. IEEE, 2016. [71] M. Freese E. Rohmer, S. P. N. Singh. V-rep: a versatile and scalable robot simulation frame- work. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2013. [72] Olav Egeland. Task-space tracking with redundant manipulators. IEEE Journal on Robotics and Automation, 3(5):471{475, 1987. [73] Adrien Escande, Nicolas Mansard, and Pierre-Brice Wieber. Hierarchical quadratic pro- gramming: Fast online humanoid-robot motion generation. The International Journal of Robotics Research, 33(7):1006{1028, 2014. [74] Valentin Falkenhahn, Frank A Bender, Alexander Hildebrandt, R udiger Neumann, and Oliver Sawodny. Online tcp trajectory planning for redundant continuum manipulators using quadratic programming. In Advanced Intelligent Mechatronics (AIM), 2016 IEEE International Conference on, pages 1163{1168. IEEE, 2016. [75] Pedro F Felzenszwalb and Daniel P Huttenlocher. Distance transforms of sampled functions. Theory of computing, 8(1):415{428, 2012. [76] David Fischinger, Markus Vincze, and Yun Jiang. Learning grasps for unknown objects in cluttered scenes. In IEEE International Conference on Robotics and Automation (ICRA), pages 609{616. IEEE, 2013. [77] Roger Frutos, Jordi Serra-Cobo, Lucile Pinault, Marc Lopez Roig, and Christian A Devaux. Emergence of bat-related betacoronaviruses: hazard and risks. Frontiers in Microbiology, 12:437, 2021. [78] Enric Galceran and Marc Carreras. A survey on coverage path planning for robotics. Robotics and Autonomous systems, 61(12):1258{1276, 2013. [79] M. Galicki. Time-optimal controls of kinematically redundant manipulators with geometric constraints. IEEE Transactions on Robotics and Automation, 16(1):89{93, Feb 2000. [80] Miros law Galicki. Real-time constrained trajectory generation of mobile manipulators. Robotics and Autonomous Systems, 78:49{62, 2016. [81] Jonathan D Gammell, Timothy D Barfoot, and Siddhartha S Srinivasa. Informed sampling for asymptotically optimal path planning. IEEE Transactions on Robotics, 34(4):966{984, 2018. [82] Jonathan D Gammell, Siddhartha S Srinivasa, and Timothy D Barfoot. Informed rrt*: Op- timal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic. In Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ International Conference on, pages 2997{3004. IEEE, 2014. 209 [83] Jonathan D Gammell, Siddhartha S Srinivasa, and Timothy D Barfoot. Batch informed trees (bit*): Sampling-based optimal planning via the heuristically guided search of im- plicit random geometric graphs. In 2015 IEEE International Conference on Robotics and Automation (ICRA), pages 3067{3074. IEEE, 2015. [84] Caelan Reed Garrett, Tom as Lozano-P erez, and Leslie Pack Kaelbling. Ffrob: An ecient heuristic for task and motion planning. In Algorithmic Foundations of Robotics XI, pages 179{195. Springer, 2015. [85] A Gasparetto and V Zanotto. A new method for smooth trajectory planning of robot manipulators. Mechanism and machine theory, 42(4):455{471, 2007. [86] M. Giftthaler, F. Farshidian, T. Sandy, L. Stadelmann, and J. Buchli. Ecient kinematic planning for mobile manipulators with non-holonomic constraints using optimal control. In 2017 IEEE International Conference on Robotics and Automation (ICRA), pages 3411{ 3417, May 2017. [87] Emile Glorieux, Pasquale Franciosa, and Dariusz Ceglarek. Coverage path planning with targetted viewpoint sampling for robotic free-form surface inspection. Robotics and Computer-Integrated Manufacturing, 61:101843, 2020. [88] Kalin Gochev, Venkatraman Narayanan, Benjamin Cohen, Alla Safonova, and Maxim Likhachev. Motion planning for robotic manipulators with independent wrist joints. In IEEE International Conference on Robotics and Automation (ICRA), pages 461{468. IEEE, 2014. [89] Marcus Gualtieri, Andreas ten Pas, Kate Saenko, and Robert Platt. High precision grasp pose detection in dense clutter. In Intelligent Robots and Systems (IROS), 2016 IEEE/RSJ International Conference on, pages 598{605. IEEE, 2016. [90] Kris Hauser and Victor Ng-Thow-Hing. Fast smoothing of manipulator trajectories using optimal bounded-acceleration shortcuts. In 2010 IEEE international conference on robotics and automation, pages 2493{2498. IEEE, 2010. [91] Dominik Henrich, Christian Wurll, and Heinz W orn. Multi-directional search with goal switching for robot path planning. In International Conference on Industrial, Engineering and Other Applications of Applied Intelligent Systems, pages 75{84. Springer, 1998. [92] Andreas Hermann, J Sun, Zhixing Xue, Steen W Ruehl, Jan Oberl ander, Arne R onnau, Jo- hann Marius Z ollner, and R udiger Dillmann. Hardware and software architecture of the bi- manual mobile manipulation robot hollie and its actuated upper body. In 2013 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, pages 286{292. IEEE, 2013. [93] J urgen Hess, Gian Diego Tipaldi, and Wolfram Burgard. Null space optimization for eec- tive coverage of 3d surfaces using redundant manipulators. In 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 1923{1928. IEEE, 2012. [94] Rachel Holladay, Oren Salzman, and Siddhartha Srinivasa. Minimizing task-space frechet error via ecient incremental graph search. IEEE Robotics and Automation Letters, 4(2):1999{2006, 2019. [95] Rachel M Holladay and Siddhartha S Srinivasa. Distance metrics and algorithms for task space path optimization. In 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 5533{5540. IEEE, 2016. 210 [96] Robert Holmberg and Oussama Khatib. Development and control of a holonomic mo- bile robot for mobile manipulation tasks. The International Journal of Robotics Research, 19(11):1066{1074, 2000. [97] Matanya B Horowitz and Joel W Burdick. Combined grasp and manipulation planning as a trajectory optimization problem. In Robotics and Automation (ICRA), 2012 IEEE International Conference on, pages 584{591. IEEE, 2012. [98] Bidan Huang, Sahar El-Khoury, Miao Li, Joanna J Bryson, and Aude Billard. Learning a real time grasping strategy. In 2013 IEEE International Conference on Robotics and Automation (ICRA), pages 593{600. IEEE, 2013. [99] Junsen Huang, Pengfei Hu, Kaiyuan Wu, and Min Zeng. Optimal time-jerk trajectory planning for industrial robots. Mechanism and Machine Theory, 121:530{544, 2018. [100] ieee. Robotic arm wields uv light wand to disinfect public spaces-ieee spectrum, 2020. Accessed: 2020-04-28. [101] Fahad Islam, Venkatraman Narayanan, and Maxim Likhachev. Dynamic Multi-Heuristic A *. In International Conference on Robotics and Automation, pages 2376{2382, 2015. [102] Fahad Islam, Venkatraman Narayanan, and Maxim Likhachev. A*-connect: Bounded sub- optimal bidirectional heuristic search. In 2016 IEEE International Conference on Robotics and Automation (ICRA), pages 2752{2758. IEEE, 2016. [103] L eonard Jaillet and Josep M Porta. Path planning under kinematic constraints by rapidly exploring manifolds. IEEE Transactions on Robotics, 29(1):105{117, 2012. [104] L eonard Jaillet and Josep M Porta. Path planning with loop closure constraints using an atlas-based rrt. In Robotics Research, pages 345{362. Springer, 2017. [105] Lucas Janson, Edward Schmerling, Ashley Clark, and Marco Pavone. Fast marching tree: A fast marching sampling-based method for optimal motion planning in many dimensions. The International journal of robotics research, 34(7):883{921, 2015. [106] Edward Johns, Stefan Leutenegger, and Andrew J Davison. Deep learning a grasp function for grasping under gripper pose uncertainty. In Intelligent Robots and Systems (IROS), 2016 IEEE/RSJ International Conference on, pages 4461{4468. IEEE, 2016. [107] Joseph L Jones and Tomas Lozano-Perez. Planning two-ngered grasps for pick-and-place operations on polyhedra. In IEEE International Conference on Robotics and Automation (ICRA), pages 683{688. IEEE, 1990. [108] Ariyan Kabir, Alec Kanyuck, Rishi K. Malhan, Aniruddha V. Shembekar, Shantanu Thakar, Brual C. Shah, and Satyandra K. Gupta. Generation of synchronized conguration space trajectories of multi-robot systems. In IEEE International Conference on Robotics and Automation (ICRA), Montreal, Canada, May 2019. [109] Ariyan M Kabir, Krishnanand N Kaipa, Jeremy Marvel, and Satyandra K Gupta. Au- tomated planning for robotic cleaning using multiple setups and oscillatory tool motions. IEEE Transactions on Automation Science and Engineering, 14(3):1364{1377, 2017. [110] Ariyan M. Kabir, Brual C. Shah, and Satyandra K. Gupta. Trajectory planning for manip- ulators operating in conned workspaces. In IEEE International Conference on Automation Science and Engineering, Munich, Germany, August 2018. 211 [111] Ariyan M. Kabir, Shantanu Thakar, Prahar M. Bhatt, Rishi K. Malhan, Pradeep Rajendran, Brual C. Shah, and Satyandra K. Gupta. Incorporating motion planning feasibility consid- erations during task-agent assignment to perform complex tasks using mobile-manipulators. In IEEE International Conference on Robotics and Automation, Paris, France, June 2020. [112] Ariyan M Kabir, Shantanu Thakar, Rishi K Malhan, Aniruddha V Shembekar, Brual C Shah, and Satyandra K Gupta. Generation of synchronized conguration space trajectories with workspace path constraints for an ensemble of robots. The International Journal of Robotics Research, page 0278364920988087, 2021. [113] Leslie Pack Kaelbling and Tom as Lozano-P erez. Hierarchical planning in the now. In Workshops at the Twenty-Fourth AAAI Conference on Articial Intelligence, 2010. [114] Mrinal Kalakrishnan, Sachin Chitta, Evangelos Theodorou, Peter Pastor, and Stefan Schaal. Stomp: Stochastic trajectory optimization for motion planning. In IEEE International Conference on Robotics and Automation (ICRA), page 4569. IEEE, 2011. [115] Sagar Kalburgi, Vishnu G Nair, and KR Guruprasad. Application of coverage path planning algorithm for milling operations. In Intelligent Systems, Technologies and Applications, pages 213{220. Springer, 2020. [116] Dejan Kaljaca, Bastiaan Vroegindeweij, and Eldert van Henten. Coverage trajectory plan- ning for a bush trimming robot arm. Journal of Field Robotics, 37(2):283{308, 2020. [117] Daniel Kappler, Jeannette Bohg, and Stefan Schaal. Leveraging big data for grasp planning. In Robotics and Automation (ICRA), 2015 IEEE International Conference on, pages 4304{ 4311. IEEE, 2015. [118] Sertac Karaman and Emilio Frazzoli. Sampling-based algorithms for optimal motion plan- ning. The international journal of robotics research, 30(7):846{894, 2011. [119] Sertac Karaman and Emilio Frazzoli. Sampling-based optimal motion planning for non- holonomic dynamical systems. In 2013 IEEE International Conference on Robotics and Automation, pages 5041{5047. IEEE, 2013. [120] Lars Karlsson, Julien Bidot, Fabien Lagrioul, Alessandro Saotti, Ulrich Hillenbrand, and Florian Schmidt. Combining task and path planning for a humanoid two-arm robotic system. In Proceedings of tampra: Combining task and motion planning for real-world applications (icaps workshop), pages 13{20. Citeseer, 2012. [121] Lydia E Kavraki and Steven M LaValle. Motion planning. Springer Handbook of Robotics, pages 109{131, 2008. [122] Lydia E Kavraki, Petr Svestka, J-C Latombe, and Mark H Overmars. Probabilistic roadmaps for path planning in high-dimensional conguration spaces. IEEE Transactions on Robotics and Automation (ICRA), 12(4):566{580, 1996. [123] Oussama Khatib. Real-time obstacle avoidance for manipulators and mobile robots. In Autonomous robot vehicles, pages 396{404. Springer, 1986. [124] Jon Kieer. Manipulator inverse kinematics for untimed end-eector trajectories with or- dinary singularities. The International journal of robotics research, 11(3):225{237, 1992. [125] Beobkyoon Kim, Terry Taewoong Um, Chansu Suh, and Frank C Park. Tangent bundle rrt: A randomized algorithm for constrained motion planning. Robotica, 34(1):202{225, 2016. 212 [126] Junggon Kim, Kunihiro Iwamoto, James J Kuner, Yasuhiro Ota, and Nancy S Pollard. Physically based grasp quality evaluation under pose uncertainty. IEEE Transactions on Robotics, 29(6):1424{1439, 2013. [127] Zachary Kingston, Mark Moll, and Lydia E Kavraki. Decoupling constraints from sampling- based planners. In International Symposium of Robotics Research, 2017. [128] Zachary Kingston, Mark Moll, and Lydia E Kavraki. Sampling-based methods for motion planning with constraints. Annual review of control, robotics, and autonomous systems, 1:159{185, 2018. [129] Zachary Kingston, Mark Moll, and Lydia E. Kavraki. Exploring implicit spaces for con- strained sampling-based planning. 2019. [130] Nikita Kitaev, Igor Mordatch, Sachin Patil, and Pieter Abbeel. Physics-based trajectory optimization for grasping in cluttered environments. In Robotics and Automation (ICRA), 2015 IEEE International Conference on, pages 3102{3109. IEEE, 2015. [131] Ross A Knepper, Todd Layton, John Romanishin, and Daniela Rus. Ikeabot: An au- tonomous multi-robot coordinated furniture assembly system. In IEEE International Con- ference on Robotics and Automation (ICRA), pages 855{862. IEEE, 2013. [132] James J Kuner and Steven M LaValle. Rrt-connect: An ecient approach to single-query path planning. In IEEE International Conference on Robotics and Automation (ICRA), volume 2, pages 995{1001. IEEE, 2000. [133] Nithyananda B Kumbla, Shantanu Thakar, Krishnanand N Kaipa, Jeremy Marvel, and Satyandra K Gupta. Simulation based on-line evaluation of singulation plans to handle perception uncertainty in robotic bin picking. In ASME 12th International Manufacturing Science and Engineering Conference, pages V003T04A002{V003T04A002. American Society of Mechanical Engineers, 2017. [134] Nithyananda B Kumbla, Shantanu Thakar, Krishnanand N Kaipa, Jeremy Marvel, and Satyandra K Gupta. Handling perception uncertainty in simulation-based singulation plan- ning for robotic bin picking. Journal of Computing and Information Science in Engineering, 18(2):021004, 2018. [135] Sulabh Kumra and Christopher Kanan. Robotic grasp detection using deep convolutional neural networks. In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 769{776. IEEE, 2017. [136] Aleksandr Kushleyev and Maxim Likhachev. Time-bounded lattice for ecient planning in dynamic environments. In IEEE International Conference on Robotics and Automation (ICRA), pages 1662{1668. IEEE, 2009. [137] Fabien Lagrioul, Neil Dantam, Caelan Garrett, Aliakbar Akbari, Siddharth Srivastava, and Lydia Kavraki. Platform-independent benchmarks for task and motion planning. IEEE Robotics and Automation Letters, 3:3765{3772, October 2018. [138] Tin Lai, Fabio Ramos, and Gilad Francis. Balancing global exploration and local- connectivity exploitation with rapidly-exploring random disjointed-trees. In 2019 Inter- national Conference on Robotics and Automation (ICRA), pages 5537{5543. IEEE, 2019. [139] Michael Laskey, Jonathan Lee, Caleb Chuck, David Gealy, Wesley Hsieh, Florian T Poko- rny, Anca D Dragan, and Ken Goldberg. Robot grasping in clutter: Using a hierarchy of supervisors for learning from demonstrations. In Automation Science and Engineering (CASE), 2016 IEEE International Conference on, pages 827{834. IEEE, 2016. 213 [140] Jean-Paul Laumond, Paul E Jacobs, Michel Taix, and Richard M Murray. A motion planner for nonholonomic mobile robots. IEEE Transactions on robotics and Automation, 10(5):577{ 593, 1994. [141] Steven M. Lavalle. Planning Algorithms. Journal of Chemical Information and Modeling, 53(9):1689{1699, 2013. [142] Steven M Lavalle and James J Kuner. Randomized Kinodynamic Planning. The Interna- tional Journal of Robotics Research, 2001. [143] Quoc V Le, David Kamm, Arda F Kara, and Andrew Y Ng. Learning to grasp objects with multiple contact points. In IEEE International Conference Robotics and Automation (ICRA), pages 5062{5069. IEEE, 2010. [144] Daniel Leidner, Wissam Bejjani, Alin Albu-Sch aer, and Michael Beetz. Robotic agents representing, reasoning, and executing wiping tasks for daily household chores. AU- TONOMOUS AGENTS AND MULTI-AGENT SYSTEMS, 2016. [145] Ian Lenz, Honglak Lee, and Ashutosh Saxena. Deep learning for detecting robotic grasps. The International Journal of Robotics Research, 34(4-5):705{724, 2015. [146] Sergey Levine, Peter Pastor, Alex Krizhevsky, Julian Ibarz, and Deirdre Quillen. Learning hand-eye coordination for robotic grasping with deep learning and large-scale data collection. The International Journal of Robotics Research, 37(4-5):421{436, 2018. [147] Miao Li, Kaiyu Hang, Danica Kragic, and Aude Billard. Dexterous grasping under shape uncertainty. Robotics and Autonomous Systems, 75:352{364, 2016. [148] Maxim Likhachev and Dave Ferguson. Planning long dynamically feasible maneuvers for autonomous vehicles. The International Journal of Robotics Research, 28(8):933{945, 2009. [149] Ryan Luna, Mark Moll, Julia Badger, and Lydia E Kavraki. A scalable motion planner for high-dimensional kinematic systems. The International Journal of Robotics Research, 39(4):361{388, 2020. [150] Jerey Mahler, Jacky Liang, Sherdil Niyaz, Michael Laskey, Richard Doan, Xinyu Liu, Juan Aparicio Ojea, and Ken Goldberg. Dex-net 2.0: Deep learning to plan robust grasps with synthetic point clouds and analytic grasp metrics. 2017. [151] Jerey Mahler, Matthew Matl, Xinyu Liu, Albert Li, David Gealy, and Ken Goldberg. Dex- net 3.0: Computing robust robot suction grasp targets in point clouds using a new analytic model and deep learning. arXiv preprint arXiv:1709.06670, 2017. [152] Jerey Mahler, Matthew Matl, Vishal Satish, Michael Danielczuk, Bill DeRose, Stephen McKinley, and Ken Goldberg. Learning ambidextrous robot grasping policies. Science Robotics, 4(26):eaau4984, 2019. [153] Jerey Mahler, Florian T Pokorny, Brian Hou, Melrose Roderick, Michael Laskey, Math- ieu Aubry, Kai Kohlho, Torsten Kr oger, James Kuner, and Ken Goldberg. Dex-net 1.0: A cloud-based network of 3d objects for robust grasp planning using a multi-armed bandit model with correlated rewards. In IEEE International Conference on Robotics and Automation (ICRA), pages 1957{1964. IEEE, 2016. [154] R. K. Malhan, R. J. Joseph, A. V. Shembekar, A. M. Kabir, P. M. Bhatt, and S. K. Gupta. Online grasp plan renement for reducing defects during robotic layup of composite prepreg sheets. In IEEE International Conference on Robotics and Automation, Paris, France, June 2020. 214 [155] R. K. Malhan, A. V. Shembekar, A. M. Kabir, P. M. Bhatt, B. C. Shah, S. Zanio, S. Nutt, and S. K. Gupta. Automated planning for robotic layup of composite prepreg. Robotics and Computer Integrated Manufacturing, 67:102020, 2021. [156] Rishi K. Malhan, Ariyan M. Kabir, Brual C. Shah, and Satyandra K. Gupta. Identifying feasible workpiece placement with respect to redundant manipulator for complex manu- facturing tasks. In IEEE International Conference on Robotics and Automation (ICRA), Montreal, Canada, May 2019. [157] B. J. Martin and J. E. Bobrow. Minimum eort motions for open chain manipulators with task-dependent end-eector constraints. In Proceedings of International Conference on Robotics and Automation, volume 3, pages 2044{2049 vol.3, Apr 1997. [158] David Mart nez, Guillem Aleny a, Pablo Jim enez, Carme Torras, J urgen Rossmann, Nils Wantia, Eren Erdal Aksoy, Simon Haller, and Justus Piater. Active learning of manipulation sequences. In Robotics and Automation (ICRA), 2014 IEEE International Conference on, pages 5671{5678. IEEE, 2014. [159] Matthew T Mason, Dinesh K Pai, Daniela Rus, Lee R Taylor, and Michael A Erdmann. A mobile manipulator. In IEEE International Conference on Robotics and Automation (ICRA), volume 3, pages 2322{2327. IEEE, 1999. [160] J McCartney, BK Hinds, and BL Seow. The attening of triangulated surfaces incorporating darts and gussets. Computer-Aided Design, 31(4):249{260, 1999. [161] Riad Menasri, Amir Nakib, Boubaker Daachi, Hamouche Oulhadj, and Patrick Siarry. A trajectory planning of redundant manipulators based on bilevel optimization. Applied Math- ematics and Computation, 250:934{947, 2015. [162] Arjun Menon, Benjamin Cohen, and Maxim Likhachev. Motion planning for smooth pickup of moving objects. In IEEE International Conference on Robotics and Automation (ICRA), pages 453{460. IEEE, 2014. [163] Mark Moll, Lydia Kavraki, Jan Rosell, et al. Randomized physics-based motion planning for grasping in cluttered and uncertain environments. IEEE Robotics and Automation Letters, 3(2):712{719, 2018. [164] Luis Montesano and Manuel Lopes. Active learning of visual descriptors for grasping using non-parametric smoothed beta distributions. Robotics and Autonomous Systems, 60(3):452{ 462, 2012. [165] Venkatraman Narayanan, Sandip Aine, and Maxim Likhachev. Improved multi-heuristic a* for searching with uncalibrated heuristics. In Eighth Annual Symposium on Combinatorial Search, 2015. [166] Timo Oksanen and Arto Visala. Coverage path planning algorithms for agricultural eld machines. Journal of eld robotics, 26(8):651{668, 2009. [167] P Olivieri, L Birglen, X Maldague, and I Mantegh. Coverage path planning for eddy current inspection on complex aeronautical parts. Robotics and Computer-Integrated Manufactur- ing, 30(3):305{314, 2014. [168] Giuseppe Oriolo and Christian Mongillo. Motion planning for mobile manipulators along given end-eector paths. In IEEE International Conference on Robotics and Automa- tion(ICRA)., pages 2154{2160. IEEE, 2005. 215 [169] Takayuki Osa, Jan Peters, and Gerhard Neumann. Experiments with hierarchical reinforce- ment learning of multiple grasping policies. In International Symposium on Experimental Robotics, pages 160{172. Springer, 2016. [170] Luigi Palmieri and Kai O Arras. A novel rrt extend function for ecient and smooth mobile robot motion planning. In 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 205{211. IEEE, 2014. [171] Jia Pan, Xinyu Zhang, and Dinesh Manocha. Ecient penetration depth approximation using active learning. ACM Transactions on Graphics, 32(6), 2013. [172] Tommaso Pardi, Vamsikrishna Maddali, Valerio Ortenzi, Rustam Stolkin, and Naresh Mar- turi. Path planning for mobile manipulator robots under non-holonomic and task con- straints. 2020. [173] Fabian Paus, Peter Kaiser, Nikolaus Vahrenkamp, and Tamim Asfour. A combined ap- proach for robot placement and coverage path planning for mobile manipulation. In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 6285{ 6292. IEEE, 2017. [174] Raphael Pelossof, Andrew Miller, Peter Allen, and Tony Jebara. An svm learning approach to robotic grasping. In Robotics and Automation, 2004. Proceedings. ICRA'04. 2004 IEEE International Conference on, volume 4, pages 3512{3518. IEEE, 2004. [175] Friedrich Pfeier and Rainer Johanni. A concept for manipulator trajectory planning. IEEE Journal on Robotics and Automation, 3(2):115{123, 1987. [176] A. Piazzi and A. Visioli. Global minimum-jerk trajectory planning of robot manipulators. IEEE Transactions on Industrial Electronics, 47(1):140{149, Feb 2000. [177] Vinay Pilania and Kamal Gupta. A hierarchical and adaptive mobile manipulator planner with base pose uncertainty. Autonomous Robots, 39(1):65{85, 2015. [178] Vinay Pilania and Kamal Gupta. Mobile manipulator planning under uncertainty in un- known environments. The International Journal of Robotics Research, 37(2-3):316{339, 2018. [179] Mihail Pivtoraiko, Ross A Knepper, and Alonzo Kelly. Dierentially constrained mobile robot motion planning in state lattices. Journal of Field Robotics, 26(3):308{333, 2009. [180] Erion Plaku, Kostas E Bekris, Brian Y Chen, Andrew M Ladd, and Lydia E Kavraki. Sampling-based roadmap of trees for parallel motion planning. IEEE Transactions on Robotics, 21(4):597{608, 2005. [181] Anthony Pratkanis, Adam Eric Leeper, and Kenneth Salisbury. Replacing the oce intern: An autonomous coee run with a mobile manipulator. In IEEE International Conference on Robotics and Automation (ICRA), pages 1248{1253. IEEE, 2013. [182] Ahmed Hussain Qureshi and Yasar Ayaz. Potential functions based sampling heuristic for optimal path planning. Autonomous Robots, 40(6):1079{1093, 2016. [183] Ahmed Hussain Qureshi, Saba Mumtaz, Khawaja Fahad Iqbal, Badar Ali, Yasar Ayaz, Faizan Ahmed, Mannan Saeed Muhammad, Osman Hasan, Whoi Yul Kim, and Moonsoo Ra. Adaptive potential guided directional-rrt. In 2013 IEEE International Conference on Robotics and Biomimetics (ROBIO), pages 1887{1892. IEEE, 2013. 216 [184] Pradeep Rajendran, Shantanu Thakar, Prahar M Bhatt, Ariyan M Kabir, and Satyandra K Gupta. Strategies for speeding up manipulator path planning to nd high quality paths in cluttered environments. Journal of Computing and Information Science in Engineering, 21(1):011009, 2021. [185] Pradeep Rajendran, Shantanu Thakar, and Satyandra K. Gupta. User-guided path planning for redundant manipulators in highly constrained work environments. In IEEE International Conference on Automation Science and Engineering (CASE), Vancouver, Canada, August 2019. [186] Pradeep Rajendran, Shantanu Thakar, Ariyan M. Kabir, Brual C. Shah, and S. K. Gupta. Context-dependent search for generating paths for redundant manipulators in cluttered environments. In IEEE International Conference on Intelligent Robots and Systems (IROS), Macau, China, November 2019. [187] Joseph Redmon and Anelia Angelova. Real-time grasp detection using convolutional neural networks. In Robotics and Automation (ICRA), 2015 IEEE International Conference on, pages 1316{1322. IEEE, 2015. [188] Caelan Reed Garrett, Tom as Lozano-P erez, and Leslie Pack Kaelbling. FFRob: Leveraging symbolic planning for ecient task and motion planning. The International Journal of Robotics Research, 37(1):104{136. [189] A. Reiter, A. M uller, and H. Gattringer. Inverse kinematics in minimum-time trajectory planning for kinematically redundant manipulators. In IECON 2016 - 42nd Annual Con- ference of the IEEE Industrial Electronics Society, pages 6873{6878, Oct 2016. [190] Alexander Reiter, Andreas M uller, and Hubert Gattringer. On higher-order inverse kine- matics methods in time-optimal trajectory planning for kinematically redundant manipula- tors. IEEE Transactions on Industrial Informatics, 2018. [191] Suresh Rewar and Dashrath Mirdha. Transmission of ebola virus disease: an overview. Annals of global health, 80(6):444{451, 2014. [192] M. Rickert, A. Sieverling, and O. Brock. Balancing exploration and exploitation in sampling- based motion planning. IEEE Transactions on Robotics, 30(6):1305{1317, Dec 2014. [193] Markus Rickert, Arne Sieverling, and Oliver Brock. Balancing exploration and exploitation in sampling-based motion planning. IEEE Transactions on Robotics, 30(6):1305{1317, 2014. [194] E. Rohmer, S. P. N. Singh, and M. Freese. Coppeliasim (formerly v-rep): a versatile and scalable robot simulation framework. In Proc. of The International Conference on Intelligent Robots and Systems (IROS), 2013. www.coppeliarobotics.com. [195] Anis Sahbani, Sahar El-Khoury, and Philippe Bidaud. An overview of 3d object grasp synthesis algorithms. Robotics and Autonomous Systems, 60(3):326{336, 2012. [196] Marcos Salganico, Lyle H Ungar, and Ruzena Bajcsy. Active learning for vision-based robot grasping. Machine Learning, 23(2-3):251{278, 1996. [197] Ashutosh Saxena, Lawson LS Wong, and Andrew Y Ng. Learning grasp strategies with partial shape information. In AAAI, volume 3, pages 1491{1494, 2008. [198] Robert E Schapire. Explaining adaboost. In Empirical inference, pages 37{52. Springer, 2013. 217 [199] Alexander Schiftner, Mathias H obinger, Johannes Wallner, and Helmut Pottmann. Packing circles and spheres on surfaces. In ACM SIGGRAPH Asia 2009 papers, pages 1{8. 2009. [200] Jonathan Scholz, Sachin Chitta, Bhaskara Marthi, and Maxim Likhachev. Cart pushing with a mobile manipulation system: Towards navigation with moveable objects. In IEEE International Conference on Robotics and Automation (ICRA), pages 6115{6120. IEEE, 2011. [201] John Schulman, Yan Duan, Jonathan Ho, Alex Lee, Ibrahim Awwal, Henry Bradlow, Jia Pan, Sachin Patil, Ken Goldberg, and Pieter Abbeel. Motion planning with sequential convex optimization and convex collision checking. The International Journal of Robotics Research, 33(9):1251{1270, 2014. [202] James A Sethian. Fast marching methods. SIAM review, 41(2):199{235, 1999. [203] Krishna Shankar, Joel W. Burdick, and Nicolas H. Hudson. A Quadratic Programming Approach toQuasi-Static Whole-Body Manipulation, pages 553{570. Springer International Publishing, Cham, 2015. [204] Aniruddha V. Shembekar, Yeo Jung Yoon, Alec Kanyuck, and Satyandra K. Gupta. Gener- ating Robot Trajectories for Conformal Three-Dimensional Printing Using Nonplanar Lay- ers. Journal of Computing and Information Science in Engineering, 19(3), 04 2019. 031011. [205] Yang Shen, Dejun Guo, Fei Long, Luis A Mateos, Houzhu Ding, Zhen Xiu, Randall B Hell- man, Adam King, Shixun Chen, Chengkun Zhang, et al. Robots under covid-19 pandemic: A comprehensive survey. IEEE Access, 2020. [206] Xiangling Shi, Honggen Fang, and Lijie Guo. Multi-objective optimal trajectory planning of manipulators based on quintic nurbs. In Mechatronics and Automation (ICMA), 2016 IEEE International Conference on, pages 759{765. IEEE, 2016. [207] Alexander Shkolnik, Matthew Walter, and Russ Tedrake. Reachability-guided sampling for planning under dierential constraints. In 2009 IEEE International Conference on Robotics and Automation, pages 2859{2865. IEEE, 2009. [208] K Sidawi, P Moroz, and S Chandra. On surface area coverage by an electrostatic rotating bell atomizer. Journal of Coatings Technology and Research, pages 1{15, 2021. [209] Catrin Sohrabi, Zaid Alsa, Niamh O'Neill, Mehdi Khan, Ahmed Kerwan, Ahmed Al-Jabir, Christos Iosidis, and Riaz Agha. World health organization declares global emergency: A review of the 2019 novel coronavirus (covid-19). International journal of surgery, 76:71{76, 2020. [210] Siddhartha S Srinivasa, Dmitry Berenson, Maya Cakmak, Alvaro Collet, Mehmet R Dogar, Anca D Dragan, Ross A Knepper, Tim Niemueller, Kyle Strabala, Mike Vande Weghe, et al. Herb 2.0: Lessons learned from developing a mobile manipulator for the home. Proceedings of the IEEE, 100(8):2410{2428, 2012. [211] Siddharth Srivastava, Eugene Fang, Lorenzo Riano, Rohan Chitnis, Stuart Russell, and Pieter Abbeel. Combined task and motion planning through an extensible planner- independent interface layer. In IEEE international conference on robotics and automation (ICRA), pages 639{646, 2014. [212] Benno Staub, Ajay Kumar Tanwani, Jerey Mahler, Michel Breyer, Michael Laskey, Yutaka Takaoka, Max Bajracharya, Roland Siegwart, and Ken Goldberg. Dex-net mm: Deep grasp- ing for surface decluttering with a low-precision mobile manipulator. In 2019 IEEE 15th 218 International Conference on Automation Science and Engineering (CASE), pages 1373{ 1379. IEEE, 2019. [213] M. Stilman. Global manipulation planning in robot joint space with task constraints. IEEE Transactions on Robotics, 26(3):576{584, June 2010. [214] Mike Stilman. Task constrained motion planning in robot joint space. IEEE International Conference on Intelligent Robots and Systems (ICRA), pages 3074{3081, 2007. [215] Rajiv Suman, Mohd Javaid, Abid Haleem, Raju Vaishya, Shashi Bahl, and Devaki Nandan. Sustainability of coronavirus on dierent surfaces. Journal of clinical and experimental hepatology, 2020. [216] Zaid Tahir, Ahmed H Qureshi, Yasar Ayaz, and Raheel Nawaz. Potentially guided bidi- rectionalized rrt* for fast optimal path planning in cluttered environments. Robotics and Autonomous Systems, 108:13{27, 2018. [217] Herbert G Tanner and Kostas J Kyriakopoulos. Nonholonomic motion planning for mo- bile manipulators. In Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065), vol- ume 2, pages 1233{1238. IEEE, 2000. [218] Mahmoud Tarokh and Xiaomang Zhang. Real-time motion tracking of robot manipulators using adaptive genetic algorithms. Journal of Intelligent & Robotic Systems, 74(3-4):697{ 708, 2014. [219] Shantanu Thakar, Liwei Fang, Brual Shah, and Satyandra K. Gupta. Towards time-optimal trajectory planning for pick-and-transport operation with a mobile manipulator. In Interna- tional Conference on Automation Science and Engineering (CASE), pages 981{987, Munich, Germany, Aug 2018. IEEE. [220] Shantanu Thakar, Ariyan Kabir, Prahar Bhatt, Rishi Malhan, Pradeep Rajendran, Brual Shah, and Satyandra K. Gupta. Task assignment and motion planning for bi-manual mobile manipulation. In IEEE International Conference on Automation Science and Engineering (CASE), Vancouver, Canada, August 2019. [221] Shantanu Thakar, Pradeep Rajendran, Vivek Annem, Ariyan Kabir, and Satyandra K. Gupta. Accounting for part pose estimation uncertainties during trajectory generation for part pick-up using mobile manipulators. In IEEE International Conference on Robotics and Automation (ICRA), Montreal, Canada, May 2019. [222] Shantanu Thakar, Pradeep Rajendran, Ariyan M. Kabir, and Satyandra K. Gupta. Manip- ulator motion planning for part pickup and transport operations from a moving base. IEEE Transactions on Automation Science and Engineering, pages 1{16, 2020. [223] Shantanu Thakar, Pradeep Rajendran, Hyojeong Kim, Ariyan M. Kabir, and Satandra K. Gupta. Accelerating bi-directional sampling-based search for motion planning of non- holonomic mobile manipulators. In IEEE International Conference on Intelligent Robots and Systems, Las Vegas, Nevada, USA, Oct 2020. [224] Shantanu Thakar and Ashwini Ratnoo. Rendezvous guidance laws for aerial recovery using mothership-cable-drogue system. IFAC-PapersOnLine, 49(1):24{29, 2016. [225] Shantanu Thakar and Ashwini Ratnoo. A tangential guidance logic for virtual target based path following. In AIAA Guidance, Navigation, and Control Conference, page 1042, 2017. 219 [226] Marc Toussaint, Helge Ritter, and Oliver Brock. The optimization route to robotics|and alternatives. KI-K unstliche Intelligenz, 29(4):379{388, 2015. [227] Terry Taewoong Um, Beobkyoon Kim, Chansoo Suh, and Frank Chongwoo Park. Tangent space rrt with lazy projection: An ecient planning algorithm for constrained motions. In Advances in Robot Kinematics: Motion in Man and Machine, pages 251{260. Springer, 2010. [228] Nikolaus Vahrenkamp, Tamim Asfour, and R udiger Dillmann. Robot placement based on reachability inversion. In 2013 IEEE International Conference on Robotics and Automation, pages 1970{1975. IEEE, 2013. [229] Jacob Varley, Jonathan Weisz, Jared Weiss, and Peter Allen. Generating multi-ngered robotic grasps via deep learning. In Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on, pages 4415{4420. IEEE, 2015. [230] Vojt ech Von asek and Robert P eni cka. Space-lling forest for multi-goal path planning. In 2019 24th IEEE International Conference on Emerging Technologies and Factory Automa- tion (ETFA), pages 1587{1590. IEEE, 2019. [231] Wei Wang, Yan Li, Xin Xu, and Simon X Yang. An adaptive roadmap guided multi-rrts strategy for single query path planning. In 2010 IEEE International Conference on Robotics and Automation, pages 2871{2876. IEEE, 2010. [232] Wei Wang, Xin Xu, Yan Li, Jinze Song, and Hangen He. Triple rrts: an eective method for path planning in narrow passages. Advanced Robotics, 24(7):943{962, 2010. [233] Zhongmin Wang and Zhu Bo. Coverage path planning for mobile robot based on genetic algorithm. In 2014 IEEE Workshop on Electronics, Computer and Applications, pages 732{735. IEEE, 2014. [234] Andrew Wells, Neil Dantam, Anshumali Shrivastava, and Lydia Kavraki. Learning feasibil- ity for task and motion planning in tabletop environments. IEEE Robotics and Automation Letters, 2019. [235] Zeyi Wen, Jiashuai Shi, Qinbin Li, Bingsheng He, and Jian Chen. Thundersvm: A fast svm library on gpus and cpus. Journal of Machine Learning Research, 19(21), 2018. [236] Jason Wolfe, Bhaskara Marthi, and Stuart Russell. Combined task and motion planning for mobile manipulation. In Twentieth International Conference on Automated Planning and Scheduling, 2010. [237] Cuebong Wong, Erfu Yang, Xiu-Tian Yan, and Dongbing Gu. Optimal path planning based on a multi-tree t-rrt* approach for robotic task planning in continuous cost spaces. In 2018 12th France-Japan and 10th Europe-Asia Congress on Mechatronics, pages 242{247. IEEE, 2018. [238] Tong Yang, Jaime Valls Miro, Yue Wang, and Rong Xiong. Non-revisiting coverage task with minimal discontinuities for non-redundant manipulators. 2020. [239] Zhiyang Yao and Satyandra K Gupta. Cutter path generation for 2.5 d milling by com- bining multiple dierent cutter path patterns. International journal of production research, 42(11):2141{2161, 2004. [240] Franziska Zacharias, Christoph Borst, Sebastian Wolf, and Gerd Hirzinger. The capability map: A tool to analyze robot arm workspaces. International Journal of Humanoid Robotics, 10(04):1350031, 2013. 220 [241] Franziska Zacharias, Daniel Leidner, Florian Schmidt, Christoph Borst, and Gerd Hirzinger. Exploiting structure in two-armed manipulation tasks for humanoid robots. In 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 5446{5452. IEEE, 2010. [242] Yong Zeng, Yakun Zhang, Junxue He, Hai Zhou, Chunwei Zhang, and Lei Zheng. Prediction model of coating growth rate for varied dip-angle spraying based on gaussian sum model. Mathematical Problems in Engineering, 2016, 2016. [243] Hui Zhang, Jinwen Tan, Chenyang Zhao, Zhicong Liang, Li Liu, Hang Zhong, and Shaosheng Fan. A fast detection and grasping method for mobile manipulator based on improved faster r-cnn. Industrial Robot: the international journal of robotics research and application, 2020. [244] Yanjun Zhang, Wenbo Li, Chao Zhang, Hanlin Liao, Yicha Zhang, and Sihao Deng. A spherical surface coating thickness model for a robotized thermal spray system. Robotics and Computer-Integrated Manufacturing, 59:297{304, 2019. [245] Yu Zheng and Wen-Han Qian. Coping with the grasping uncertainties in force-closure analysis. The International Journal of Robotics Research, 24(4):311{327, 2005. [246] J. Zhou, J. Zhou, Y. Zheng, and B. Kong. Research on path planning algorithm of intelligent mowing robot used in large airport lawn. In 2016 International Conference on Information System and Articial Intelligence (ISAI), pages 375{379, 2016. [247] Jiaji Zhou, Robert Paolini, Aaron M Johnson, J Andrew Bagnell, and Matthew T Mason. A probabilistic planning framework for planar grasping under uncertainty. IEEE Robotics and Automation Letters, 2(4):2111{2118, 2017. [248] Matt Zucker, Nathan Ratli, Anca D Dragan, Mihail Pivtoraiko, Matthew Klingensmith, Christopher M Dellin, J Andrew Bagnell, and Siddhartha S Srinivasa. Chomp: Covari- ant hamiltonian optimization for motion planning. The International Journal of Robotics Research, 32(9-10):1164{1193, 2013. 221

Abstract (if available)

Abstract Mobile manipulators are essentially robotic arms that can move around and hence provide flexibility in the kinds of tasks they can do. This significantly reduces down-time of such robots as compared to fixed manipulators. Moreover, moving the manipulator and mobile base concurrently can make mobile manipulators more efficient as compared to moving them one at a time as done traditionally. In certain scenarios, it may be even be necessary to move the mobile base and the manipulator concurrently. Motion planning for mobile manipulators with concurrent motions of the mobile base and the manipulator especially with a nonholonomic mobile base is a very challenging problem. In this dissertation, specialized techniques for motion planning of mobile manipulators are presented for three inherently important tasks. These tasks are namely object pick-up, transportation, and area coverage. ? In the first task, the goal is to pick-up objects while the mobile base is in motion. The dissertation individually focuses on mobile base, manipulator and gripper motion planning under object pose uncertainty. If a mobile manipulator is tasked to pick-up an object from a table while the base is still moving, there are several decisions the robot must make. It must figure out how to move the mobile base such that it reaches close to the object since the intermediary point where the manipulator picks up the object is unknown. Moreover, it must determine when on the mobile base path should the manipulator start moving. Furthermore, the robot must figure out how to grasp the object such that it is the easiest to execute the motions. And finally, if there is uncertainty in the pose of the object, how should the gripper move so that the probability of grasping is high. These challenges are addressed in this dissertation by having: (a) a discrete search-based planner for the mobile base with specialized heuristics to guide it towards the object and a cost function for the time-delay caused by the manipulator motion; (b) three specialized techniques for determining manipulator motion on a moving mobile base and the grasping strategy; (c) an active learning-based method for determining the gripper speed and gripper closing speed for successfully picking up objects with pose uncertainty. ? In the next task, the objective is to carry large objects through a cluttered environment with a nonholonomic mobile manipulator. The goal is to find a good quality solution for this challenging planning problem quickly. A bi-directional sampling-based method for growing rapidly exploring random trees with focused sampling for the mobile base and the manipulator has been developed for this. The focusing accelerates the trees towards the desired configurations in the workspace of the robot. Moreover connecting the two trees is challenging since the mobile base is nonholonomic. Hence, a heuristic for choosing how to connect the two trees has been developed that focuses only on attempting connections between appropriate kind of tree nodes only. ? The final task is developing an area coverage planner for a mobile manipulator for spray-based disinfection of surfaces. In this regard, a framework to disinfect surfaces represented by their point clouds and execute motions with a mobile manipulator has been developed. The first step is to project the point cloud on a plane and generate a polygon. On the polygon multiple spray paths are then generated using a branch and bound-based tree search algorithm such that each spray path covers the entire area of the polygon. Mobile manipulator trajectory to follow one of the spray path is then generated using a successive refinement-based parametric optimization. Once the trajectory is generated, there is a need to make sure that the joint velocities of the mobile manipulator are regulated appropriately such that each point on the surface receives enough disinfectant dose. To this end, the time intervals between the robot path waypoints are computed such that enough disinfectant liquid is sprayed on all points of the point cloud that results in thorough disinfection of the surface. ? The dissertation then focuses on executing tasks with a bimanual mobile manipulator. If tasks are to be executed with such a robot, assigning appropriate task to the appropriate manipulator is important. If the tasks are not assigned correctly, the notification of failure will be received only after expensive motion planners fail to find a solution. This can be highly time-consuming. In this regard, a two-layered architecture for task-agent assignment and motion planning of bimanual mobile manipulators for executing complex tasks has been developed. Search trees are used in temporal windows to determine feasible task assignments of agents using task and spatial constraint-based heuristics. A caching scheme has been introduced for moving between different trees so as to avoid re-planning of those portions of the robot motion that were successful. This greatly reduces the number of calls to the motion planner as compared to direct motion planning after task-agent assignment.

Linked assets

University of Southern California Dissertations and Theses

Conceptually similar

PDF

Robot trajectory generation and placement under motion constraints

PDF

Trajectory planning for manipulators performing complex tasks

PDF

Intelligent robotic manipulation of cluttered environments

PDF

Contingency handling in mission planning for multi-robot teams

PDF

Speeding up trajectory planning for autonomous robots operating in complex environments

PDF

Algorithms and systems for continual robot learning

PDF

Learning affordances through interactive perception and manipulation

PDF

Hierarchical tactile manipulation on a haptic manipulation platform

PDF

Planning and learning for long-horizon collaborative manipulation tasks

PDF

Process planning for robotic additive manufacturing

PDF

Target assignment and path planning for navigation tasks with teams of agents

PDF

Motion coordination for large multi-robot teams in obstacle-rich environments

PDF

Accelerating robot manipulation using demonstrations

PDF

Physics-based data-driven inference

PDF

Efficiently learning human preferences for proactive robot assistance in assembly tasks

PDF

AI-driven experimental design for learning of process parameter models for robotic processing applications

PDF

Automated alert generation to improve decision-making in human robot teams

PDF

Decision support systems for adaptive experimental design of autonomous, off-road ground vehicles

PDF

Learning from planners to enable new robot capabilities

PDF

Informative path planning for environmental monitoring

Asset Metadata

Creator Thakar, Shantanu (author)

Core Title Planning for mobile manipulation

School Viterbi School of Engineering

Degree Doctor of Philosophy

Degree Program Mechanical Engineering

Degree Conferral Date 2021-08

Publication Date 07/06/2021

Defense Date 04/19/2021

Publisher University of Southern California (original), University of Southern California. Libraries (digital)

Tag area coverage planning,disinfection,grasping,high degree of freedom systems,manipulator planning,mobile manipulation,mobile manipulator,mobile robot,motion planning,nonholonomic mobile manipulator,nonholonomic planning,OAI-PMH Harvest,robotic arm,task and motion planning,Transportation

Format application/pdf (imt)

Language English

Contributor Electronically uploaded by the author (provenance)

Advisor Gupta, Satyandra K. (committee chair), Culbertson, Heather (committee member), Oberai, Assad (committee member)

Creator Email shantanuthakar@gmail.com,sthakar@usc.edu

Permanent Link (DOI) https://doi.org/10.25549/usctheses-oUC15276668

Unique identifier UC15276668

Identifier etd-ThakarShan-9702.pdf (filename)

Legacy Identifier etd-ThakarShan-9702

Document Type Dissertation

Format application/pdf (imt)

Rights Thakar, Shantanu

Type texts

Source University of Southern California (contributing entity), University of Southern California Dissertations and Theses (collection)

Access Conditions The author retains rights to his/her dissertation, thesis or other graduate work according to U.S. copyright law. Electronic access is being provided by the USC Libraries in agreement with the a...

Repository Name University of Southern California Digital Library

Repository Location USC Digital Library, University of Southern California, University Park Campus MC 2810, 3434 South Grand Avenue, 2nd Floor, Los Angeles, California 90089-2810, USA

Repository Email cisadmin@lib.usc.edu