Motion planning for robotic arms integrated on mobile platforms for internal logistics tasks

Universidade do Minho Escola de Engenharia António João Gonçalves Matos Motion planning for robotic arms integrated on mobile platforms for internal logistics tasks January 2025 Universidade do Minho Escola de Engenharia António João Gonçalves Matos Motion planning for robotic arms integrated on mobile platforms for internal logistics tasks Master’s Dissertation Master’s in Mechanical Engineering Specialization in Mechatronic Systems Work developed under the supervision of: Professor José Mendes Machado Professor Luís Filipe Castro Freitas Louro January 2025 i DIREITOS DE AUTOR E CONDIÇÕES DE UTILIZAÇÃO DO TRABALHO POR TERCEIROS Este é um trabalho académico que pode ser utilizado por terceiros desde que respeitadas as regras e boas práticas internacionalmente aceites, no que concerne aos direitos de autor e direitos conexos. Assim, o presente trabalho pode ser utilizado nos termos previstos na licença abaixo indicada. Caso o utilizador necessite de permissão para poder fazer um uso do trabalho em condições não previstas no licenciamento indicado, deverá contactar o autor, através do RepositóriUM da Universidade do Minho. Licença concedida aos utilizadores deste trabalho Atribuição CC BY https://creativecommons.org/licenses/by/4.0/ ii Acknowledgments Firstly, I would like to express my deepest gratitude to my supervisor José Machado, for his support throughout the development of this work and to Professor Luís Louro for his invaluable guidance in all matters relating to robotics. I want to express my heartfelt thanks to Professor Estela Bicho for her generosity in sharing knowledge and for her willingness to help me whenever I needed it. A special acknowledgment goes to the team involved in this project, namely Professor Sérgio Monteiro and my colleagues Ivan Castro, João Veríssimo, João Diogo, and João Araújo, with special attention to the latter for putting me through this delicate phase. I am also thankful to the Bosch team for their collaboration with the University of Minho, with special recognition to Tiago Carvalho and Damásio Eurico for their dedication during our regular meetings to monitor the project’s progress. I am grateful to the entire MarLab team at the Industrial Electronics Department for warmly welcoming me during these past months and for providing the tools and resources needed to accomplish this work. I also want to express my gratitude to Paulo Bourbon from Europneumaq for allowing me to conduct tests with the gripper used in this project. I appreciate my family, whose unwavering support has been my rock during this challenging period. Their encouragement was fundamental. Thank you to the Department of Mechanical Engineering professors for the knowledge imparted during my academic journey over the past five years. Lastly, I want to thank my friends who shared this challenging journey with me. Special thanks to the Roubocopo group. iii STATEMENT OF INTEGRITY I hereby declare having conducted this academic work with integrity. I confirm that I have not used plagiarism or any form of undue use of information or falsification of results along the process leading to its elaboration. I further declare that I have fully acknowledged the Code of Ethical Conduct of the University of Minho. iv Abstract The developed work in this dissertation is part of the MIAR project, which focuses on using synchronized Mobile Intelligent Autonomous Robots (MIAR) to automate logistics operations at Bosch Car Multimedia BrgP. The autonomous mobile manipulator must perform pick-andplace operations involving Targets, boxes containing certain electronic products, between a storage buffer and access ramps connected to the assembly lines. The purpose of this dissertation was to develop motion planning for the KUKA LBR iiwa 14 R820 manipulator to execute the intended tasks. The use of a redundant arm in this project aimed to create collision-free trajectories in confined spaces. Firstly, a simulation model was created at CoppeliaSim to represent the Bosch shop floor where the mobile manipulator will operate. Under this scenario, several tasks were performed, such as determining the number of boxes the manipulator could handle from the dollies and checking the maximum number of boxes that could be transported on top of the mobile platform. To carry out the planning tests, the collaborative gripper 2FGP20 was selected to meet the project’s requirements, and customdesigned fingers were simulated at Autodesk Fusion to withstand the imposed loads during manipulation. Additionally, new optimized ramps were introduced to ensure that the arm could access boxes. A method of communication between CoppeliaSim and MoveIt, software used for trajectory generation, was implemented. This communication was established using ROS2 via two launch files. A benchmarking study was conducted among ten planners, identifying T-RRT as the most suitable for the project. Subsequently, the planner was customized to ensure better results across diverse manipulation scenarios, and a post-processor was utilized to verify if it would lead to significant improvements, considering the project's stringent requirements. In conclusion, the manipulator demonstrated satisfactory performance, given the complexity of its working environment. Some limitations were identified, and suggestions for future improvements were proposed. Keywords: Global Planners, Motion Planning, Pick-and-Place, Smoothness. v Resumo O trabalho desenvolvido nesta dissertação enquadra-se no projeto MIAR, que se foca na utilização de Robôs Autónomos Inteligentes Móveis (MIAR) sincronizados para automatizar as operações de logística na Bosch Car Multimedia BrgP. O manipulador móvel autónomo necessita de realizar operações de pick-and-place envolvendo Targets, caixas contendo determinados produtos eletrónicos, entre um buffer de armazenamento e rampas de acesso ligadas às linhas de montagem. O objetivo desta dissertação foi desenvolver planeamentos de movimento para o manipulador KUKA LBR iiwa 14 R820 poder executar as tarefas pretendidas. A utilização de um braço redundante neste projeto foi com a intenção de se criar trajetórias livres de colisões em espaços confinados. Primeiramente, foi criado um modelo de simulação no CoppeliaSim para representar o chão de fábrica na Bosch, no qual o manipulador móvel irá operar. Neste cenário, foram realizadas diversas tarefas, tais como determinar a quantidade de caixas que o manipulador poderia alcançar dos dollies e verificar a quantidade máxima de caixas que poderiam ser transportadas em cima da plataforma móvel. Para a realização dos testes de planeamento, foi selecionado o gripper colaborativo 2FGP20, de forma a satisfazer os requisitos do projeto, e foram simulados dentes personalizados no Autodesk Fusion para suportar as cargas impostas durante a manipulação. Além disso, foram introduzidas novas rampas otimizadas para garantir que as caixas pudessem ser acedidas pelo braço. Foi desenvolvido um método de comunicação entre o CoppeliaSim e o MoveIt, software que é utilizado para a geração de trajetórias. Esta comunicação foi estabelecida por ROS2 através de dois launch files. Um estudo de benchmarking entre dez planeadores foi realizado, identificando o T-RRT como o mais adequado para o projeto. Posteriormente, o planeador foi personalizado de forma a garantir melhores resultados em diversos cenários de manipulação, e um pós-processador foi adicionado para verificar a existência de melhorias significativas, perante os rigorosos requisitos do projeto. Concluiu-se que o manipulador demonstrou um desempenho satisfatório perante a complexidade do ambiente de trabalho do projeto. Foram ainda identificadas algumas limitações e propostas sugestões de melhorias futuras. Palavras-chave: Pick-and-Place, Planeadores Globais, Planeamento de Movimentos, Suavidade. vi Contents Abstract ..................................................................................................................................... iv Resumo ....................................................................................................................................... v 1 Introduction ........................................................................................................................ 1 1.1 Motivation ................................................................................................................... 2 1.2 Objectives .................................................................................................................... 3 1.3 Structure of the Dissertation ....................................................................................... 4 2 State of the Art ................................................................................................................... 6 2.1 The Importance of Collaborative Robots in Industry .................................................. 6 2.2 Automatic Systems for Feeding and Collecting Boxes from Production Lines ........... 9 2.3 Motion Planners ........................................................................................................ 14 2.3.1 Global Planners .................................................................................................. 15 2.3.2 Local Planners ..................................................................................................... 24 2.4 Motion Planning and Control for Autonomous Mobile Manipulators ...................... 28 3 Theoretical Foundations and Development Tools ........................................................... 31 3.1 Fundamental Concepts of Motion Planning .............................................................. 31 3.2 Motion Planners Utilized in the MIAR Project .......................................................... 33 3.2.1 PRM* .................................................................................................................. 34 3.2.2 LazyPRM* ........................................................................................................... 35 3.2.3 KPIECE ................................................................................................................. 35 3.2.4 T-RRT .................................................................................................................. 36 3.2.5 Bi-TRRT ............................................................................................................... 37 3.2.6 BiEST ................................................................................................................... 38 3.2.7 ProjEST ................................................................................................................ 39 3.3 Hardware Components ............................................................................................. 39 vii 3.3.1 Forward Kinematics ............................................................................................ 40 3.3.2 Inverse Kinematics ............................................................................................. 49 3.4 Software Tools ........................................................................................................... 53 3.4.1 ROS2 Middleware ............................................................................................... 53 3.4.2 MoveIt Library .................................................................................................... 59 3.4.3 CoppeliaSim ........................................................................................................ 64 3.4.4 Autodesk Fusion ................................................................................................. 66 4 Developed Work ............................................................................................................... 67 4.1 System Architecture .................................................................................................. 67 4.2 Simulation Model ...................................................................................................... 68 4.3 Selection of the Gripper ............................................................................................ 73 4.4 Development of the Gripper Fingers ......................................................................... 78 4.4.1 Modeling the Gripper Fingers ............................................................................ 79 4.4.2 Evaluation and simulation of the fingers ........................................................... 80 4.4.3 Fingers Test ........................................................................................................ 84 4.5 Maximum Number of Targets to be Manipulated .................................................... 86 4.6 Configuration in MoveIt ............................................................................................ 90 4.6.1 Launch KUKA LBR iiwa 14 R820 + 2FGP20 in Rviz .............................................. 92 4.6.2 MoveIt-CoppeliaSim Communication ................................................................ 94 4.7 Ramp Analysis and Optimization ............................................................................... 97 4.8 Pick-and-Place Operations in MoveIt/CoppeliaSim ................................................ 102 5 Analysis of Results .......................................................................................................... 109 5.1 Selection of the Ideal Motion Planner ..................................................................... 109 5.2 Analysis of the Motion Planners Comparison ......................................................... 112 5.3 Optimization of the Motion Planner ....................................................................... 116 5.4 Manipulation Operations Using the Selected Motion Planner ............................... 118 xiv List of Tables Table 1 - Motion range and speed specifications of the KUKA LBR iiwa 14 R820 joints. ........ 40 Table 2 - Denavit-Hartenberg parameters table for the KUKA LBR iiwa 14 R820 robot. ........ 42 Table 3 - Functional and non-functional requirements for MIAR project gripper. ................. 75 Table 4 - Comparison of product attributes. ............................................................................ 76 Table 5 - Topics and messages used for the MoveIt-CoppeliaSim communication. ............... 96 xv List of Acronyms Acronym Description AMR Autonomous Mobile Robot AS/RS Automated Storage and Retrieval System BrgP Braga Plant BP Bounce Posture CAD Computer-Aided Design CAE Computer-Aided Engineering CHOMP Covariant Hamiltonian Optimization for Motion Planning D-H Denavit-Hartenberg DMP Dynamic Movement Primitive DoF Degrees of Freedom EST Expansive Space Trees FK Forward Kinematics GC Global Configuration GTP Good-to-Person HRC Human-Robot Collaboration HUMP Human-like Upper-limb Motion Planner IK Inverse Kinematics KMR Kuka Mobile Robot KPIECE Kinodynamic Planning by Interior-Exterior Cell Exploration LiDAR Light Detection and Ranging MIAR Mobile Intelligent Autonomous Robots OMPL Open Motion Planning Library PRM Probabilistic Roadmap Method ROS Robot Operating System RRT Rapidly Exploring Random Tree RULA Rapid Upper Limb Assessment SEW Shoulder-Elbow-Wrist SEWv Virtual Shoulder-Elbow-Wrist xvi SKU Stock-Keeping Unit SP Start posture SRDF Semantic Robot Description Format S-R-S Spherical-Rotational-Spherical STOMP Stochastic Trajectory Optimization for Motion Planning TAP Target Am Pose TEB Time Elastic Band TP Target Posture T-RRT Transition-based Rapidly Exploring Random Tree URDF Unified Robot Description Format XML Extensible Markup Language YAML Yet Another Markup Language 1 1 Introduction At Bosch Car Multimedia Braga Plant (BrgP), as well as in other factories within the group, the movement of boxes containing electronic products, known as Targets, across the shop floor between various assembly lines is a complex task. Currently, Targets are moved manually, a time-consuming process that often results in missed delivery deadlines. This issue has several implications for operational performance and production costs. The present dissertation is part of the “Connected Manufacturing - Digital Transformation” Program (ref: SIFN-01-9999-FN-179826 – I&D) of the Bosch & UMinho partnership, more specifically the Project CM.5G.P16 – MIAR – “Flexible system to transport products between shared processes with Mobile Intelligent Autonomous Robots (MIAR) synchronized with production lines”. The MIAR project aims to develop intelligent, flexible, and automated solutions that enable the transport and positioning of products within established deadlines, relying on a combination of advanced technologies and solutions in areas such as collaborative robotics, automation, computer vision, and IoT. Within the scope of the MIAR project, it is necessary to address the previously mentioned situation by implementing innovative technologies. The goal is to move the Targets from manual to automated through solutions that guarantee faster procedures, respecting predefined timeframes. The solutions to be implemented aim to use synchronized autonomous mobile manipulator robots integrated with production lines to perform pick, transport, and place operations for the Targets. For this purpose, vision systems are required to identify the Targets and potential obstacles to the movement of the autonomous mobile robot, such as humans, since the environment in which the robot will operate is shared with factory workers. Furthermore, since the intention is to use more than one autonomous mobile manipulator robot in the established operations, it is important to ensure that no collisions occur between them during the execution of tasks. To achieve this, it is necessary to develop a flexible fleet management system designed for the respective robots. It is also essential to control the movement of the redundant anthropomorphic robotic arm mounted on the mobile platform to ensure the successful execution of pick, transport, and place tasks. 2 In the context of my dissertation, the goal is to develop and test methods for generating collision-free pick-and-place movements for the robotic arm integrated into the mobile robot. The robotic arm is responsible for picking the Targets from the storage buffer and placing them onto the access ramps leading to the assembly lines. It must also collect empty boxes from the ramps and return them to the storage buffer. As these robots must coexist with human operators, in addition to safety, an important requirement is that their movements are legible to make the robot's behavior more transparent to the workers around them. 1.1 Motivation Due to the constant technological evolution in Industry 4.0, the use of automated systems to control industrial processes has grown. The main purpose is to maximize the productivity and flexibility of the processes by integrating autonomous machines in monotonous and hazardous operations and reducing human intervention (NECULA et al., 2022). Currently, tasks such as transporting and handling components across production lines remain challenging in manufacturing environments. The increasing demand for adaptable workspaces requiring more flexible tasks has increased the need for collaborative robots in the industry. The use of collaborative robots in shared environments with humans enables them to assist workers in various tasks, particularly repetitive ones (Weidemann et al., 2023). The MIAR project is concerned with implementing a promising approach to ensure the precise and safe placement and collection of boxes in the production lines. This is achieved by integrating an omnidirectional mobile platform equipped with a redundant collaborative manipulator. This system is designed to work with human workers in a shared industrial space. As such, the robotic arm's movements must be safe and human-readable during all the pickand-place operations. Moreover, the robotic arm's environment is expected to be filled with access ramps that are presented as obstacles to the manipulator’s movements. The robot must generate collision-free trajectories to precisely deliver the boxes to the designated ramps without colliding with them during the entire process. Consequently, the study of the redundancy of the manipulator with the mobile platform’s position will bring good benefits to the execution of the intended operations. The proposed goal brings the opportunity to realize an in-depth 3 analysis of the available motion planning techniques to create a smooth and efficient trajectory throughout all operational phases. 1.2 Objectives The main objective of this dissertation is to develop methods and algorithms for generating safe and precise movements for the robotic arm, ensuring the accurate and secure placement of boxes onto the feeding lines, as well as the simulation with the manipulator to validate the developed models. The planning of trajectories for the redundant arm consists of executing various manipulation tasks. These tasks involve picking up and placing Targets between the storage buffer and the autonomous mobile robot (AMR) and, in turn, picking up and placing these boxes onto the access ramps. To fulfill this primary goal, a series of preliminary tasks must be undertaken, such as familiarizing with the Robot Operating System (ROS) and MoveIt frameworks. The creation of a simulation scenario replicating the real environment at Bosch Car Multimedia BrgP using the physical simulator CoppeliaSim as well as determining the number of boxes the AMR can transport across the shop floor are fundamental to box manipulation management. Another important subtask to consider is the study of possible modifications in the “as is” shot floor, particularly the access ramps, which in the current scenario reveal difficulties for the placement of the Targets. Alongside these suggested changes, this research will also extend to the search for an appropriate gripper in the market for the robotic arm. The gripper must be capable of grasping the boxes for manipulation while satisfying the project's requirements. To ensure the robotic arm's movements are legible and safe, a comprehensive overhaul of the collision-free trajectory planners will be conducted. A comparative analysis of these planners will be made to select the most proper one for the pick-and-place operations. This dissertation also aims to settle a means of communication between MoveIt, the motion planner library responsible for projecting the robot’s movements, and the physical simulator CoppeliaSim, which will transmit all the physical interactions and overall operations. As expected outcomes, the goal is to achieve a robotic arm capable of performing safe and human-readable movements during pick-and-place operations of boxes in the production 4 area. This is particularly important as the robot will coexist in an environment with human operators, where human-robot interaction must be as efficient as possible. 1.3 Structure of the Dissertation This dissertation is divided into six chapters, each explained below. The current chapter serves as the introduction, addressing the problem definition, providing context for the MIAR project, and outlining the motivations and goals established for this work. The second chapter exposes the current growth in the use of robots in the industry, focusing on the evolution of collaborative robots in shared tasks. It also includes a general review of collision-free motion planners, especially sampled-based, optimization-based, and human-like planners. It addresses an analysis of the industry's existing automatic feeding and collection systems for boxes, and it exhibits an overview of solutions similar to the robotic approach presented in the MIAR project, emphasizing control by motion planning for mobile platforms and integrated robotic arms. The third chapter explores theoretical foundations important for properly understanding the developed work and delves into the impact of redundancy in the robotic arm's kinematics study. It also lays out the planners under examination in the project for the pick-and-place trajectories. Lastly, it details all the software and hardware necessary to realize the respective work. The fourth chapter represents the core of this dissertation. It describes the system architecture of the MIAR project and details all the practical tasks carried out during the project, commencing with the creation of the simulation scenario in CoppeliaSim, the study of the number of boxes the AMR can dislocate, and the selection of an appropriate gripper for the manipulation tasks. It includes the modeling and simulation of potential gripper fingers to be used in the project. The design of optimal access ramps to facilitate pick-and-place operations for the Targets is also examined. Other tasks were also executed, such as configuring the manipulator with the gripper in MoveIt, replicating the robot’s workspace environment to the Rviz, and establishing communication between MoveIt and CoppeliaSim. The fifth chapter thoroughly compares the motion planners introduced in Chapter 3 to identify the most suitable one for the project. Consequently, it includes the results and 5 analysis of the benchmarking study. It also explores customized planners and examines how their adjustments influence the performance of the pick-and-place operations. The final chapter provides a comprehensive summary of the work accomplished throughout this research, highlighting the conclusions drawn from the developed solution. It discusses the possible limitations of the proposed approach and outlines suggestions for future research. The dissertation concludes with an annex and appendices that include technical drawings of the access ramps, the fingers designed for the selected gripper, and the results of the motion planners comparison. 6 2 State of the Art This chapter begins by discussing the evolution of collaborative robots in the industry and provides a literature review on automated solutions for the collection and placement of boxes on production lines. It also summarizes motion planning methodologies that ensure collisionfree movement, categorizing them into global and local planners. Finally, the chapter highlights some applications of these methodologies in mobile manipulator robots. 2.1 The Importance of Collaborative Robots in Industry Industry 4.0 represents a major transformation in industrial processes. This digital industry relies on cyber-physical systems and the Internet of Things to connect databases between systems and equipment, such as sensors and processors, to promote a safer and more efficient industry (Toktaş-Palut, 2022). In modern industry, robots are the fastest-expanded mechanical solution implemented in the industrial sector (S. Liu & Liu, 2020). Based on the World Robotics 2023 report, the worldwide annual installation of industrial robots has tripled from 2012 to 2022, presenting a slight increase of 5% from 2021 to 2022. This growth can be checked in Figure 1 – (a) graph. Besides, between 2021 and 2022, there was a notable increase in the annual Installations of Industrial robots by customer industry, mainly in the electrical/electronics sector of 10% and in the automotive sector of 16%, as depicted in Figure 1 – (b). Figure 1 - (a) Annual installations of Industrial robots - World; (b) Annual Installations of Industrial robots by customer industry – World. Adapted from (IFR International Federation of Robotics, 2023). 7 Companies adopt more and more industrial robots for their extensive usability in operational processes such as manufacturing and logistics, especially robotic arms, to execute repetitive, exhausting, and dangerous tasks like arc and spot welding, carrying high loads, solving manual labor problems, and ensuring higher production efficiency. These types of robots are usually in a robotic cell equipped with sensors and protective fences to surround the robot’s workspace to ensure the safety of human workers (Weidemann et al., 2023). Industrial robots have been defined in different ways over the years. The Robot Institute of America (1979) states that an industrial robot is “a reprogrammable, multifunctional manipulator designed to components or specialized devices, through programmed motions for the performance of a variety of tasks” (Considine & Considine, 1986). According to the international standard EN ISO 10218-1:2011 (International Organization for Standardization, 2011a), which specifies safety requirements for industrial robots, an industrial robot is “an automatically controlled, reprogrammable, multipurpose manipulator, programmable in three or more axes, which can be either fixed in place or mobile for use in industrial automation applications.” Despite the continued growth of traditional robots, there is an increasing demand for more dynamic work environments where tasks are less structured, and workers can collaborate alongside robots (Palmieri & Scoccia, 2021). In these cases, the robots don’t have protective fences and work aided by safety sensors and image processing through cameras. Consequently, the study and development of collaborative robots have expanded. Collaborative robots, also known as cobots, emerged in the 1990s. A collaborative robot is defined as a robot designed for direct interaction with a human within a defined collaborative workspace (ISO 10218-2:2011 (International Organization for Standardization, 2011b)). In contrast to traditional robots, the purpose of cobots is to work alongside humans in a variety of industrial applications like palletizing, machine tending, assembly, and injection molding, as well as in other areas such as healthcare and assistance for people in need. Cobots have been steadily evolving over the years to make collaboration with humans more efficient and safer and to enable improvements in workplace ergonomics. They will primarily be used for repetitive tasks that require high precision and load-lifting capabilities (Weidemann et al., 2023). 14 2.3 Motion Planners Robotic arms are particularly suited for processes that involve picking and placing components, where timing and precision are critical. For a robotic arm to successfully transport boxes, as required for the MIAR Project, it is essential to plan its movements carefully to avoid collisions with obstacles in the working area and with the robot itself. As manipulators are increasingly required for challenging operations, often working in tight spaces and collaborating with humans, the importance of effective motion planning continues to grow. Motion planning has been a subject thoroughly explored for years (LaValle, 2006), which consists of determining a robot's motion from an initial state to a target state, avoiding environmental obstacles, and satisfying constraints, such as joint and torque limits (Latombe, 1991). The motion planning algorithms can be categorized into: • Classical (Deterministic/Combinatorial) Methods: These methods define an explicit representation of the topology of the configuration space. They are rigorous in discretizing the configuration space and searching for the best path, often leading to higher computational demands as the dimensionality of the configuration space increases. The combinatorial methods are decomposed by the cell decomposition, potential fields, and roadmap approaches (Choset et al., 2005). • Grid Methods: These approaches discretize the configuration space into a grid structure, facilitating searches for viable paths (Maria & Rodrigues, 2022). • Sampling-Based Methods: These techniques utilize deterministic or random sampling to explore the configuration space, making them particularly adept at handling highdimensional problems. • Optimization-Based Methods: These methods aim to generate optimal trajectories based on specific criteria, such as minimizing path length, motor efforts, or computation time. Optimization-based methods can produce efficient and smooth paths but might require significant computational power. • Human-Like Motion Planning Methods: With the increasing integration of robots into shared workspaces alongside humans, more and more motion planning algorithms prioritize legibility and predictability. These methods aim to generate intuitive movements for humans, enhancing safety and collaboration (Gulletta et al., 2021). 15 However, motion planning algorithms are typically classified into two types of path-planning methods for obstacle avoidance: the global and local methods (Hwang & Ahuja, 1992). The following sections will discuss the concept of each method and provide an overview of some existing motion planning algorithms for each. 2.3.1 Global Planners The global motion algorithms (Hwang & Ahuja, 1992) consist of methods that process information about the robot’s workspace before executing the movement. These methods are used when the motion planning must be meticulously planned. The process in this planner is made offline. An example is planning motion trajectories for industrial robots in environments where avoiding obstacles is crucial. In the global approach, since the workspace is static, the motion planning for robotic arms becomes easier, resulting in generated movements that are more faithful to human movements and providing more predictable actions to the human operators around the robot’s workspace. Some motion planning algorithms classified as global methods, such as sampling-based planners and optimization-based Planners, are presented below. 2.3.1.1 Sampling-based Planners Sampling-based motion planning has been one of the most researched path-planning methods in recent years. These methods are widely adopted in high-dimensional spaces (Hsu et al., 1997). The sample-based methods also referred to as probabilistic methods, use random or deterministic sampling functions to explore the configuration space (C-space). The main purpose of these planners is to solve path-planning problems by constructing graphs or trees through the connection of sampled configurations (Choset et al., 2005). Therefore, these planners can be divided into two main phases: the first phase involves sampling the configuration space to create a roadmap, and the second is searching this roadmap to find an optimal path (Barland, 2012). Among all types of planners, the most popular are the Probabilistic Roadmap (PRM) and the Rapidly Exploring Random Tree (RRT) (Elbanhawi & Simic, 2014). 2.3.1.1.1 Probabilistic Roadmap Method The Probabilistic Roadmap Method (PRM), introduced by Kavraki et al. (1996), is a planning method that searches for free collision paths for robotic manipulators with many degrees of freedom (DoF), known for effectively solving complex motion planning problems. 16 This method involves creating a roadmap (a graph) in the configuration space and searching for the optimal path. Thereby, the algorithm comprises two distinct phases: the learning phase and the query phase. In the learning phase, the algorithm constructs a probabilistic roadmap by continuously generating numerous random configurations within the robot’s Collision-free space (Cfree). These collision-free configurations are represented as nodes in a graph, and due to a local planner, free collision paths are calculated by binding the nearby configurations (nodes) together, generating the edges of the roadmap. The query phase consists of finding the most feasible path between a specified initial configuration (qinit) and a final configuration (qgoal) on the roadmap created in the learning phase. The method attempts to find the nodes on the roadmap closest to the nodes representing the initial and final configurations and then establishes a connection between those nodes. Subsequently, a path-finding algorithm is performed to find the shortest path between the configurations. An advantage of the PRM algorithm is the ability to handle many types of robots with arbitrary DoF, and it doesn’t require detailed information about the robot’s configuration space. It only requires the ability to verify whether a given configuration is collision-free. Figure 8 shows an example of the construction of a roadmap. Figure 8 - Example of building a roadmap on a robot. Obstacles are represented in blue. Adapted from (Eric O. Scott, 2015). There have been several extensions to the PRM planner over the years, such as the Lazy PRM algorithm developed by Bohlin & Kavraki (2000) In this method, the collision checking is only carried out after a path between the initial and final configurations is found, significantly reducing computational time in many scenarios. To do this, this approach initially considers that all the roadmap nodes and edges are collision-free before searching for the shortest path, and only afterward are the nodes and edges checked for collisions. 17 2.3.1.1.2 Rapidly Exploring Random Tree The Rapidly Exploring Random Tree (RRT) developed by LaValle (1998) was designed to solve motion planning problems with algebraic and differential constraints in highdimensional spaces. This probabilistic method creates a graph in the shape of a tree that is expanded rapidly within the robot’s configuration space. To implement this method, defining the environment, the initial and goal points, and the maximum number of samples is necessary. The tree is rooted from the initial state and grows incrementally by selecting and connecting random points until it either reaches the goal position, the number of iterations, or the planning time expires (Kang et al., 2019). A new point, corresponding to a new robot state, is randomly generated for each iteration. The nearest point to the tree is searched, and a local planner attempts to extend the tree to the sampled point. If the new point is collisionfree, it is added to the tree, creating a new branch. When the algorithm is unable to find a solution in some instances, the problem can be addressed by executing the algorithm with more samples. Unlike the PRM, which requires the construction of a roadmap and the connection of nearby configurations, in the RRT, the samples are obtained uniformly, causing the tree to grow into regions of the collision-free space that have not yet been explored. As a result, RRT is widely used in high-dimensional spaces and nonholonomic and kinematic motion planning tasks (Gasparetto & Zanotto, 2010). The RRT planner is often faster than the PRM planner because it focuses on finding the nearest neighbor point for each iteration. At the same time, the PRM searches through multiple neighbor points around a random sample. Figure 9 illustrates an example of the construction of a configuration tree. Figure 9 - Example of the building of an RRT tree. Adapted from (Barland, 2012). 2.3.1.1.3 RRT-Connect The RRT-Connect algorithm, developed by Lavalle & Kuffner (2000), also known as bidirectional RRT, was designed for efficient single-query path planning. This approach rapidly 18 finds solutions by exploring the configuration space with two random trees that grow toward each other with a simple heuristic. The algorithm starts by creating two trees: one “forward” tree rooted in the initial robot configuration and the other “backward” that starts at the goal configuration. In this single-query probabilistic planner (B. Sciavicco & Oriolo, 2009) the trees expand incrementally by selecting a sample of the Cfree. One of the trees then reaches out toward the sample, creating a new configuration. The algorithm attempts to connect the configuration to the nearest vertex of the other tree through a “Connect” function that iteratively tries to link the two trees. If an obstacle is found in the connection, the trees switch roles, and the algorithm starts the next iteration. RRT-Connect is considered probabilistically complete, which means that the probability of finding a solution increases with longer planning time. It is also usually faster than the standard RRT, especially in less cluttered environments, mainly because it focuses on connecting the two trees, while RRT explores with a single tree until it reaches the goal state. 2.3.1.1.4 RRT* The Rapidly Exploring Random Tree Star (RRT*) algorithm developed by Karaman & Frazzoli (2011) is a variation of the single-tree RRT algorithm designed to ensure feasible and optimal paths according to a specified cost function. RRT* addresses the asymptotic optimality, which means that as number of samples increases, the probability of finding the optimal path approaches one. This algorithm modifies the RRT by introducing a process that continuously rewires the search tree. For each iteration, the algorithm starts by considering that the minimal cost connection will come from the closest sample to the tree. Therefore, a new sample point is added to the tree, and the connections of nearby configurations are reevaluated to ensure that they contribute to the most cost-effective path. This iterative process is continuously optimized until an optimal path is found. Although the RRT* provides significantly better solutions, it requires increased computational cost since the process of rewiring the tree demands additional calculations, leading to longer computational times and a slower convergence rate. Nevertheless, adding asymptotic optimality in this sampling-based method increases the time complexity by only a constant factor over the standard RRT and still provides better paths. 2.3.1.2 Optimization-based Planners Optimization-based planners address motion planning tasks as optimization problems, where the goal is to compute the most efficient trajectory that satisfies a set of constraints 19 related to the robot. To find an optimal trajectory, an object function must be defined, with the user selecting what parameters wants to optimize, such as time, energy, or other factors (Barland, 2012). These planners often use either gradient-based methods, which optimize by incrementally adjusting the trajectory, or non-gradient methods, which explore a broader range of possible solutions (Lynch & Park, 2017). Although these methods are widely used due to their capacity to generate trajectories that optimize cost, these approaches don’t always ensure an effective solution. Among the several trajectory optimization approaches, some techniques stand out. 2.3.1.2.1 Covariant Hamiltonian Optimization for Motion Planning Zucker et al. (2013) introduced an optimization-based motion planner named Covariant Hamiltonian Optimization for Motion Planning (CHOMP) algorithm that uses covariant gradient and functional gradient techniques to optimize robot trajectories for smoothness and obstacle avoidance. This approach is based on the potential field work of Quinlan & Khatib (1993). Unlike most motion planners that treat planning and optimization as separate stages in trajectory generation, CHOMP merges these phases through gradientbased optimization techniques to create an entirely optimizing trajectory. CHOMP models a trajectory as a spring-mass system, where the path internal energy is related to the length and smoothness, and the external energy is related to the obstacles. This algorithm seeks to identify the most efficient path that minimizes the total energy of the trajectory. The trajectory's cost is defined by two components: the smoothness functional of the planner, which measures the dynamics quantities of the trajectory, which aims to reduce quantities like the squared velocity norm during the execution time, and the obstacle-avoidance functional that implements cost functions in the robot’s operational space to keep the algorithm complexity low (Gulletta et al., 2021). The planner quickly adjusts the trajectory to avoid collisions while optimizing dynamic quantities like the robot's joint velocities and accelerations. 2.3.1.2.2 Stochastic Trajectory Optimization for Motion Planning Another optimization-based motion planner, known as Stochastic Trajectory Optimization for Motion Planning (STOMP), was developed by Kalakrishnan et al. (2011). This algorithm was designed for robotic arms to generate smoothness trajectories that avoid obstacles and minimize costs related to constraints in a given time. Unlike gradient optimization methods like CHOMP, STOMP doesn´t require gradients for the planning. As 20 mentioned, it optimizes general costs like torque limits, energy, and constraints included in cost functions. This planner starts by randomly generating, at each iteration, a set of noisy trajectories around an infeasible initial trajectory through a normal probability distribution. These noisy trajectories are evaluated based on a cost function that includes terms like trajectory smoothness and obstacle avoidance. The results from the evaluation are used to iteratively update to a trajectory with lower cost. The higher-cost trajectories contribute less to the final optimized path. It is considered that both initial and final arm postures in the trajectory are permanent during the optimizing process, and the duration of a given trajectory is fixed. STOMP benefits from its ability to quickly compute distance queries and collision costs using distance fields and spherical approximations. Since STOMP is not a gradient-based approach, it has the advantage of overcoming local minima issues, a common problem in CHOMP. Furthermore, practical experiments revealed that STOMP presents a better success rate in various manipulation operations than CHOMP. 2.3.1.3 Human-like Planners There is an increasing interest in ensuring that robots collaborate with humans on shared tasks. Human-Robot Interaction/Collaboration (HRI-C) is enhanced when robots possess an anthropomorphic appearance and exhibit movements similar to those of humans. This makes their actions less unpredictable and aids human perception in understanding robotic behavior. In other words, humans can intuitively follow a robot's movements and anticipate its future actions (Gulletta et al., 2020). Even though probabilistic-based approaches have been extensively explored and studied over the past decade, these planners usually produce unpredictable trajectories (Rafael & Ferreira, 2022). Unstable movements, such as sudden operating speeds by robots, can cause anxiety among nearby humans and pose dangers in shared environments (Zacharias et al., 2011). Human-like morphology and movement are key characteristics for effective human-robot interactions. Robotic movements should be perceived as natural, predictable, and capable of demonstrating the underlying intent of the action without the need for verbal communication (Gulletta et al., 2021). Therefore, the study of legible and smooth robotic movements has led to the development of various human-like approaches inspired by the human motor principles presented below. 2.3.1.3.1 RULA Criterion with RRT-Connect Zacharias et al. (2011) developed a global method for redundant robots using the RULA (Rapid Upper Limb Assessment) ergonomic research criterion to generate human-like 21 movements. In this method, the criterion is applied to identify the areas in the workspace of a manipulator where it exhibits configurations similar to those of a human. RULA measures stress and strain levels in human upper limb postures on a 1 to 7 scale. The lower the RULA score, the more natural the robot configuration is (McAtamney & Corlett, 1993). The initial and desired configurations of the arm are determined through inverse kinematics guided by RULA. Subsequently, the trajectory to move the arm between the two configurations is generated using the RRT-Connect sampling algorithm, thereby defining a collision-free trajectory. To find the natural configurations, the robot’s dexterous workspace is composed of regions with a reachability index for measuring the success in finding inverse kinematics solutions. Thereafter, the RULA criterion is applied to this map, where points with high RULA scores are excluded, indicating less natural postures. A drawback of this method is that if we set a lower RULA score requirement for planning, we will have fewer valid manipulator configurations, resulting in fewer inverse kinematics solutions. Additionally, this method also overlooks the typical temporal parameterization seen in human movements, leading to an unrealistic planning time for the robot. 2.3.1.3.2 Bi-RRT Algorithm with TAPs Xie et al. (2011) introduced the TAP “Target Am Pose” concept for planning human-like arm movements in robotic manipulators. Before starting a task, humans project their arm's final pose, called TAP. TAPs are defined through a qualitative assessment using performance indices that compare the robotic arm's configuration with the human arm's pose in specific target positions. In a collision-free path, a minimum jerk model is employed to generate smooth movements. To apply the model, the initial posture of the robotic arm, the start point, the endpoint, and a set of waypoints must be known. Next, the minimum jerk model creates a path for the finger through all these points. TAPs are calculated at each waypoint and target position to ensure the robot's movement patterns mimic human arms. The IK-JJL inverse kinematics algorithm enables the end-effector to follow the finger path while reaching the designated TAPs. In a scenario with obstacles, a global space analysis identifies high-risk collision areas. Crucial points are selected to create TAPs for the arm that avoid those obstacles. The arm's path is segmented, and the Bi-RRT algorithm is applied to each segment to determine a collision-free path. A disadvantage of this solution is the lack of consideration for typical human motion velocity profiles, such as the bell-shaped velocity profile (Gulletta et al., 2021). 22 2.3.1.3.3 Analytical Inverse Kinematics Solver for Anthropomorphic 7-DOF Redundant Manipulators with Human-Like Configuration Constraints W. Liu et al. (2017) proposed an analytical inverse kinematics algorithm for seven degrees of freedom robot manipulators with shoulder-elbow-wrist (SEW) configuration. This algorithm is designed to replicate human configurations from recorded human movements. It introduced the concept of key positions, which are cartesian positions of the joints of the manipulator. A correspondence method named wrist-elbow-in-line allows mapping key positions from human demonstrations to the manipulator. The mentioned key positions correspond to the shoulder, elbow, wrist, and end-effector/hand (Figure 10 – (a)). Only the key positions of the manipulator's elbow and wrist joints are used as constraints to define the robot arm configuration since the key position of the shoulder is known because it’s fixed, and the task determines the end-effector. In this method, the key position of the wrist must be aligned with the manipulator's elbow and end-effector key positions, ensuring all remain within the same reference plane. Additionally, the key position of the manipulator's elbow must be as close as possible to the human elbow's key position (Figure 10 – (b)). When mapping the key positions from the human demonstration to the robot, the robot's joint limits and link lengths must be considered. Therefore, this solution allows the robot’s elbow key position to be the closest to the human’s elbow key position by adjusting iteratively the robot’s wrist key position until a valid analytical inverse kinematics solution is found. A disadvantage of this solution lies precisely in solving the correspondence problem. As a strategy of iterating key positions is applied, the robot's configuration ceases to follow a human-like posture in areas near the joint limits of the robotic arm. Figure 10 - (a) Kinematic structure of a robot and human arm and their respective key positions; (b) Correspondence method wrist-elbow-in-line. Adapted from (W. Liu et al., 2017). 23 2.3.1.3.4 Human-like Upper-limb Motion Planner Gulletta et al. (2021) proposed a motion planning algorithm named HUMP - Humanlike Upper-limb Motion Planner. This planner was implemented for point-to-point movements and pick-and-place operations, does not require a high computational cost, and contemplates biological temporal parameterization through Fitts' Law. In this global method, trajectories are pre-planned offline to avoid collisions during the robot’s path before starting its movements through sensory input information from the workspace. To provide a smooth and human-like motion, the target posture (TP), which corresponds to the robot configuration that places the robot’s end-effector in the intended pose, must be chosen to minimize the angular jerk of the movement between the robot’s start posture (SP) and the TP. This movement in joints-space is referred to as direct movement (Figure 11 – (a)). If obstacles are anticipated in the direct movement, bounce Postures (BP) are selected to facilitate a back-and-forth movement, which involves moving from SP to BP and back to SP. In this way, this algorithm considers the direct movement and back-and-forth movement, which are used on paths with obstacles. When these two movements are combined, a collision-free trajectory is defined (Figure 11 – (b)). In the proposed method, the movement was composed of pick, place, and move segments (Figure 11 – (c)). The pick and place movements combine three phases: transport, approach, and retreat. The approach phase corresponds to the moment when the manipulator moves closer to grasp the object in the pick segment and when the object is placed in the desired position in the place segment. Meanwhile, the retreat phase corresponds to the moment when the object is lifted in the pick segment and when the manipulator moves away from the placement position in the place segment. The move segment includes the transport phase, which moves the gripper toward the desired positions. The approach and retreat phases are linked by the actions of grasping and ungrasping the manipulated object. All these phases require the selection of a TP and the generation of direct movement. The transport phase is the only one that involves bounce postures to generate back-and-forth movements and subsequently create a composite movement, as it is the only phase that considers objects along the path. 30 Rastegarpanah et al. (2021) proposed a generic framework designed to automate the process of removing and sorting components from electric vehicle batteries with the help of mobile manipulators. The framework enables mobile manipulators to pick up components from a table and place them in the correct bins. The framework comprises navigation, where the robot moves around the workstation, considering the objects in the environment to reach the intended locations. The navigation is handled through a global and local planner approach. The global method uses laser scan sensors that recreate the workspace in a 2D grid map, and the mobile base planning is solved by the A* algorithm. The RRT planner plans the manipulator's trajectory with an inverse kinematics solver to generate a collision-free path. The gripper's grasping actions are controlled via ROS. 31 3 Theoretical Foundations and Development Tools This chapter provides the necessary theoretical background to support the concepts applied in this dissertation. Firstly, it aims to clarify some basic concepts of motion planning that are essential for the project’s development. While the previous chapter provided an overview of motion planning and included examples of various planners, Section 3.2 highlights the importance of selecting an appropriate motion planner for manipulation tasks and details the planners deployed in the MIAR project. This chapter also introduces the manipulator used in the MIAR project, offering a comprehensive examination of its kinematics properties through the discussion of both forward and inverse kinematics. At last, this chapter offers a comprehensive overview of the software and frameworks utilized in the context of this dissertation. 3.1 Fundamental Concepts of Motion Planning Within the scope of motion planning, it is important to differentiate two concepts: path planning and trajectory planning. Path planning is a component of the broader motion planning problem aimed at determining a collision-free path between two configurations in the world space without concern for dynamics, the duration of motion, or other constraints (Lynch & Park, 2017). On the other hand, Trajectory planning not only deals with geometric (kinematical) problems but also with dynamic problems, such as managing the system's masses, inertias, actuator limits, and external forces (Choset et al., 2005). It also considers the temporal aspect of motion planning. Trajectory planning uses the path obtained from path planning that indicates the position and orientation of points in cartesian space and joint space and determines the time profiles of position, velocity, and acceleration for each link, which are then provided to the controllers within the robot’s control system. The complexity of motion planning increases with the number of degrees of freedom (DOF) in the robot. As the number of paths between the start and goal position increases, so does the number of trajectories, making the problem significantly more challenging and requiring greater computational power. A scheme illustrating how robot motion planning works can be shown in Figure 14. 32 Figure 14 – Schematic representation of robot motion planning. Adapted from (S. Liu & Liu, 2020). It is important to highlight that the generated planning can occur in two different spaces: Cartesian Space (also known as Task Space or Operational Space) and Joint Space or Configuration Space. The operational space refers to the three-dimensional cartesian space that defines the position and orientation of a robot’s end-effector. The joint space is defined by a vector of joint coordinates, whose components are the translational and angular displacements of each joint of a robotic link. Usually, planning movements in Cartesian space requires less computational effort because the trajectories are defined by position and orientation coordinates. In contrast, the joint space has a higher dimensionality, as it depends on each joint of the robot. Since these joints are independent, the search for a motion solution involves a larger search space (Gulletta et al., 2020). Regarding the operational and joint spaces, the workspace is defined as the subset of the task space that the end-effector's frame can reach. In other words, the workspace of a manipulator is described as a set of points that can be reached by its end-effector and depends on the length of the links and the joint limits. The workspace can further be decomposed into Reachable Space, which consists of the volume of the workspace that the robot can reach from at least one orientation, and dexterous space, which is the volume of the workspace that the robot end-effector can reach in any orientation. The dexterous workspace is a specific part of the reachable workspace. By default, when calculating the workspace of a manipulator, the end-effector is dictated by the end of the last link of the robot. However, depending on the task, the end-effector can vary. For example, in the MIAR Project, the manipulator needs to grasp a set of boxes, so the end-effector, in this case, is the gripper. Thereby, the workspace depends on the tool-frame transformation (Figure 15) that is attached to the tip of the manipulator (Craig, 2017). 33 Figure 15 - The representation of a robot's tool frame in relation to the base frame. Adapted from (Craig, 2017). 3.2 Motion Planners Utilized in the MIAR Project Selecting the most appropriate motion planning algorithm is necessary for a robotic manipulator to execute collision-free paths successfully. However, the selection of the most suitable planner is a difficult assignment since there are numerous effective strategies available for motion planning problems. There isn’t a single global planning algorithm suitable for every type of motion planning problem, and all the methods have their weaknesses and strengths (Lynch & Park, 2017). When selecting a motion planning method for robotic systems, several critical factors must be considered to ensure the method is appropriate for a specific application (LaValle, 2006). These factors include: trajectory distance, shorter trajectories often lead to faster task completion; execution time, minimizing cycle time directly impacts productivity in industrial settings; computational complexity, certain algorithms have longer processing times, which can slow down the entire system; planning approach, whether it involves creating a solution based on the robot’s environment before the trajectory is initiated or building the trajectory dynamically as the robot moves; robot’s dynamic constraints; and the nature of the obstacles, whether the obstacles are rigid or deformable (Gomes De Brito, 2018). As already stated, the main focus of this dissertation is to develop legible motions for a redundant manipulator in internal logistics tasks, so the selection of the ideal motion planner 34 is a topic of crucial importance in the development of this work. The manipulator has to operate in a restricted space where the placement of the boxes in the access ramps presents a significant challenge. As a result, it is required to create a full trajectory representing all the operations of pick or place, considering all the obstacles within the manipulator’s workspace that could affect its planning. Given the nature of these environments, a global motion planner is used to execute the intended tasks. Although the collaborative manipulator will operate in an environment shared with human operators, it will not perform collaboratively directly with them. In other words, the manipulator will not directly interact with the agents around it. Hence, this project excludes the use of human-like planners, as the primary goal is to generate safe and smooth trajectories for the manipulator while ensuring it can complete tasks in the shortest time possible, which is critical to meet work deadlines. Human-like planners are typically slower and demand greater computational resources, making them less suitable for this application. The global planners used in this project are the RRT, RRT-Connect, RRT*, T-RRT, BiTRRT, PRM*, LazyPRM*, ProjEST, BiEST, and KPIECE. The planners will be presented in more detail next, except for the RRT, RRT-Connect, and RRT* planners, which have already been analyzed in Chapter 2.3. 3.2.1 PRM* PRM* is an advanced version of the standard sampling-based motion planning algorithm PRM designed to find optimal paths in high-dimensional configuration spaces. This extended method approaches optimality by systematically increasing the number of connection attempts as the roadmap expands. As already explained, the Probabilistic Roadmap strategies generate nodes through random samples of the robot’s configuration space. While PRM connects each node to its nearest neighbors from a fixed radius, PRM* uses a sphere with a connection radius for each newly sampled node. The radius of these spheres is scaled logarithmically based on the number of neighboring nodes around the new node (Karaman & Frazzoli, 2011). This method ensures that every node is linked to enough neighbors, facilitating the discovery of shorter paths. Therefore, this approach makes the roadmap by connecting the nodes within the specified radius. The algorithm uses graph search techniques, like Dijkstra's or A* algorithms, to find the shortest path between the start and goal configurations within the roadmap. In this planner, the phases of building the roadmap 35 and searching occur simultaneously, and like the RRT* planner, PRM* is also considered asymptotic optimality (Alandes, 2015). 3.2.2 LazyPRM* The LazyPRM* is an asymptotically optimal sampling-based motion planning algorithm that uses lazy collision checking to enhance pathfinding efficiency in high-dimensional configuration spaces, meaning that it only defers collision checks on an edge when it is part of a potential candidate path. As the name suggests, this is a version of PRM* (Hauser, 2015). While PRM* performs collision checks during the roadmap construction phase, leading to higher computational costs, Lazy PRM integrates lazy collision checking. Still, it does not guarantee optimality, LazyPRM* incorporates lazy state validity checking into the PRM* planner. This algorithm creates random nodes, and each node is connected to its neighbors based on a connection radius to discover optimal paths. Unlike PRM*, the edges between the nodes aren´t immediately checked for collisions. Edges are assumed to be initially valid and are checked only when a candidate's path to the goal is found. If an edge is detected to be in collision in the checking, it is removed from the roadmap, and alternative paths are explored. This strategy avoids many edges that cannot be on an optimal path, significantly reducing computational overhead. 3.2.3 KPIECE The Kinodynamic Planning by Interior-Exterior Cell Exploration (KPIECE) algorithm (I. Sucan & Kavraki, 2008) is a sampling-based motion planner specifically designed to handle systems with complex dynamics. Unlike the vast majority of planners, which require explicit state sampling, KPIECE projects the high-dimensional state space onto a one or multi-level grid-based discretization, creating a set of cells that represent different regions of the space. This discretization guides the planner in estimating the coverage of the state space and allows for identifying underexplored areas. Since KPIECE is a tree-based planner, it creates a tree of motion from the initial state that grows through the cells, connecting feasible new states. As new states are added, KPIECE updates the coverage information for the corresponding cells and adjusts its exploration strategy to concentrate on areas of the grid with less coverage. During the exploration, the algorithm distinguishes between interior cells, surrounded by explored cells, and exterior cells, which are on the boundary of the explored regions. By taking 36 preference on exterior cells, KPIECE expands the boundaries of the explored space, promoting a complete exploration. The process continues until a path from the initial state to the goal region is found. 3.2.4 T-RRT The Transition-based Rapidly Exploring Random Tree (T-RRT) is a sampling-based motion planning algorithm that computes paths in high-dimensional cost spaces by combining randomized path planning and stochastic optimization methods (Jaillet et al., 2010). This is an extended version of the RRT algorithm, which utilizes the exploratory strengths of the planner to navigate through the space. T-RRT incorporates a stochastic transition test to guide the exploration toward lower-cost areas within the robot's configuration space. T-RRT uses this transition test to decide whether to accept or reject new potential states to the growing tree based on the cost function variation relative to the local motion that connects the current state to the new potential state (Figure 16). The cost function considered in the costmap can represent various criteria, such as energy consumption or the distance to maximize between the robot and surrounding objects to create paths with high clearance. Therefore, T-RRT favors solutions that minimize the accumulated cost, leading to paths that are considered not only feasible but also safe. The planner also has a temperature parameter that is dynamically adjusted through the search process and influences the acceptance probability of states. At each accepted uphill transition, this parameter is decreased to avoid over-exploring in regions of high cost, and in case the uphill transition between the states is rejected, the temperate is increased to help the exploration to find new potential states (Devaurs et al., 2013). Thus, this algorithm starts by initializing a tree with an initial configuration and setting the initial temperature parameter. The algorithm samples a random state, and the existing tree attempts to extend toward the sampled state, generating a potential new one. The cost of the new state is compared to the cost of the parent state, and the transition test determines the acceptance probability based on the cost difference. If the new state is accepted, it is added to the tree. At the same time, the temperature parameter is adjusted based on the success rate of the state transition, allowing the algorithm to adapt dynamically to the cost map. This process repeats until the tree reaches the goal state. Compared to the RRT planner, the T-RRT produces better paths because the RRT explores the configuration space to find feasible paths but doesn’t consider a cost function. On the other hand, T-RRT isn’t asymptotically optimal, 37 meaning it doesn´t converge towards an optimal solution like the RRT* planner, which aims to find the shortest path in configuration space by continuously refining its tree structure. Nevertheless, T-RRT offers faster and more feasible paths in high-dimensional spaces than RRT*, which can take time to find a solution. Figure 16 - Example of a construction process of the Transition-based RRT planner. The red zones represent high cost area of the cost map. Adapted from (Jaillet et al., 2010). 3.2.5 Bi-TRRT The Bidirectional Transition-based Rapidly Exploring Random Tree (Bi-TRRT) is an adaption of the T-RRT algorithm that merges the advantages of bidirectional search with costsensitive exploration (Devaurs et al., 2013). Similar to the RRT-Connect planner, Bi-TRRT considers two growing trees, one rooted from the initial configuration and the other from the goal state, and tries to connect them. Each tree explores the cost map independently and has its own temperature parameter. The algorithm alternates between growing each tree. At each iteration, one of the trees attempts to extend toward a sampled random state of the configuration space. Then, the transition test determines whether the new state is accepted or rejected based on the cost difference between the new and the parent state. If the new state passes the transition test, it is added to the tree, and the other tree tries to expand toward this new node. If the trees are unable to connect, they switch roles, and the process is repeated. This process continues with the trees alternately expanding toward randomly sampled states until the junction is made. While RRT* generally converges to better solutions compared to Bi-TRRT, the latter often discovers solutions more quickly. 38 3.2.6 BiEST Expansive Space Trees (EST) is a tree-based motion planning algorithm built for robots with many DoFs that incrementally builds a roadmap through the configuration space, favoring the exploration of less explored areas through the concept of visibility (Hsu et al., 1997). The planner comprises the expansion and connection phases, which are iteratively executed. In the expansion phase, a node in the tree is selected for expansion based on its weight function, which is defined by the number of sampled configurations within its neighborhood up to a specified distance threshold. Nodes with lower weights, indicating fewer nearby configurations, are prioritized for expansion. This ensures the tree grows toward less explored areas, preventing repeated exploration of already covered regions and encouraging expansion into new directions. Then, random states are sampled in the neighborhood of the selected node of the tree. The group of nodes near the free node that can connect in a straight line without encountering obstacles is called the visibility set (Alandes, 2015). The planner connects those sampled configurations that contribute to the visibility region with the free node, leading to the expansion of the tree. Then, the process repeats. In the connection phase, the algorithm tries to connect the tree to the goal configuration. If the connection is not possible, the tree continues to expand. While the PRM planner generates random nodes throughout the configuration space and tries to connect them, later searching for the best path between the initial and goal nodes, the EST planner focuses its exploration solely on the relevant space regions to reach the solution. This approach allows the EST to avoid unnecessary computation when creating a roadmap for the entire configuration space. Therefore, this planner EST is ideal for single-query path planning problems and is well suited for environments with narrow passages. The BiEST algorithm is a bidirectional version of the EST planner that explores the configuration space by growing two trees simultaneously, one from the start configuration and another from the goal configuration. For each tree, it expands toward the node with the lowest weight, and random configurations are sampled. The tree expands toward the configurations within the visibility region of the selected node. The two trees grow toward each other, and when the visibility region of one of the trees intersects with the other, the planner attempts to connect the newly added nodes. The visibility region of a tree is the union of the visibility regions of all its nodes. As the two trees grow toward each other, the 39 exploration space decreases, allowing the BiEST planner to find solutions more quickly than the EST planner. 3.2.7 ProjEST The Projection-based Expansive Space Tree algorithm (ProjEST) is an extended version of the EST planner that incorporates a projection function to map the high-dimensional state space into a lower-dimensional space (Hsu et al., 1997). This projection enables ProjEST to impose a grid on the projected space, allowing it to focus the expansion of the tree in underexplored areas. The algorithm selects nodes for expansion based on a weight function, prioritizing regions that are less densely sampled, and uses the projection to guide the sampling and connection of the nodes near the tree. The planner uses a default projection associated with the state space if no projection is defined. The algorithm attempts to connect the tree, which roots from the initial node, to the goal configuration, iterating until a valid path is discovered. By reducing the dimensionality of the problem during exploration, ProjEST can converge on solutions more quickly than the standard EST planner. 3.3 Hardware Components The robotic arm defined for the motion planning tasks in the scope of the MIAR project is the collaborative manipulator KUKA LBR iiwa 14 R820 (Figure 17). This is a redundant manipulator with 7 degrees of freedom. A manipulator is considered kinematically redundant when it has more degrees of freedom than those strictly necessary to perform a given task. Manipulators with 7 DoF are generally redundant, which allows for greater manipulability. The reason for using this type of robot in the case study is primarily due to its redundancy, which enables more precise and dexterous movements. This is beneficial for performing tasks and allowing the robotic arm to exhibit smoother and legible movements. The redundant degree of freedom allows the manipulator to perform secondary tasks beyond achieving a specific position and orientation, such as avoiding obstacles. The 7-DoF anthropomorphic robot features an S-R-S kinematic structure consisting of a spherical shoulder, a rotational elbow, and a spherical wrist (Faria et al., 2018). 46 According to Figure 17, the angle θ1𝑣 is located on the xy plane since the robot’s first joint rotates around the horizontal plane. To obtain the angle, it is necessary to determine the distance from the robot’s shoulder to its wrist. As the position and orientation of the robot’s tip are known due to the general homogenous transformation matrix and the vector from the base to the shoulder is 𝑝 02=[0 0 𝐿1], because the link 1 only rotates axially, then the vector from the shoulder to the tip is computed by the Equation 3.11. 𝑝 27= 𝑝 07− 𝑝 02 3.11 Since the vector from the wrist to the robot’s tip is 𝑝 67=[0 0 𝐿4] and the real orientation between these two frames can be acquired by the rotation matrix R 07 derived from T 07, as presented above in Equation 3.2, the shoulder-wrist vector is calculated using the Equation 3.12. 𝑝 26= 𝑝 27− 𝑝 67 R 07 3.12 The θ1 angle, positioned in the xy-plane, establishes the x and y coordinates of the tip and, consequently those of the wrist, and is thus influenced by 𝑝 26,𝑥 and 𝑝 26,𝑦. However, in case the vector 𝑝 26 is colinear with the standard unit vector in the direction of the x-axis [0,0,1], which means that the wrist of the robot is aligned with the shoulder direction, then θ1𝑣 is undefined because 𝑝 26,𝑥 and 𝑝 26,𝑦 are zero. This scenario represents a singularity. To address this issue, when such a condition arises, θ1𝑣 takes the value 0. This condition is verified by the cross product of both colinear vectors, which must be 0 (Equation 3.13). θ1𝑣 ={𝑎𝑡𝑎𝑛2(𝑝 26,𝑦,𝑝 26,𝑥), 𝑖𝑓 ‖𝑝 26×[0 0 1]‖>0 0, 𝑖𝑓 ‖𝑝 26×[0 0 1]‖=0 3.13 As shown in Figure 20, the virtual joint θ2𝑣 is based on the projection of the shoulderwrist vector on the z-axis and the xy-plane, but it also depends on the angle Ф, the latter being determined by the law of cosines (Equation 3.14). 47 Ф=arccos(𝑑𝑠𝑒2+‖𝑝 26‖2−𝑑𝑒𝑤2 2 𝑑𝑠𝑒 ‖𝑝 26‖) 3.14 Figure 20 - Illustration of the virtual manipulator used to calculate FK. Adapted from (Faria et al., 2018). Depending on whether the elbow of the robot is upwards or downwards, the value of θ2𝑣 will differ. That being said, another parameter, called Global Configuration (GC), is considered. This parameter is associated with specific joints and takes a value of 1 if the joint's configuration is positive and -1 otherwise. In this case, if the elbow is up (θ4> 0), the GC will be 1, and if the elbow is down, the GC will be -1 (Equation 3.15). θ2𝑣= atan2(√𝑝 26,𝑥2+ 𝑝 26,𝑦2,𝑝 26,𝑧)+𝐺𝐶4Ф 3.15 With the virtual angles known, the virtual position of the robot's shoulder ( 𝑝 04𝑣) can be determined by applying the previously described FK process to obtain the homogeneous transformation matrix T 04𝑣. Given that, the shoulder and wrist positions are identical in both the real and virtual configurations, as seen in Figure 19, the vectors linking the base to the shoulder ( 𝑝 02 and 𝑝 02𝑣) and the base to the wrist ( 𝑝 06 and 𝑝 06𝑣) are therefore equivalent. The only difference lies in the vectors linking the base to the elbow ( 𝑝 04and 𝑝 04𝑣) owing to the different elbow positions in the real and virtual manipulators. As the arm angle is defined as 48 the angle between the two planes: SEW and SEWv, the parameter will be computed by calculating the angle between the normal vectors of these planes. The cross-product of two vectors spawns a vector that is orthogonal to both. The normal vector of the plane SEW can be determined by the cross-product of the vectors belonging to the plane, namely the vectors linking the shoulder to the elbow ( 𝑝 24) and the elbow to the wrist ( 𝑝 26) (Equation 3.18). 𝑝 24= 𝑝 04− 𝑝 02 3.16 𝑝 26= 𝑝 06− 𝑝 02 3.17 𝑉𝑆𝐸𝑊= 𝑝 24× 𝑝 26 3.18 The normal vector for the plane SEWv is through the vectors 𝑝 24𝑣 and 𝑝 26𝑣 (Equation 3.21). 𝑝 24𝑣= 𝑝 04𝑣− 𝑝 02𝑣 3.19 𝑝 26𝑣= 𝑝 06𝑣− 𝑝 02𝑣 3.20 𝑉𝑆𝐸𝑊𝑣= 𝑝 24𝑣× 𝑝 26𝑣 3.21 Before formulating the arm angle, it is important to find whether the parameter is positive or negative. The cross product of the normal vectors defined in Equations 3.18 and 3.21 generates a vector that is collinear with the vector that connects the shoulder to the wrist (𝑝 26). If these vectors have the same direction, the sign of the arm angle is positive, otherwise, it is negative. To conclude, the arm angle results from the dot product to the two normal vectors VSEW and VSEWv (Equation 3.22). Ѱ=𝑠𝑖𝑔𝑛Ѱ arccos(𝑉𝑆𝐸𝑊𝑣 ∙𝑉𝑆𝐸𝑊 ) 3.22 49 3.3.2 Inverse Kinematics The inverse kinematics (IK) consists of finding the joint values of the robot based on the position and orientation coordinates of the end-effector, that is, there is a conversion from the cartesian space to the joint space. As previously indicated, 7-DOF manipulators are generally redundant, and this redundancy problem is addressed in the IK. As the target pose is specified by six variables (three for position and three for orientation), it becomes necessary to solve a system of six nonlinear equations involving seven unique variables corresponding to the robotic arm's joints. This results in an indeterminate system, indicating that the inverse kinematics problem is under-constrained. Faria et al. (2018) proposed an analytical approach for solving the inverse kinematics of 7-DOF manipulators designed to prevent joint limits and singularities. For this purpose, beyond the coordinates of the desired pose, the parameters arm angle (Ѱ) and global configuration (GC) are introduced as inputs to the problem. Thus, solving the arm's inverse kinematics requires these three variables, whereby for a given target pose, the desired arm angle is specified, along with the GC values for the shoulder, elbow, and wrist joints (values of 1 or -1), indicating whether θ2, θ4, and θ6 are positive or negative. In IK for 7-DOF manipulators, there are eight possible solutions for a specific hand pose and known arm angle: four with the elbow (θ4) bent backward and another four with the elbow bent forward. Within each of these eight configurations, there is an infinity of possibilities in the null space due to the seventh degree of freedom, which allows continuous movements of the robot without affecting the end-effector’s pose. According to Faria et al. (2018), to determine the real joints of the robot, it is necessary to first calculate the value of joint θ4, as it is the only joint independent of the arm angle parameter. As such, by applying the Equation 3.12 again, it´s possible to have the real shoulder-wrist vector. Since the shoulder position doesn’t change relative to the base, the vector 𝑝 02 remains the same as the virtual one, the vector 𝑝 07 is derived from the desired position, the matrix R 07 is established using the desired orientation of the robot’s endeffector (Yaw, Pitch, Roll) and the Roll-Pitch-Yaw transformation matrix (Equation 3.4), and the vector 𝑝 67 is once again the vector that links the wrist to the end-effector by the z-axis. Thus, based on Figure 20, the angle θ4 is achievable through the law of cosines. As mentioned earlier, in the IK, the GC is an input that describes the robot's configuration. Since the remaining joint angles depend on the upward or downward orientation of the robot's 50 elbow, it is essential to introduce the GC4 parameter in Equation 3.23 to specify whether θ4 will be positive or negative. θ4=𝐺𝐶4arccos(‖𝑝 26‖2−𝑑𝑠𝑒2−𝑑𝑒𝑤2 2 𝑑𝑠𝑒 𝑑𝑒𝑤 ) 3.23 In order to discover the other joint angles, the real manipulator´s elbow position must be found. To accomplish this, it is first calculated the pose of the virtual elbow, using the process previously applied in the FK, whereby θ1𝑣, Ф angle and θ2𝑣 are determined with the help of Equations 3.13, 3.14 and 3.15. It is important to remind that the value of the virtual joint θ3𝑣 is zero. The pose of the virtual elbow is dictated by the T 03𝑣 or T 04𝑣 matrices through the D-H convention, where R 03𝑣 or R 04𝑣 represent the orientation of the frames in the elbow presented in the Figure 18. Considering that the arm angle defines the angle between the real arm configuration and its virtual/non-redundant configuration, the real elbow pose will correspond to the virtual elbow pose when rotated by an Ѱ angle around the axis passing through the elbow and wrist (Equation 3.24). 𝑅 03= 𝑅 0Ѱ 𝑅 03𝑣 3.24 Given this logic, Rodrigues's rotation formula can be used to convert the virtual elbow pose to the real elbow pose. The Rodrigues's rotation formula allows the rotation of the vectors from the rotation matrix around a specified axis by a given angle (Equation 3.25). R=I3+(sin𝜑) 𝐾+ (1−cos(𝜑)) 𝐾2 3.25 , where I3 is the identity matrix, 𝜑 is the rotation angle around the axis, and K is the crossproduct matrix associated with the unit vector k, that represents the rotation axis. Applying this formula to the case study results in the following Equation 3.26. 𝑅 0Ѱ=I3+(sinѰ) [𝑝 26 ×]+(1−cos(Ѱ)) [𝑝 26 ×]2 3.26 51 Replacing the Equation 3.26 on the Equation 3.24, 𝑅 03 can be formulated as follows: 𝑅 03=𝐴𝑠sin(Ѱ)+𝐵𝑠cos(Ѱ)+ 𝐶𝑠 3.27 , where: 𝐴𝑠=[𝑝 26 ×] 𝑅 03𝑣 𝐵𝑠=−[𝑝 26 ×]2 𝑅 03𝑣 𝐶𝑠=[𝑝 26 𝑝 26 𝑇] 𝑅 03𝑣 The matrix 𝑅 03 can also be expressed using the Denavit-Hartenberg convention (Equation 3.28) through the multiplication of T 01, T 12 and T 23, as previously detailed. R 03 =[cos(𝜃1)cos(θ2)cos(𝜃3)− 𝑠𝑖𝑛(θ1) 𝑠𝑖𝑛(θ3) cos(𝜃1) 𝑠𝑖𝑛(θ2)cos(𝜃3) 𝑠𝑖𝑛(θ1)+cos(𝜃1)cos(θ2)sin(𝜃3) cos(𝜃1) 𝑠𝑖𝑛(θ3)+cos(𝜃2)cos(θ3)sin(𝜃1)𝑠𝑖𝑛(θ1) 𝑠𝑖𝑛(θ2)cos(𝜃2)sin(θ1)sin(𝜃3)− 𝑐𝑜𝑠(θ1) 𝑐𝑜𝑠(θ3) −𝑠𝑖𝑛(θ2) cos(𝜃3) cos(𝜃2)−𝑠𝑖𝑛(θ2) 𝑠𝑖𝑛(θ3)] 3.28 By analyzing the matrix 𝑅 03, it can be observed that θ2 can be determined as follows: θ2=arccos (𝑎𝑠32sin(Ѱ)+ 𝑏𝑠32cos(Ѱ)+𝑐𝑠32) 3.29 Based on Equation 3.29, and taking into consideration the signal of θ2 given by the global configuration parameter (GC2), it can be concluded that the real joint θ2 can be expressed as follows: θ2=GC2 arccos (𝑎𝑠32sin(Ѱ)+ 𝑏𝑠32cos(Ѱ)+𝑐𝑠32) 3.30 Applying the same logic, it is possible to determine the values of the joints θ1 and θ3, which depend on θ2 . 52 θ1=atan2(GC2 [𝑎𝑠22sin(Ѱ)+ 𝑏𝑠22cos(Ѱ)+𝑐𝑠22],GC2 [𝑎𝑠12sin(Ѱ) + 𝑏𝑠12cos(Ѱ)+𝑐𝑠12]) 3.31 θ3=atan2(GC2 [−𝑎𝑠33sin(Ѱ)− 𝑏𝑠33cos(Ѱ)−𝑐𝑠33],GC2 [−𝑎𝑠31sin(Ѱ) − 𝑏𝑠31cos(Ѱ)−𝑐𝑠31]) 3.32 The calculation of the remaining joints of the robot will depend on 𝑅 03, which can now be determined since the joints θ1, θ2, and θ3 are known; on 𝑅 07, which is defined by the robot's pose, as established in Equations 3.4 and 3.10; and on 𝑅 34, which is obtained from the matrix 𝑇 34, presented in Equation 3.3, and the value of joint θ4, which is now known. With all these matrices recognized, the matrix 𝑅 47 can be expressed by Equation 3.33. 𝑅 47= 𝑅 34𝑇 𝑅 03𝑇 𝑅 07 3.33 Following the same reasoning presented earlier for the representation of the 𝑅 03 matrix, 𝑅 47 can be expressed as follows: 𝑅 47=𝐴𝑤sin(Ѱ)+𝐵𝑤cos(Ѱ)+ 𝐶𝑤 3.34 , where: 𝐴𝑤= 𝑅 34𝑇 𝐴𝑠𝑇 𝑅 07 𝐵𝑤= 𝑅 34𝑇 𝐵𝑠𝑇 𝑅 07 𝐶𝑤= 𝑅 34𝑇 𝐶𝑠𝑇 𝑅 07 Given the algebraic rotation matrix 𝑅 47 (Equation 3.35), derived from the homogeneous matrix 𝑇 47, and considering the global configuration parameter of the manipulator's wrist (GC6), joint θ6, followed by joints θ5 and θ7, can be determined using Equations 3.36, 3.37 and 3.38, respectively. R 47 =[cos(𝜃5)cos(θ6)cos(𝜃7)− 𝑠𝑖𝑛(θ5) 𝑠𝑖𝑛(θ7) −cos(𝜃7) 𝑠𝑖𝑛(θ5)−cos(𝜃5)cos(θ6)sin(𝜃7)cos(𝜃5) 𝑠𝑖𝑛(θ6) cos(𝜃5) 𝑠𝑖𝑛(θ7)+cos(𝜃6)cos(θ7)sin(𝜃5)cos(𝜃5)cos(θ7)− 𝑐𝑜𝑠(θ6) 𝑠𝑖𝑛(θ5) 𝑠𝑖𝑛(θ7) 𝑠𝑖𝑛(θ5) 𝑠𝑖𝑛(θ6) −𝑠𝑖𝑛(θ6) cos(θ7) 𝑠𝑖𝑛(θ6) 𝑠𝑖𝑛(θ7)cos(𝜃6)] 3.35 53 θ6=GC6 arccos (𝑎𝑤33sin(Ѱ)+ 𝑏𝑤33cos(Ѱ)+𝑐𝑤33) 3.36 θ5=atan2(GC6 [𝑎𝑤23sin(Ѱ)+ 𝑏𝑤23cos(Ѱ)+𝑐𝑤23],GC6 [𝑎𝑤13sin(Ѱ) + 𝑏𝑤13cos(Ѱ)+𝑐𝑤13]) 3.37 θ7=atan2(GC6 [𝑎𝑤32sin(Ѱ)+ 𝑏𝑤32cos(Ѱ)+𝑐𝑤32],GC6 [−𝑎𝑤31sin(Ѱ) − 𝑏𝑤31cos(Ѱ)−𝑐𝑤31]) 3.38 In summary, it can be concluded that the calculation of the joints for the redundant 7degree-of-freedom robot depends on the desired pose, as well as on the arm angle and the global configuration parameters. 3.4 Software Tools For the development of this project, a set of software tools was utilized. Accordingly, this section begins with a brief introduction to the ROS2 framework. Next, it explores the MoveIt library, which is used for motion planning in robotic systems, establishes its relationship with the OMPL library and highlights Gazebo and RViz plugins. Finally, this chapter presents the physical simulator CoppeliaSim and the software Autodesk Fusion. 3.4.1 ROS2 Middleware One of the most important tools to take into consideration in this work is the Robot Operating System (ROS) framework. ROS is an open-source framework consisting of communication tools and a set of plug-and-play libraries that aid in creating complex systems. ROS is considered a specialized middleware for robotics, providing communication infrastructure, hardware abstraction, low-level device control, exchange of messages between processes, and more (Serrano, 2015). Although it is independent of the type of programming language used, ROS primarily utilizes C++ and Python for its development (Figure 21). 54 Figure 21 - Languages used in ROS communication tools. Adapted from (The Robotics Back-End, 2018). ROS was created to facilitate code reuse in robotics research and development. It features an integrated repository that allows users to utilize pre-existing code for various robotic applications. ROS supports a wide range of robotic components, including aerial, ground, humanoid, and underwater robots, as well as different types of sensors, such as GPS receivers. As such, it serves as a platform that enables the integration of various software tools with different purposes, such as OpenCV and MoveIt. For instance, ROS can be integrated with OpenCV to enable data captured by cameras to be processed by advanced algorithms for more efficient image processing, such as object detection, motion tracking, and pattern recognition (Serrano, 2015). In the MIAR Project, ROS was used to connect the selected motion planner for the redundant manipulator’s movements in the logistics tasks and the physical software that serves as a robotic simulation environment. In this project, the operating system used is Ubuntu 22.04, and the recommended ROS distribution is ROS2 Humble Hawksbill. ROS2 Humble is an advanced version of the ROS framework. Retaining some ROS concepts is important to ensure a correct understanding of the steps taken in the project in Chapter 4. 3.4.1.1 ROS Structure ROS can be divided into workspaces containing packages. Each package can contain small programs called nodes, which enable connections between different systems. 3.4.1.1.1 ROS Workspace A ROS workspace is a directory that starts empty and creates a specific structure when built using the Colcon build tool. It includes a source (src) subdirectory containing the source code for the ROS packages. Additionally, the workspace has three other directories: the build directory, where intermediate files are stored; the install directory, where all created packages 55 are installed; and the log directory, which holds various logging information about the builds made in the workspace. 3.4.1.1.2 Packages Packages are the basic unit of software organization in ROS. Each package can contain libraries, executables, or scripts, which are used to structure and modularize the code. Packages can be created using either CMake or Python, allowing a workspace to include packages of different build types. Figure 22 illustrates a workspace structure containing a CMake package developed in C++ and a Python package developed in Python, both located in the source folder. Figure 22 - Workspace with CMake and Python packages. 3.4.1.1.3 Nodes A node is an executable file that performs computation. Nodes are created inside packages. These processing units are independent and can communicate with other nodes. (Tavares, 2015). Figure 23 shows an example of a robotics application composed of nodes. This system consists of three ROS packages, each containing a set of nodes initiated separately and communicating with each other. 62 Figure 29 depicts the high-level system architecture of the move_group node, which is a central component in MoveIt. This ROS node operates as an integrator, coordinating different components to offer a set of ROS actions and services for users. There are three primary ways to access the services and actions of move_group: move_group_interface package, which provides a simple and effective C++ interface that communicates with the move_group node via ROS topics, services, and actions. It enables functionality for most tasks a user may need, such as defining the target’s pose, motion planning, adding objects in the environment, and attaching or detaching objects from the robot; moveit_commander package through Python; Via a Graphical Interface, using the motion planning plugin for Rviz, the ROS visualization tool. Rviz is a 3D visualizer designed for the ROS environment (da Costa, 2019). It allows the display of sensor data and the robot’s state in real-time, providing a virtual model of the robot. It presents sensor data through ROS topics and services, which helps understand the robot's perception of its environment. This simulator is mainly used to validate planning solutions in virtual environments, usually working in parallel with the physical software Gazebo, before being applied to the real robot. It enables visualization of the robot's trajectory, helping to identify potential issues. Gazebo is a 3D simulator designed to recreate environments for multiple robots. It includes dynamic and kinematic physics to accurately simulate real-world conditions such as friction, damping, and other environmental factors. The descriptive robot format used in Gazebo is the Simulation Description Format (SDF). As the Rviz, Gazebo is used to test and validate robotic behaviors before real operations. (Koenig & Howard, 2004) The move_group retrieves different types of information from the ROS parameter server: the robot_description parameter, which provides the robot’s URDF file, and the robot_description_semantic parameter, which provides the robot’s SRDF file. It also searches for other MoveIt configuration data like joint limits, kinematics, motion planning settings, and perception information. These config files are also generated within the MoveIt config package of the robot by the MoveIt Setup Assistant. 63 Figure 29 - High-level system architecture of the MoveIt move_group node. Adapted from (PickNik Robotics, 2024a). In MoveIt, even if the trajectory has already been planned for a given task, it still needs to be executed. To do so, MoveIt uses controllers to execute the trajectory for certain hardware. The ros2_control framework standardizes the interaction between the robot hardware and the software controllers (Amadi et al., 2024). It provides a flexible interface to control different types of hardware, whether for real robots, physical simulations, or mock setups. This framework comprises the controller manager node, the controllers required for trajectory execution, and hardware interfaces. The controller manager node manages controller switching by activating, deactivating, and loading controllers as needed. It connects the controllers to the required hardware interfaces and manages MoveIt by activating the appropriate controllers. The users can also access the controller manager through ROS services. The controllers are plugins based on control theory that evaluate input commands and calculate outputs to control robot joints. The most used controller plugins are the joint_trajectory_controller, which controls the trajectories of robot joints; the joint_state_controller, which publishes the status of robot joints; and the position, velocity, and effort controllers, which send the desired position, velocity, and effort commands, respectively, to the hardware interface. As mentioned earlier, these controllers are configured in the ros2_controllers.yaml file. The hardware interface is used to send and receive commands from the hardware. There are several hardware interfaces, the most common being the Joint Command Interface, which supports joint control, and the Joint State Interface, which supports reading the state of the joints (position, velocity, effort). In case the hardware interface of a robot in MoveIt doesn’t already exist, the user can create its own. 64 Several hardware plugins are available regarding the hardware with which MoveIt can communicate. Among these stands the real hardware of a robot that corresponds to the robot’s system interface and the gazebo_ros2_control/GazeboSystem, which integrates the control of the robot with the Gazebo simulator, allowing realistic physical simulations. If the goal is to simulate the robot’s behavior without relying on a physical simulator or the real robot, fake hardware can be used in MoveIt through the mock_components/GenericSystem plugin. This plugin is mainly used for offline testing of the ros2_control framework, as it simulates ideal robot behavior by mirroring commands to their states. Since in the project, the validation tests of the trajectories generated from the manipulator were not to be supported by the Gazebo physical simulator, then the GenericSystem plugin was used. To visualize a robot in MoveIt, as well as plan and execute its motions, ROS 2 launch files are required. The Python launch files handle the loading of all the configuration parameters present in the moveit_config package. To achieve this, MoveIt uses the MoveItConfigsBuilder utility to load these parameters, which include the robot description and semantic description files (URDF and SRDF), motion planning and kinematics plugins, trajectory execution settings, and other necessary configurations. Once the MoveIt configuration parameters are loaded, the move_group node must be launched to initiate the Move Group C++ Interface. It’s also important to launch the tf2 library to manage coordinate transforms, especially the static transform publisher node, which establishes the static transform between the robot’s base and world frames. The robot state publisher node must be launched to read the robot’s joint states, compute frame transformations using the URDF file, and publish these transforms to tf2. Finally, the ros2_control framework must be set up for the robot to execute a trajectory. To start the ros2_control, the controller manager node must be launched and spawn the required controllers and the hardware plugin needed for the trajectory execution. 3.4.3 CoppeliaSim Although MoveIt does perform motion planning for the manipulator for the various pickand-place operations, it is essential to validate the planning tests before applying them to realworld scenarios. To do this, the movements must first be simulated in a dynamic analysis. While Gazebo is the simulator that typically works alongside MoveIt, this project will utilize CoppeliaSim to verify the arm's movements in a digital environment. 65 CoppeliaSim (Figure 30) is a physical simulator that aids the development and validation of planning algorithms and robotic systems (Bogaerts et al., 2020). This software allows the creation of any robot and the design of complex environments with multiple robots. It includes a physical engine that enables robotic behaviors that are more similar to real solutions. The selection of CoppeliaSim for this project is due to its compatibility with various operating systems, including Windows and Linux, making it suitable to work in Ubuntu 22.04, which is the required version for the project. CoppeliaSim also supports the creation of robotic models in the working environment by importing CAD files such as stl, and can also read URDF files like in MoveIt. This compatibility allows the same robot model to be used in both MoveIt and the physical simulator. Furthermore, CoppeliaSim can also establish external communication with other software because it’s compatible with ROS. These strengths make it a versatile tool for connecting the motion planning of the robotic arm in MoveIt with the robot's physical model in CoppeliaSim. CoppeliaSim is organized into models, which are individual elements that can represent a robot, sensors, or even a person, and a scenario, which comprises all the models that compose the simulation space. Each model is represented by a hierarchy tree that includes the physical model, which considers all the physical properties, such as mass and moments of inertia, and the graphical model, which is associated with the visual representation of the model in the scenario. The tree can also have joints connecting the model with any other scenario component. CoppeliaSim allows the control of different models by associating them with child scripts. The proprietary language for these scripts is Lua or Python. In this work, CoppeliaSim replicates the manipulator's movement during the pick-andplace operations through ROS and validates the planned motions by recreating a digital twin of the robot’s workspace. 66 Figure 30 - CoppeliaSim interface. 3.4.4 Autodesk Fusion Lastly, Autodesk Fusion was used, which is a Computer-Aided Design (CAD), ComputerAided Engineering (CAE), and Computer-Aided Manufacturing (CAM) software developed by Autodesk. This software (Figure 31) allows users to define and model various components, set properties, and assemble different models. It also helps validate products by subjecting them to statistical, thermal, dynamic, and other analyses. In the context of this work, Autodesk Fusion was utilized to model the access ramps and any other essential components for the robot's workspace during the planning of the pick-and-place operations. Additionally, the software was used to simulate the performance of the custom fingers designed for the selected gripper of the robot, as detailed in Section 4.4.2. Figure 31 - Autodesk Fusion Interface. 67 4 Developed Work The present chapter encompasses the practical elements undertaken in the development of this project. The chapter begins with a description of the system architecture of the MIAR project, emphasizing the module where this dissertation fits. It then addresses the creation of the simulation model of the robotic arm’s workspace and the selection of a suitable gripper for the desired operations, including the development of customized gripper fingers. Furthermore, it outlines the configuration and control of the set: manipulator and gripper for MoveIt and the integration with the CoppeliaSim software. The last section exposes the implementation of motion algorithms for the robotic arm’s pick-and-place operations. 4.1 System Architecture As previously indicated, this dissertation is part of the MIAR project, whose objective is to transport Targets from an intermediate storage buffer to supply ramps. Thus, developing a system architecture encompassing several modules was necessary. Figure 32 illustrates the global architecture of the MIAR system. Figure 32 - Global architecture of the MIAR system. This architecture is centered around the core module, Fleet Management, which is responsible for managing the fleet of vehicles and contains a map of the production lines' supply area. This module receives data from the production lines through the Bosch system 68 and transmits the information to each vehicle via the local module Service Manager. This module receives the service of the Fleet Management and divides it into tasks that are sent to the Task Manager. This submodule is within the Movement Controller Module, which includes several submodules dedicated to controlling the AMR and planning the manipulator’s movements. The task manager ensures the proper execution of assigned tasks by activating the appropriate submodules in the correct sequence. The architecture also includes the Environment Perception local module that comprises several submodules responsible for acquiring and processing sensory information, either through LiDAR or the vision system. This dissertation focuses primarily on the Arm Planning submodule, which is tasked with planning and controlling the arm’s movements. However, developing this submodule requires understanding the mobile manipulator's conditions. This includes a clear definition of the tasks to be performed by the mobile manipulator and its workspace during the operations. 4.2 Simulation Model As previously mentioned, the main aim of this work is to develop safe and smooth trajectories for the KUKA LBR iiwa 14 R820 manipulator. The respective arm is mounted on the KUKA KMR iiwa 14 mobile platform. In the Bosch Car Multimédia BRgP shop floor, the Targets are positioned on top of dollies, which are transport carts used to move loads within the facility. The Targets are manually transferred from the dollies to different assembly lines by PoUPs, a designation established for the operators responsible for the specific task. The objective of the MIAR project's "To Be" challenge is to replace these PoUPs with autonomous mobile manipulator robots to perform the task. An example of the intended work layout is shown in Figure 33. 69 Figure 33 - Intended work layout at Bosch Car Multimedia BrgP. As can be seen in Figure 33, the blue zone corresponds to the initial location of the dollies loaded with their respective Targets. First, the mobile platform must move to the Dolly Supply zone so that the robotic arm can pick up the Targets and place them on top of the platform. Afterward, the platform moves to the different workstations, represented in yellow in Figure 33, allowing the arm to deliver the Targets to the appropriate stations. It's important to note that the corridors where the autonomous mobile manipulator robots operate are 1.2 meters wide, which limits their movement during the execution of the desired tasks. The corridor next to Dolly Supply is wider, allowing mobile platforms to rotate when picking up boxes. Several preliminary parameters must be considered to carry out the simulation model, namely the maximum number of Targets positioned on top of the dollies that the arm can reach and the maximum number of Targets that the mobile platform can transport from the Dolly Supply to the different workstations. The selection of the gripper for the robotic arm is also extremely important for the manipulation of the Targets. Thereby, some fundamental elements required to recreate the intended work layout at the Bosch Car Multimedia BRgP shop floor were modeled using Autodesk Fusion software. These elements include the access ramps and the dolly channel. Other essential components, like the Targets and the dollies, were provided as CAD files by Bosch. 70 There are two types of Targets to be manipulated, both of which are plastic PP containers from the Utz Group (Utz Group, 2014). One container is larger, measuring 600 x 400 x 220 mm, while the other is smaller, measuring 400 x 300 x 220 mm. Both containers have loose lids. Figure 34 presents these containers and their corresponding CAD models. Figure 34 – Containers-RAKO from Utz Group and their respective CAD models. In this dissertation, the larger containers will be referred to as "600x400 boxes" and the smaller ones as "400x300 boxes" based on the outer dimensions of their bases (Figure 35). (a) (b) Figure 35 - (a) Outer dimensions of the base of the larger container; (b) Outer dimensions of the base of the smaller container. Similar to the Targets, there are two types of dollies, each designed for a specific kind of container. Currently, the dollies are positioned together at the Dolly Supply without any secure mechanism to hold them in place (Figure 36). 71 Figure 36 - Dolly Supply zone for MIAR Project. As a result, the dolly channels are used to retain and guide the respective dollies. An example of this mechanism, along with its CAD model, can be seen in Figure 37. Figure 37 – Dolly Channel and CAD representation. Finally, the access ramps are responsible for storing the boxes for a certain period of time. In the current shop floor, the ramps have either two levels, which include an upper and a lower level, referred to as one-level ramps, or three levels, consisting of an upper, middle, and lower level, referred to as two-level ramps. The upper and middle levels are designated for holding full boxes from the Dolly Supply provided by the PoUP, while the lower level contains the empty boxes left by the human operators. The CAD models of the one and twolevel ramps can be checked in Figure 38 and Figure 39, respectively. There are two types of ramps corresponding to each kind of Target, where the width of each level of the ramp is the box length with an additional 10 mm, implying high precision in the picking and placement of the boxes. Appendix A shows these four ramps without the roller rails in more detail. For this project, the one-level ramps will be used for the pick-and-place operations. Thus, the mobile manipulator has to place the boxes retrieved from the Dolly Supply on the upper level of the ramps, pick the empty boxes from the lower levels, and transport them back to the Dolly Supply. 78 Based on the applied method, the most appropriate solution was the 2FGP20 gripper from OnRobot (Figure 44). This gripper can communicate using ROS, which is beneficial for integrating with the motion planning software MoveIt, which is responsible for planning the robot's trajectories. Figure 44 - 2FGP20 front view (dimensions in mm). Adapted from (OnRobot, 2023). 4.4 Development of the Gripper Fingers As stated in Section 4.3, one of the requirements is that the gripper must have a minimum distance between its fingers greater than 400 mm to grasp the boxes successfully. Although the stroke of the 2FGP20 gripper is 260 mm, its opening is insufficient for grabbing the Targets. To ensure effective manipulation of both boxes, it is essential to develop custom gripper fingers. Using Autodesk Fusion software, the desired fingers were modeled, and through a Finite Element Analysis (FEA), the fingers were tested to verify whether they could meet the requirements to be used in a real application. The FEA method is useful for simulating and identifying causes of failure in mechanical systems to prevent failures before manufacturing. 79 4.4.1 Modeling the Gripper Fingers Typically, parallel gripper fingers are composed of simple plates that apply pressure to the manipulated object. The tangential component of the applied pressure must be sufficient to reach a static equilibrium, balancing the gravitational force and preventing the object from falling. However, the boxes under study feature a groove corresponding to their outermost surface, which has a thickness of only 6 mm, as can be seen in Figure 45, which illustrates the profile view of both Targets. Figure 45 - Profile view of both Targets with the outer dimensions. To address this, the gripper fingers were inspired by how a human hand typically grasps these types of boxes. The fingers were designed in an L shape, allowing them to compress and handle the boxes, as shown in Figure 47. This design approach reduces the pressure required on the outer surfaces of the box by applying load components in both the horizontal and vertical directions. As detailed in Figure 45, the groove width is 15 mm, which means the minimum aperture for the 400x300 boxes must be 370 mm. To ensure a safe grip, the design of the fingers allows a distance of 368 mm when the gripper is closed. This design enables the gripper to clamp the boxes in 2 mm if necessary (Figure 46). When the gripper is open, the distance between the fingers reaches 628 mm because the gripper's stroke is 260 mm. This results in a total clearance of 28 mm in relation to the 600x400 boxes. Consequently, high precision is required when planning the grasping and releasing operations of the robot. The dimensions of each finger can be verified in Appendix B. In the CAD model, fillets were applied 80 to the perpendicular surfaces where the boxes are grasped, aiming to reduce stress concentrations and facilitate mesh generation. (a) (b) Figure 46 – (a) Maximum Gripper Aperture; (b) Minimum Gripper Aperture. One of the main problems of the 2FGP20 gripper is that its flange is decentralized 50 mm in relation to its center, as reported in Figure 44. Another issue is that only one of the gripper fingers is movable. Since the two boxes to be manipulated have a length difference of 200 mm if both fingers have the same length, a significant misalignment would occur between the gripper’s flange and the center of one of the boxes. Such misalignment is undesirable, as it could create a moment during manipulation tasks that might compromise the robot's operations. To mitigate this problem, the fingers must have different sizes. Therefore, the length of each finger was adjusted interactively until reasonable dimensions were achieved. This ensures that the distance from the robot flange to the center of each box isn´t too different. Since these fingers are intended for initial tests, one of the Bosch requirements was that they should be made of PLA. Figure 47 - Gripper holding the two types of Targets. 4.4.2 Evaluation and simulation of the fingers After creating the geometric model of the fingers, the pre-processing of a linear static analysis was made, where inputs like the materials, mesh, and loading conditions were 81 established. Regarding loads, the fingers were subjected to two forces: a horizontal force representing the maximum gripping force of 400 N and a vertical force corresponding to the maximum weight of the Targets. This latter force is responsible for provoking tensile stresses in the upper surface of the fingers and compressive stresses in the lower surface. Since the two fingers withstand the boxes of 8 kg, an acting force of 39.24N was applied to the inner surfaces of each finger. Regarding boundary conditions, each finger is fixed to the claws at the base of the gripper, which constrains all six degrees of freedom (DoF) of the model. The mesh is a crucial factor to consider, as its quality significantly impacts the simulation results. A higher mesh density, which means more elements by unit of volume, leads to more accurate results. However, higher computation costs will be required if the mesh is too refined. Since each finger is voluminous, solid elements were considered, specifically quadratic tetrahedral elements. These elements are composed of 4 corner nodes, 6 mid-side nodes, and 6 edges. Second-order elements generally yield more realistic results than linear elements because they can have curved edges to better capture geometric curvatures. This feature was considered in the mesh generation (Figure 48). Figure 48 - Pre-processing of the mobile gripper finger. Instead of using local mesh refinement, where local mesh controls are applied to different regions of the model, an adaptive mesh refinement was used. In this method, the solver automatically identifies critical stress regions and refines the mesh by employing smaller elements in those areas. As the mesh density increases, the stress levels tend to rise until the results stabilize. From that point, further mesh refinement does not yield significant differences in the results. A convergence study was conducted to achieve what is known as a converged mesh. This method involves successively refining the mesh until the change in a critical parameter, usually stress, between two consecutive iterations is less than a certain 82 percentage (Madier, 2021). In this case, a percentage of 5% was set. To avoid excessive computational times during the mesh generation, a maximum of 6 iterations was defined (Figure 49). Figure 49 - Mesh generation parameters in Autodesk Fusion. Following pre-processing, the fingers were subjected to static analysis to determine their ability to support the applied loads during the grasping actions. Due to the high applied loads, a safety factor of 4:1 based on critical transportation was considered for comparison (PHD Inc., 2001). In the post-processing phase, the results were analyzed. Based on the results obtained, it can be concluded that the fingers can withstand the applied loads, as the values for stress, strain, and displacement are low, and the safety factor is high throughout the part. As expected, the maximum stress and strain occur on the perpendicular internal surfaces due to the moment generated by the gripping force. The maximum displacement is observed at the tips of the fingers, as this gripping force is significantly greater than the weight of the box. Moreover, the stress on the movable finger (Figure 50 – (a)) is slightly higher than that on the fixed finger (Figure 50 – (b)) because the movable finger is longer, resulting in a greater moment at its tip. 83 (a) (b) Figure 50 – (a) Static simulation on the movable finger; (b) Static simulation on the fixed finger. Given the high safety factor, it´s possible to understand that the fingers were overdimensioned. The design can be optimized to reduce the weight of the fingers. A shape optimization was considered, as shown in Figure 51. However, since these fingers were intended solely for initial testing, this study has not been conducted. Therefore, the fingers used were those described earlier, with a total weight of 1 kg. 84 Figure 51 - Fingers shape optimization. For safety precautions, the study was also conducted on the boxes to determine if they could withstand the gripper's maximum gripping force, which can reach high levels. As anticipated, the maximum gripping force did not impact the boxes (Figure 52). Given that the maximum experienced stress is 3.1 MPa, it can be concluded that the box remains within the elastic regime and doesn´t suffer plastic deformation because the box has a yield stress of 30.30 MPa. Figure 52 - Static simulation of the boxes to be manipulated. 4.4.3 Fingers Test The fingers were manufactured using 3D printing, and manipulation tests using a Doosan robot were conducted at Europneumaq facilities to validate the simulations. The tests demonstrated that the gripper could successfully grasp the two types of boxes (Figure 53). 85 (a) (b) Figure 53 - (a) Grip test of the 400x300 box; (b) Grip test of the 600x400 box. In this project, the gripper mounted with the designed fingers has a total weight of 4.7kg. Although, at first glance, it appears that the arm can transport the boxes since the total weight to be handled is 12.7 kg, this doesn’t necessarily mean that the arm can manipulate the maximum weight of the Targets. As previously mentioned, the height of the gripper must not be too large because it would affect the arm’s desired performance. This limitation happens because the payload of the arm varies depending on the distance from the manipulator's mounting flange to the center of gravity of the object being manipulated, as depicted in Figure 54. Figure 54 - Payload diagram of KUKA LBR iiwa 14 R820 manipulator. Adapted from (KUKA, 2019). Due to the significant difference in weight between the gripper and the box, it can be inferred that the center of gravity of this system would be located somewhere within the 86 Target. By analyzing the payload diagram, it is evident that for the manipulator to work with the Gripper+Target system, the value of Lz must be less than approximately 100 mm. However, the gripper height alone already exceeds this value, as shown in Figure 44. Therefore, it is concluded that even though the gripper meets the initial project requirements, its considerable distance from the flange to its base inevitably reduces the payload capacity of the arm. 4.5 Maximum Number of Targets to be Manipulated Since the Kuka LBR iiwa 14 R820 robotic arm is mounted on top of the mobile platform and given the fact that a dolly at the Bosch factory floor can support more than one Target, it is important to determine the maximum number of Targets the robotic arm can reach. An analysis was conducted for the mobile platform in different positions, concluding that the maximum number of Targets that the robotic arm can grasp from the dolly is four (Figure 55). Beyond four boxes, the arm cannot grasp any additional Targets from above. (a) (b) Figure 55 – (a) Manipulator grasping the first box from the dolly; (b) Maximum number of Targets placed on a dolly that the arm can reach. Another crucial aspect to consider in the study is the maximum number of Targets the mobile platform can transport to the workstations. This number depends on the robotic arm's ability to place the boxes along the platform. Since the Targets to be handled can have two different dimensions, the analysis was carried out for three distinct cases: the maximum number of 600x400 boxes placed on the base of the mobile platform, the maximum number of 400x300 boxes that can be placed on the base of the platform, and the maximum number of Targets comprising both types of dimensions. For the first case, it was determined that two 87 600x400 boxes could not fully fit on the base of the KMR. Additionally, the arm can only place two stacked boxes due to its configuration, which does not allow for the addition of a third box (Figure 56). Figure 56 – Study of the maximum number of 600x400 boxes on the platform. Regarding the 400x300 boxes, various distributions of these boxes at the platform's base were examined to determine which would allow for the maximum number of boxes. It was observed that the KMR could transport 4 boxes (Figure 57). Figure 57 - Study of the maximum number of 400x300 boxes on the platform. In the last case, different distributions were also considered for the two types of boxes. It was concluded that only two 400x300 boxes and two 600x400 boxes could be transported (Figure 58). 94 Figure 66 - KUKA LBR iiwa 14 R820 with 2FGP20 gripper in Rviz. 4.6.2 MoveIt-CoppeliaSim Communication In MoveIt, when a robot is executing a trajectory to a specific point, its joints, including the gripper joint, are always read by the /joint_states topic. To avoid trajectory errors in CoppeliaSim, it is essential for the robot to replicate the same joint movements as in MoveIt without any delay. Hence, ROS2 communication was made between the two software. The communication logic followed the diagram in Figure 67, where CoppeliaSim sends the poses of multiple objects in the manipulator's workspace to MoveIt, while MoveIt continuously sends the robot's joint values to CoppeliaSim. Figure 67 – MoveIt-CoppeliaSim Communication Diagram. 95 A node named /joint_state_subscriber was created to subscribe to the /joint_states topic, reading the robot’s joint values that are provided in the sensor_msgs/msg/JointState message, which contains data describing the state of the controlled joints in MoveIt, including the joint names, positions, and velocities (ros2.org, 2020). This node publishes the robot joints to a new topic whose message type is readable in CoppeliaSim. For this purpose, the message type: std_msgs::msg::String was used. In the CoppeliaSim MIAR project scenario, a child script associated with the robotic arm was made. This script subscribed to the newly created topic and applied the joint positions in real time to the CoppeliaSim manipulator. To synchronize the opening and closing of the gripper joint in CoppeliaSim with its counterpart in MoveIt, a topic named fechar_gripper was created in the main code and was responsible for executing the gripper’s finger movement. Each time the gripper is commanded to grasp or release an object in MoveIt, the gripper's joint position is set as the message to be published on this topic. In CoppeliaSim, the gripper’s child script subscribes to this topic, reads the joint position, and applies it as the value for its joint. To plan the robot’s movement in MoveIt, the arm must be aware of the obstacles around it to detect them and provide collision-free trajectories. To solve this problem, every model in CoppeliaSim that impacts the manipulator’s workspace received a child script. This script reads the models' poses relative to the arm's base frame and publishes them in a topic, with orientation given in quaternion format. To make these objects appear in the Rviz environment, a set of code was developed to read the poses from the topics associated with each CoppeliaSim model and collision objects were created by reading the models' stl files to generate meshes. The objects were then published to the /collision_object MoveIt topic, which is responsible for placing objects in Rviz. In Table 5, it is possible to visualize the message types along with the associated topics that facilitate communication between the two software. 96 Table 5 - Topics and messages used for the MoveIt-CoppeliaSim communication. Topic Message Goal /joint_states sensor_msgs/msg/JointState Used to know the states of the robot’s joints. /joint_state_string std_msgs/msg/String Publishes the robot’s joint positions to CoppeliaSim. /fechar_gripper std_msgs/msg/String Sends the gripper joint position in the grasp and release actions. /caixa400_300_position_orientation geometry_msgs/msg/Pose Sends the pose of the objects located in the robot’s workspace. /caixa400_300_position_orientation_1 /caixa400_300_position_orientation_2 /caixa400_300_position_orientation_3 /caixa600_400_position_orientation /caixa600_400_position_orientation_1 /caixa600_400_position_orientation_2 /caixa600_400_position_orientation_3 /carcaca_position_orientation /plataforma_position_orientation /rampa1nivel_410_position_orientation /rampa1nivel_610_position_orientation /collision_object moveit_msgs/msg/CollisionObject Creates collision objects in Rviz environment. For a more detailed understanding, Appendix C provides a visualization of how the communication is established between the nodes and topics responsible for sending the manipulator’s joints from MoveIt to CoppeliaSim. The nodes and topics involved in sending the poses of the objects in the CoppeliaSim simulation scenario to Rviz are presented in Appendix D. These connections were obtained using the ROS tool rqt_graph, where the nodes are represented by an oval format and the topics by a rectangle. The difference in the poses generated by the two software in relation to the world frame can significantly compromise the arm's ability to manipulate objects correctly in both scenarios. Accuracy errors in data transmission exceeding 14 mm can prevent the arm from gripping the boxes properly. It is essential that both scenarios are identical. Thus, the URDF, used to visualize the manipulator with the gripper attached in Rviz, was utilized to create them 97 in the CoppeliaSim environment. This ensures no differences between the two scenarios (Figure 68). Figure 68 - CoppeliaSim-MoveIt Communication. 4.7 Ramp Analysis and Optimization An important aspect that needed to be studied before starting the pick-and-place operations was to confirm whether the “as is” access ramps would enable the manipulator to pick the boxes. A study was carried out to investigate this situation. Concerning the two-level ramps, it was confirmed that this type of ramp is unsuitable for the pick and place tasks. For both the operations of picking empty boxes from the lower level and depositing full boxes on the middle level, the robotic arm did not have configurations that would enable it to perform these tasks. The dimensions of the set arm and gripper and slight height differences between access levels prevent the manipulator from approaching the ramp without colliding with it. Furthermore, due to its high height, the arm cannot reach or drop the full boxes on the upper level (Figure 69). This issue automatically limits the possibility of increasing the height between the levels. 98 Figure 69 - Attempt to collect or place Targets by the manipulator on the different levels of the ramp. Regarding the one-level ramps, with the KMR positioned at different distances from the 1200 mm corridor, it was concluded that the redundant manipulator could not pick up empty boxes from the lower level because the arm links collided with the upper level (Figure 70 – (a)). Several solutions were proposed to allow the arm to access this type of ramp and grab the boxes, such as modifying the height of the ramp levels. In an iterative process, onelevel ramps with different height levels were evaluated based on the distance between the KMR and the ramps in the corridor. It was concluded that the lower level height should not be reduced. Doing so would not only cause ergonomic strain on the human operators on the opposite side of the ramp, forcing them to bend further to place the boxes, but would also compromise the path planning for the manipulator. Lowering the level would require the platform to move closer to the ramp so that the arm could reach the box, resulting in increased difficulty when removing the box due to the reduced distance between the platform and the ramp. For the upper level, an increase in the height of 200 mm was determined to prevent the robot from colliding with the ramp. However, the height could not exceed 300 mm, regardless of the platform’s distance from the ramp. This proposal, however, was deemed unsuitable because it would require that all motions to remove the box from the ramp be performed using inverse kinematics (IK) instead of a global planner due to the limited manipulation space that would significantly increase the time required for the planner to find a feasible solution. This strategy also would force the robot to remove the boxes from the lower level by the side of the ramp, as there would not be enough clearance to remove the box directly between the platform and the ramp. Another suggestion was to design the ramp levels as a staircase (Figure 70 – (b)). This design would eliminate the issue of the arm colliding with the upper level when picking a box from the lower level. However, it would reduce the storage capacity of the upper 99 level and once again create ergonomic disadvantages for the human operators, as the upper level would need to be lowered for the arm to reach it. Figure 70 - (a) Collision of the manipulator's links with the ramp upper level; (b) Proposed staircase ramp. It was suggested that the front bar of the upper level be cut to facilitate the entry of the robotic arm into the ramp for reaching the lower level. At the front part of the upper level, only the roller rails located at the corners would remain to support the boxes placed at the level's entrance. The support structure that holds all the roller rails and connects them to the rest of the ramp structure would be repositioned further back not to hinder the arm's entry into the ramp. A study was carried out to define the optimal entrance cut for the ramps, ensuring their suitability for the project. From this study, it was concluded that the ideal ramp for the 600x400 boxes should have an entrance cut of 500x350 mm, while the ramp for the 400x300 boxes should have an entrance cut of 300x350 mm. Besides, both ramps would require the levels to be raised by 20 cm (Figure 71). Figure 71 - "To be" one-level ramp. 100 With the lower level raised by 20 cm, it would be possible to perform arm movements to pick up the box without resorting to numerous intermediate inverse kinematic (IK) movements. Another advantage is that the platform could be positioned farther away from the ramp, enabling direct removal of 600x400 boxes from the lower level. The increased height of the lower level in the pick operation would facilitate the placement planning of the boxes in the platform, as the lower level of the ramp would be closer to the platform's base level (Figure 72). For picking operations, the platform should be positioned 455 mm away from the ramps that store the 600x400 boxes and 435 mm from the ramps that hold the smaller boxes. For placing operations, the platform must be set at a distance of 185 mm from the ramps. The technical drawing of the new type of ramp can be seen in Appendix E. Figure 72 - Picking the 600x400 box on the optimized ramp. A study was also conducted to evaluate whether the proposed modifications for the one-level ramps could render the two-level ramps suitable for future applications. In this scenario, the upper and middle levels were designed with a cut of 500x350 mm for the 600x400 boxes and a 300x350 mm cut for the 400x300 boxes. Using the original "as is" level heights, it was observed that the arm still collided with the levels during the placement tasks (Figure 73 – (a)). However, it could now reach the empty boxes on the lower level (Figure 73 – (b)). 101 (a) (b) Figure 73 - (a) Collision between the robotic arm and the two-level ramp with entrance cuts; (b) Robotic arm retrieving an empty box from the two-level ramp with entrance cuts. Further analysis was conducted to investigate how adjusting the heights of the ramp levels could improve their suitability for manipulation operations. From this study, it was concluded that all levels needed to be lowered to enable the robot to place boxes on the upper and middle levels without collisions (Figure 74). Figure 74 - Box placement and retrieval across different levels of the optimized two-level ramp. An example of an optimized two-level ramp that supports pick-and-place operations is presented in Figure 75. 102 Figure 75 - Example of an optimized two-level ramp. Although the arm can reach all levels of the two-level ramps, these ramps were not considered for the pick-and-place operations studied in this work due to issues of spatial clutter and the necessity of reducing the height of all levels. Such height adjustments would impose ergonomic strain on human operators, particularly when placing boxes on the lower level. Moreover, for the arm to access the lower level, it must be stretched, requiring the platform to be positioned closer to the ramp. This is problematic for larger boxes, as the limited space between the platform and the ramp would necessitate repositioning the platform further away from the ramp to remove the box. Thus, executing a single manipulation operation would require additional platform movements, complicating the overall process. 4.8 Pick-and-Place Operations in MoveIt/CoppeliaSim In the MIAR project, there are eight manipulation scenarios for the pick-and-place operations, four of which relate to picking and placing the two types of Targets on the access ramps and four of which relate to picking and placing the respective Targets on the dollies. So, eight operation codes must be developed, each adapted to the conditions of its specific scenario. 103 The correct execution of trajectories in the manipulator for the MIAR project was managed using only two launch files. The first launch file is responsible for creating the Coppelia scenario for MoveIt, and the second is responsible for executing the desired task. Regarding the first launch file, it is composed of a set of nodes that are initiated sequentially. Each node represents a process and is defined by its executable, the package it belongs to, and optional parameters such as the node name and output. Event handlers were used to register specific transition events between nodes to ensure the sequential execution of nodes (Open Robotics, 2024c). This is achieved using the RegisterEventHandler action along with the OnProcessExit event handler. The OnProcessExit is triggered when a specified node finishes execution, allowing the dependent node to start only after its predecessor has finished. This structured method ensures that all processes are executed in the correct order, respecting dependencies and preventing potential conflicts during execution. The first nodes in this sequence publish each model present in the physical simulator scenario as collision objects in Rviz. Depending on the number of models present in CoppeliaSim, the Rviz environment is adapted to match the simulation. Nothing is published as a collision object if the robot has no obstacles in the scenario. After the sequential run of the nodes responsible for publishing the poses, the final node settles the communication between the manipulator in MoveIt and the one in CoppeliaSim, as previously explained. As already exclaimed, the second launch file handles the execution of the pick-andplace operations. Depending on the manipulation scenario selected by the user, one of the eight possible operations will be executed. So, this launch file depends on the desired operation and takes the name of the CoppeliaSim model the arm must pick or place as input. Consequently, this launch file is composed of a sequence of two nodes, both of which receive the launch file's input as an argument. The first one corresponds to a node that reads the pose of the CoppeliaSim model to be manipulated, provided as an argument, and publishes it in Rviz. The second node executes the planning code for the specified object. By structuring the process in this way, the scenario is always read before proceeding with the manipulation operation. If there is any positional variation in the models in CoppeliaSim, MoveIt will update this change, and the subsequent planning will take this into account. Following this logic, these codes could be easily adapted to real-world applications. In the first launch file, cameras would detect the real-time positions of the objects relative to the arm depending on where the platform stops near the ramps or dollies. In the second launch file, the camera would identify 110 success rate, path planning time, and a path smoothness metric to validate whether the trajectory is smooth. The latter metric measured the angle between three consecutive points, where values closer to 0 indicate smoother trajectories due to a higher rate of straight-line segments. This study concluded that the RRT-Connect planner is a good option for highcomplexity scenarios. Given this overview, it can be concluded that selecting the appropriate motion planner depends on the distribution of the manipulation scenario. Consequently, a motion planning benchmarking study was conducted for the MIAR project. The experiments were carried out on a personal computer using Ubuntu 22.04. The computer consisted of an MSI Katana 15 B13VGK-2040 laptop equipped with a 13th-generation Intel Core i7 processor, 32 GB of RAM, a 1 TB solid-state drive (SSD), and an NVIDIA GeForce RTX 4070 GPU. It is noteworthy that the comparison was exclusively conducted using MoveIt, employing the pick-and-place codes outlined in section 4.8. A group of planners was compared across four different scenarios corresponding to the pick-and-place operations of the two types of boxes on the access ramps. These scenarios were considered due to their critical importance to the overall project and their complexity related to the limited space available for placing the boxes on the ramps. To take benefit of the best planner, the worst-case scenario was considered for each planning, where the robot’s workspace was fully occupied by the maximum possible number of objects. For example, when the manipulator needed to pick up or place 600x400 boxes on the ramp, it was assumed that the platform was already full of 400x300 boxes, and vice versa. The comparison involved the planners RRT-Connect, T-RRT, BiEST, KPIECE, and LazyPRM*, which were highlighted earlier in the reviewed studies. Optimized versions of some mentioned planners, such as PRM* and RRT*, were also included in the benchmarking. Additionally, planners like Bi-TRRT, RRT, and ProjEST were considered after satisfactory results were obtained during preliminary testing in the indicated scenarios. When it comes to choosing the benchmarking metrics, the analysis considered total planning time, total execution time, path length, and the success rate in finding a solution to measure the performance of each planner. A smoothness factor was included to evaluate the quality of the generated paths. This parameter is derived from the metric used in the benchmarking study by Cohen et al. (2012) to measure path smoothness. For a path composed of n segments, the angle α i between two consecutive segments is calculated, and the 111 following Equation 5.1 is applied. The lower the value of this metric, the smoother the trajectory is considered. 𝐾=1𝑛 ∑𝛼𝑖2 𝑛 𝑖=2 5.1 In MoveIt, each planner includes multiple parameters that can significantly influence the performance of the sampling-based planners. Since modifying and testing each parameter individually by planner would be a time-consuming process, all the mentioned planners were compared using the default parameters provided by the MoveIt 2 package. A planning time of 1 second was specified for each planner, as the goal is to achieve fast planning, along with the constraints and motion kinematics explained in Section 4.8. Each planner was simulated ten times for each operation. The optimization-based planner CHOMP was initially considered for this benchmarking study. However, this planner typically generates long processing times, as highlighted in (S. Liu & Liu, 2022). Given the defined constraints and the settled planning time, CHOMP was unable to find a solution without adjustments to its parameters. As a result, this planner was excluded from the benchmarking study. The benchmarking process was inspired by the OMPL Planner Arena from MoveIt, utilizing Tukey Boxplots to compare the performance of each planner across different metrics. Similar to the Planner Arena, the plot analyses in this study exclude data from failed attempts by the planners. This exclusion is crucial for correctly interpreting the studied plots because some planners may present data from the total number of planning attempts, while others might show a subset. To address this issue, as done in the Planner Arena, a table showing the success rate of each planner is presented alongside the plots (Moll et al., 2015). As the goal is to identify a suitable planner for all the required operations on the Bosch shop floor, any planners that fail to compute a solution within the 10 defined attempts are automatically excluded from consideration. The analysis of the obtained results is thoroughly addressed in the subsequent section. 112 5.2 Analysis of the Motion Planners Comparison As explained in section 5.1, a benchmarking study of ten OMPL planners was performed for the scenarios involving the access ramps, using the planning and execution times, path length, a smoothness metric, and the success rate in finding a solution as evaluation metrics. The results are presented below for the place operation involving the 600x400 boxes, which consists of placing the boxes on the upper level of the ramp. Due to the many metrics and planners included in the comparison, the analyses for the remaining operations are presented in Appendix F. In the operation of placing the 600x400 boxes, it was observed that all the planners provided relatively short planning times for both the placement and retreat paths (Figure 80). However, RRT-Connect and BiEST exhibited slightly higher values compared to the others. Figure 80 - Comparison of planning and execution times for the place operation of the 600x400 boxes. A significant discrepancy was observed between the planners in terms of execution time. The planners that demonstrated the best results were ProjEST, RRT, RRT*, PRM*, and TRRT. This latter planner contained outliers, one in the place execution time and two in the retreat execution time. These outliers resulted from execution times that exceeded the upper whisker, meaning values higher than the upper quartile (Q3) by 1.5 times the interquartile range. Although LazyPRM* and KPIECE had a high place and retreat execution times, it is 113 notable that half of their values are concentrated as their median values were below 10 seconds. These execution time values are reflected in the path length of the entire operation generated by each planner (Figure 81). Planners with higher execution times, such as BiEST and RRT-Connect, produced longer paths, while planners like RRT, ProjEST, and PRM* yielded shorter paths. Figure 81 - Comparison of the path length for the place operation of the 600x400 boxes. Using the metric explained earlier, smoothness was observed. Planners producing longer paths tended to create smoother trajectories (Figure 82). These planners prioritized the motion constraints specified in the operation code rather than finding the fastest path to the goal. As a result, they generated longer and unnecessary paths to yield more linear paths, while other planners allowed larger angles between the segments that composed the path. 114 Figure 82 - Comparison of the smoothness factor for the place operation of the 600x400 boxes. Regarding the success rate in finding a feasible solution, most planners successfully generated a valid path, except the RRT-Connect planner, which failed in one attempt. Only ProjEST, RRT, PRM*, and KPIECE consistently found a solution on the first attempt (Figure 83). Figure 83 - Number of attempts for the place operation of the 600x400 boxes. From the pick operation of the 600x400 boxes presented in Appendix F, it is possible to analyze that only the Bi-TRRT and T-RRT planners achieved good results in terms of planning time and execution time for both the pick and retreat movements of the task, with Bi-TRRT showing outliers for the pick execution time. The remaining planners exhibited high execution time values for the pick movement, with very dispersed data. These results are also expressed in the path length metric, as these two planners generated the shortest paths. Regarding the smoothness factor, the standout planners were once again Bi-TRRT, TRRT, and RRT*. Unlike the previous operation, all planners successfully found a path in this operation. Bi-TRRT, T-RRT, PRM*, and LazyPRM* have achieved high success rates. Based on this analysis, it can be concluded that these two first planners were the best options for this operation. 115 For the place operation with the 400x300 boxes, all the planners generally presented good planning and execution times, with the ProjEST, Bi-TRRT, and RRT planners standing out. However, the RRT-Connect, LazyPRM*, and BiEST planners showed high execution times. The planners that created the shortest paths were Bi-TRRT, ProjEST, RRT, RRT*, and PRM*. Despite the favorable results presented by the Bi-TRRT planner, it is important to note that it could not find a feasible solution in all 10 attempts. This planner failed one time to find a solution for the placing operation and, consequently, for the retreating movement. RRT-Connect, BiEST, and LazyPRM* were the planners that contributed to smoother trajectories. However, once again, considering the success rate, the RRT-Connect and BiEST planners failed to find a solution at least twice. Therefore, the RRT-Connect, Bi-TRRT, and BiEST planners were excluded from the selection of the most appropriate planner for the MIAR project, regardless of their good performance in other operations. In the pick operations with the 400x300 boxes, only two planners stood out in the generated times: Bi-TRRT and T-RRT. While Bi-TRRT demonstrated shorter planning times, TRRT provided shorter execution times. Although this interpretation might suggest that T-RRT generated shorter paths, when analyzing the plot based on the operation length, it is verified that Bi-TRRT generated shorter solutions. This is confirmed by its lower mean, smaller interquartile range, and shorter whiskers. Despite being faster at generating solutions with minimal distances, both Bi-TRRT, T-RRT, and KPIECE displayed trajectories with less smoothness. This is confirmed by their higher smoothness factor values, which are approximately 0.05. In contrast, the RRT-Connect planner excelled in this metric. When examining the success rate, several planners, namely BiEST, ProjEST, RRT, RRT*, and PRM*, failed to find always a solution. Over 60% of the attempts executed by ProjEST, RRT, and RRT* were unsuccessful. Among all the planners, Bi-TRRT was the only one that consistently found a solution on the first attempt. The conducted benchmarking study concluded that many planners could not produce paths for the operations involving the manipulation of the 400x300 boxes. This lower success rate compared to the tasks involving the 600x400 boxes can be attributed to the limited space between the platform and the access ramps. The smaller boxes are positioned at the base of the platform, making the required manipulator movements more challenging than picking or placing the larger boxes in the conceptual structure located at the back of the KMR. Among all the considered planners only three were able to find suitable solutions in all four scenarios, 116 which were KPIECE, LazyPRM* and T-RRT planners. Since T-RRT delivered the best overall results, performing well across all evaluation metrics, it was chosen as the planner for the MIAR project. 5.3 Optimization of the Motion Planner T-RRT planner demonstrated satisfactory results for most box manipulation tasks. However, as presented in the previous analysis, its performance on the smoothness metric was the worst in the place operation for 400x300 boxes, and it also showed high values for the pick operation. One alternative to address this problem is further limiting the orientation constraint for the manipulator's end-effector. This adjustment would ensure safer handling of the filled 400x300 boxes and reduce the variability in trajectory generation. The T-RRT planner was customized by tweaking its parameters to achieve faster solutions without fully compromising path quality through an iterative process. The initial temperature, parameter that was mentioned in Section 3.2.4, was augmented as it is recommended that it should be high at the beginning of the algorithm to enable a broader exploration (Kavraki Lab, 2024). The max_states_failed parameter was reduced to accelerate the temperature increase after a few failed transitions to new states, while the temp_change_factor was raised to promote faster temperature adaptations during failed transition tests. It is important to emphasize that while these changes may increase the search for solutions, they could lead to poorer paths being found. On the other hand, to ensure safer handling and reduce tilting during manipulation, the orientation constraint of the end-effector was restricted for the 400x300 boxes. This constraint was also adjusted for the placement code of the 600x400 boxes, as they are placed full on the upper level of the ramp. MoveIt includes post-processing algorithms to time parameterization kinematic trajectories, considering the velocity and acceleration values. To prevent jerky movements of the manipulator and improve smoothness, the Ruckig jerk-limited smoothing algorithm was used during the pick-and-place operations (PickNik Robotics, 2024d). The performance comparison between this custom T-RRT and the default one for the benchmarking metric across the different operations is depicted in Appendix G. Another post-processing optimization technique that can be employed is using CHOMP as a post-processor for OMPL (PickNik Robotics, 2024e). In this approach, the custom T-RRT finds a solution, and CHOMP subsequently optimizes the trajectory. The CHOMP trajectory 117 initialization method fillTrajectory was selected to ensure that the computed path from T-RRT was received. Similarly to the previously considered process, the CHOMP parameters were modified iteratively to guarantee a solution in the shortest possible time. The maximum number of iterations after establishing a collision-free trajectory was reduced, the weight of smoothness in the cost function was considered, a minimum obstacle clearance was defined, and the enable_failure_recovery parameter was set to true, which enables CHOMP to adjust its parameters in case the first attempt with the specified parameters fails. The performance of this parameter is also illustrated in Appendix G. For operations involving the 400x300 boxes, it was observed that the custom T-RRT could generate faster paths, even though the manipulator was more restricted. The execution time values were similar to the default planner, but the path length was shorter with more concentrated values, as indicated by the smaller interquartile range. This reduced variation in values may be attributed to restrictions imposed in the codes that force the algorithm to execute trajectories in a certain direction consistently. In both operations, the use of restricted orientation constraints contributed to smoother paths, particularly in the placement scenario, where the reduction was significant. Regarding the T-RRT with CHOMP, it was verified that although the OMPL quickly found initial solutions and the CHOMP parameters were adjusted, the post-processing time remained lengthy. The planning time ranged from 6 to 7 seconds, which is too long for practical real-world applications. For the smoothness metric, this planner achieved low values and, in the pick operation, even outperformed the custom T-RRT. Although not included in Appendix G, in the ten simulations conducted for each planner, both were able to find solutions, usually in one or two attempts. In one simulation of the pick operation, the planner T-RRT with CHOMP required six attempts to find a solution for the retreat movement to the home position. This result was reflected in the outlier in the planning time for the retreat movement in the respective operation. Regarding the pick and place operations involving the larger boxes, the custom T-RRT planner generally demonstrated shorter planning times than the default planner. However, its execution times were slightly higher in both operations. In turn, the default planner exhibited higher mean execution times for the place operation due to the presence of outliers. The custom planner produced shorter trajectories for the path length metric in both operations. Concerning the smoothness metric, both planners displayed similar values for the pick operation, as the retreat phase consists of a short movement without any collision objects 118 in its path. Nonetheless, custom T-RRT had better results in the place operation. As previously discussed, this performance behavior may result in the restricted orientation constraint applied to this operation. The custom T-RRT with CHOMP planner accomplished results similar to those of the custom T-RTT. Nevertheless, it required longer planning times due to the postprocessing step, resulting in values of around 6 to 7 seconds. From all this analysis, it can be concluded that both the customized T-RRT planner and the planner in conjunction with CHOMP delivered better results in the smoothness metric for small box operations. This outcome was desired, as the default T-RRT planner exhibited high values for this metric. The planners had similar results for operations with larger boxes, although the two customized planners generated shorter paths than the default one. It is also evident that adding CHOMP as a post-processor did not significantly enhance the performance of the custom T-RRT. The results remained similar, with the additional drawback of increased planning time. This is likely attributed to the parameter adjustments made to CHOMP, where the concern was minimizing the post-processing time rather than improving the planner's smoothness. Therefore, based on the realized study, the customized T-RRT is the most suitable planner for the working environment present in the MIAR project. 5.4 Manipulation Operations Using the Selected Motion Planner The customized T-RRT motion planner demonstrated the best performance, according to the studies conducted earlier. The comparison focused solely on scenarios where the boxes were stored on the access ramps due to the challenging nature of this environment for the manipulator during the tasks. However, this evaluation did not include the four additional scenarios where the robot picks and deposits the Targets in the dollies located at the Dolly Supply area. Therefore, the chosen motion planner was tested for the eight manipulation scenarios to ensure its suitability for the MIAR project. This analysis considered all possible pick-and-place operations involving the transport of the Targets. Unlike the previous benchmarking studies, these tests incorporated real-time communication between MoveIt and CoppeliaSim. This analysis verified that the planning times occasionally exceeded expectations compared to the results obtained solely in MoveIt. This slight discrepancy can be attributed to the continuous communication between the two software systems, which imposes greater 119 computational demands on the system. Despite this, the planner successfully identified feasible solutions for the various case scenarios. The chosen planner was determined to effectively accomplish all intended tasks with satisfactory results. Figure 84 shows the outcomes of the picking operations for the 600x400 boxes and 400x300 boxes from the ramp. (a) (b) Figure 84 - (a) Picking 600x400 boxes from the ramp. The task can be seen in: https://youtu.be/4_dr845m8Tw; (b) Picking 400x300 boxes from the ramp. The task can be seen in: https://youtu.be/8CKV2Hu8QAo. Similarly, the placement of the 600x400 boxes on the ramp, as well as the placement of the 400x300 boxes, can be checked in Figure 85. (a) (b) Figure 85 - (a) Placing 600x400 boxes in the ramp. The task can be seen in: https://youtu.be/a5ITRRm8M_8; (b) Placing 400x300 boxes in the ramp. The task can be seen in: https://youtu.be/GSF92bFSyIc. 126 Chitta, S., Sucan, I., & Cousins, S. (2012). MoveIt! [ROS Topics]. IEEE Robotics & Automation Magazine, 19(1), 18–19. https://doi.org/10.1109/MRA.2011.2181749 Choset, H., Lynch, K. M., Hutchinson, S., Kantor, G., Burgard, W., Kavraki, L., & Thrun, S. (2005). Principles of Robot Motion: Theory, Algorithms, and Implementations. MIT Press. Cohen, B., Sucan, I. A., & Chitta, S. (2012). A generic infrastructure for benchmarking motion planners. 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, 589–595. https://doi.org/10.1109/IROS.2012.6386228 Coleman, D., Sucan, I., Chitta, S., & Correll, N. (2014). Reducing the Barrier to Entry of Complex Robotic Software: a MoveIt! Case Study. Considine, D. M., & Considine, G. D. (1986). Robot Technology Fundamentals. https://api.semanticscholar.org/CorpusID:59927231 Craig, J. (2017). Introduction to Robotics: Mechanics and Control (4th ed.). Pearson. da Costa, P. G. (2019). Motion Planning in Cartesian Space for the Collaborative Redundant Robot Sawyer. Devaurs, D., Simeon, T., & Cortes, J. (2013). Enhancing the transition-based RRT to deal with complex cost spaces. 2013 IEEE International Conference on Robotics and Automation, 4120–4125. https://doi.org/10.1109/ICRA.2013.6631158 Dheenadhayalan. (2021). fusion2urdf ROS2. GitHub. https://github.com/dheena2k2/fusion2urdf-ros2?tab=readme-ov-file Doyle-Kent, M., & Kopacek, P. (2021). Adoption of Collaborative Robotics in Industry 5.0. An Irish industry case study. IFAC-PapersOnLine, 54(13), 413–418. https://doi.org/10.1016/j.ifacol.2021.10.483 Dragan, A. D. (2015). Legible Robot Motion Planning. https://api.semanticscholar.org/CorpusID:62974192 Elbanhawi, M., & Simic, M. (2014). Sampling-Based Robot Motion Planning: A Review. IEEE Access, 2, 56–77. https://doi.org/10.1109/ACCESS.2014.2302442 Engemann, H., Du, S., Kallweit, S., Cönen, P., & Dawar, H. (2020). OMNIVIL—An Autonomous Mobile Manipulator for Flexible Production. Sensors, 20(24), 7249. https://doi.org/10.3390/s20247249 127 Eric O. Scott. (2015, October 25). PRM with Ob-maps.gif. https://en.wikipedia.org/wiki/Probabilistic_roadmap#/media/File:PRM_with_Obmaps.gif Faria, C., Ferreira, F., Erlhagen, W., Monteiro, S., & Bicho, E. (2018). Position-based kinematics for 7-DoF serial manipulators with global configuration control, joint limit and singularity avoidance. Mechanism and Machine Theory, 121. https://doi.org/10.1016/j.mechmachtheory.2017.10.025 Fox, D., Burgard, W., & Thrun, S. (1997). The dynamic window approach to collision avoidance. IEEE Robotics & Automation Magazine, 4(1), 23–33. https://doi.org/10.1109/100.580977 Gasparetto, A., & Zanotto, V. (2010). Optimal trajectory planning for industrial robots. Advances in Engineering Software, 41(4), 548–556. https://doi.org/10.1016/j.advengsoft.2009.11.001 Gomes De Brito, J. N. (2018). Manipulador robótico para poda automática (Projeto RoMoVi). Groover, M. P. (2007). Automation, Production Systems, and Computer-Integrated Manufacturing (3rd ed.). Prentice Hall Press. Guertler, M., Tomidei, L., Sick, N., Carmichael, M., Paul, G., Wambsganss, A., Hernandez Moreno, V., & Hussain, S. (2023). WHEN IS A ROBOT A COBOT? MOVING BEYOND MANUFACTURING AND ARM-BASED COBOT MANIPULATORS. Proceedings of the Design Society, 3, 3889–3898. https://doi.org/10.1017/pds.2023.390 Gulletta, G., Erlhagen, W., & Bicho, E. (2020). Human-Like Arm Motion Generation: A Review. Robotics, 9(4), 102. https://doi.org/10.3390/robotics9040102 Gulletta, G., Silva, E. C. E., Erlhagen, W., Meulenbroek, R., Costa, M., & Bicho, E. (2021). A Human-like Upper-limb Motion Planner: Generating naturalistic movements for humanoid robots. International Journal of Advanced Robotic Systems, 18, 172988142199858. https://doi.org/10.1177/1729881421998585 Hai Robotics. (2022). ACR SYSTEMS Power Different Sectors. Hauser, K. (2015). Lazy collision checking in asymptotically-optimal motion planning. 2015 IEEE International Conference on Robotics and Automation (ICRA), 2951–2957. https://doi.org/10.1109/ICRA.2015.7139603 128 Hollerbach, J. M. (1985). OPTIMUM KINEMATIC DESIGN FOR A SEVEN DEGREE OF FREEDOM MANIPULATOR. https://api.semanticscholar.org/CorpusID:10553087 Hsu, D., Latombe, J.-C., & Motwani, R. (1997). Path planning in expansive configuration spaces. Proceedings of International Conference on Robotics and Automation, 2719– 2726. https://doi.org/10.1109/ROBOT.1997.619371 Hwang, Y. K., & Ahuja, N. (1992). Gross motion planning—a survey. ACM Computing Surveys, 24(3), 219–291. https://doi.org/10.1145/136035.136037 IFR International Federation of Robotics. (2023). World Robotics 2023. International Organization for Standardization. (2011a). Robots and robotic devices — Safety requirements for industrial robots Part 1: Robots. In ISO Standard No. 102181:2011. https://www.iso.org/standard/51330.html International Organization for Standardization. (2011b). Robots and robotic devices — Safety requirements for industrial robots Part 2: Robot systems and integration. In ISO Standard No. 10218-2:2011. International Organization for Standardization. (2016). Robots and robotic devices — Collaborative robots. In ISO Standard No. 15066:2016. Iossifidis, I., & Schoner, G. (2004). Autonomous reaching and obstacle avoidance with the anthropomorphic arm of a robotic assistant using the attractor dynamics approach. IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA ’04. 2004, 4295-4300 Vol.5. https://doi.org/10.1109/ROBOT.2004.1302393 Jaillet, L., Cortés, J., & Siméon, T. (2010). Sampling-Based Path Planning on ConfigurationSpace Costmaps. IEEE Transactions on Robotics, 26(4), 635–646. https://doi.org/10.1109/TRO.2010.2049527 Kalakrishnan, M., Chitta, S., Theodorou, E., Pastor, P., & Schaal, S. (2011). STOMP: Stochastic trajectory optimization for motion planning. 2011 IEEE International Conference on Robotics and Automation, 4569–4574. https://doi.org/10.1109/ICRA.2011.5980280 Kang, G., Kim, Y. B., Lee, Y. H., Oh, H. S., You, W. S., & Choi, H. R. (2019). Sampling-based motion planning of manipulator with goal-oriented sampling. Intelligent Service Robotics, 12(3), 265–273. https://doi.org/10.1007/s11370-019-00281-y 129 Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7), 846–894. https://doi.org/10.1177/0278364911406761 Kavraki, L. E., Svestka, P., Latombe, J.-C., & Overmars, M. H. (1996). Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation, 12(4), 566–580. https://doi.org/10.1109/70.508439 Kavraki Lab. (2024). TRRT Class Reference. OMPL. https://ompl.kavrakilab.org/classompl_1_1geometric_1_1TRRT.html#details Khatib, O. (1986). Real-Time Obstacle Avoidance for Manipulators and Mobile Robots. In Autonomous Robot Vehicles (pp. 396–404). Springer New York. https://doi.org/10.1007/978-1-4613-8997-2_29 Kobayashi, M., & Motoi, N. (2022). Local Path Planning: Dynamic Window Approach With Virtual Manipulators Considering Dynamic Obstacles. IEEE Access, 10, 17018–17029. https://doi.org/10.1109/ACCESS.2022.3150036 Koenig, N., & Howard, A. (2004). Design and use paradigms for gazebo, an open-source multi-robot simulator. 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE Cat. No.04CH37566), 2149–2154. https://doi.org/10.1109/IROS.2004.1389727 KUKA. (2019). LBR iiwa Operating Instructions. KUKA AG. (2024). KMR IIWA. Kuka.Com. https://www.kuka.com/en-de/products/amrautonomous-mobile-robotics/mobile-robots/kmr-iiwa Latombe, J. C. (1991). Robot Motion Planning. The Springer International Series in Engineering and Computer Science. In Kluwer Academic (1st ed). Kluwer Academic. Lauretti, C., Cordella, F., & Zollo, L. (2019). A Hybrid Joint/Cartesian DMP-Based Approach for Obstacle Avoidance of Anthropomorphic Assistive Robots. International Journal of Social Robotics, 11(5), 783–796. https://doi.org/10.1007/s12369-019-00597-w Lavalle, S., & Kuffner, J. (2000). Rapidly-Exploring Random Trees: Progress and Prospects. Algorithmic and Computational Robotics: New Directions. LaValle, S. M. (1998). Rapidly-exploring random trees : a new tool for path planning. The Annual Research Report. https://api.semanticscholar.org/CorpusID:14744621 130 LaValle, S. M. (2006). Planning Algorithms. Cambridge University Press. https://doi.org/10.1017/CBO9780511546877 Likhachev, M., & Ferguson, D. (2009). Planning Long Dynamically Feasible Maneuvers for Autonomous Vehicles. The International Journal of Robotics Research, 28(8), 933– 945. https://doi.org/10.1177/0278364909340445 Liu, S., & Liu, P. (2020, December). A Review of Motion Planning Algorithms for Robotic Arm Systems. Liu, S., & Liu, P. (2022). Benchmarking and optimization of robot motion planning with motion planning pipeline. The International Journal of Advanced Manufacturing Technology, 118. https://doi.org/10.1007/s00170-021-07985-5 Liu, W., Chen, D., & Steil, J. (2017). Analytical Inverse Kinematics Solver for Anthropomorphic 7-DOF Redundant Manipulators with Human-Like Configuration Constraints. Journal of Intelligent & Robotic Systems, 86(1), 63–79. https://doi.org/10.1007/s10846-016-0449-6 Lynch, K. M., & Park, F. C. (2017, March 3). Modern Robotics - Mechanics, Planning, and Control. Madier, D. (2021). Practical Finite Element Analysis for Mechanical Engineers (1st ed.). Dominique Madier . Maria, R., & Rodrigues, P. (2022). Motion Planning for Autonomous Sorting Tasks using the Collaborative Saywer Robot. https://hdl.handle.net/1822/87341 Matheson, E., Minto, R., Zampieri, E. G. G., Faccio, M., & Rosati, G. (2019). Human–Robot Collaboration in Manufacturing Applications: A Review. Robotics, 8(4), 100. https://doi.org/10.3390/robotics8040100 Matúšová, M., Bučányová, M., & Hrušková, E. (2019). The future of industry with collaborative robots. MATEC Web of Conferences, 299, 02008. https://doi.org/10.1051/matecconf/201929902008 McAtamney, L., & Corlett, E. N. (1993). RULA: a survey method for the investigation of work-related upper limb disorders. Applied Ergonomics, 24(2), 91–99. https://doi.org/10.1016/0003-6870(93)90080-S mhubii. (2021). lbr_fri_ros2_stack. GitHub. https://github.com/lbrstack/lbr_fri_ros2_stack 131 mhubii. (2024). lbr_ros2_control. LBR-STACK. https://lbrstack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_ros2_control/doc/lbr_ros2_ control.html Moll, M., Sucan, I. A., & Kavraki, L. E. (2015). Benchmarking Motion Planning Algorithms: An Extensible Infrastructure for Analysis and Visualization. IEEE Robotics & Automation Magazine, 22(3), 96–102. https://doi.org/10.1109/MRA.2015.2448276 NECULA, M., CÂMPEAN, E., & MORAR, L. (2022). DEFINING THE CHARACTERISTICS NEEDED FOR THE COBOTS DESIGN USED FOR THE SUPPLY OF CNC MACHINES . Applied Mathematics, Mechanics, and Engineering, 65(Special I), 171–177. Niermann, D., Petzoldt, C., & Freitag, M. (2023). Intuitive and Flexible Process Control for Autonomous Mobile Robots: A Case Study in a Large Logistics Enterprise. Procedia CIRP, 120, 153–158. https://doi.org/10.1016/j.procir.2023.08.028 OnRobot. (2023). DATASHEET 2FGP20. Open Robotics. (2024a). Understanding actions. ROS2 Documentation: Humble. https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/UnderstandingROS2-Actions/Understanding-ROS2-Actions.html Open Robotics. (2024b). Understanding nodes. ROS2 Documentation: Humble. https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/UnderstandingROS2-Nodes/Understanding-ROS2-Nodes.html Open Robotics. (2024c). Using event handlers. ROS2 Documentation: Humble. https://docs.ros.org/en/humble/Tutorials/Intermediate/Launch/Using-EventHandlers.html#using-event-handlers Palmieri, G., & Scoccia, C. (2021). Motion Planning and Control of Redundant Manipulators for Dynamical Obstacle Avoidance. Machines, 9(6), 121. https://doi.org/10.3390/machines9060121 Peixoto, R. T. (2015). Validação de design de armazém automatizado com uso de simulação discreta. PHD Inc. (2001). GRIPPER SELECTION GUIDE. PickNik Robotics. (2024a). Concepts. MoveIt 2 Documentation. https://moveit.picknik.ai/main/doc/concepts/concepts.html PickNik Robotics. (2024b). Move Group C++ Interface. MoveIt 2 Documentation. https://moveit.picknik.ai/humble/index.html 132 PickNik Robotics. (2024c). MoveIt Setup Assistant. MoveIt 2 Documentation. https://moveit.picknik.ai/humble/doc/examples/setup_assistant/setup_assistant_ tutorial.html PickNik Robotics. (2024d). Time Parameterization. MoveIt2 Documentation. https://moveit.picknik.ai/humble/doc/examples/time_parameterization/time_par ameterization_tutorial.html PickNik Robotics. (2024e). Using CHOMP Planner. MoveIt2 Documentation. https://moveit.picknik.ai/humble/doc/how_to_guides/chomp_planner/chomp_pla nner_tutorial.html PickNik Robotics. (2024f). Using OMPL Constrained Planning. MoveIt 2 Documentation. https://moveit.picknik.ai/humble/doc/how_to_guides/using_ompl_constrained_pl anning/ompl_constrained_planning.html Quinlan, S., & Khatib, O. (1993). Elastic bands: connecting path planning and control. [1993] Proceedings IEEE International Conference on Robotics and Automation, 802– 807. https://doi.org/10.1109/ROBOT.1993.291936 Rafael, P., & Ferreira, R. (2022). Solução robótica segura e flexível para alimentação de máquina ferramenta. Rano, I., & Iossifidis, I. (2013). Modelling human arm motion through the attractor dynamics approach. 2013 IEEE International Conference on Robotics and Biomimetics (ROBIO), 2088–2093. https://doi.org/10.1109/ROBIO.2013.6739777 Rastegarpanah, A., Gonzalez, H. C., & Stolkin, R. (2021). Semi-Autonomous Behaviour Tree-Based Framework for Sorting Electric Vehicle Batteries Components. Robotics, 10(2), 82. https://doi.org/10.3390/robotics10020082 Robotics, C. (2023). Robotize your manual warehouse . Robotics, H. (2022). ACR SYSTEMS Power Different Sectors. Roodbergen, K. J., & Vis, I. F. A. (2009). A survey of literature on automated storage and retrieval systems. European Journal of Operational Research, 194(2), 343–362. https://doi.org/10.1016/j.ejor.2008.01.038 ros2.org. (2020, October 9). sensor_msgs/msg/JointState Message. Ros2.Org. https://docs.ros2.org/foxy/api/sensor_msgs/msg/JointState.html Saoji, S., & Rosell, J. (2020). Flexibly configuring task and motion planning problems for mobile manipulators. 2020 25th IEEE International Conference on Emerging 133 Technologies and Factory Automation (ETFA), 1285–1288. https://doi.org/10.1109/ETFA46521.2020.9212086 Serrano, D. (2015). Introduction to ROS-Robot Operating System. NATO Science & Technology Organization. Shimizu, M., Kakuya, H., Yoon, W.-K., Kitagaki, K., & Kosuge, K. (2008). Analytical Inverse Kinematic Computation for 7-DOF Redundant Manipulators With Joint Limits and Its Application to Redundancy Resolution. IEEE Transactions on Robotics, 24(5). https://doi.org/10.1109/TRO.2008.2003266 Sucan, I. (2017). moveit::planning_interface::MoveGroup Class Reference. ROS.Org. https://docs.ros.org/en/jade/api/moveit_ros_planning_interface/html/classmovei t_1_1planning__interface_1_1MoveGroup.html Sucan, I. A., Moll, M., & Kavraki, L. E. (2012). The Open Motion Planning Library. IEEE Robotics & Automation Magazine, 19(4), 72–82. https://doi.org/10.1109/MRA.2012.2205651 Sucan, I., & Kavraki, L. (2008). Kinodynamic Motion Planning by Interior-Exterior Cell Exploration. 449–464. Tavares, P. (2015). Planeamento de trajetórias em Manipuladores em ambientes industriais. Faculdade de Engenharia da Universidade do Porto. Thakar, S., Fang, L., Shah, B., & Gupta, S. (2018). Towards Time-Optimal Trajectory Planning for Pick-and-Transport Operation with a Mobile Manipulator. 2018 IEEE 14th International Conference on Automation Science and Engineering (CASE), 981– 987. https://doi.org/10.1109/COASE.2018.8560446 Thakar, S., Rajendran, P., Kabir, A. M., & Gupta, S. K. (2022). Manipulator Motion Planning for Part Pickup and Transport Operations From a Moving Base. IEEE Transactions on Automation Science and Engineering, 19(1), 191–206. https://doi.org/10.1109/TASE.2020.3020050 The Robotics Back-End. (2018). What is a ROS Node? https://roboticsbackend.com/whatis-a-ros-node/ Toktaş-Palut, P. (2022). Analyzing the effects of Industry 4.0 technologies and coordination on the sustainability of supply chains. Sustainable Production and Consumption, 30, 341–358. https://doi.org/10.1016/j.spc.2021.12.005 134 Trost, P., Kartnig, G., & Eder, M. (2022, September). Simulation study of Autostoresystems. https://doi.org/10.34726/3044 Utz Group. (2014). RAKO utz clever-creative-sustainable-relevant. Vemula, A., Muelling, K., & Oh, J. (2021). Path Planning in Dynamic Environments with Adaptive Dimensionality. Proceedings of the International Symposium on Combinatorial Search, 7(1), 107–115. https://doi.org/10.1609/socs.v7i1.18386 Weidemann, C., Mandischer, N., van Kerkom, F., Corves, B., Hüsing, M., Kraus, T., & Garus, C. (2023). Literature Review on Recent Trends and Perspectives of Collaborative Robotics in Work 4.0. Robotics, 12(3), 84. https://doi.org/10.3390/robotics12030084 Wesley Reis, K., Antônio dos Reis, M., Tourinho Lima, R., & Alves Ferreira Neto, N. (2019). ESTUDO DE DESEMPENHO DE PLANEJADORES DO MOVEIT! PARA UM MANIPULADOR ROBÓTICO DE 6DOF. Xie, B., Zhao, J., & Liu, Y. (2011). Human-like motion planning for robotic arm system. 2011 15th International Conference on Advanced Robotics (ICAR), 88–93. https://doi.org/10.1109/ICAR.2011.6088543 Xing, B., Gao, W.-J., Battle, K., Marwala, T., & Nelwamondo, F. (2010). Simulation and optimization for batch order picking problem: the application of ant colony algorithm. 1661–1672. Yang, S., Liu, P., & Pears, N. (2023). Benchmarking of Robot Arm Motion Planning in Cluttered Environments. 2023 28th International Conference on Automation and Computing (ICAC), 1–6. https://doi.org/10.1109/ICAC57885.2023.10275283 Yang, Y., Merkt, W., Ivan, V., & Vijayakumar, S. (2018). Planning in Time-Configuration Space for Efficient Pick-and-Place in Non-Static Environments with Temporal Constraints. 2018 IEEE-RAS 18th International Conference on Humanoid Robots (Humanoids), 1–9. https://doi.org/10.1109/HUMANOIDS.2018.8624989 Zacharias, F., Schlette, C., Schmidt, F., Borst, C., Rossmann, J., & Hirzinger, G. (2011). Making planned paths look more human-like in humanoid robot manipulation planning. 2011 IEEE International Conference on Robotics and Automation, 1192– 1198. https://doi.org/10.1109/ICRA.2011.5979553 Zucker, M., Ratliff, N., Dragan, A. D., Pivtoraiko, M., Klingensmith, M., Dellin, C. M., Bagnell, J. A., & Srinivasa, S. S. (2013). CHOMP: Covariant Hamiltonian optimization 135 for motion planning. The International Journal of Robotics Research, 32(9–10), 1164–1193. https://doi.org/10.1177/0278364913488805