Development of a robotic system to assist neurosurgeons in minimally invasive stereotactic procedures

Carlos André de Oliveira Faria janeiro de 2020 UMinho | 2020 Development of a robotic system to assist neurosurgeons in minimally invasive stereotactic procedures Universidade do Minho Escola de Engenharia Carlos André de Oliveira Faria Development of a robotic system to assist neurosurgeons in minimally invasive stereotactic procedures janeiro de 2020 Tese de Doutoramento Programa Doutoral em Engenharia Biomédica Trabalho efetuado sob a orientação de Prof. Estela Bicho Prof. Wolfram Erlhagen Carlos André de Oliveira Faria Development of a robotic system to assist neurosurgeons in minimally invasive stereotactic procedures Universidade do Minho Escola de Engenharia 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-NãoComercial-CompartilhaIgual CC BY-NC-SA h tt p s:/ / crea t ive c o mm on s .org / licen s es / by - nc - s a / 4.0/ [Esta licença permite que outros remisturem, adaptem e criem a partir do seu trabalho para fins não comerciais, desde que lhe atribuam a si o devido crédito e que licenciem as novas criações ao abrigo de termos idênticos.] ii Acknowledgments Working towards this thesis was certainly an interesting and challenging journey. A journey I would not be able to undertake without the assistance, guidance and friendship of some amazing people. In first place, express my heartfelt gratitude to my advisors, Prof. Estela Bicho, Prof. Wolfram Erlhagen and Dr. Manuel Rito, for their expertise, guidance and encouragement through rough seas and calm waters. To my colleagues and friends of the MARLab, Tiago Malheiro, Flora Ferreira, Luís Louro, Toni Machado, Gianpaolo Gulletta, Weronika Wojtak, Carolina Vale, João Sepúlveda and many more to mention, I thank you for your counsel, for your friendship, and for all the fun we had through these years. To Prof. Giancarlo Ferrigno and Prof. Elena de Momi I thank you for the opportunity to work at the NearLab next to such brilliant and inspiring people and for the chance to participate in the ACTIVE project. It was an experience I will forever cherish. To Prof. João Vilaça and to Prof. Jaime Fonseca, I am grateful for your knowledge, guidance, and for welcoming me in the AAILab. Without their assistance, this project would not be possible. To the Centro Hospitalar e Universitário de Coimbra, the Hospital da Luz Guimarães, and Hospital di Garda Milano, I am grateful for allowing me to conduct research at your facilities, and for the time and resources provided. Finally and at the top of the list, I thank to my mother, to my father, to my brother and to my dearest Rita, they are the closest to my heart and to whom I owe it all. This work was partially supported by the NETT Project (FP7-PEOPLE-2011-ITN-289146); by the ACTIVE Project (FP7-ICT-2009-6-270460) and by the Foundation for Science and Technology, Portugal (grant number SFRH/BD/86499/2012). 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 Resumo Desenvolvimento de um sistema robótico para auxílio de neurocirurgiões em procedimentos estereotáxicos minimamente invasivos Os robôs revolucionaram a indústria e estão a ser aplicados em aplicações mais complexas e críticas, incluindo cirurgias. O método estereotáxico foi desenvolvido para alcançar estruturas intracranianas sem visualização direta e através de uma abordagem minimamente invasiva, baseada num sistema de coordenadas. Devido ao formato das cirurgias estereotáxicas e funcionais, a adequabilidade de robôs como assistentes aos neurocirurgiões é aparente. Os robôs são agentes precisos, repetíveis, e configuráveis, frequentemente nomeados para tarefas de orientação de ferramentas. Apesar destas vantagens, apenas um número limitado de robôs cirúrgicos chegou ao mercado. Sistemas robóticos cirúrgicos atualmente em prática exibem elevados custos com um lento retorno financeiro, uma limitação causada em parte pela dificuldade em transferir tecnologia dos centros de investigação para as salas de operação. Nesta tese, olhámos para os sistemas robóticos neurocirúrgicos-chave para identificar os denominadores comuns e as características mais procuradas, como substituição da frame estereotáxica, e uma solução robótica acessível, flexível, interactiva, e precisa. Propomos duas soluções para abordar este problema. Primeiro, adaptámos um manipulador industrial de 6 graus-de-liberdade com um sistema de controlo monolítico, como prova de conceito e ferramenta de investigação. A segunda solução consiste numa arquitectura de controlo modular, construída sobre um modelo de componente e comunicação bem estabelecido, com um comportamento determinístico e com suporte para restrições de tempo real críticas, testado num manipulador de 7 graus de liberdade. No cerne da arquitectura modular está um método analítico de cinemática inversa para manipuladores redundantes capaz de resolver unicamente o espaço nulo, evitar singularidades, e limites de juntas. Este método oferece ao manipulador a flexibilidade necessária para se adaptar ao espaço de trabalho disponível que é partilhado com a equipa cirúrgica e com outros equipamentos. A arquitectura proposta inclui uma lista de componentes de controlo e supervisão interconectados para executar as diferentes tarefas cirúrgicas. Desenvolvemos e validámos um novo algoritmo para determinar os parâmetros reais do modelo cinemático de manipuladores. Testámos a precisão de aplicação do sistema in vitro e registámos um erro médio de 0.567 mm e um erro máximo de 1.094 mm. A vantagem da solução proposta é a sua performance, flexibilidade em integrar novos módulos, e na capacidade de melhorar componentes sem ter de renovar a solução por completo, ou escalar os custos. Palavras-chave: Cirurgia Estereotáxica, Controlo de Alto-nível, Robótica. v Abstract Development of a robotic system to assist neurosurgeons in minimally invasive stereotactic procedures Technology is often the answer to most of our problems. Robots revolutionized the industry and are finding their way into more complex and critical applications, including surgery. The stereotactic method was developed to reach intracranial brain structures without direct visualization and through a minimally invasive approach, based on a coordinate system. Due to the format of stereotactic and functional surgeries, the suitability of robots as a neurosurgeon’s assistants is apparent. Robots are accurate, repeatable, and configurable agents often assigned to tool guidance. Despite these advantages, only a handful of surgical robots reached the market. Current surgical robotic systems in practice exhibit high up-front costs with slow investment return, a limitation in part caused by the difficulty in transferring technologies from the research center into the surgical room. In this thesis, we examine the principal neurosurgical robotic systems to identify the common denominators and the most requested features, replacement of the stereotactic frame, affordable access to flexible, interactive, and accurate robotic solutions. We propose two solutions to address these subjects. First, we adapted an industrial 6 DoF manipulator with a monolithic control system, as a proof-of-concept and research tool. The second solution consists of a modular control architecture, built on top of a well-established component and communication model, with deterministic behavior and supporting hard-real-time constraints, tested in a 7 DoF manipulator. At the core of the modular architecture, is a novel analytic inverse kinematics method for serial redundant manipulators capable of uniquely solving the nullspace, avoiding singularities, and joint limits. This method provides the manipulator with the flexibility to adapt to the available workspace, which is shared by the surgery team and by other equipment. The proposed architecture also includes a list of interconnected controllers and supervisor components to perform the different surgical tasks. We also developed and validated a new algorithm to determine the real kinematic model parameters of manipulators, parameters that may differ from nominal values. We tested the system’s application accuracy in vitro and registered a mean error of 0.567 mm, and a maximum error of 1.094 mm. The real advantage of the proposed solution lies in its performance, flexibility to integrate new modules, and in the capacity to upgrade individual components without the need to overhaul the entire solution, or escalate the costs. Keywords: High-level Control Systems, Robotics, Stereotactic Surgery. vi Contents Acknowledgments...................................... iii Resumo .......................................... v Abstract .......................................... vi ListofFigures........................................ xv ListofTables ........................................ xvi Nomenclature........................................ xvi 1 Introduction 1 1.1 Contributions..................................... 2 1.2 Outline........................................ 3 2 Stereotactic and Functional Neurosurgery 5 2.1 Stereotactic Imaging and Technology . . . . . . . . . . . . . . . . . . . . . . . . . 5 2.1.1 Stereotactic Imaging . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 2.1.2 Stereotactic Technology . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 2.1.3 Frameless Technique . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 2.2 StereotacticProcedures................................ 11 2.2.1 Deep Brain Stimulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 2.2.2 Stereo-Electroencephalography . . . . . . . . . . . . . . . . . . . . . . . . 14 2.2.3 OtherProcedures .............................. 17 3 Robotic Neurosurgery 21 3.1 Robotic assistant: neurosurgeons’ expectations and demands . . . . . . . . . . . . . 21 3.2 State-of-the-art robotic systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 3.2.1 Specifically for Stereotactic Neurosurgery . . . . . . . . . . . . . . . . . . . 25 3.2.2 General and capable of Stereotactic Neurosurgery . . . . . . . . . . . . . . . 34 3.3 Current perspectives and Future directions . . . . . . . . . . . . . . . . . . . . . . . 38 4 Industrial Manipulator Adaptation 41 4.1 RoboticSystemRole ................................. 41 4.2 Setupdescription................................... 42 4.2.1 MotomanMH5................................ 43 vii 6.29RobotStatecomponent.................................125 6.30 Transformations component. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125 6.31 Task Supervisor component. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 6.32 Decision process to guarantee a feasible trajectory. . . . . . . . . . . . . . . . . . . 129 6.33 Set of poses that align the end-effector frame ( {E} ) z-axis with the vector created from the two input points ( p1 and p2 ). The number of redundant poses n depend on the sampling step ( κ ). For clarity, only the zand x-axis of the test poses are represented in thepicture.......................................130 6.34 Task Supervisor motion state machine. . . . . . . . . . . . . . . . . . . . . . . . . 132 6.35RobotFRIcomponent. ................................134 6.36 Representation of the last joint position (x0) and the 4 preceding values. . . . . . . . . 134 6.37V-REPFRIcomponent. ................................135 6.38 Registration component. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136 6.39 Surgery Plan component. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137 6.40 Procedure Coordinator component. . . . . . . . . . . . . . . . . . . . . . . . . . . 138 6.41 Directcontrolwindow..................................140 6.42Registrationwindow. .................................141 6.43Surgeryplan......................................141 6.44Procedurewindow. ..................................142 7.1 Example of acquired point ( p ) contained in the plane defined by the last joint axis ( nn ) and the position of the flange. The acquired point must not be contained in the line that passes through the last joint axis, i.e. p6=p∅. ....................145 7.2 The relationship between two lines in space can be parameterized by the shortest distance between lines d (dual part), and the projected angle between the lines γ (real part) (Ketchel and Larochelle, 2006). The common normal is, ˆe =kn1×n2k . The distance between lines is, d= (c2−c1)·e . The angle between lines can be determined from, γ= atan2 (kn1×n2k,n1·n2)...........................149 7.3 The line relationship categorization process. . . . . . . . . . . . . . . . . . . . . . . 151 7.4 Example of the robot motion that generates the tracked point trajectory for the second joint. The thicker lines represent the position of the tracked point acquired relative to the robot base frame. The thinner lines represent the best fitting circle to the trajectories. . . 155 7.5 Acquired point trajectories. For each revolute joint, the motion axis is represented at the centerofrotation....................................156 7.6 Phantomboxconcept. ................................160 7.7 AssembledPhantomBox................................160 7.8 Pre-imaging procedure of the phantom. . . . . . . . . . . . . . . . . . . . . . . . . 161 7.9 Interlockpoint. ....................................161 xiv 7.10 Phantom meshes after registration with the interlock calibration (red) and the ICP (blue). The CT phantom point cloud is represented by gray-colored dots and the model point cloud is either represented by red-colored dots for the interlock calibration, and blue-colored dots for the ICP calibration. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163 7.11 Model stalks locally registered to the stalks in the CT dataset. Model stalks’ outline is represented by the blue dots while the real stalks from the CT image are represented by agrayshape/dots. ..................................164 7.12 Marker placement equipment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164 7.13 Marker placement setup and example. . . . . . . . . . . . . . . . . . . . . . . . . . 165 7.14 Post marker placement CT image scan of the phantom. . . . . . . . . . . . . . . . . 165 7.15 System accuracy results. The model stalks (blue outline) are registered to the image stalks (gray dots) and the target, 5 mm above each stalk tip, is represented by the red color while the center position of each marker is identified by a green dot and a black outline.........................................166 8.1 Schematic representation of an Extended Modular Architecture. . . . . . . . . . . . . 168 8.2 Stereo vision system setup. Stereo camera fixed on a bracket, connected to a computer viaUSB2.0. .....................................170 8.8 Static Analysis - Detection error (translation and rotation component) of an immobile objectforeachtestset.................................181 8.9 Dynamic Analysis - Tracking error of a moving object across all test sets. Low, medium and high linear and angular speeds depicted in left and right figures. Statistically significant difference of median translation error among all angular speeds. Statistically significant difference of median rotation error among all angular speeds, except between low and mediumangularspeeds. ...............................181 8.10 Time and Frequency analysis of brain motion in the 3 acquisitions, 25, 33 and 58 seconds. Left plots depict the displacement of the cloud of features’ centre of mass throughout the acquisition. Continuous component of the signal was subtracted. Right plots represent the Fast fourier transform (FFT) analysis of the displacement of the cloud of features’ centerofmass.....................................182 8.11 Force Dimension Omega Driver component diagram. . . . . . . . . . . . . . . . . . . 185 8.12 Representation of the OmegaOrientation variable. . . . . . . . . . . . . . . . . . . . 186 8.13 Haptic Control Modes component diagram. . . . . . . . . . . . . . . . . . . . . . . 187 xv List of Tables 3.1 Most successful robotic systems and projects directed at stereotactic neurosurgery. . . . 24 4.1 Yaskawa Motoman MH5 robotic arm specifications. . . . . . . . . . . . . . . . . . . 43 4.2 Target point ( pt ) Cartesian coordinates from ZD coordinates in each of the frame mounting positions........................................ 49 5.1 KUKA LBR iiwa / Med 7 R800 robotic arm specifications . . . . . . . . . . . . . . . . 69 6.1 Classic Denavit-Hartenberg parameters of a 7-DoF anthropomorphic manipulator. . . . . 93 6.2 Descriptive statistics of computational times recorded of different parts of the algorithm, measured from 10000 samples, in microseconds (µs). ................109 7.1 Initial, start and end joint coordinates used during the trajectory execution. For readibility, the joint positions are displayed in degrees, except for the Cobra’s θ3 that is displayed in millimeters.......................................157 7.2 Denavit-Hartenberg parameters for the studied robots. The measured Denavit-Hartenberg parameters ( ameas,i , αmeas,i , dmeas,i and θmeas,i ) and the differences to the nominal values ( aerror,i , αerror,i , derror,i and θerror,i ) for each joint are presented. The a and d parameters are represented in millimeters, the αand θin degrees. Values truncated at 1e−9. . . 158 8.1 TestPerformed.....................................179 8.2 Registration residual errors. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 180 xvi Chapter 1 Introduction The new technology introduced to the robotic surgical field should conform to the three iron pillars of the health-care system: quality, cost-effectiveness, and accessibility. The constant search for new and superior medical technology is primarily driven for the sake of improving health-care quality. The quality principle can be objectively measured by the precision of the tool placement, the incidence of surgical and long-term complications, duration of operative times and so on. Most published reports on research systems and commercially available products tend to present their results in terms of accuracy, effectiveness, and safety against the traditional approach or other homologous systems. On the other hand, cost-effectiveness and accessibility fall into a secondary objective, which is evident through the lack of studies that assess the immediate and/or long-term cost-effectiveness of the advertised technology. A recent report on a retrospective cohort study involving 416 hospitals and 24000 patients who underwent radical nephrectomy between 2003 and 2015, revealed a trending increase on the use of robotic-assistive technology with higher hospital costs averaging more $2700 per patient (Jeong et al., 2017). Fiani et al. evaluated the impact of robot-assisted neurosurgery on the quality of the health-care service and the immediate economic burden using as an example the MAZOR SpineAssist robotic system (Fiani et al., 2018). Considered one of the most affordable spinal surgery systems approved by FDA, it still has setup costs that amount to $550,000 including an annual maintenance service that costs about 10% of the list price, consumables, and implants that amount to $1500 per case. One can argue that the faster operating times, reduced number of intra/postoperative complications or the decrease in technical support and maintenance of operative materials may result in long-term savings for the medical center. The reality is that surgical robotic systems have considerably high up-front costs for the medical institution with no immediate investment return, which makes it only available to an exclusive list of first world health-care providers. Two possible strategies to cope with this problem involve: 1. simplification and acceleration of the surgical robot development process; 2. endowing the surgical robot with a versatile control system. It naturally follows that new technology draws more diversity to the robotics market, which eventually leads to more accessible robotic solutions. Additionally, systems equipped with the right tools can be reassigned 1 to multiple procedures, thus expanding its potential and balancing the cost-efficiency scale. It is also true that transferring surgical devices into the operating room represents a difficult enterprise (section 3.3). Since the first surgical robot, three decades ago, many prototypes have been built, several validated but only a few are currently applied in real surgical scenarios. Sanchez et al. (2014) list 3 factors that contribute to this reality: • limited access to research and operation on living organisms; • difficulty to start a business in today’s medical industry; • lack of adequate design methodologies to facilitate the technological transfer, specifically in robots developed in research centers. Other not so obvious deterrent is the limited access to previous technology as a result of either: commercial products whose information is not disclosed, or projects conducted in private repositories by research centers. Finally and to preface the rest of the document, it worth noting that the theme and motivation for this project materialized from a direct interaction with the end-users, the Neurosurgery team of the Centro Hospitalar e Universitário de Coimbra. 1.1 Contributions Since the inception of this project, we soon realized the multi-disciplinarity, complexity, and human/financial resources required for the development of a complete robotic system towards robotic neurosurgery. This thesis will focus on the process and development of an high-level control architecture for robotic systems for neurosurgical applications. Our main goal is precisely the development of a modular, distributable and flexible control system to facilitate the technology transfer of neurosurgical robotic systems. The thesis main contributions are: 1. Identification and comparison between the main imaging and tool guidance technology applied to stereotactic neurosurgery; 2. Revision and comparison of the key robotic systems for neurosurgical applications, detailing the features common among these systems and putting them in line with user expectations (Faria et al., 2015a); 3. Adaptation of an industrial manipulator and implementation of a control system to operate as a tool guidance device and directly replace the stereotactic frame (Faria et al., 2016); 4. Design and implementation of a modular high-level control architecture for neurosurgical robots with hard-real-time support, deterministic behavior and well-defined component and communication model; 5. Creation of a virtual scenario and virtual robot as a replica of a neurosurgical operating room with a real robotic system that is controlled by the same high-level control architecture; 6. Development of an analytic inverse kinematics method for redundant manipulators capable of avoiding joint limits, singularities and uniquely solving the nullspace (Faria et al., 2018); 2 7. Development of a kinematic calibration method to determine the real Denavit-Hartenberg parameters of a serial manipulator model with variable kinematic structures and joint types/dispositions (Faria et al., 2019); 8. Development of an in vitro method to assess the system’s application accuracy based on a phantom with several distributed targets to mimicking the human brain; 9. Development of two extension modules to the proposed high-level control architecture, to feature teleoperation and to include a stereo-vision system capable of tracking and quantifying the brain surface movement in open-skull neurosurgery (Faria et al., 2014). The source code repository with the material developed in the context of this thesis can be found at https://github.com/neuebot/NeueBot. 1.2 Outline This thesis is organized in 9 chapters. In this section, we briefly elaborate on the structure of the document and outline the key points of each chapter. In chapter 2 we introduce the topic of Stereotactic and Functional Neurosurgery. As the surgical application to the robotic system in development, it is fundamental to describe not only the technology involved but also the methodologies, benefits, and limitations. It is listed various imaging modalities applied to stereotactic and functional neurosurgeries, as well as the different stereotactic frames historically and currently used. The chapter finishes with a description of several neurological ailments that are indicated for stereotactic neurosurgery, relating them to procedural variations. Although most of the technology applied to stereotactic neurosurgery is listed in chapter 2, the category of neurosurgical robots deserved its chapter, chapter 3. This chapter serves to introduce the theme of robotic systems for neurosurgical tasks, the neurosurgeons’ expectations and demands, current perspectives and future directions. At the core of the chapter, we enumerate the key players in surgical robots for neurosurgical applications, going into detail about their main features and how they can be perceived as advantages or disadvantages. To limit the scope of the listed systems, we chose to include systems that either obtained clearance for clinical trials or are commercially available. After the introductory chapters, in chapter 4 we elaborate on a simple robotic system solution to operate as a direct replacement to the stereotactic frame. A 6-DoF industrial robotic manipulator with suitable repeatability, reach and compactness is adapted to perform a surgical procedure. It is described how the different system modules were implemented to directly handle the surgical coordinates and the end-effectors designed and fabricated to interface with the surgical instrumentation. In chapters 5, 6, 7 and 8 we present and detail on a modular high-level control architecture, an alternative to the monolithic solution from chapter 4. Chapter 5 starts by describing the differences between both approaches and how they affect system development, technology transfer and ultimately the cost and accessibility of surgical robots. The concepts of Component-Based Software Engineering are explored together with a list of frameworks and libraries to implement modular control software for 3 robotic applications. These ideas crystallize as system design decisions and the architecture layout. This chapter also contains the real and virtual robotic setup descriptions based on a 7-DoF anthropomorphic manipulator. Finally, we converge on the task description, which specifies the robotic system role throughout the procedure. In chapter 6 we go into detail about the system software components that amount to the proposed high-level control architecture. The control architecture divides into two layers, the Core and the Application Layer. The Core components include controllers for the robot motion in task and joint space, as well as new kinematic solving module specially design for safe and deterministic operation of redundant manipulators. The Application components bridge the user-interface with the core layer to provide the expected functionality for the neurosurgical task. The setup introduced in chapter 5 and detailed in chapter 6 is calibrated and tested in chapter 7. This chapter is divided into two parts, the first-named ‘Kinematic Convention and Calibration’ where it is proposed and tested a Denavit-Hartenberg parameter detection method to determine the real robot parameters and thus improve its precision. The method was proposed and tested in 6 serial manipulators with distinct kinematic dispositions. The second part of the chapter describes a method to assess the robotic system’s application accuracy. It relies on a cost-effective phantom device with several targets and stereotactic instrumentation to drive small markers to the target location. The precision is measured with a CT imaging scan as it provides better spatial resolution and correctness. One of the advantages of a modular architecture is the possibility to branch out the system to alternative applications. Two extensions to the high-level controller are proposed in chapter 8, one module to track the brain surface is presented as a system extrapolation for open-skull surgeries; and a teleoperation module was implemented and tested for a potential application in kidney stone removal surgery. 4 Chapter 2 Stereotactic and Functional Neurosurgery The surgical act on itself is one of the most complex tasks performed by humans with the objective of intrusively diagnosing, treating a pathological condition, or improving any bodily function. In particular, the field of neurosurgery has been historically considered an observation realm and labeled “primum non noncere”, as the surgical outcome often deteriorated the patient condition even further. The current state of neurosurgery is a culmination of centuries of advances in a wide range of science and medicine areas ranging from anesthesia, antisepsis, hemostasis, instrumentation and more recently of imaging technology (Ormond and Hadjipanayis, 2014). This thesis will focus on a rather contemporary sub-group of general neurosurgery called ‘Stereotactic and Functional Neurosurgery’. This chapter in particular aims to introduce the core concepts involved with this surgical procedure. 2.1 Stereotactic Imaging and Technology The stereotactic method was first described by Robert Clarke and Victor Horsley in 1906, as a system to define the brain volume in Cartesian coordinates and to access subcortical structures using a 3D positioning device. Lacking the imaging technology to identify intracranial structures, their system did not see much use beyond in vitro animal testing (Horsley and Clarke, 1908). It was not before 1947 that Spiegel and Wycis noted the applicability of stereotaxy in subcortical functional neurosurgery procedures. They pioneered the development of methods to determine stereotactic coordinates based on anatomical landmarks (Spiegel et al., 1947). These findings were mostly based on imaging techniques such as ventriculography (Dandy, 1918) and angiography (Moniz et al., 1931). In 1953, Talairach reported the stereotactic angiography, the first imaging technique to allow direct visualization of intracranial lesions (Talairach et al., 1954). Additionally, and to cope with the limited applicability of contemporary visualization technology Talairach and Tournoux developed a consistent and reliable method to indirectly locate anatomic cerebral structures. Their system relies on easily detectable anatomic references, the intercommisural line that passes through the superior edge of the Anterior Commissure (AC) and the inferior edge of the Posterior Commissure (PC). The AC-PC line establishes the foundation for the proportional grid system, an orthogonal system of lines parallel 5 and perpendicular to the AC-PC that divides the brain in parallelograms, Figure 2.1. It takes into account the skull size and is based on a landmark with definite relationship to gray central nuclei and sufficiently precise relationship to the telencephalon, the AC-PC line (Talairach and Tournoux, 1988; Benabid et al., 2009). The Talairach coordinate system is one of the first and certainly the first widely accepted Atlas to describe the neural architecture of the human brain, being particularly useful to locate structures not visible by traditional radiologic methods. Figure 2.1: Talairach proportional grid system based on the AC-PC line. Other structures located using grid parcellation: SS, sylvian sulcus; STS, superior temporal sulcus; LFS, lower frontal sulcus; RS, rolandic sulcus; CS, calcarine sulcus; MF, motor fibers; PT, pyramidal tract; upper diagram, lateral; lower diagram, anteroposterior (reprint copyright license agreement with Springer Nature). Thenceforth, the field of Stereotactic Neurosurgery has witnessed continuous expansion, providing remarkable nonor minimally-invasive procedures to accurately localize and treat lesions or abnormal tissue. 2.1.1 Stereotactic Imaging The developments on stereotaxy have been spearheaded by the advent and development of imaging technology. As of today, the stereotactic and functional surgery planning relies on a wide spectrum of imaging exams. The most common modalities are Computed Tomography (CT) and variations of Magnetic Resonance Imaging (MRI, MRA, MRV, fMRI, etc). Other imaging sources such as Positron Emission Tomography (PET), Single Photon Emission Computed Tomography (SPECT) and ultrasound are also employed (Gorgulho et al., 2009). Computed Tomography is a radiological imaging exam that uses X-rays to acquire axial slices of a defined volume. CT scans are relative fast, when compared to MRI for example and enjoy high accuracy and low distortion. On the other hand, the exam exposes the patient to a significant dose of radiation, and the brain structures cannot be differentiated due to the low contrast of brain tissue (similar Hounsfield units). Although some lesions are visible in pre-operative CT, for the most part, CT-guided stereotaxy is predicated on a brain atlas and the relation to ventricular landmarks, such as the AC-PC line (Hariz and 6 Zrinzo, 2009; Schulder and Oubré, 2009). Anatomical and probabilistic functional atlases have been historically used to support stereotactic planning, whenever direct visualization of target structures is not possible or when the exam introduces significant distortions. Parallel to other advances in stereotactic technology, also the brain atlas has been evolving to better predict anatomical variability, deliver a high-resolution volumetric model that builds on functional population-based atlases (Nowinski, 2009). Magnetic Resonance Imaging uses strong electromagnetic fields to polarize matter at an atomic level. The scanned material is differentiated by determining the rate at which the atoms return to the equilibrium state. MRI provides high discrimination of brain structures, which allow the images to be used as the patient’s personal brain atlas. Moreover, by modifying the properties of the polarizing signal - MRI sequences (T1-, T2-weighted, FLAIR, proton density, etc) - the radiologist may enhance specific tissues or structures in detriment of others. It is recurrent practice to use radiocontrast agents to enhance the contrast between brain tissues both in MRI (gadolinium contrast) as well as in CT (iodine-based contrast) studies. On the negative side, MRI imaging is susceptible to distortions caused by external electromagnetic fields, which provoke geometric inaccuracies (Hariz and Zrinzo, 2009; Schulder and Oubré, 2009). The surgical planning of today’s image-guided neurosurgery often relies on multimodality imaging studies, harnessing the advantages of each technique in order to accurately reach the target structures and avoid surrounding vasculature or eloquent brain regions. Combining different radiological datasets involves a method called “image fusion”. Each image set has its own 3D coordinate system that needs to be registered into another to create a coherent output. In order to match a set of homologous points in two reference frames, one needs common references, such as: anatomical references/surfaces/volumes, skin fiducial markers or bone implanted markers. These markers must be simultaneously visible in MRI/CT imaging and during the actual procedure in the perioperative space by a localizer system (Hartov and Roberts, 2009). Not delving in exhaustive details, one can split the registration methods into: • Point Based Co-registration, a set of reference points are located in both spaces, and an algorithm based on singular value decomposition (SVD) or a variant of iterative closes point (ICP) is employed to match point-pairs. This simple and robust strategy, is perhaps the most widely used approach. • Surface Based Co-registration, per-definition relies on capturing, representing and matching surfaces. Anatomical surfaces are digitized using contact sweeps, laser scanners or stereoscopy, into a set of points that form a mesh. The transformation between the two surfaces is computed using an ICP algorithm. • Volume Based Co-registration, is a method to calculate the spatial transformation between volumetric datasets based on similarity and mutual information. The transformation results from an optimization of the geometric relationship between neighbor image elements. These relationships are predicated in physiological assumptions. 7 In the planning software, the medical team selects the target and entry points of the electrode insertion trajectory, avoiding vessels and ventricles. Based on the selected trajectories, the planning software computes the stereotactic coordinates for each electrode. A phantom device is used in the operating room to visually confirm the stereotactic coordinates (Figure 2.3b). The phantom is attached to a stereotactic ring (similar to the one fixated on the patient’s skull) and simulates the target point to be reached by the electrode. The stereotactic frame is mounted onto the phantom’s stereotactic ring and is adjusted to the desired coordinates. A stylet is then placed in the stereotactic frame guide and if the stylet tip and the phantom tip are coincident the computed coordinates are confirmed. The frame is subsequently removed from the phantom, placed in the patient’s stereotactic ring and the stylet is used to mark the scalp entry point. Then the frame is moved aside to make the scalp incision and to drill the burr hole in the skull to access the brain (Figure 2.3c-2.3d). Afterward, the frame is adjusted once again in order to advance the electrodes/cannulas to the defined depth (Figure 2.3e). Multi micro-electrodes are used to map the sensorimotor region by recording the neuroelectrical activity near the planned target. Initially, these electrodes are positioned along the planned trajectory with the aid of guiding cannulas, 10 mm to 15 mm before the target. After this, they are iteratively lowered – millimeter by millimeter – until they are positioned 5 mm from the target and then half a millimeter between iterations, recording the neuroelectrical signals during each step. In the end, the data recorded are analyzed to select the closest location to the sensorimotor region within the nucleus (Figure 2.3e). The same recording micro-electrodes have a macro-stimulation lead, which is used to stimulate the previously located sensorimotor region. Following an iterative approach once more, the current and the depth of leads is increased (Figure 2.3e-2.3f). During each step, the team of neurologists qualitatively evaluates the patient’s symptoms, seeking the best response and verifying side effects. After finding the ideal electrode placement and stimulation signal properties, the micro-electrodes are replaced with a definitive quadripolar macroelectrode. Intra-operative imaging is used to check if the macro-electrode placement coincides with the micro-electrode position. The macro-electrode is later connected to an implanted pulse generator or neuropacemaker. If bilateral brain stimulation is required, all the intra-operative processes must be repeated for the other side. Due to the long duration of the procedure, the neurosurgeon may choose to implant the implanted pulse generator afterward or in the following day. 2.2.2 Stereo-Electroencephalography Back in 1953, Talairach and Bancaud began a series of studies based on implanted depth electrodes in human patients. Advances in the stereotactic radiologic visualization of a number of brain structures permitted the neurosurgeon to establish a spatial coordinate system to map the brain, the stereotactic atlases. These atlases would gradually cover more telencephalic structures and concurrently new methods to reach these areas would be developed. Two decades of uninterrupted expansion culminated in a new surgical exploration methodology to investigate epilepsy, reported in 1973 by Talairach and Bancaud, the Stereo-electroencephalography (SEEG) (Talairach et al., 1962; Talairach and Bancaud, 1973). 14 Surgical treatment of refractory epilepsy presented indisputable evidence of success. The surgical procedure is to resect or to control-damage the epileptogenic zone (EZ). The success of surgical treatment is thus directly dependent on a clear segmentation of the EZ as well as the delimitation of the nearby eloquent brain regions, e.g. primary motor, sensory, speech, visual cortex, etc. The study of ictal symptomatology, the radiologic image data, and non-invasive electroclinical studies may present non-conclusive evidence of the EZ nor discard the proximity to functional areas. The SEEG method relies on the introduction of several multi-contact “depth” electrodes to record the disturbed electrical activity of the brain in the vicinity of the supposed EZ. These electrodes provide a time accurate high-resolution information regarding the focus and propagation of the seizure onset. The methodology proposed by Talairach and Bancaud relied on angiography and X-ray images. To guarantee spatial coherency and correctness, images had to be calibrated to eliminate enlargement, displacing or distorting artifacts. This proved to be a complex and time-consuming process. Besides, the reported Tailarach stereotactic frame used to position the electrodes was limited to lateral orthogonal trajectories, which limits the implantation site workspace. The emergence and advancement of new imaging techniques such as CT and MRI proved to be a major improvement in the surgery planning phase, by simplifying and shortening the process with inherent greater spatial resolution and accuracy. In parallel, new stereotactic frames designs, frameless methods, and even robotic actuated implantation have pushed the procedure’s precision and repeatability to its excellence standard, reduced the setup time and the discomfort to the patient, among other advantages (Kratimenos et al., 1993; Guenot et al., 2001; Ortler et al., 2011; Cardinale et al., 2013; Dorfer et al., 2014; Munyon et al., 2015). Even though the core principles behind the SEEG procedure are standard, different medical centers slightly differ in their approach in terms of imaging techniques, implantation setup or execution. To keep the document consistent with the previous Deep Brain Stimulation subsection 2.2.1, we will describe the SEEG surgical workflow as performed in Coimbra Hospital and University Center. Once indicated for surgery, the patient first undergoes a gadolinium-enhanced MRI examination, including volumetric T1-weighted and volumetric T2-weighted fluid-attenuated inversion recovery (FLAIR) sequences for targeting. The collected image data sets are used in the surgery planning step to establish the electrode trajectories, which should reach around and beyond the putative epileptogenic zone while avoiding any considerable vases or important brain structures. In the surgery day, a volumetric and contrast-enhanced CT scan is acquired of the patient’s head, with the stereotactic reference ring and fiducial localization plates attached. Both MR And CT image data sets are registered and the coordinates of the planned electrode trajectories are confirmed or adjusted, Figure 2.4a. Aside from the typical entry and target point, other metrics are recorded for each electrode path such as: bone thickness, distance between the inner skull to the cortical surface and distance from the cortical surface to the target. Given the usually large number of electrodes implanted, the surgeons usually opt for checking the coordinates of the first trajectory with a phantom device (similar to the DBS procedure Figure 2.3b). The stereotactic frame with its guide is now attached to the reference ring fixed to the patient’s head. 15 (a) Pre-operative trajectory planning with stereotactic CT registered with pre-operative MRI. (b) Skin and skull piercing step. Drill depth controlled by a stopper mechanism. (c) Dura mater and capillary vases coagulation. (d) Attachment of the screw port. (e) Stylet introduced to open the electrode path. (f) Electrode depth controlled implantation and fixation. Figure 2.4: Steps in the procedure for Stereo-Electroencephalography. The stopper at the guide is adjusted according to the bone thickness in the path of the selected electrode trajectory, to prevent the drill piece from damaging the cortical surface. A sleeve that fits the thin drill is introduced in the stereotactic guide, and both the skin and skull are pierced, Figure 2.4b. The drill is withdrawn and a monopolar coagulator is passed through the guide to perforate the dura mater and coagulate surrounding capillary vases, Figure 2.4c. Then, a 15 to 35 mm screw is fixed in the 16 drilled skull hole. It will work as an access port to the electrode, Figure 2.4d. Then, the stopper is adjusted to match the distance between the entry point and the “deep” target point, calculated as such: to the screw length, 5 mm are subtracted, corresponding to the screw thread that enters the skull, added the skull thickness, plus the distance between inner skull to the cortical surface, and from that to the deep-seated target. A stylet is inserted through the screw port to clear a path within the brain tissue for the electrode, Figure 2.4e. The stylet is then replaced by a semi-flexible electrode, which itself has a washer acting as a stopper mechanism to prevent going beyond the target point. The washer in each electrode is length adjusted previously to implantation. The electrode has a screwed cap that fits the screw port, sealing and locking the electrode in place, Figure 2.4f. The process is thoroughly repeated for each indexed electrode. The choice of completing the electrode insertion process for each iteration, instead of drilling the holes sequentially, is part of a thoughtful strategy to limit the leakage of cerebrospinal fluid as well as the entrance of air in the skull cavity. By doing so, it minimizes the brain shift phenom, which is further constrained by the “anchor effect” of the introduced electrodes. After implantation, the electrodes are connected to an electroencephalogram which records the brain signals for a period lasting between a couple of days to several weeks. The signals from electrode contacts are monitored to establish the location of the epileptogenic focus, based on the irritative zone (interictal spikes and high-frequency oscillations) and the seizure onset zone (electric activity at ictal onset and ictal baseline shift). Before the electrodes removal, the neurosurgeon applies a stimulating signal, to the vicinity of the delimited region, to map cortical and subcortical eloquent brain areas. Bipolar coagulation may be applied if the electrode contacts register marked epileptic activity, and prior stimulation supports the remoteness of eloquent brain areas. 2.2.3 Other Procedures The applicability of the stereotactic technique extends well beyond the domain of functional electrode implantation. In order to keep this introductory chapter concise, instead of mentioning each other surgical procedure, we chose to partition them into categories: biopsy, destructive and resection surgery. The goal of this subsection is to generally describe the concepts and goals of these operations, with an emphasis on the surgical process. To keep a coherent line of thought, we narrowed the scope to minimally invasive brain procedures. Stereotactic Biopsy is the least invasive surgical method for tissue sampling known. It can be performed under local or shallow anesthesia, suited for patients with poor medical status and not amenable for general anesthesia such as infants or people of advanced age. While neuroimaging studies with CT or MRI indicate the presence or not of lesions, precise histological diagnosis is crucial for selecting the best treatment of intracranial masses and infiltrating lesions. The minimally invasive nature of the procedure yields its low morbidity and mortality rates. Stereotactic brain biopsy is considered a relatively safe surgery prescribed to patients with: multiple, poorly defined, hard to access or radiosensitive lesions (Elder et al., 17 2009). The patient initially undergoes an imaging examination preferably a combination of CT and MRI for high tissue contrast with low spatial distortion. The lesions are located in relation to the established stereotactic frame of reference, and the instrumentation guidance trajectory selected to avoid, whenever possible, sulcus, cortical arterial or venous structures. Likewise to other stereotactic procedures, the patient’s head is shaved and sterilized, the stereotactic frame attached and sterile drapes applied. Usually, local anesthesia is administered, followed by the scalp incision and skull drilling. The dura is punctured with a blunt tip probe, and then, the biopsy needle advances to the stereotactic target. Intra-operative imaging can be performed to confirm target location (Field et al., 2001; McGirt et al., 2005). Stereotactic Resection and Evacuation Surgery encompasses procedures that involve access to a volume defined region of the brain for lesion removal or drainage, i.e. resections (Harris et al., 2005; Kelly, 1992) and abscess/hematoma evacuation (Brouwer et al., 2014). These procedures are prescribed for an ample variety of neurological pathologies: tumors, cysts, hematomas, stem lesions, etc. Volumetric stereotaxy differs from the point-in-space surgeries mostly during the planning phase. The surgery plan of the later can be summed by entry, target and a few more point coordinates, volumetric stereotaxy depends heavily on information such as the volume and shape of the lesion (Kelly, 1998). Previous to the surgery a volumetric imageology study is conducted and information such as evacuation volume or lesion boundaries are extrapolated. The surgical approach is decided upon the histology and anatomical location of the lesion. As for any other stereotactic procedure, a head fixating device is attached to the patient’s head that remains immobile or calibrated to the instrumentation guiding apparatus. The scalp is prepared, incised and the craniotomy performed. The size of the brain access hole depends on the procedure and on the instrumentation used. • In resection or excision surgeries, a wider access is required to remove the sectioned tissue or to pass instrumentation. Naturally, in these surgeries, the size of the burr hole is proportional to the dimensions of the tissue to remove. In case of, stereotactic endoscopy removal (e.g. intra-ventricular colloidal cyst), the burr hole is dimensioned according to the conduit introduced, which creates the channel to pass the endoscope and micro-instrumentation required for section and cauterization. • In abscess/hematoma evacuation, a small incision suffices to pass the fine tube that perforates the lesion walls and aspirates its contents. In some cases, an ultrasonic disintegration device is employed to facilitate the liquidification and suction of the contents. Depending on the procedure and resources of the surgical center, a tracking device is used to verify instrumentation placement in respect to the surgical field, and/or an imaging device is used to confirm the effectiveness of the treatment (volume resected/evacuated). Volumetric stereotaxis has several advantages when compared to the open brain approach: i) facilitates lesion location by the surgeon, ii) imparts a three-dimensional volume and shape of the lesion to address, iii) it enables a minimally invasive procedure with respect to the configuration of the lesion, normal brain, 18 and vascular anatomy, and finally iv) it indicates where the lesion ends and normal tissue begins by means of a real-time display, interactive software, and stereotactic instrument tracking. Stereotactic Destructive Surgery aims to achieve selective lesioning, inactivation or modulation of abnormal brain tissue. we will specifically target radiofrequency ablation, laser-induced thermal therapy (LITT) and brachytherapy, some of the most common and successful types of minimally invasive stereotactic ablative surgeries. Ablative or lesioning procedures were historically one of the first functional neurosurgeries to treat drug recalcitrant movement and non-psychotic psychiatric disorders (Greenberg et al., 2010; Curry et al., 2012). Many physical principles have been employed to disrupt pathological neuronal activity such as radiofrequency (RF) heating, chemical destruction, ionizing radiation, mechanical methods (leucotomy), focused electromagnetic waves and lasers. RF ablation is perhaps the most popular method with several advantages ranging from well-circumscription of lesions to seamless temperature, target control, and treatment versatility. The minimally invasive procedure rests on the generation of a high-frequency oscillating electromagnetic field, created by the implanted lesion electrode at the intracranial treatment site paired with a large area dispersive electrode placed at the skin surface. The high-frequency oscillation of ionized particles provokes a temperature increase radial to the lesion electrode, which permanently damages the surrounding tissue when it heats beyond 45◦ (Cosman et al., 1984). Functional ablative procedures are arguably being replaced by brain stimulation procedures (refer to subsection 2.2.1), which advertise the same therapeutic output with additional advantages: i) reversibility, ii) versatility, iii) safety and iv) novelty (Hariz and Hariz, 2013). Laser Induced Thermal Therapy or LITT, relies on the application of a laser fiber directly to the deepseated target, and by photo-coagulation the tissue surrounding the fiber tip is heated to temperatures between 55 and 95 ◦ C. The surgical placement of the optic fiber follows a similar procedure to the described SEEG electrode placement (refer to subsection 2.2.2), the greatest difference being the use of MRI compatible instrumentation. The patient is placed in the MRI and the fiber placement verified both visually and with a low-power test pulse. An accurate and real-time map of tissue temperature is acquired by magnetic resonance thermal imaging. More than one laser position and activation may be used to cover the full target volume. LITT is of particular use in neuro-oncology to treat systemic cancer (Carpentier et al., 2012; Buttrick and Komotar, 2016). Radiotherapy is predicated on a differential radiosensitivity between neoplasic and normal brain tissue. Two different approaches exist: teletherapy (Andrews et al., 2004) and brachytherapy (Nachbichler and Kreth, 2018; Magill et al., 2017). The first is a closed-skull, non-invasive procedure based on multiple collimated beams of radiation, stereotactically directed towards the treatment site. It is performed with high-energy radiation technology such as linear accelerators (LINAC), the gamma knife, or other charged particles produced by cyclotrons/synchrocyclotrons. Alternatively, interstitial brachytherapy is an invasive procedure that involves the direct permanent or temporary placement of single to multiple radioactive sources within the prescribed tumor volume. Compared to teletherapy, the invasive brachytherapy achieves higher therapeutic ratios through selective administration of radiation with a steep falloff near normal brain 19 tissue. It causes a well-defined small region of neoplasic tissue necrosis that develops radially from the radioactive seed, which is subsequently removed by macrophage activity. Surgery planning involves the determination of the volume and shape of the target, the choice of isotope, number of seeds, activity, and location within the tumor. The stereotactic surgical workflow for both ablative or brachytherapy treatments involve the accurate placement of electrodes/catheters, which is a procedure similar to those described in subsections 2.2.1 and 2.2.2. Aside from the therapeutic principle underlying each treatment, the most noticeable difference to the described procedures lies on the pre-operative planning step. A careful study of the brain target volume and shape is required for a precise configuration of the equipment used, and subsequent control over the radiation isodoses or the isothermal ablative surfaces. 20 Chapter 3 Robotic Neurosurgery For clearness and topic containment, we avoided referring to robotics in the medical introductory chapter 2, even though the neurosurgery field and the robotics realm extensively overlap. It was nothing more than a matter of time until someone realized the applicability of robotics within an operating room. Robots are by definition mechanically automated devices, programmable to perform ordinary and repetitive tasks with utter precision and consistency. In stereotactic surgery, more so than in open surgery, the whole procedure can be broken down to a sequence of well-defined steps conducted in reference to absolute coordinates, in order to accurately guide instrumentation to the surgical field. Such a formulaic approach closely resembles the typical industrial robot task. In fact, over the past years, it has become increasingly evident that robotic systems significantly enhance the surgeon’s surgical capabilities (Nathoo et al., 2003; Eljamel, 2007; Nathoo et al., 2005), as evidenced by fewer intraoperative complications and an improved patient outcome. The application of robotic technology in the surgical field is relatively recent (late 90s), but it has already been adopted in a vast number of medical domains: neurosurgical, orthopedic, laparoscopic, thoracic etc. In this chapter we will be focusing on the robotic technology that targets stereotactic neurosurgery, listing the most successful and distributed systems, and discussing their features and advantages (Faria et al., 2015a). 3.1 Robotic assistant: neurosurgeons’ expectations and demands How do robotic systems improve the work conditions for neurosurgeons, neurologists and other staff? What tasks can be delegated to the robot? What are the benefits for the patient? What can be expected of a neurosurgical robot? These are some of the most common and fundamental questions often posed to and by developers regarding robotic neurosurgery, which will be addressed below. As stated previously, typical stereotactic surgery lasts several hours, throughout which the surgical team must remain completely focused. After attending stereotactic surgeries and brainstorming with neurosurgeons and robotic engineers, we were able to answer the first two questions (which are somewhat 21 related) and concluded that a simple and intuitive robotic system may improve the standard procedure in various aspects and thus: • Enable the coordinates and electrode’s path information to be managed between the planning software and the robotic controller software, instead of manually handling this information. • Avoid stereotactic frame and mechanical driver slacks or loose parts. • Avoid the slow process of repeatedly mounting and setting the frame and driver coordinates for both the phantom and patient. • Allow neurosurgeons to select and insert electrodes in eccentric trajectories, overcoming the constraints imposed by the stereotactic frame apparatus. This is extremely helpful when more than a single trajectory is needed, such as during SEEG, where up to 20 electrodes need to be inserted in a single procedure. • Enable the robotic manipulator to handle multiple end-effectors and surgical instrumentation to execute restrained skull drilling and the swift positioning of electrodes with improved precision. The manipulator can limit these tasks so that they are executed specifically along the predefined path, instead of executing them on the basis of a marked entry point. • Enable medical teams to easily take control over the task of increasing the depth of electrodes - while evaluating the patient’s symptoms - by simply interacting with a robot graphic interface, which aids neurosurgeons with that task. • Ensure flexibility and ease in changing the entry point once the burr hole is performed and in the event of encountering an unexpected vascular structure after opening the dura matter. • Reduce the risk of data loss or human errors. • Enable online monitoring of the absolute coordinates of the instrumentation tips, based on their physical dimensions and on the manipulator position in relation to the base referential. • Finally and most importantly, making frameless surgery under robotic guidance possible. It is important to note that, even though frameless surgery implies no frame, the transformation between the instrument guiding device and the patient must be constant. The most common approach to this problem relies on the use of a Mayfield 3-point pin fixation device (Integra LS, Plainsboro, New Jersey), in order to immobilize the patient’s skull. A rigid link connecting the Mayfield and the instrument guiding device then ensures the constant transformation. There are computational solutions in IGS for the active robot compensation of patient motion, which presents acceptable accuracy but this is still rather limited in the compensation time delay (Haidegger et al., 2009). Robotic systems enhance accuracy, precision and steadiness (Cardinale and Mai, 2011), which are directly reflected in fewer intraoperative complications and produce a positive impact on the patient’s outcome (Nathoo et al., 2003; Camarillo et al., 2004). It is not only the patient but also the healthcare institution that benefits from shorter patient recovery times and lower occupancy rates. When consulted about the expectations related to the robotic system for stereotactic neurosurgery, neurosurgeons look forward to: i) a simple system of intuitive usage, ii) a cost-effective solution, iii) a small, easily mountable and movable device. Thus, apart from the main goal of positioning and manipulating 22 surgical equipment, the most sought-after assets reside in the human factors and in the integrability of the robotic system. These aspects should thus be targeted by engineers when devising a robotic platform for stereotaxy. 3.2 State-of-the-art robotic systems Since the first report of a robotic neurosurgical system in 1985, a wide range of neurosurgical solutions has been developed (Kwoh et al., 1988). In order to keep this section concise, we chose to include the most successful robotic systems or projects directed at stereotactic neurosurgery that either reached the market or were clinically tested, with reported in vivo results 1 (see Table 3.1). Robotic platforms for endovascular or radiosurgery were not included in this review. The listed robotic systems were divided into three categories according to user-interaction (see Nathoo et al. (Nathoo et al., 2005)): • Supervisory Controlled, the robot motion performed during the operation is explicitly or implicitly specified by the surgeon offline. During the procedure, the robot moves autonomously under the surgeon supervision. • Telesurgical, the robotic manipulator (slave) is directly controlled by the surgeon through an input device like a joystick (master), which is usually endowed with force feedback. • Shared Control, surgeon and robot share the control over the surgical instrumentation. The surgeon still controls the procedure while the robot provides steady-hand manipulation or active-restrain over surgical safety areas. The list is organized in two parts to differentiate robotic systems: those developed specifically for stereotactic neurosurgery and general robotic systems, which are capable of performing/assisting stereotactic neurosurgery although they were not solely designed for it. 1 The Robocast and NeuRobot projects did not report clinical trials, but were involved in major European-funded programs, and were included for their contribution. 23 Figure 3.4: Renaissance MARS robot (Courtesy of Mazor Robotics, Inc.). bulk and costs associated to large robots. The system’s cost was initially aimed to be under 100 000 USD, unlike other robotic solutions which range from 300 000 to 500 000 USD (Joskowicz et al., 2005, 2006). A recent article in an investment research platform revealed the listed system price to be 849 000 USD and the disposables 1 500 USD (Capital, 2014). 3.2.1.5 RONNA G3 The RONNA G3 system is the third generation of a robotic neuronavigation system that is the product of a cooperation between the Faculty of Mechanical Engineering and Naval Architecture from the University of Zagreb and the University Hospital Dubrava. Tailored for stereotactic frameless minimally invasive surgery, more specifically brain biopsy, the system prototype has been upgraded and was recently tested in clinical trials. The system features three main components: i) a robotic arm mounted on a universal mobile platform, ii) a surgery planning system and iii) a global tracking system (Dlaka et al., 2018). The RONNA G3 system is designed to work with a single robotic arm - master, which acts as a passive navigation guide leaving the neurosurgeon in charge of drilling and handling the surgical instrumentation. An extended version of the system includes a dual-arm system, being the second arm - assistant - responsible for inserting the operating instruments in the tool guide. The patient registration is based on retroreflective fiducial markers, either freely distributed on the head and secured by bone screws or assembled to a single cross-pattern support. The fiducials are located in the stereotactic image scans using the system’s 30 planning software (RONNAplan), through a manual or automated process. Perioperative and after fixating the patient’s head with a Mayfield clamp, the markers are located and tracked by a wide range infrared stereo camera (Polaris Spectra, Northern Digital Inc., Canada). It tracks the coarse position of the markers in the patient and of the robotic arm(s). While the global tracker information is used to assure safe and collision-free motion planning, a high-precision localization device (RONNAstereo) identifies the exact position of the markers which is used for the image-patient registration (Švaco et al., 2017; Dlaka et al., 2018). Figure 3.5: RONNA G3 neuronavigation robotic system: A) master robot; B) assistant robot; C) universal mobile platforms; D) optical tracking system; E) control and planning software interface (reprint copyright license agreement with John Wiley and Sons). The robotic arm mobile platform is fixed to the operating table to ensure a stable link throughout the procedure. Once the registration is concluded, the RONNAstereo end-effector is replaced by a tool guide and the localization procedure is verified by pointing at anatomical landmarks. Thence, the robot moves to a pre-planned trajectory for the skull drilling and needle insertion steps. An in vitro study reported the system’s mean positioning accuracy to be 0.65 mm with a maximum error of 1.89 mm (standard deviation of 0.39 mm) (Dlaka et al., 2018). The error was strongly affected by the proximity between the markers and the target points, with lower errors in shallow targets. In the reported clinical trial, the authors measured the entry and target point errors of 2.24 mm and 2.33 mm respectively, as the Euclidean distance between the planned and effective points (Švaco et al., 2017). The RONNA G3 system poses itself as a precise frameless neuronavigation robotic system, however, more clinical trials are required to assess the system’s performance. The project is funded by the Croatian Scientific Foundation (Research project ACRON, grant no. 4192). 31 3.2.1.6 Robocast The Robocast – an acronym of Robot and Sensor integration for Computer Assisted Surgery and Therapy project (FP7 ICT-2007-215190) – aimed to create a modular system to integrate image-guided navigation and robotic devices for keyhole surgery (Figure 3.6). The project developers envisaged a human-robot interface with context-intuitive communication, embedded haptic feedback, a multiple robot chain with kinematic redundancy, an autonomous trajectory planner and a high-level controller (Comparetti et al., 2011a; De Momi et al., 2009). Figure 3.6: Robocast robot (Courtesy of De Momi, E. and Ferrigno, G. - Robocast Project). The Robocast system consists of an optical and electromagnetic tracking system, ultrasound and three robotic actuators with haptic devices. The first robot, or gross positioner, is the Pathfinder robot with 6 DoF. There is another called fine positioner, which is the MARS (Renaissance) parallel robot with 6 DoF to further improve accuracy. The third is a linear piezo actuator to ensure the linear insertion of electrodes or biopsy probes. The optical tracking system is used to register the intraoperative environment according to the preoperative plan. A single DoF haptic feedback actuator is used to control probe depth (De Lorenzo et al., 2011). The software platform can be divided into six subsystems: preoperative planning, human-computer interface, sensor manager, high-level controller, haptic controller and safety check (De Momi and Ferrigno, 2010). After the neurosurgeon has selected the target and entry area, the preoperative planning software autonomously calculates the lower risk optimal entry point and trajectory (De Momi et al., 2009, 2013). Human-Computer Interface allows the surgeon to interact with the navigation system, while the sensor manager assembles data from the ultrasound and tracking system and inputs this to the system control 32 center. The high level controller manages information from the preoperative planning and sensor manager subsystems, and iteratively calculates the gross positioner and fine positioner kinematics (Comparetti et al., 2012b). The haptic controller interfaces the linear actuator robot with the haptic device, transmitting a force-feedback reaction to the surgeon so that the probe will be moved. Finally, the safety check module runs regular state verifications for each subsystem; in the case of failure, it stops the probe movement (Comparetti et al., 2011b). The technical accuracy of the iterative targeting approach based on continuous optical feedback was evaluated in vitro, in optimal and noise-induced conditions. The largest reported translation median error was 0.6 mm and 0.4 mm for the entry and target points, respectively. While the largest rotation median error was 6.5×10−3 rad (Comparetti et al., 2012b). The accuracy reported fits the requirements for clinical applications. The Robocast project ended in 2011 and has been continued by the Active project - an acronym for Active Constraints Technologies for Ill-defined or Volatile Environments (FP7-ICT-2009-6-270460) (Active Project, 2012; De Momi et al., 2014), which proposes an integrated redundant robotic platform. This relies on two autonomous cooperating robotic manipulators for neurosurgery, which form a light and agile system with 20 DoF. 3.2.1.7 Rosa The Rosa robotic system (MedTech SAS, Montpellier, France) is the latest generation of neurosurgical computer-controlled robots for stereotactic surgery (Figure 3.7). The Rosa system comprises a mechatronic part consisting of a 6 DoF serial robotic manipulator and a control software part for neurosurgery planning, registration, and guidance (Medtech S.A, 2012). Figure 3.7: MedTech Rosa (Courtesy of MedTech Surgical). 33 The planning software (Rosana, MedTech) allows for the merging of different and complementary imaging techniques when studying the best surgical approach. The patient initially undergoes an MRI exam (with or without contrast, various supported sequences) to visualize the target anatomical structures, and to plan the optimal guiding trajectory (Gonzalez-Martinez et al., 2014; Serletis et al., 2014). This plan is then registered to a CT scan, performed near the time of surgery, and which serves as the reference due to its homogeneous geometric accuracy. An intraoperative Flat-Panel CT can be integrated into the surgery workflow to compensate for brain shift or robot registration errors (Lefranc and Le Gars, 2012; Lefranc et al., 2014b). After uploading the plan to the Rosa system, the robot is firmly fixed to the skull clamp. The surgery team may choose to register the robot to the intraoperative scene in a frame-based (Leksell frame) or frameless approach. The frameless method is carried out using fiducial markers attached to the scalp/skull, or via the Rosa patented automatic surface scan. The latter combines robot motion and laser telemetry to provide a non-invasive registration (Lefranc et al., 2014a; Medtech S.A, 2010). The robot is draped after a satisfactory registration and, upon the surgeon’s command, automatically moves to the planned guiding trajectory. It remains in a locked state while the entry point is marked and prepared. Scalp incision and skull drilling are performed with a cordless power drill (Gonzalez-Martinez et al., 2014). The neurosurgeon may choose to insert the probes or electrodes manually through the adapted reducers held by the arm, or may use the haptic robot interface to lower the instruments (Lefranc et al., 2014b). This shared-control feature allows for intuitive interaction and control by the neurosurgeon with tremorless and motion restriction advantages. Lefranc et al. (Lefranc et al., 2014a) present a study comparing different modalities of image and robot registration with a phantom and in actual procedures. The Rosa system achieves an accuracy below 1 mm for frame-based and fiducial registration, and a 1.22 mm accuracy for frameless surface registration, both with CT as well as reference imaging2. The greatest asset of the Rosa system, when compared to the other solutions, is its flexibility. It is easily integrated into the institution workflow and is reported to be well-accepted (Lefranc et al., 2014b). No other robotic system offers so many options regarding robot registration. The Rosa system provides consistent and accurate instrument guidance while keeping surgery times comparable to conventional methodologies (Lefranc and Le Gars, 2012; Lefranc et al., 2014b; Gonzalez-Martinez et al., 2014). With regard to its negative aspects, users point to the robot’s learning curve and bulk dimensions, which limit the neurosurgeon’s workspace. 3.2.2 General and capable of Stereotactic Neurosurgery 3.2.2.1 MKM The MKM system (Carl Zeiss, Oberkochen, Germany) stands for ”Multicoordinate Manipulator”, and consists of three components: 1) an operating microscope mounted on 2) a 6 DoF motor-driven robotic arm, and 3) 2 Surface registration with MRI scans are error-prone due to image-related distortions, leading to significantly lower overall accuracy. 34 a computer workstation (Pillay, 1997). Its initial goal was to serve as a frameless stereotactic navigation system, by joining the concepts of intraoperative microscopy and neuronavigation in minimally invasive IGS (Roessler et al., 1997). The surgical procedure is planned and based on preoperative image scans, which are then registered to the intraoperative scene using scalp or bone fiducials. Inside the operating room, the neurosurgeon visualizes the neuroimaging plan, superimposed onto the microscope optical field, and showing the entry point, target point, lesion contours and other structure markings (Pillay, 1997; Roessler et al., 1998). Several advantages arise from this fusion: the potential to outline and minimize the size and shape of skin incision, craniotomy, and corticotomy; the capacity to decide between different surgical approaches and the possibility of performing more aggressive resections with a lower risk of damaging surrounding structures (Roessler et al., 1997). Willems et al. (Willems et al., 2001) extended the applicability of the MKM system by introducing an instrument holder for frameless stereotactic procedures to be mounted on the microscope. This instrument holder, also developed by Carl Zeiss, consisted of an extension arm which rigidly fixed to the microscope with a large channel for tool guidance. Plastic reducers are fitted to the channel to constrain different instrumentation, for probe guidance or bone drilling (Willems et al., 2001). The MKM software was equipped with a “tool mode” module, which sets the instrument holder so as to align it with the surgery planned trajectories, rather than the optical axis (Willems et al., 2003). Additionally, instead of telemanipulating the microscope with a spherical sensor joystick, the microscope holder automatically moves to the predefined position (manual repositioning possible). During instrument insertion, however, the system movements are disabled for safety reasons (Willems et al., 2001). In vitro and in vivo studies were performed with the mounted instrument holder to assess the MKM system’s accuracy. Willems et al. (Willems et al., 2001) reported a slightly lower application accuracy with the robot when compared to the BRW frame; yet, there was a comparable target localization error. Willems et al. (Willems et al., 2003) reported an average biopsy localization error of 3.3 mm and 4.5 mm, depending on the registration method used (bone screws or scalp adhesive fiducials). While this is acceptable for brain biopsy procedures, further accuracy is required for functional neurosurgery. The MKM system presents a rapid, flexible and reliable alternative to stereotactic frames in biopsy brain surgeries and stereotactic neurosurgery guidance (Willems et al., 2001). On the other hand, its high costs of acquisition, bulky structure and lack of mobility, constitute some of its negative features (Willems et al., 2003; Lefranc et al., 2014b). 3.2.2.2 NeuRobot NeuRobot 3 stemmed from the European Community funded project ROBOSCOPE to provide a joint solution for common problems in Neurosurgery. The project involved a robotic arm (NeuRobot) and a simulator 3 Do not confuse this with another system called the NeuRobot (Hongo et al., 2006, 2003), which is a telecontroled micromanipulator system with a master-slave control hierarchy to perform minimally invasive procedures using an endoscope and three robotic arms. There is also another system, also called NeuroBot, which is used in skull-based surgeries (Handini, 2004). 35 image-guided system, ROBO-SIM. Focusing on the robot platform, the NeuRobot is described by Auer et al. (Auer et al., 2002) as ”an active manipulator with inbuilt robotic capabilities” that includes: active constraint mechanisms of the manipulator motions based on mapped permitted regions, a precise pattern control and the capacity to automatically track moving features (Figure 3.8). Figure 3.8: NeuRobot (Courtesy of Prof. Brian Davies, at Imperial College of London). The robotic manipulator has no more than 4 DoF to manipulate instrumentation around a pivot point – the burr hole entry point in stereotaxy. These 4 DoF control the probe orientation around the Yaw, Pitch, Endoscope rotation and the position along an Endoscope depth DoF, which implies that the NeuRobot cannot reach the pivot point on its own and must, therefore, be previously positioned. This is one of the system’s disadvantages since, if more than one trajectory is required, the robot needs to be repositioned and readjusted to the surgery table (Davies et al., 2000). The manipulator includes a control mechanism developed from a flight-simulator experience by Fokker control systems b. v., and enhances precise motion and force-control using low force inputs (Auer et al., 2002). Special attention was paid to safety issues. The system thus includes: a dead man’s switch and a workspace which is physically constrained in a safe operating volume based on MRI segmented data. An ultrasound imaging system is used to track tissue deformation during the procedure, and the probe position is dynamically compensated in real-time. The NeuRobot was able to operate autonomously, yet it raised concerns about ”who is in-charge” of the surgery (Davies et al., 2000). Despite its advantages, the system is still dependent on a stereotactic frame to register the robot with the surgery reference (Davies et al., 2000). The robot was initially projected to hold and manipulate a neuroendoscope but, as stated by the authors, it could in principle be used to handle other stereotactic instrumentation. One remarkable advantage of the NeuRobot system is the integrated ROBO-SIM software, which enables the same manipulator to be used in real or simulated interventions to train and help neurosurgeons to become acquainted with the system (Auer et al., 2002). 36 3.2.2.3 Evolution 1 The Evolution 1 robotic system (Universal Robot Systems, Schwerin, Germany) was specially designed for neurosurgical and endoscopic applications for micro scale brain and spine procedures. Unlike the previous examples, Evolution 1 is a 4 DoF hexapod with a parallel actuator, which combines high accuracy with great payload capacity. Its 6 mechanical parallel axes work as a spherical joint to move a platform with a slider mechanism that holds the endoscope. The parallel actuator approach enhances motion precision achieving an absolute positioning accuracy of 20 µ m and a motion resolution of 10 µ m, even under loads of up to 500 N (Nimsky et al., 2004; Zimmermann et al., 2004). Evolution 1 is able to compute the movement of all axes in less than 120 µ s. It comprises a universal adapter enabling it to incorporate different types of surgical instrumentation like endoscopes and high-speed drills. Due to the rather reduced working range, however, it must be pre-positioned in the desired orientation, approximately 5 cm above the entry point. Its user-interface is a touch screen and a master joystick device to control end-effector motion and speed (Nimsky et al., 2004; Zimmermann et al., 2004). Following IGS methodology, end-effector instrumentation follows a trajectory set preoperatively and based on MRI scans and planning software (VectorVision, BrainLab). Intraoperatively, the patient’s face is scanned for surface recognition by using infrared technology or laser surface scanning. Later, this information is matched with the preoperative MRI to ensure that the robot knows its position in relation to the surgery reference frame (Zimmermann et al., 2004). The main advantages of Evolution 1 are high precision and steady positioning/manipulation of endoscope, smooth and slow execution of movement within critical anatomical areas while handling surgical equipment. This system can be potentially adapted to assist stereotactic surgeries. However, a high payload capacity is superfluous since the weight factor is not an important aspect for the instrumentation and tasks at hand. Consequently, a parallel actuator is not always the best choice as it is typically large, thus restraining the neurosurgeon’s workspace, and possesses a relatively limited reach/flexibility. 3.2.2.4 neuroArm / SYMBIS The award-winning system, neuroArm, was developed by Dr. Garnette Sutherland from the University of Calgary in association with engineers from Macdonald Dettwiler and Associates (MDA). It was introduced in 2002, and was recently acquired and renamed SYMBIS (IMRIS, Winnipeg, Canada). The project’s main goal is to take advantage of the MR-environment and haptic feedback technology, adding 3D image reconstruction and high-end hand-controller design. It claims the title of being the first image-guided, MR-compatible surgical robot, capable of microsurgery and stereotaxy. It consists of two 7+1 DoF manipulators, which are semi-actively actuated in a master-slave control type and moved by hand control at a remote workstation. The human-robot interface filters undesired hand tremors and can scale the movement of the controls in relation to end-effectors (Pandya et al., 2009; Sutherland et al., 2008). The neuroArm is built for neurosurgery precision tasks so that each arm has a limited payload of 0.5 kg, a force output of 10 N, a tip speed that ranges from 0.5 to 5 mm/s and sub-millimetric accuracy. Patient safety was a paramount concern throughout the development of the robotic system, and features 37 Figure 3.9: University of Calgary neuroArm (Courtesy of neuroArm Project, at University of Calgary). such as active workspace constraints were added in case the robot leaves the safe operating zone. These policies granted neuroArm the approval of the Canadian Standards Association in 2007, as well as that of Institutional Ethics and Investigational Testing by the University of Calgary and Health Canada in 2008 (Figure 3.9). This robotic system is capable of microsurgery and stereotaxy, which has granted it a place among the general robotic platforms (Sutherland et al., 2003). Despite increasing surgery time, its precision, steadiness, and compatibility with planning software have resulted in reduced trauma and blood loss (Pandya et al., 2009). The end-effector positioning can be verified by overlaying 2D and 3D MRI preoperative and intraoperative information, respectively. After positioning, a Z-Lock feature is used to restrict the tool motion along the defined longitudinal trajectory. The main advantage of the neuroArm system also constitutes a drawback in some types of stereotactic neurosurgery, due to the need for an MRI scanning machine during the entire surgery with the associated maintenance and acquisition costs. Furthermore, the robotic system costs are also considerably higher since the robotic manipulator is manufactured exclusively with non-ferromagnetic materials (primarily titanium and polyetheretherketone) (Sutherland et al., 2008). 3.3 Current perspectives and Future directions If one is to compare the most successful robotic systems/projects for stereotactic procedures, one will find several similarities. All the systems follow a very standard and similar surgical protocol related to the IGS paradigm. The main differences are mostly related to technical aspects. Starting with the robot structure itself, most systems rely on serial instead of parallel actuators. Parallel robots excel at precision associated with larger payload requirements; even so, a larger payload capacity 38 will seldom be a requirement in stereotactic procedures. Additionally, parallel actuators have very limited access to the surgical target and typically occupy more space next to the patient. Serial manipulators, on the other hand, present greater flexibility and compactness, while still providing a larger workspace, i. e., easier access to surgical targets and trajectories. It is important to note the Renaissance system’s unorthodox solution, which takes advantage of the sturdiness of parallel actuators to miniaturize and create a portable robot. Although its narrow workspace prevents its use in SEEG applications, it is used in DBS and biopsy surgeries. The number of the manipulators’ DoF is application-dependent, but this tends to vary between 4 and 7, except for the Robocast project’s robot, which follows a multi-robotic 13 DoF approach (for enhanced precision). The number of manipulation DoF affect not only the workspace but the robot’s dexterity and flexibility, thus conditioning surgical planning. Fewer DoF and a more reduced workspace mean less flexibility, which directly influences how the robot should be placed in order to reach the planned trajectories, often implying obstructions to the medical team’s workspace and vision of the surgical field. Although more DoF and high dexterity is generally an advantage regarding collision avoidance problems, a larger number of joints – particularly in serial manipulators – means more sources of errors that accumulate along the robotic chain. Most of the robotic systems and projects for stereotactic neurosurgery enable a frameless approach and are gradually becoming detached from the dependency on stereotactic frames. While frameless is one of the flags of robotic systems, the accuracy and repeatability of frameless systems are still surpassed by frame-based systems (Lefranc et al., 2014a; Cardinale et al., 2013). Especially in the case of functional neurosurgery in deep-seated targets, frame-based is still the preferred solution. This is because the frameless approach maximizes accuracy and precision at the entry point rather than at the target point, as in the arc-centered approach (Bjartmarz and Rehncrona, 2007; Zrinzo, 2012). Improving efficiency and developing new frameless registration/fixation methods constitutes a timely endeavor and a research opportunity. The robotic systems listed converge in other aspects, such as their portability and embedded imaging and planning technology. The lack of mobility in systems like Surgiscope and MKM is seen as a disadvantage. The fact that they are easy to transport and quick/easy to set up is certainly a premise for future robotic system developers. Additionally, the system’s modularity and the possibility of choosing from different surgical approaches depending on the clinical case greatly improves the system’s acceptance. Safety is of paramount concern and should be addressed from the early stages of the system’s development (Taylor and Stoianovici, 2003). It is the most cited reason underlying a medical team’s apprehension in the face of robotic technology (Lavallee et al., 1992). To achieve clinical clearance, a robotic system must at no single point of failure lead to a loss of control and injury to the patient. Critical safety systems like these are typically endowed with redundant position encoders and mechanical limits for speed and exerted forces. Any sensory mismatch or consistency failure should cause the robot to freeze or go limp, while assuring a safe retract mechanism to resume the surgery in a traditional fashion (Talamini et al., 2003; Kortenkamp et al., 2008). Regarding sterilization, the system parts which are in direct contact with the patient must be either disposable or robust enough to withstand autoclaving or other sterilization 39 uparams and fdata contain vectors of integer and floating point numbers respectively. The errorcode contains an integer value. The message received has 5 analogous fields. The rCommand is expected to contain an integer incremented by one from the command sent. The rText command generally contains a string with a small description of the errorcode received. The ruparams and rfdata contain vector of integer and float values. The errorcode returns: 0 , successful command; -1 , command not accepted; -2 , normal connectivity with the robot controller not allowed (e.g. alarm); -3 , YARP not running; -4 , implemented exception handling; -5, other errors. The implemented command functions check on the system’s state and availability to perform the required action, before transmitting motion commands such as Cartesian or joint motion. 4.3.2 Registration In an effort to keep the procedure as close as possible to the current surgery procedure, we decided to use the ZD frame as the head fixating device and surgery frame reference. The preoperative planning is conducted according to the established standard and the instrumentation trajectories identified in the surgery reference frame. Then, a point based co-registration approach is employed to find the transformation matrix that maps the robot to the surgery reference frame, refer to subsection 2.1.1. The general idea consists of selecting a set of points with known coordinates in the frame of reference (surgery frame), and use a measuring device to detect these points in their own reference system. The closest path closes the loop and achieve this registration is to operate the robotic manipulator as the detection device. The ZD base ring contains several divots engraved in its structure at well-defined positions ( Sph1,2,3,4 ) in the surgery reference frame, Sph1= [−126.1,+18.8,0.0], Sph2= [+126.1,+18.8,0.0], Sph3= [−126.1,−18.8,0.0], Sph4= [+126.1,−18.8,0.0]. (4.1) The robotic arm is maneuvered to sequentially touch each of these points, and their position ( Rph1,2,3,4 ) is recorded relative to the robot’s base frame using forward kinematics, Figure 4.5. Once the two sets of identified in both surgery and robot reference frame, the rigid transformation that maps one set of points into the other ( RTS ) is computed based on a least squares fitting algorithm (Horn, 1987). Horn proposed an optimization algorithm to find the affine relationship between pairs of measurements in different reference frames. The algorithm takes 3 or more pairs of points and starts by computing the translating component of the transformation that is based on the distance between centroids. The scaling between the two sets of points is calculated using the root mean square deviations of each set of points from their respective centroid. The quaternion representing the best fitting rotation is computed 46 Figure 4.5: Point based co-registration process. A “pivot” end-effector tool is used to touch four selected divot markings on the reference ring, sequentially. from the eigenvector with the most positive eigenvalue of a symmetric 4×4 matrix. Matrix that results from arithmetic operations between sums of products of coordinates from both sets of points. 4.3.3 Coordinate Convention The Coordinate Convention module is no more than a package the handles the surgical trajectory coordinates from the preoperative plan. The developed library accepts two types of inputs: i) the Cartesian coordinates of the target and entry point as defined in the surgery reference frame; or the ii) ZD frame coordinates (we worked with the available ZD frame, although the process can be replicated to other commercial frames). To act as a direct replacement for the frame, the control software should handle the frame coordinates as output from the image planning software and translate them into coordinates known in the robot space. 4.3.3.1 Cartesian coordinates Target and Entry Points In case the target ( pt ) and entry points ( pe ) are specified in Cartesian coordinates, the surgery trajectory representation as a vector (evt) in space is straightforward, evt=pt−pe,(4.2) however, the robotic system usually requires a completely defined transformation matrix as a motion reference. It is not possible to uniquely construe a reference frame, from a directional vector in space, because one orientation variable is unspecified. This is a typical case where the robot kinematic chain is 47 considered redundant because the number of DoF required for the robot to conform to the trajectory is less (5) than the number of structural DoF (6). The homogeneous transformation matrix that represents a surgical trajectory ( T1 ) is constructed from the target and entry points as depicted, Figure 4.6a. (a) Definition of the trajectory frame. (b) Calculation of the redundant orientation angle. Figure 4.6: Representation of a surgical trajectory reference frame extracted from the target and entry points. The position sub-matrix ( pt ) is directly defined by the target Cartesian coordinates. By definition, we chose to assign the surgery trajectory unit vector (eˆvt) to the rotation sub-matrix z-axis vector, T1=         Rx,x Ry,x eˆvt,x pt,x Rx,y Ry,y eˆvt,y pt,y Rx,z Ry,z eˆvt,z pt,z 0 0 0 1         (4.3) The end-effector angle around z-axis is undefined, and as a result the rotation sub-matrix vectors Rx and Ry are unspecified. We start by calculating temporary axes ( R0x and R0y ), based on an arbitrary vector 3 , R0y=       right, if (front ·eˆvt) = 1, eˆvt×front keˆvt×frontk,if (front ·eˆvt)<1,(4.4) which follows, R0x=Ry×eˆvt kRy×eˆvtk.(4.5) The angle φis calculated from, φ= atan2(−eˆvt,y,eˆvt,x).(4.6) 3To facilitate the vector representation, let front = [1 0 0],right = [0 1 0] and up = [0 0 1] 48 Let, rotx(θ) =      1 0 0 0 cos θ−sin θ 0 sin θcos θ     ,rotx(ψ) =      cos ψ0 sin ψ 0 1 0 −sin ψ0 cos ψ     ,rotz(φ) =      cos φ−sin φ0 sin φcos φ0 0 0 1     . (4.7) The final rotation matrix of T1 is calculated as a product of the temporary matrix and the rotation matrix around the z-axis (rotz(φ)), R=    R0xR0yeˆvt         cos φ−sin φ0 sin φcos φ0 0 0 1     .(4.8) The transformation matrix here proposed, defines the target position and an initial orientation. Although the reference frame does not contain information about the entry point, it can be directly calculated by translating along the z-axis of a distance of keˆvtk . During the procedure, the user has the opportunity to adjust the end-effector orientation around the z-axis, while keeping it aligned with the surgical trajectory. In this case, the new motion reference is obtained by changing φ(4.8). 4.3.3.2 Zamorano-Dujovny coordinates The system is also endowed with an algorithm to convert ZD coordinates to a reference frame representing a trajectory. It should be noted that the ZD frame is designed to have a single point fixation (subsection 2.1.2) and can be mounted in the 4 quadrants of the base ring (positions 0 ◦ , 90 ◦ , 180 ◦ , 270 ◦ ). Consequently, the ZD coordinates depend on the stereotactic frame mounting position, Figure 4.7. The A, B and C coordinates specify the target position (Table 4.2), whilst the D and E coordinates Table 4.2: Target point (pt) Cartesian coordinates from ZD coordinates in each of the frame mounting positions. 0◦90◦180◦270◦ pt,x B A B A pt,y A B A B pt,z C C C C specify the surgical trajectory orientation. The D coordinate represents a rotation around A-axis, and E represents a consecutive rotation around B-axis. Then, the rotation matrix can be extrapolated from D and E as, R0=                  roty(−(90◦−D)) rotx(−(90◦−E)),if frame mounted at 0◦, rotx(−(90◦−D)) roty(90◦−E),if frame mounted at 90◦, roty(90◦−D) rotx(90◦−E),if frame mounted at 180◦, rotx(90◦−D) roty(−(90◦−E)),if frame mounted at 270◦, (4.9) 49 Figure 4.7: Example of ZD coordinates with the stereotactic frame mounted in position 0◦. A final rotation is applied to point the z-axis trajectory vector towards the target point from the entry point, R=R0rotx(180).(4.10) 4.3.3.3 Rotation Matrix and Euler Angles The NXC100 controller works exclusively with Euler angles to represent rotations while the controller application with rotations in the form of matrices and quaternions. Matrix to/from quaternion conversions are unambiguous, but the same cannot be said for Euler to/from matrix form conversions, as there are up to 12 sequences divided between the proper Euler angles and Tait-Bryan angles. The convention used by the NXC100 controller is the extrinsic Tait-Bryan x−, y− and z− , that relates to the angle coordinates (θ, ψ, φ). Translating from angle coordinates to a matrix form, rotx,y,z(θ, ψ, φ) = rotz(φ) roty(ψ) rotx(θ)(4.11) and from the matrix form to the angle coordinates, θ= arctan2(r32, r33) ψ= arctan2(−r31, r32 sin θ+r33 cos θ) φ= arctan2(−r21,−r11) (4.12) The rij variables correspond to the matrix coefficient at ith line and jth column. 50 4.3.4 Robotic Motions To keep the system simple yet functional, it was used the motion commands of the NXC100 controller. The commands listed below are common among industrial controllers. For this particular application, three types of motions were used. • Joint motion, typical point-to-point motion where the position of each joint is interpolated to move from the initial to the target position individually, but synchronously; the end-effector trajectory in Cartesian space is not relevant. • Linear motion, each joint executes a coordinated motion, such that the end-effector describes a line in Cartesian space to reach the specified Joint or Cartesian position; • Incremental motion, similar to Linear motion, however, it is specified the increment of the end-effector position in Cartesian coordinates (only), instead of the absolute target; Additional parameters for the motion commands include joint and Cartesian velocity limits, the tool dimensions, and the reference frame whenever Cartesian coordinates are involved. The motion planning was divided into the approach and the tool handling phase. The approach phase involves the movements between the robot home or safe position and the stereotactic trajectory – manipulator is at least 250 mm away from the patient’s head. The tool handling movement phase starts when the manipulator end-effector is already aligned with the stereotactic trajectory and it just translates along this trajectory. To enforce safety, the manipulator joint speed is maxed at 15% during its approach phase, and 3 to 5% during the tool handling phase. As neither high-forces nor velocities are required in stereotactic neurosurgeries, functioning at a lower speeds causes less stress to the joints, makes the robot motions more predictable and provides the operator more control over the robot actions (Fei et al., 2001). 4.3.5 User Interface The control application launches with the MainWindow, where the user can input the Server Port field with the name of the peer YARP component to connect to (in this case, the “/translator”). It expects a running YARP server, as well as a running module that interfaces with the robotic application. If the communication is established a green led is light up below the connection button, and the “Mode” label will indicate the NXC100 controller functioning mode. If the mode is set to Remote the user can now remotely control the manipulator; also an additional group of buttons appears under the “General Functions” panel to manipulate basic robot functions such as, activate the servos or trigger the joint brakes, Figure 4.8a. When a new procedure is started, the registration window shows up and asks the user to move the robot (with the attached pivot) to reach the reference ring divots. The divots should be touched in a specific order, recording the robot’s position after reaching each one. When the final position is recorded, the transformation between the robot and the reference ring is calculated. For more information about this step please refer to subsection 4.3.2. After the registration step, a question dialog appears to select the coordinate convention to use (check subsection 4.3.3). If the user is working with ZD coordinates a window such as Figure 4.8b appears, 51 otherwise, a similar window shows up to introduce target and entry point coordinates. These coordinates should be directly available from the planning software. Once the trajectory coordinates are checked and confirmed, a new window will pop-up Figure 4.8c, again, with different coordinates depending on the chosen convention. (a) MainWindow, connection interface, basic robot controls and button to launch procedure. (b) ZDCoordinates, ZD frame coordinate insertion interface; an alternative interface for target and entry point input is available by user choice. (c) ZDTask, window that shows the motion options available for each selected trajectory. Figure 4.8: Graphical user interfaces of the control application. The ZDTask window Figure 4.8, appears with the inserted trajectories. The GUI contains several tips to guide the neurosurgeon throughout the process, prompting messages between each step and to verify any robot motion. The “Trajectory Selection” panel contains a list of the mutually exclusive trajectories 52 previously inserted as well as an Instructions label to guide the neurosurgeon through each step. Below, the “Task Selection” panel contains several buttons that permit the user to maneuver the robot during the sequence of motions to surgically introduce the electrode. The robot actions can be summarily described by a finite-state-machine, where each button represents an input and each step (Selection, Approach, Adjust, Drill, Retreat, Guide) a state. A led next to the button will indicate a successful transition to the new state, which depends on the user allowance, reaching the target frame within the precision interval and the absence of any hardware, software interruptions. 4.4 Procedure Workflow The developed control application provides a platform for the user to insert and execute the preoperative plan when connecting the control application to the robot’s workstation. The system’s user interface guides the surgeon through the execution of this plan - that defines actual trajectories of electrodes toward the target – by following a sequential task-based controlled process for positioning and orienting the robotic arm according to the selected trajectories. This process is divided into six main steps, presented below. To assess the system’s performance, it is conducted a complete demonstration of the robotic role in an assisted stereotactic neurosurgery using the developed control application 4 . It is portrayed the different postures assumed by the supervisory-controlled robotic manipulator, when performing the different tasks, using snapshots of the actual system. At the same time, it is shown how the system’s user interface dynamically changes as the procedure advances. Approaching the selected trajectory After selecting the trajectory, the Approach task is ready to be executed. It is calculated the end-effector coordinates to position the robot’s tool holder z-axis collinear to the stereotactic trajectory, distanced 350 mm from the target. Then, the robot executes a sequential Joint motion from its home/safe position to this new calculated reference. Immediately after, another command is sent to move the robot in a Linear Motion along its tool reference z-axis until reaching a distance of 250 mm from the target. The neurosurgeon can now attach the surgical tool to the tool holder, and adjust the robot’s orientation to a more favorable posture. This change in the robot’s orientation involves rotating the robot’s end-effector around its tool’s z-axis in steps of 10◦ , using an Incremental motion and without changing any other position or rotation variables. Assisted-drilling When executing the Drill task, the manipulator advances its end-effector in an Incremental motion collinear to the electrode’s trajectory towards the target. The distance between the tool and the target was set in accordance with the drill length. After reaching the target position, the joint brakes are engaged and the neurosurgeon may perform the drilling step. The tool holder controls the drill orientation as well as its depth, to avoid damaging structures beyond the skull’s inner wall, Figure 4.9. 4 The procedure demonstration can be visualized at http://marl.dei.uminho.pt/public/videos/ RoboticNeurosurgeryDemonstration.html 53 Figure 4.9: Performing assisted drilling, with orientation and depth-control. Returning to a safe position At this point, the Retreat task is the only step available. By selecting it, the robot moves its end-effector in a Linear motion along the electrode’s trajectory out-wards/away from the patient’s head, until a distance of 250 mm from the target. The current tool is substituted by the electrode’s microdrive. Assisted electrode’s guidance Once more, the robot advances its end-effector collinear to the electrode’s trajectory towards the target in an Incremental motion (again, for demonstration, the distance was set in accordance to the microdrive with the defined electrode depth), and assisted electrode’s guidance is executed. The surgery phantom device is used to simulate the target coordinates instead of the dummy model in order to demonstrate the electrode, positioned along the selected trajectory, reaching the selected target, Figure 4.10. Figure 4.10: Holding microdrive to position electrode at the surgical target. Finishing the procedure - returning to home position In the end, the robotic manipulator retracts in a composed motion mirroring the initial approach movement. Initially, the robot moves its end-effector outwards and along the trajectory in a Linear motion until reaching 350 mm from the patient’s head. Then, and after removing the electrode’s microdrive, the robot moves to the home position in a Joint motion. By following a sequence of centripetal motions when approaching or moving away from the patient, it makes the robot action more predictable and avoids movements between trajectories that could potentially 54 collide with the patient. For safety reasons and when initiating a new procedure – before approaching a selected trajectory – the robotic controller checks the robot and connection state, and if the arm is at its home position. If the robotic system gathers all the ideal conditions to operate, the user is informed and asked to give consent to move the robotic arm. Whenever the robot moves, the servo power is activated, and the user is informed that it is moving. When the robot is not moving, its joint brakes are activated. At any moment the user can stop/prevent the robot movement, either using a hardware or software trigger - Emergency and Hold buttons - found at the programming pendant, robot controller or in the graphical user interface. If the desired position/orientation is not reached, in case robot movements are stopped during the process or, for example, if the robot reaches a joint limit, an alarm is triggered reporting the position and orientation errors. When executing a stereotactic procedure using the developed control application, task buttons are enabled/disabled according to the next step of the procedure, guiding the user through the execution of the procedure. During the robot motion, all task buttons are disabled. A green/red color is used to notify the user for the success of performing the required task. 4.5 Discussion Robotic intervention in neurosurgery is a growing trend with new market available systems and continuous technological breakthroughs. Robotic agents offer multiple advantages when compared to more traditional hardware like stereotactic frames, namely, in terms of improved precision, consistency and flexibility. However, market available robotic systems are pricey and closed. Here it was presented a simple solution that has the potential to work as a direct replacement to the stereotactic frame, combining the advantages of a robotic system while scaling down on system complexity. It provides an easily integrable, expandable and low-cost system for stereotactic neurosurgery. Even though the system is not permitted to perform neurosurgery as-is, it can be used as a proof-of-concept instrument, or in a research environment to catalyze the development of new systems for stereotactic neurosurgery. 55 Components are implemented by subclassing the TaskContext object, a base class that defines the context/environment where each task is executed. At the core of the component is the ExecutionEngine, which is responsible for processing asynchronous operations, calling plugin functions and executing its programs. The ExecutionEngine is launched as soon as the TaskContext is created. It calls on the “*Hook()” implemented functions according to the task state or state transition, Figure 5.2. Figure 5.2: TaskContext state diagram. The component has 4 regular states: Init, PreOperational, Stopped and Running. After creation, the component is at the Init state, at construction it transitions to PreOperational or Stopped (default). If the component is at the PreOperational state, it needs to “configure()” before calling the “start()” method. The code in the “configurationHook()” and “cleanupHook()” is used to read/write XML files, to allocate/free resources, or to perform any other operation that may compromise the real-time requirement. The real-time code belongs to the “startHook()”, “updateHook()” and “stopHook()”. The ExecutionEngine runs in a thread allocated by an Activity object. At construction, a period (0 if aperiodic), a priority and a scheduler are passed to build the Activity, which in its turn sets the pace for the ExecutionEngine. As depicted in Figure 5.3, the component’s interface comprises Operations and OperationCallers , Input and Output Ports , and Properties . Additionally, the component’s functionality can be augmented by adding the provided (scripting, marshalling, etc) or custom plugins. 5.2.3.2 Data Ports Data flow ports are the communication mechanism to send or receive a stream of data. An output port publishes data that can be received by a connected input port - both supporting multiple connections. The output port sends data whenever prompted, while the input port splits into: i) standard port, polls new data, ii) event port, that triggers a new running step once data arrives. Reading and writing data ports are always real-time and thread-safe, as long as the transferred data assignment operator is as well. A port is defined by a unique name within the component, and a data-type. Any data-type can be exchanged by a port, as long as its type and transport are known. By default, standard C++ and 62 Figure 5.3: Schematic representation of a TaskContext. “std :: vector < double > ” data types are supported. To include custom data-types, one must register the type and transport with the type system 3 . Finally, the communication middleware allows transparent data port connection regardless of the involved component’s proximity (local, inter-process or distributed). The data port connection, much like any other interface type, can be effectuated in the coded functions or at run-time using the deployer tool (more information below). 5.2.3.3 Operations The Operations and OperationCallers define what functions the component provides and requires, respectively. Implemented as C++ functions, operations can take multiple arguments and return a single value. They can be called from different processes or across the network as long as the types and transports of its arguments and returns are known. When setting up a component’s operation, one must specify a name, a function pointer, as well as an execution type flag. The later indicates whether the operation is executed in the client’s or the owner’s thread. Client or caller thread execution does require data locks to avoid concurrent access to shared resources and is often used in non-real-time operations or “getter” functions. Owner thread execution does not require data locks as the operation is uniquely executed by the component’s ExecutionEngine. This approach is typically used for process-intensive operations or operations that can be concurrently called from several places. The OperationCallers are the operation proxies implemented in the caller’s component. They are tasked with calling or sending an operation from a remote component: 3There are tools integrated into the OROCOS toolchain to expedite the typekit creation and registration. 63 • calling an operation is a synchronous method, i.e. blocks the current thread execution until the operation returns; • sending an operation is an asynchronous non-blocking method, i.e. it immediately returns a SendHandle, which can be polled to check the current status or to collect the result once the operation concludes. Calling or sending operations implicates memory allocation to collect the return values, a non-schedule operation that may compromise the real-time requirement. To deal with this dilemma, the deployment tool uses a real-time memory allocator that turns to a fixed amount available. 5.2.3.4 Properties Properties and Attributes are component “member variables” that store information and share the perk of being accessible and writable in run-time. They are commonly used to configure the component’s parameters. Properties can be read or stored from XML files, thus befitting variables of persistent nature (e.g. robot link lengths), while the lifetime of the attributes overlaps with that of the application. Accessing and writing these variables is a real-time procedure but not thread-safe. 5.2.3.5 Deployment The deployer provided by the OROCOS framework is itself a component with permission to load and configure other components using an orocos script file (“.ops” or “.osd”) an XML file. However, the deployer can only load components within the same process. The deployment process starts by importing components, auxiliary libraries, and typekits. The components are then loaded (created) and configured - setting the Activity(ies) parameters, loading, and connecting services and connecting ports. Depending on each component initial state (PreOperational or Stopped), they can be either configured or started. 5.3 Modular Surgical Robots Taylor et al. (1991) report a robot for orthopedic surgical applications. Being one of the first surgical robots, it was soon realized the scaling complexity of these systems and the need for a flexible, distributed solution that could incrementally integrate new sensors, actuators, boards, etc. A more detailed look at the controller software of the system called “ROBODOC” (Kazanzides et al., 1992) introduces the first task partitioned architecture with 5 modules to address servo actuation, motion control, surgery sub-tasks, safety and to monitor real-time. The architecture design complies with the surgical application requirements and enables an incremental and flexible development. There are however, several complex computational requirements involved to guarantee the synchronicity and real-time requirements of these components that run across different processes/computers. This design would eventually evolve to the cisst-SAW Framework (Kazanzides et al., 2010) whose features were already discussed in subsection 5.2.2. 64 Daniele Comparetti et al. (2014); Comparetti et al. (2012a) report a high-level surgical robot architecture for multiple neurosurgery scenarios, a joined effort from the European funded project ACTIVE (FP7-ICT-2009-6-270460). The control scheme involves a complex setup with several frameworks and middlewares including ROS, OROCOS and CORBA to deliver a versatile system capable of neuronavigation, cooperative control, and teleoperation. The system comprises several components from different middlewares that work in a distributed environment to manage the robot action from trajectory planning to servo interpolation passing through force control and telemanipulation. However, only limited information is disclosed about the system’s modules and the source code is not available, which makes it difficult to reuse or apply this research. Muradore et al. (2015); Bonfè et al. (2014) describes the development of a comprehensive cognitive system towards surgical tasks, I-SUR European project (FP7-ICT-2009-6-270396). The technologies developed during this project extend to soft organ model fabrication, surgical planning and robot operation in deformable bodies, intuitive user-interface design and intra-operative event handling. One product of this project is a component-based software architecture motivated by the need to merge sensory data and to maneuver several robotic apparatus including the ISUR robot with a macro unit (3 DoF delta configuration) coupled to a micro unit (4 DoF serial arm) to move and orient the needle; and a 6 DoF serial manipulator that handles the ultrasound probe. The control architecture was implemented in five OROCOS components to take advantage of its real-time properties: 1) motion planner, 2) trajectory generator, 3) variable admittance control, 4) robot driver and a 5) supervisor. Similarly to the previous listed system, there is no in-depth information about the actual modules besides concepts, so any subsequent research that aims to add or improve to the system’s components needs to re-implement the whole architecture from scratch. Sandoval et al. (2018) introduce a framework for robot-assisted minimally invasive surgery based on torque control of a redundant 7-DoF serial robot. The system categorizes as a shared-control platform that requires the user to initially manually move the robot to the incision cut where the trocar is placed. Thenceforth, a Cartesian compliance control moves the robot in accordance with a Remote Center of Motion (RCM) constraint. A 3D vision system synchronized with the surgical workspace feeds information to the Cartesian controller that generates the trajectories performed by the robot’s end-effector. A secondary task is embedded in the controller to permit the robot’s nullspace motion triggered by a user input force, without affecting the higher priority surgery task. The system’s force control approach makes it less viable for autonomous neuronavigation tasks that are fundamentally based on positions in the work frame. 5.4 Design of a Surgical Robot Controller In the current chapter, we introduce our approach of a modular architecture towards surgical robotic applications. After elaborating on the pros and cons of the modular methodology, and selecting the most appropriate framework to implement our solution, it is necessary to clarify some of the choices made in the design of our control architecture. The first real design choice relates to the manipulator kinematic structure. From the analysis of the surgical procedure and determination of the robotic gesture, it was concluded that a serial robotic 65 manipulator would be more flexible and compact, providing a broader workspace for an easier access to the surface or deep surgical targets from normal or eccentric trajectories. The ideal number of DoF was also discussed in section 3.3, and would ideally be comprehended between 4 and 7. Having presented a solution for 6 DoF surgical robot prototype (refer to chapter 4) (Faria et al., 2013, 2016), we decided to develop a 7 DoF prototype for completeness. Other reasons weighted on the choice of a 7 DoF manipulator: i) the existence of a certified robot model for human collaboration in medical applications, the KUKA LBR Med 4 , and ii) the inherent manipulator redundancy that permits the robot to perform secondary tasks such as, to conform to the operating room space, avoid joint limits and singularities. The capacity to use the extra-DoF to perform these secondary motion objectives is an innovation when compared to the commercial 5 and 6-DoF systems (Table 3.1). In terms of user-interaction, we define our approach as a Supervisory Controlled system that acts as a passive agent to precisely position surgical instrumentation according to pre-defined trajectories. To demonstrate the developed system’s potential we also show how it can be easily adapted to a Telesurgical application. As for feasibility, a neuronavigation system does not have extraordinary requirements. The separation of the preoperative imaging stage from the intraoperative procedure and the limited intersection between the tool positioner and intraoperative imaging systems mean that the robot materials do not need to comply with the CT or MRI scanners. The robotic system is designed so that only the end-effector interacts with the the surgical tools. Thus, the end-effector is the only part of the system that needs to be sterilized/able and covering the robot’s body with sterile drapes often suffices to avoid biological contamination. A lot of emphases was put on having a deterministic system with predictable movements. While this proposition has several ramifications, in terms of robot control, special care was taken to avoid reaching singularities or joint limits (semi-singularities) using a novel Inverse Kinematics method and to generate consistent and safe movements. The physical “dead man” switches and emergency stop buttons have been translated and implemented in software. The system receives external signals that, at the moment, are software generated but can easily be adapted to handle hardware triggers. Additionally, the joint brakes are activated whenever the robot is not moving and on top of that, the end-effector is designed to be: sterilizable, permit quick tool exchange, and in case of failure, allow the robotic system retraction to proceed with the standard tools. We opted to use a touch-based registration method similar to the one presented in subsection 4.3.2 as it does not require additional hardware. Other popular registration techniques using fiducial markers or the facial surface, can be later incorporated by taking advantage of the incremental system development model. The software implementation is actually where the previous arguments on component-based technology and its perks meet some of the requirements for surgical robots. Firstly, by partitioning the control code into functional blocks we create a structure that is easier to configure and validate. Each part of the controller is more clearly contained and has a detailed interface, so it can be individually and thoroughly tested. Special attention was paid to ensure that the framework where we implemented our system was real-time compliant. Any safety-critical robotic system should impose real-time constraints. It also adds to the system 4https://www.kuka.com/en-de/industries/healthcare/kuka-medical-robotics 66 predictability and consistency as it guarantees that the system meets absolute time deadlines. The critical components that require real-time run in a single host with an RTOS and communicate intra or interprocess. The HMI and the application layer components can run in the same host using a non-real-time scheduler or can run in another host within the same network. The developed software components are documented using Doxygen 5 and the Qt Creator plugin. The code is versioned under SVN 6 in an online repository. The component’s design and structure are well detailed according to the OROCOS framework standard. The system is set up in an Ubuntu 14.04 (kernel version 3.18.20) host patched with a Xenomai 2.6.5 RT-kernel. For a detailed description of the system and included components refer to chapter 6 5.5 Architecture Layout Before describing in depth each of the developed components, we introduce a representative diagram of the robotic manipulator controller’s architecture Figure 5.4. The structure is divided into two main groups: the Core and the Application. The Core group stands for a set of components responsible for executing coordinated taskand joint-space robot movements in a deterministic and reliable way, i.e. each action is uniquely and unequivocally defined and executed only when the entirety of the motion is possible. Unlike the Core group, the Application components are designed with the surgical task in mind. Its components form the bound between the user and the generic core functionality, handling coordinates and motion sequences adjusted to the task. This high-level controller was designed to keep a minimalist interface with the hardware setup and thus reduce the dependency on the device’s available API. In regard to the robot actuation, the interface between the Core components and the physical robot control unit is limited to the communication of joint variables (positions and external forces) at a defined time step. The components colored in blue (Procedure Coordinator, Task Supervisor and Robot State) represent ‘server units’, i.e. capable of communicating with components residing in other processes or across the network. The components colored in green are embedded in other applications. The Human Machine Interface component is integrated with the graphical user interface created using Qt, while the V-REP Plugin component is integrated in the plugin created for the V-REP simulator. Both components function as clients that interact with the blue components. 5.6 Setup description To evaluate the performance of the developed controller architecture, the solution was tested in a robotic manipulator, model KUKA LBR iiwa 7 R800 (c.f. Figure 5.5 and Table 5.1) with different end-effectors that interface the robot flange with the surgical tools. 5www.doxygen.org/ 6https://subversion.apache.org/ 67 «component» Cartesian Trajectory Controller «component» Kinematics and Statics Module «component» Task Supervisor «component» Joint Trajectory Controller «component» Robot FRI «component» V-REP FRI «component» Robot State «component» Transformations «component» Procedure Coordinator «component» Surgery Planner Application CORE Robot «component» V-REP Plugin «component» Human Machine Interface Figure 5.4: Schematic representation of the Modular Architecture developed. 5.6.1 KUKA LBR iiwa 7 R800 The LBR iiwa series (KUKA AG, Augsburg, Germany) are the first lightweight sensitive robots designed for human-robot collaboration scenarios. These are serial anthropomorphic manipulators with 7-DoF and highly sensitive joint torque sensors ( ±2% of maximum torque) distributed along its kinematic chain. The recently announced KUKA LBR Med product line goes one step forward to push their technology for an easier integration in the medical field. The International Electrotechnical Commission (IEC) released a Certification Body report attesting the LBR Med R800 and R820 to be compliant with the norms IEC 60601-1:2005 and IEC 60601-1:2005/AMD1:2012 for medical electrical equipment, as well as the norm IEC 62304:2006 for Medical Device Software – Software Life Cycle Processes. The similar structure and operation interface of the LBR iiwa and the LBR Med facilitates the transition between working with either model. Thus, one can assume that the transition of our solution to a LBR Med model would be seamless. Despite the similarities, the Med model includes redundant integrated torque sensors, internally routed cables and IOs (no exposed ports), hygiene-optimized surfaces along with a wide range of hardware and software safety features. 68 Figure 5.5: KUKA LBR Med 7 R800 model. Table 5.1: KUKA LBR iiwa / Med 7 R800 robotic arm specifications Axes 7 Payload 7 kg H. Reach 800 mm Weight 23.9 / 25.5 kg Repeatability 0.1 mm Installation Position Any Protection Class IP 54 Another disruptive and inviting feature of the KUKA LBR iiwa robots has to do with their Sunrise.OS programming interface that operates fully on JAVA - arguably the most popular programming language as of date. The advantages of this design choice are twofold: allow the user to make use of many of the language perks, and also the familiarity to work with JAVA instead of a proprietary language, as is common among other robotic manufacturers. The Sunrise.OS ships with the Sunrise.Workbench, the IDE used to install and configure the robot station, safety specifications, IOs; and where the robot applications are programmed, managed and synchronized with the controller. The software bundle provided by KUKA includes a couple of useful packages for fast application development, among them two are particularly relevant for this project: the Human Robot Collaboration and the Connectivity FRI. Certified by the norm EN ISO 13849-1:2008, the Human Robot Collaboration module enables the user interact with the robot in Performance level d with an architecture category 3, where single faults should be detected and do not cause a loss of the safety function. In practice the system enforces a safe interaction with torque/force, velocity monitoring and collision detection. From this module, a feature called “hand-guidance” was included in our project. This module permits the user to manually move the 69 entire robot structure, which can be particularly useful since it is the most intuitive and quickest method to conform the robot to the desired pose. The hand-guidance mode imposes virtual constraints to avoid driving any joint beyond their limits and takes into account the dynamics of the robot and any attached tool for gravity compensation. The Connectivity FRI package provides the FRI (historically Fast Research Interface (Schreiber et al., 2010)) an interface via which the robot controller can communicate continuously and in real-time with an external system, Figure 5.6. It was design for laboratory experimentation with the purpose of developing new applications and algorithms to control the LBR iiwa robot. To operate the robot through FRI three Figure 5.6: KUKA Sunrise.Connectivity FRI overview. elements are needed: the robot application (server) coded in JAVA and executed directly in the Sunrise Cabinet, the client application coded in C++ and running in a remote host, and a FRI channel. This channel connects the remote host to the cabinet through a physical Ethernet connection using a UDP protocol for best-effort communication of binary data packages. The client of the FRI connection is described in more detail in subsection 6.6.1. Communication via FRI is established via the KUKA Option Network Interface (KONI) that provides a rea-time capable Ethernet connection with clock rates up to 1ms. Once the connection is configured and established the server requires a continuous and steady communication with the client at the agreed frequency, hence the reason for the real-time requirements of the control architecture components. A mismatching latency, communication jitter or dropped communication messages deteriorates the connection quality, which is usually labeled as POOR, FAIR, GOOD or EXCELLENT. Connection state affects directly the session state, a parameter that defines if the FRI connection is in a monitoring state (MONITOR_WAIT or MONITOR_READY) or in a commanding state (COMMAND_WAIT or COMMAND_ACTIVE), Figure 5.7. In a monitoring state, only server-to-client uni-directional communication is supported. While the FRI connection defaults to a monitoring state (independent from the connection quality), it can only transition to a commanding state if the quality rises to GOOD or EXCELLENT. In any session state, the client receives information about the connection state and quality; axis-specific coordinates such as, actual position, torque, setpoint position and torque, external torque; time stamps and other operation related variables. If the connection is at COMMAND_ACTIVE the client can then send new target joint positions for the robot to follow. Note that, the controller internally does the best effort to reach the target coordinates from the 70 START MONITOR_WAIT MONITOR_READY COMMAND_WAIT COMMAND_ACTIVE newFRISession Connection Dropped Quality≥ GOOD Connection from Client Accepted FRIJointOverlay End of Motion Overlay Figure 5.7: Connectivity.FRI session state diagram. current position within a time-step disregarding the torques or velocities generated. Despite the velocity and acceleration limits imposed for the joint trajectory interpolation 6.3, a software limiter was implemented at the lowest level (immediately before sending the coordinates to the robot controller) to guarantee that the commanded joint position displacement always falls within a safe threshold. Project Management Although the high-level control of the robotic system is handled by the control architecture, our solution still depends on the robotic controller 7 to drive the joints and provide feedback about the system state. A robot application was developed to combine the remote real-time control through FRI connection with the certified hand-guidance mode to re-position the arm. The program was implemented as a state-machine with three basic states: Initial or Stopped state, Hand-guidance state and the Remote control state. The transition between states is event-driven and triggered either from the designed end-effector (refer to subsection 5.6.2) or directly from the pendant console. Transition to any of the actuation states (Hand-guidance or Remote) involves a continuous user-input, or a two-step confirmation. If any exception is caught or the communication lost, the robot defaults to the Stopped state. The FRI Session parameters are configured during the initialization of the robotic application. During this step, the local and remote IP and port addresses are specified along with the time-step and the receiver multiplier 8 . The communication with the remote host is set to an update cycle of 5 ms with a multiplier factor of 1. 5.6.2 Other equipment: Micro-drive and Designed tools To guarantee a correct fit between the surgical tools (e.g. the micro-drive and the drill fittings) and the robotic system, a custom end-effector was designed and created. The Leibinger micro-drive for stereotactic 7the Sunrise Cabinet in this case 8 Factor that defines the rate at which the remote host should communicate with the controller, actual_rate = send_rate × factor. 71 <<Interface>> Surgery Trajectories + AddTrajectory(...): bool + RemoveTrajectory(...): bool + ShowTrajectory(...): bool + SetSurgeryReference(...): bool <<Interface>> End-effector Change + GetCurrentEndEffector(...): bool + AttachEndEffector(...): bool + MoveEndEffectorTool(...): bool V-REP Robot Component - Component Period: double Current Joint Velocities Target Joint Positions Current Joint Positions V-REP End-Effector Component  - Component Period: double Next Joint Positions End-Effector Position End-effector Change V-REP Trajectories Component Surgery Trajectories Target Hollow Joint Positions Current Hollow Joint Velocities Current Hollow Joint Positions <<Interface>> Surgery Trajectories + AddTrajectory(...): bool + RemoveTrajectory(...): bool + ShowTrajectory(...): bool + SetSurgeryReference(...): bool <<Interface>> End-effector Change + GetCurrentEndEffector(...): bool + AttachEndEffector(...): bool + MoveEndEffectorTool(...): bool «components» V-REP Plugin Figure 5.15: OROCOS components embedded in the developed V-REP plugin. simulation cycle, the mediating concurrent queues that store the information from the control architecture are inspected, and the corresponding V-REP Regular API functions called from the main application running thread9. The V-REP Robot component is responsible for exchanging information about the robot joint coordinates between the control architecture and the virtual robots. These messages are exchanged at a steady frequency, defined by the Component Period parameter, Figure 5.15. Similar to the case with the real KUKA robot, the information exchanged between the control and the machine is limited to joint coordinate data. The V-REP Trajectories component allows the user to create visual cues related to the surgical trajectories. These trajectories are pure-graphical and semi-opaque shapes generated from the target and entry coordinates relative to the surgery reference frame, Figure 5.16. Finally, several end-effector models were added to the scene, some can be actuated along their working axis, Figure 5.17 and 5.18. The V-REP End-Effector Component allows the user to switch between different end-effectors, as well as to control their tool movement (if actuated). 5.8 Task Description As presented in chapter 2, stereotactic neurosurgery is inherently a strict, well-defined procedure in terms of surgical target positions and how to approach them. Objectively, the stereotactic surgical can be summarized to the guidance of tools along linear trajectories. This problem was previously discussed (see subsection 4.4) for the use of an industrial manipulator. The role of the robotic system is similar to the previously presented industrial robot adaptation. However, since the robot is for the most part controlled from the 9Calling Regular API functions from the communication threads results in faulty behavior. 78 Figure 5.16: Virtual representation of the surgical trajectories in the V-REP scenario. Figure 5.17: Concept of probe guide end-effector with actuated depth control. Figure 5.18: Concept of trepan guide end-effector with actuated depth control. modular architecture, one has more thorough control over each action, the capacity to finely tune each action to the task requirements, and the ability to react in real-time and within a single control cycle to any event. Moreover, the flexible structure of the control architecture permits a seamless addition or replacement of components or processes and facilitates porting the solution to different actuators. The robotic manipulator assistance in a stereotactic surgery can be broke down to two steps: i) the Approach to the selected trajectory, and the ii) Tool Handling - linear movements along the entry-target trajectory, Figure 5.19. 79 START Registration Surgery Reference Point Selection Point touch with Actuator Compute Registration Matrix Surgery Plan Input Target and Entry Points Confirmation of Surgical Trajectories Procedure Coordinator - Approach Trajectory Selection Pose and Nullspace Selection FLE <εReady Autonomous Hand-Guidance Nullspace Motion Hand-Guidance Motion Arc / Joint Motion Final Adjustment Verify and Lock Selected Trajectory Aligned with Trajectory Procedure Coordinator - Tool Handling Tool Change Position Drill Position Tool Insertion Position Return to Home Position END Figure 5.19: Diagram of Stereotactic Neurosurgery assistance tasks performed by the robotic system. The user may choose to perform the Registration or input the Surgery Plan first, but both steps are required prior to initiating the surgical procedure. If the system operator chooses to perform the intra-operative registration first, then an interface is prompt to introduce the coordinates of the known registration points relative to the surgery reference frame. Once concluded, the robot changes to an hand-guidance mode and the end-effector is manually driven to touch the -registration points, all-at-once or each one sequentially. The two sets of registration points, relative to the surgery reference frame and the robot base frame, are processed by an algorithm that outputs the rigid transformation that relates both references. The transformation is accepted if the Fiducial Localization Error (FLE) is below an established error threshold (ε). Since the current system has no integration with the surgery planning software, the user is required to manually introduce the surgery entry and target coordinates (relative to the surgery reference frame) in the system. When both steps are concluded the system shifts to the Procedure Coordinator mode. For each surgical trajectory, the robot operation starts with the Approach step. When a trajectory is selected, the system performs a feasibility analysis to select the most appropriate pose and nullspace 80 configuration to approach the trajectory, guaranteeing the motion feasibility also expected for the tool handling step. The best fitting robot poses and configurations are reported to the user who can select the solution that better suits the workspace environment (pre-visualization in a virtual scenario). Once the robot pose and nullspace solutions are chosen for the selected trajectory, the robot attempts to move from its home position to the respective approach trajectory pose. The system permits two different interactions, autonomous or hand-guided motion. In autonomous mode, the robot first executes a nullspace motion to the selected nullspace solution and then moves to the final pose. Alternatively, the robotic system user may manually drive the manipulator to a pose proximal to the selected trajectory in task space, keeping an appropriate configuration. The robot then performs a small adjustment to exactly fit the end-effector to the linear trajectory defined by the entry and target points. With the robot-end effector aligned with the surgical trajectory, the Tool Handling step begins. Since the surgical trajectory is described by a line in space, the relevant robot poses in space are completely described by the distance of the end-effector to a known point along the trajectory, the entry or target point, Figure 5.20. The approach point refers to the final position of the approach movement, i.e. the point in Figure 5.20: Relevant procedure points defined along the entry-target trajectory. In the target-entry direction and from the target point one finds the tool point at a distance of dtool . In the target-entry direction and from the entry point one finds the approach point at a distance of dapp , the tool-switching point at a distance of dtool and the skull drill point at a distance of ddrill. task space where the end-effector will first align with the entry-target trajectory. This point is distanced of 200 mm from the entry point. The tool-switching point marks the position to where the robot retracts so the user may switch between surgical tools. This point is distanced from the entry point of 100 mm. The drill point refers to the point where the robot should hold its end-effector to passively assist the neurosurgeon during the scalp incision and skull drilling step. The position of this point is measured relative to the entry point and the distanced is affected by the length of the drill bit, in order to stop its advance at the interior wall of the skull. Finally, the tool point marks the position where the robot end-effector will be placed in order to allow the surgical tool holder to drive the probe, electrode, etc. to its final position. The position of the tool point is determined relative to the target point and depends on the length of the surgical tool used. When the procedure for a single trajectory is concluded, the user is prompt to return the robotic arm to its home position. The robot operator may choose to let the manipulator move autonomously to its home position. If so, the robot executes a two-step motion, first a linear motion until reaching the approach 81 position and then the final arc/joint motion. Alternatively, the operator may manually drive the robot away from the surgical trajectory using the hand-guidance module. Before starting a new surgical trajectory procedure, the robot always starts from its home position. For a better understanding of the procedure workflow and user-interaction refer to the video at https://youtu.be/OllE5Cr4sls. 82 Chapter 6 Developed Components In this chapter we list the different components that form the high-level control architecture. As previously mentioned, these components are split into two layers, the Core Layer that includes: • Cartesian Trajectory Controller; • Kinematics and Statics Module; • Joint Trajectory Controller; • Robot State and Transformations; • Task Supervisor; • Fast Research Interface. The second layer - Application Layer - includes: • Registration; • Surgery Plan; • Procedure Coordinator. Finally, the Graphical User Interface is also described. Despite being an application, it includes OROCOS components that act as proxies to communicate directly and transparently with the high-level control architecture. 6.1 Cartesian Trajectory Controller The Cartesian Trajectory Controller is the component assigned with generating timed trajectories according to the desired robot’s end-effector motion in task space. It is based on the open-source Orocos Kinematics and Dynamics C++ Library - KDL. The component provides an interface to execute Cartesian space trajectories such as: i) linear, ii) arc, iii) composed, iv) nullspace motions if the robotic manipulator is intrinsically redundant or v) a sequence of the previous. The concept of trajectories should be understood as a combination of two concepts, the geometric path drawn in tri-dimensional space and the implicated velocity profile. 83 The component provides an interface to generate robot trajectories in task space. It outputs a vector with frames, generated at a fixed time sampling rate and the time-stamp of each frame the end-effector should pass through, Figure 6.1. The component’s interface is mainly composed of operations that can be called to request a specific type of trajectory with the associated parameters. Once the trajectory frames are computed, the result is dispatched to the Kinematics and Dynamics Module. Thus, this component does not operate with real-time requirements. Cartesian Trajectory Controller - Max Vel: double - Max Acc: double - Time Step: double Generate Trajectory Vector Poses & Times Error Port <<Interface>> Generate Trajectory - Eq. Radius: double + GenLinearTrajectory(...): void + GenArcTrajectory(...): void + GenComposedTrajectory(...): void + GenPauseTrajectory(...): void + GenNullspaceTrajectory(...): void <<Interface>> Vector Poses & Times + GetTrajectory(...): void Figure 6.1: Cartesian Trajectory Controller component and interfaces. 6.1.1 Geometric Path The required end-effector geomtric path is either defined by a function describing its shape in space (point, linear, arc or nullspace motion) or by a set of ‘way-points’ (composed motion). In the end, both functions and way-points create a path in space that defines the end-effector position and orientation using poses. For clarity, a pose is henceforth referred to as a transformation frame, or simply frame. 6.1.1.1 Linear Path The interface to create a linear path comprises a start and an end frame related to the same reference system, along with an object to mediate the rotation interpolation. If the linear path is a pure translation - the start and end frames share the same rotation sub-matrix - then it can be described by a vector in space relating the start and end frame positions, Figure 6.2a. On the other hand, if the linear path involves also a rotation, an interpolation algorithm is applied to rotate from the initial frame rotation to the final rotation. The algorithm takes both rotation matrices and determines the resulting axis-angle rotation, and then linearly interpolates the angle around the determined axis, Figure 6.2b. 6.1.1.2 Arc Path Two alternative interfaces are provided to generate arc or circular paths in Cartesian space. The first option requires a start frame, an arc center position in the same reference system as the start frame, an arc direction vector that specifies the plane where the arc is inscribed, an arc angle scalar and an end rotation 84 0.5 0.4 0.3 0.2 0.1 0 -0.04 0 0 0.04 (a) Linear trajectory executed along the start frame z−axis with no rotation. 0.5 0.4 0.3 0.2 0.1 0 -0.04 0.04 0 0 (b) Linear trajectory executed along the start frame z−axis with a rotation of 45◦around the same axis. Figure 6.2: The required start and end frames are represented with larger coordinate frames. The smaller coordinate frames represent the intermediate frames which the end-effector should pass through. matrix. The plane where the arc is inscribed ( ˆvarc ), is defined by the external product of the arc direction vector ( ˆvdir ) and the unit vector ( ˆvcen ) computed from the start frame position to the arc center position (Figure 6.3), ˆvarc =ˆvdir ׈vcen (6.1) Figure 6.3: Example of arc generated with the variables specified in equation (6.1). This option provides complete and explicit control over the arc geometry in Cartesian space. The second interface requires the start and end frames, and a point non-colinear with the line that crosses both frame positions ( pref ), the reference point, Figure 6.4. The position of the start, end-frames and the reference point define the plane where the arc is inscribed ( ˆvarc ). The center point of the arc is found along the line that bisects the segment from the start to the end frame positions, at a distance of the of the segment length from both points. Similarly to the linear path, the rotation interpolation is applied if the start frame’s rotation sub-matrix differs from the end rotation matrix, Figure 6.5. 85 Figure 6.4: Example of arc generated with the start, end frame and the reference point. 6.1.1.3 Composed Path The composite segment is a path generated from a set of ordered way-points. The component interpolates between the provided way-points following two different approaches. Depending on the motion required, one might specify: i) mandatory way-points, i.e. the generated path links linearly and inclusively the specified input frames (Figure 6.6a); ii) proximal way-points, i.e. the generated path makes an over-fly near a way-point to avoid sharp turns (Figure 6.6b). A path involving proximal way-points requires the user to provide an additional variable that codes the radius of the arc created to transition between line segments. Lastly, there is a concept intrinsic to any of the path modalities previously presented that is fundamental to the determination of the velocity profile, the path length. The path length associated with translations is no more than the sum of the Euclidean distances between each segment start and end positions. While the concept might be straightforward when the motion is reduced to a pure translation, there is not a direct correlation between path length and the amplitude of a rotation or a nullspace motion. In order to maintain a coherent and safe time-frame for the generated path, a metric is used to quantify these non-translation movements into a quantifiable path length. To account for the rotation, a concept named ‘equivalent radius’ is introduced. This variable is a parameter to the path function interfaces and establishes the radius of a hypothetical arc formed by the equivalent rotation (axis-angle notation). With this heuristic, it is possible to convert an angle into a representative path length, i.e. the arc distance. When the requested path transformation involves both translation and rotation, the final path length is equal to the greatest scalar between the Euclidean distance of the translation and the rotation equivalent arc distance. Two factors code the scale of both translation and rotation path lengths, in order for them to complete their paths synchronously. Thus, the largest path length part has a scale factor of 1, while the smallest counterpart has a scale that matches the ratio between path lengths. 86 0.2 0.1 0 0.2 -0.04 0 0 0.1 (a) −90◦ vertical arc trajectory ( xz − plane) with no end-effector rotation. 0.2 0.1 -0.2 -0.1 0 0.04 0 0 (b) 90◦ horizontal arc trajectory ( yz −plane ) with no end-effector rotation. 0.2 0.1 0 -0.04 -0.2 -0.1 0 0 (c) 90◦ vertical arc trajectory ( xz−plane ) with an end-effector rotation around the y− axis of 90◦. 0.2 0.1 0 0 0.1 0.2 0.04 0 (d) −90◦ horizontal arc trajectory ( yz−plane ) with an end-effector rotation around the x−axis of −90◦. Figure 6.5: Arc trajectory examples in orthogonal planes for easier visualization. Both the arc radius, angle, plane and end-effector orientation can be finely tuned. 6.1.1.4 Nullspace Path We call ‘nullspace motion’ to the joint motion of the robotic manipulator without altering its end-effector frame in relation to its base reference system, a property shared by kinematically redundant manipulators. Although at first it makes little sense to introduce the concept of nullspace as a part of the Cartesian Trajectory Controller component, in practice this possibility to adjust the robot posture without moving its end-effector will prove useful as a mechanism to handle secondary objectives 1 . The nullspace motion is parametrized by a single coordinate ( ψ ) and a single end-effector pose that is maintained throughout the 1We will further elaborate on the topic of ‘nullspace’ and its applications on the section 6.2. 87 between: (b)ase, (e)lbow, (w)rist and (f)lange (see Figure 6.9). Figure 6.9: Manipulator generic structure, joint variables and DH frames assigned. The 7-DoF manipulator model of LBR iiwa 7 R800 from KUKA AG is used to depict the shape of an anthropomorphic arm without offsets. The forward kinematics problem is easily solved once the DH parameters are determined. Four parameters are assigned to each joint, which convert to a transformation matrix that establishes the relation between one assigned reference frame (i−1) and the next (i), i−1Ti=         cos θi−sin θicos αisin θisin αiaicos θi sin θicos θicos αi−cos θisin αiaisin θi 0 sin αicos αidi 0 0 0 1         .(6.4) The product of these matrices from the base to the flange, 0T7=0T11T22T33T44T55T66T7 , returns the manipulator’s pose in task space. 6.2.2.1 Self-Motion Parameters To address the global and local self-motion manifolds, two additional parameters are introduced in the calculation of the inverse kinematics. The first parameter – Global Configuration ( GC ) – uniquely specifies the branch of the inverse kinematics solutions for the global configuration manifold. The second parameter – arm angle ( ψ ) – introduced by Hollerbach (1985), indicates the elbow position in the redundancy circle. Both the GC and ψ variables are directly determined in the forward kinematics problem from the joint angles, and are passed as parameters to the proposed inverse kinematics algorithm. The Global Configuration GCk parameter is split into 3 variables that express the sign of the joint angle coordinates at the shoulder ( GC2 ), elbow ( GC4 ) and wrist ( GC6 ). These variables subsequently exert control over the manipulator’s arrangement in space. The GCkis given by, GCk=     1,if θk≥0 −1,if θk<0. ,∀k∈ {2,4,6}.(6.5) The manipulator’s arrangement in space is directly affected by the GCk at the shoulder, elbow and wrist. The arm angle, represents the angle formed by the shoulder-elbow-wrist plane (SEW) and the reference plane (SE v W), as shown in Figure 6.10. The elbow redundancy depends solely on the structure of the manipulator and on the shoulder-wrist vector. The center of the ψ circle is located at half the distance 94 along the straight line connecting the shoulder to the wrist, and its curve is inscribed in the plane defined by the shoulder-wrist vector. (a) Representation of the arm angle ( ψ ), as the angle between the real ( E ) and virtual elbow (Ev) in the redundancy circle. (b) Configuration of the real and virtual manipulator at the same pose. Figure 6.10: The “real manipulator” depicts a 7-DoF manipulator at an arbitrary pose. The “virtual manipulator” is the non-redundant (6-DoF) replica of the same manipulator reaching the same pose. The real manipulator is transformed into the virtual manipulator by blocking the 3th joint at zero (θ3= 0). The selection of the reference plane is not straightforward and has been a topic of discussion in several works (see e.g. (Shimizu et al., 2008; Yan et al., 2014)). The first approach (Kreutz-Delgado et al., 1992; Dahm and Joublin, 1997) was to define the reference plane based on the shoulder and wrist positions, and an arbitrary vector. Whilst this method is the easiest to compute and establishes a clear relation between the arm angle and the task space, it also raises algorithmic singularities that derive from the possibility of the arbitrary vector being collinear with the shoulder-wrist vector. In this case, the reference plane can no longer be determined since the vectors that describe the plane are linearly dependent. Shimizu et al. (2008) introduced a workaround to the algorithmic singularity by defining the reference plan based on a virtual manipulator that is a non-redundant image of the original 7-DoF manipulator, with θ3= 0 . If no joint limits are imposed to the virtual manipulator, provided a target pose within its workspace, one can find a finite set of solutions for the inverse kinematics problem. These solutions, depend on the virtual manipulator configuration, which was not addressed in the referred paper. Yan et al. (2014) presented the “dual arm angle” algorithm, which uses two perpendicular arbitrary vectors to define the reference plane, following the same approach as Kreutz-Delgado et al. (1992). The algorithm follows the principle that the shoulder-wrist vector cannot be collinear with both arbitrary vectors at the same time, so when the singularity occurs for one of the arbitrary vectors, they switch to the other one to generate the reference plane. To develop an algorithm that solves global configuration self-motion along with the elbow redundancy, it should guarantee that: 1) the definition of the reference plane needs to account for the joint configuration, 95 the GC parameter; 2) the homotopic property within c-bundles should be extended to the nullspace definition – function of ψ . That is, any function in task-space should be continuously deformed into a function of the ψ parameter for a specific global configuration branch. The method from Yan et al. satisfies the first premise, but not the second. For this reason, we extended the method originally proposed by Shimizu et al. (2008) to consider the robot joint configuration. 6.2.2.2 Elbow redundancy The arm angle, ψ , is the angle between the vectors normal to the real manipulator SEW plane and the virtual (non-redundant) manipulator SE v W plane (Figure 6.10). There is however a small caveat to the statement, since the same plane can be defined by both its vector and its negative vector. This indetermination is relevant to our problem because the definition of the reference SEW plane vector depends on the global configuration of the virtual manipulator, i.e. if the elbow is upwards or downwards. To uniquely determine a reference plane vector, we associate the robot configuration variable GC to the calculation of the reference plane. It is easy to prove that the shoulder and wrist positions are common to the real and virtual robot for the same target pose (Figure 6.10). Therefore, we only need the elbow position of the virtual manipulator to find the reference plane and consequently calculate ψ . Inverse kinematic expressions for the 6-DoF virtual manipulator are applied to discover its elbow position. The joint positions of the virtual manipulator will be represented by a superindex θv i. Let the target pose be represented by a transformation matrix 0T7 , that combines a position component 0p7∈R3 and a rotation component 0R7∈SO(3) . To simplify the notation of the following equations, we list the vectors from: base to shoulder ( 0p2 ), shoulder to elbow ( 2p4 ), elbow to wrist ( 4p6 ) and wrist to flange (6p7) according to the DH parameters: 0p2=h0 0 dbsiT 2p4=h0dse 0iT 4p6=h0 0 dewiT 6p7=h0 0 dwf iT. The virtual elbow joint ( θv 4 ) is the first to be calculated, since it only depends on the manipulator’s kinematic structure and on the shoulder-wrist vector. The shoulder-wrist vector is calculated from: 2p6=0p7−0p2−0R76p7(6.6) and θv 4is computed using the law of cosines θv 4=GC4arccos k2p6k2−(dse)2−(dew)2 2dse dew !.(6.7) 96 Figure 6.11: Representation of the virtual manipulator and variables required for the θv 2calculation. The elbow configuration variable ( GC4 ) – the signal of the 4th joint angle – is required to uniquely define the reference plane (6.7). Since θv 3 is 0, the shoulder-elbow vector ( 2p4 ), as well as the elbow-wrist vector ( 4p6 ) are aligned in the xy -plane. The joint θv 1 is thus responsible for moving the virtual arm to the wrist xand yposition coordinates. However, if the shoulder-wrist vector ( 2p6 ) is aligned to the z -axis of joint 1 ( 0R1,z ), then joint 1 is no longer defined and an algorithmic singularity occurs. Hence, the calculation of θv 1branches into, θv 1=     atan2 (2p6,y,2p6,x),if k2p6×0R1,zk>0 0,if k2p6×0R1,zk= 0 .(6.8) The virtual shoulder joint θv 2 is the last missing variable to find the virtual elbow position. As can be seen in Figure 6.11, φcan be calculated from the law of cosines: φ= arccos (dse)2+k2p6k2−(dew)2 2dse k2p6k!,(6.9) and the value of θv 2, which depends on the elbow configuration, is: θv 2= atan2 q(2p6,x)2+ (2p6,y)2,2p6,z+GC4φ. (6.10) With θv 1 , θv 2 , θv 3 and θv 4 we can calculate the pose of the virtual elbow from the base ( 0Tv 4 ) using the forward kinematics (6.4). The normal vector to the plane ( vv sew ) is now calculated as the cross product of the unit vectors that link shoulder-elbow and shoulder-wrist: vv sew = 0pv 4−0pv 2 k0pv 4−0pv 2k!× 0pv 6−0pv 2 k0pv 6−0pv 2k!.(6.11) The normal vector to the real arm SEW plane ( vsew ) is calculated using the same formula with the position 97 of the real elbow instead, vsew = 0p4−0p2 k0p4−0p2k!× 0p6−0p2 k0p6−0p2k!.(6.12) Let ψ∈[−π, π] and in particular ψ be 0 at the reference plane. The sign of the arm angle parameter is determined as, sgψ=sgn hd vv sew ×d vsew·2p6i(6.13) and ψas, ψ=sgψarccos d vv sew ·d vsew.(6.14) Since the arm angle, ψ , is now determined, along with the joint configuration ( GC ) and the final robot pose ( 0T7 ), there exists a full description of the robot configuration in task space, which maps to an unique manipulator configuration in joint space. 6.2.3 Inverse Kinematics The described inverse kinematics algorithm is based on the standard approach of dividing the manipulator into the upper and lower arm, responsible for positioning and orientation respectively. In the same way that forward kinematics computes the final pose ( 0T7 ) and self-motion parameters ( ψ , GC ) from the joint positions, reciprocally, the inverse kinematics requires these three variables to generate a set of joint positions. The first step consists of determining the shoulder-wrist vector ( 2p6 ). The same equation used for the virtual manipulator (6.6) can be applied for the real robot, since the shoulder and wrist positions are coincident. Moreover, because the shoulder to wrist vector is the same, also the elbow joint ( θ4 ) can be calculated from (6.7). The θ4 is the only joint angle that does not depend on ψ . In order to determine the other joint angles, we need to find the position of the real manipulator elbow. To do so, first we calculate the virtual elbow pose at the reference plane ( 0Tv 4 ), following the algorithm described in section 6.2.2.2. The real elbow pose is no more than the virtual elbow pose, rotated around the shoulder-wrist axis (2p6) of ψ, 0R4=0Rψ0Rv 4(6.15) which is equivalent to 0R3=0Rψ0Rv 3,(6.16) because θ4is the same in the virtual and the real manipulator, hence 3Rv 4=3R4. The elbow redundancy rotation matrix ( 0Rψ ) codes the rotation of the angle ψ around the shoulder-wrist 98 vector (2p6), Figure6.10a. It is calculated using the Rodrigues’s rotation formula in matrix notation, 0Rψ=I3+ sin(ψ)hd 2p6×i+ (1 −cos(ψ)) hd 2p6×i2(6.17) where hd 2p6×iis the cross-product matrix for the unit vector d 2p6. Substituting (6.17) into (6.16) and following the notation in Shimizu et al. (2008), 0R3 can be obtained in terms of three auxiliary matrices As,Bsand Cs, 0R3=Assin(ψ) + Bscos(ψ) + Cs(6.18) where, As=hd 2p6×i0Rv 3 Bs=−hd 2p6×i20Rv 3 Cs=d 2p6d 2p6 T0Rv 3. The real values of θ1 , θ2 and θ3 are now determined analytically by combining the elements 2 of the 0R3(θ1,2,3)matrix – derived from DH parameters – in trigonometric operations, 0R3(θ1,2,3) =      ∗cos θ1sin θ2∗ ∗sin θ1sin θ2∗ −sin θ2cos θ3cos θ2−sin θ2sin θ3     .(6.19) The ∗ symbol indicates omitted elements of the matrix, which are not required for joint position calculations. It is important to note that these joint angles are subject to the global configuration parameter GC , thus: θ1= atan2 (GC2[as22 sin ψ+bs22 cos ψ+cs22], GC2[as12 sin ψ+bs12 cos ψ+cs12]) (6.20) θ2=GC2arccos (as32 sin ψ+bs32 cos ψ+cs32)(6.21) θ3= atan2 (GC2[−as33 sin ψ−bs33 cos ψ−cs33], GC2[−as31 sin ψ−bs31 cos ψ−cs31]).(6.22) Once we know the rotation matrix relative to the shoulder joints ( 0R3 ), it is straightforward to compute the rotation matrix of the wrist joints (4R7) 4R7=Awsin(ψ) + Bwcos(ψ) + Cw(6.23) 2The notation mij is used to specify the element of the Mmatrix at the ith line and jth column 99 where, Aw=3R4 TAsT0R7 Bw=3R4 TBsT0R7 Cw=3R4 TCsT0R7. and now we can analytically extrapolate the joint positions of the wrist joints from the elements of the 4R7(θ5,6,7)matrix, 4R7(θ5,6,7) =      ∗ ∗ cos θ5sin θ6 ∗ ∗ sin θ5sin θ6 −sin θ6cos θ7sin θ6sin θ7cos θ6     .(6.24) Respecting the global configuration parameter, we compute the remaining joint angles, θ5= atan2 (GC6[aw23 sin ψ+bw23 cos ψ+cw23], GC6[aw13 sin ψ+bw13 cos ψ+cw13]) (6.25) θ6=GC6arccos (aw33 sin ψ+bw33 cos ψ+cw33)(6.26) θ7= atan2 (GC6[aw32 sin ψ+bw32 cos ψ+cw32], GC6[−aw31 sin ψ−bw31 cos ψ−cw31]) . (6.27) In conclusion, the joint angles are uniquely determined based on a target pose ( 0T7 ), and two auxiliary parameters, the arm angle (ψ) and the joint configuration (GC). 6.2.4 Limits and Singularities Joint limits and singularity avoidance are two crucial conditions to guarantee a stable and feasible motion in task space. The described method maps joint limits and singularities as intervals of the elbow redundancy circle; these intervals specify whether ψ will drive the robot to a joint limit violation or singularity violation (see Figure 6.12). Feasible intervals are labeled Ψi,j , where the index indicates interval j of the ith joint. The final interval (Ψall) is the intersection of the feasible intervals of all joints, Ψall = 7 \ i=1 Ψiand Ψi= nj [ j=1 Ψi,j.(6.28) The novelty of the proposed method has to do with the specification of the joint configuration when mapping the joint limits and singularities to the redundancy space. To determine these intervals, we will use the relation between joint angles and the elbow self-motion expressed in section 6.2.3. These relations are described in joints of pivot-type through a tangent function (6.20), (6.22), (6.25) and (6.27), and in joints of hinge-type through a cosine function (6.21) and (6.26). It is important to stress that the elbow 100 Figure 6.12: Example of joint limits and singularity intervals inscribed in the elbow redundancy circle for joint i . The ψ(θl i) and ψ(θu i) represent the arm angles where the respective joint limits are met, and ψsing is a singular arm configuration. An avoid interval is set next to the singular point with δrepresenting the unilateral spacing. joint angle, θ4, does not depend on the arm angle and is therefore omitted from the following analysis. The expressions that represent the joint position, θi , as a function of the arm angle, ψ , can be summarized in two generic shapes: one for the pivot joints (i= 1,3,5,7) where3, θi(ψ) = atan2(GCk[ansin ψ+bncos ψ+cn], GCk[adsin ψ+bdcos ψ+cd]) (6.29) and one for the hinge joints4(i= 2,6), θi(ψ) = GCkarccos(asin ψ+bcos ψ+c).(6.30) where k= 2 for i∈ {1,2,3} and k= 6 for i∈ {5,6,7} . Having two generic functions to represent the relation between joint angles and the arm angle simplifies the analysis of the θi(ψ) function. It is possible to determine the intervals of ψ that correspond to feasible joint angles by solving these equations as a function of the joint limits, θl iand θu i. This is explained next. 6.2.4.1 Pivot Joints In order to determine the arm angle intervals that map to the joint limits, or the singular arm angle, we need to differentiate these expressions. Simplifying the notation of (6.29) let θi(ψ) = atan2(u, v) , where u=GCk(ansin ψ+bncos ψ+cn) v=GCk(adsin ψ+bdcos ψ+cd) 3an, bn and cn are the signed coefficients of the first input of atan2 in equations (6.20), (6.22), (6.25) and (6.27), whereas index ad, bdand cdrepresent the signed coefficients of the second input. 4a, b and care the signed coefficients of acos in equations (6.21) and (6.26) 101 its total derivative5is given by, dθi dψ =∂θi ∂u ∂u ∂ψ +∂θi ∂v ∂v ∂ψ =vu0 v2+u2−uv0 v2+u2.(6.31) After simplification we obtain the following expression, dθi dψ =atsin ψ+btcos ψ+ct v2+u2(6.32) where, at=GCk(cnbd−bncd) bt=GCk(ancd−cnad) ct=GCk(anbd−bnad). Using the Weierstrass-substitution method, the stationary points (ψ0) are given, if they exist, by, ψ0= 2 arctan  at±qa2 t+b2 t−c2 t bt−ct .(6.33) Depending on the value of a2 t+b2 t−c2 tthree cases may arise: •a2 t+b2 t−c2 t>0,with stationary points; •a2 t+b2 t−c2 t<0,without stationary points; •a2 t+b2 t−c2 t= 0,indeterminate joint angle. With stationary points The generic profiles of the function (6.29), when it has stationary points, is depicted in Figure 6.13. Stationary points represent the maximums, minimums or inflection points of the θi(ψ) function. Shimizu et al. (2008) state that stationary points can be proved to be either global maximums or minimums, and elaborate on how the joint limits can be represented on the elbow redundancy circle, based on this premise (Figure 6.13a). This assumption, however, cannot be extended to a general purpose manipulator. It is common in industrial manipulators to feature actuators with ample working range, close or even beyond the interval of [−π, π] . When the target pose and joint configuration ( GC ) are specified, the resulting joint positions may be near to a joint limit. Thus, a change in the arm angle may lead to a joint angle variation such that, θi(ψ)<−π∨θi(ψ)> π . This leads to a discontinuity because the atan2 function maps the arm angle to the joint angle in the [−π, π] range. When the joint angle overshoots this limit, the returned joint angle goes from π to −π or vice-versa (see Figure 6.13b). Hence, the stationary points can not be considered global maximums or minimums. Without stationary points If the function is monotonic, there are no stationary points and there is a one-to-one correspondence between ψ and θi . The function θi(ψ) is not fully defined in the interval 5In points where both partial derivatives exist, the function atan2(u, v)can be differentiated as arctan(u/v). 102 (a) Joint angle varies within [−π, π].(b) Joint angle variation crosses the {−π, π}boundary. Figure 6.13: Pivot joint θi(ψ)profile with stationary points. [−π, π] , due to the existence of a point of inversion, where the joint angle is discontinued (see Figure 6.14). This phenomenon has nothing to do with kinematic singularities, it derives from the range where atan2 (a) Discontinuity of joint angle at ψ= {−π, π}. (b) Discontinuity of joint angle at ψ∈ ]−π, π[. Figure 6.14: Pivot joint θi(ψ)profile without stationary points. maps the arm to the joint angle. Since the joint limits are commonly found within the [−π, π] interval, this inversion occurs from outside the joint limits and causes no trouble. Indeterminate Joint Angle A singular point exists if the third condition is verified ( a2 t+b2 t−c2 t= 0 ). The exact singular arm angle ψsing is directly determined from the stationary points of equation (6.33), ψsing = 2 arctan at bt−ct.(6.34) However, it cannot be labeled a stationary point because neither θi(ψ) nor its derivative is defined at that point. In a numerical approximation scheme it is not feasible to setup a condition that triggers on a single 103 timeitimef 1 1.5 2 (a) Different values of αand K= 0.1. timeitimef 1 2 1.5 (b) Different values of Kand α= 20. -1 -0.5 0 0.5 1 1.5 2 2.5 -1 -0.5 0 0.5 1 1.5 2 2.5 -1 -0.5 0 0.5 1 1.5 2 2.5 -1 -0.5 0 0.5 1 1.5 2 2.5 (c) Variation of the joint angles for different values of α(K= 0.1). -1 -0.5 0 0.5 1 1.5 2 2.5 -1 -0.5 0 0.5 1 1.5 2 2.5 -1 -0.5 0 0.5 1 1.5 2 2.5 (d) Variation of the joint angles for different values of K(α= 20). Figure 6.21: Variation of the arm angle (ψ) and joint angles along the trajectory. In conclusion, the Kinematics Solver uses the concept of global joint configuration control in positionbased inverse kinematics calculation and applies the same parameter to derive the expressions that map the joint angles to arm angle. This relation enables the mapping of the joint limits and singularities in the nullspace represented by the redundancy circle. It was shown a method to reliably compute intervals of feasible elbow positions to avoid joint limits and singularities. A simple metric to compute the arm angle from the interval of feasible arm angles was also discussed. Being an analytic solution, the time required for each operation is limited and quantifiable, which makes this method adequate for real-time control systems. This solution relies on simple mathematical operations that are implemented and fully optimized in mathematical software libraries, making it a very light and fast algorithm. The proposed algorithm is specifically relevant in scenarios where the task execution requires continuous and predictable motion execution, avoiding jumping between configurations, passing singularities or reaching joint limits. 110 6.2.6 Component’s Operation and Other Functionalities The component operates in one of two different modes, 1. Trajectory Following Control; 2. Online Trajectory Generation. In the Trajectory Following Control mode, the component receives a complete trajectory in task space frames from the required interface Vector Poses & Times (Cartesian Trajectory Controller) and processes it to generate a vector of corresponding joint positions. When the poses are received from the Cartesian Trajectory Controller component, the joint trajectory is processed when a call is made to the “Compute Trajectory” interface. It handles both the Cartesian space as well as the nullspace trajectories. If the Online Trajectory Generation mode is active instead, the component operates in real-time receiving the target end-effector frames from the “Target Frame” input port and computing the next target joint positions that then sent through the output port. In this mode, the component also calculates the external forces and torques at the end-effector from the measured external joint torques, using the statics solver (subsection 6.2.7). The “Kinematics” interface provides two methods to calculate Inverse kinematics. The Inverse Kinematics() requires the target end-effector frame as well as the two redundancy parameters GC and ψ . The Adjusted Inverse Kinematics() only requires the end-effector frame to calculate the joint position solution. The GC parameter is assigned to the current manipulator value and the ψ is calculated based on (6.39). The CalculateJointTrajectory() and CalculateNullspaceTrajectory() methods from the “Compute Trajectory” interface generate a joint trajectory from the vector of end-effector frames / nullspace parameter, following a logic similar to the scheme in Figure 6.18. Four different outcomes are possible: i) the joint trajectory is feasible and is forwarded to the Joint Trajectory Controller; ii) the trajectory is not feasible from the current ψ (needs to be adjusted prior to trajectory execution); iii) the trajectory is not possible under no ψof the current GC (requires a manipulator configuration change); iv) the trajectory is not feasible. In order to guarantee that the robot is capable of performing the required trajectory with a minimum deviation from the current ψ , two additional methods are available. The AnalyseTrajectory() checks through each of the trajectory’s frames arm angle intervals (for each configuration) Figure 6.19, and returns a set of possible ψ intervals for each configuration. The ScoresTrajectory() attributes a score to a discrete set of ψ values within the possible intervals, based on the distance from the current arm angle and on the need for a manipulator reconfiguration (change of GC ), and returns a code with one of the four possible outcomes referred above. Finally, the EvaluatePosesManipulability() method classifies a set of poses based on a metric involving the manipulability and the proximity to limits and singularities. The method evaluates all the input poses and, if inverse kinematic solution exist, it returns for each configuration the pose and arm angle that achieve the highest manipulability / distance to limits and singularities metric. More information about this method in subsection 6.2.6.4. 111 6.2.6.1 Interval Avoidance Strategy To analyze the trajectory feasibility in terms of the four possible outcomes, we applied a backtracking algorithm that combines the sets of feasible intervals ( Ψall ) from the last trajectory frame back to the initial frame. This process is carried out in the AnalyseTrajectory() method and requires an additional parameter - ψdmax ∈R+ - that codes the maximum arm angle displacement between frames / iterations. The algorithm returns a set of initial conditions that guarantee that the robot is capable of performing the received task space trajectory within the same configuration ( GC ) and with a maximum arm angle displacement (ψdmax) between iterations. Instead of using the feasible intervals calculated directly for each frame, the algorithm combines the intervals of the current frame ( Ψall(i) ) with what we call the “shadow of the intervals” ( Ψshd ) from the next frame in time. The “shadow intervals” are no more than the feasible intervals of the next frame in time ( Ψall(i+ 1) ) enlarged by ψdmax on each upper and lower bounds. Thus, the current frame feasible intervals are the intersection, Ψall(i) = Ψall(i)∩Ψshd,(6.43) which guarantees that if the arm angle of the current frame is in a feasible interval, it can proceed to a feasible interval of the next trajectory frame (Algorithm 1). input :traInt a vector of trajectory intervals of size sz,ψdmax output:final feasible intervals finInt // Create the ''shadow'' intervals shdInt vector 1shdInt ←vector(sz, interval_set(empty)); 2shdInt[sz] = traInt[sz] 3for i←sz −1to 0do 4foreach interval in traInt[i+ 1] do // Truncate intervals between [−π, π] 5lo ←max(interval.lower() −ψdmax,−π); 6up ←min(interval.upper() + ψdmax, π); 7shdInt[i]←shdInt[i]∪interval_set(lo, up) 8end // Intersect ''shadow'' with current intervals 9shdInt[i]←shdInt[i]∩traInt[i] 10 end 11 finInt =shdInt[0] Algorithm 1: Algorithm to adjust feasible intervales of a trajectory. 6.2.6.2 Nullspace Scoring From the trajectory analysis, one obtains a set of feasible arm angle intervals for the different global configurations. In order to select the appropriate initial pose for the robotic manipulator to execute the 112 Figure 6.22: Example of standard trajectory with feasible intervals computed from the method presented in subsection 6.2.4, and after applying the interval adjustment algorithm. trajectory, we created an algorithm that scores the feasible nullspace intervals ( ψm ). The goal is to assign the lowest score to the redundancy parameters better suited to perform the trajectory - minimize the distance to the current parameters and maximize the distance to joint limits and singularities. These intervals are initially discretized into evenly-spaced arm angle samples with the default separation step of ∆s= 1◦. ψs=n∆s+ψl m, n = 0,1,··· , N (6.44) where, N=&ψu m−ψl m ∆s'(6.45) Then, the score of each sample (σ(ψs)) is calculated as a variation of (6.39), σ(ψs) = kψ−ψsk+ K ψu m−ψl m 2  e −αψs−ψl m ψu m−ψl m−e−αψu m−ψs ψu m−ψl m  (6.46) thus, the lowest the score, the best arm angle alternative. The scoring mechanic was implemented so that the end-user can ultimately alter the arm angle to a position that better fits the workspace while still accounting for a robot configuration that can accomplish the trajectory. 6.2.6.3 Nearing Singularities Another question is raised when the manipulator moves along a trajectory composed of a sequence of frames or continuously follows an online trajectory with short time intervals and approaches a singular configuration. 113 As defined in subsection 6.2.4, the strategy to handle singular arm angles - ψsing - detected in pivot joints, is to set an avoiding interval, [ψsing −δ, ψsing +δ] , to surround the singular point. The singular point is detected when a2 t+b2 t−c2 t from the coefficients of 6.33 is zero, or below a tolerance value in real systems. If the singularity interval is created only when the condition verifies, the manipulator’s current arm angle might be near or already within the singularity interval. If that is the case, the manipulator does not have time to adjust the arm angle to avoid the interval and pass near the singular point, with the consequent position discontinuity. While this phenom can be predicted in trajectories known beforehand, it is unavoidable when the component is in the “Online Trajectory Generation” mode following an end-effector frame that is updated in short time intervals. To cope with this situation, we adjust the singularity interval ( δ ) with the proximity to the singular point, which occurs when a2 t+b2 t−c2 t tends to zero. Consider the variable ε⇐ ka2 t+b2 t−c2 tk that codes the “nullspace proximity” to a singular point, and two additional variables: •δmax ∈R+, parameter that codes the maximum δ; •dε∈R+, parameter that codes the border proximity εof a singular point vicinity; The δis then calculated according to the following quintic polynomial function, δ=     δmax 10 ε−dε dε3−15 ε−dε dε4+ 6 ε−dε dε5,if ε < dε 0,if ε≥dε ,(6.47) which generates the smooth transition curve (Figure 6.23), that allows the manipulator to gradually avoid Figure 6.23: Singularity avoid interval as a function of the “nullspace proximity” parameter. going near singularities. 6.2.6.4 Evaluating Poses Up to this point, we were capable of mapping the limits and singular points of the robot mechanism to the nullspace of the inverted kinematic expressions. By doing so, we can take advantage of the robot’s redundant structure to navigate through full-defined task space poses or trajectories. This method implements an 114 optimization function to solve extra degrees of redundancy, for example, whenever the task does not completely define the position and orientation. The proposed pose evaluation method takes as inputs the pose variations relative to one or more redundant coordinate(s) (position and/or orientation), and grades the inputs based on a metric that involves the manipulability score and the distance to limits and singular points that will henceforth be named modified manipulability for simplicity, see (6.54). As for the applicability of the algorithm, consider the problem of selecting the manipulator pose to align with the linear electrode insertion trajectory (target to entry point). The axial symmetric property of the electrodes leads to a redundant orientation coordinate, around the axis that aligns with the trajectory. Manipulability. Introduced by Yoshikawa (1985a,b) it directly relates to the “easiness of changing arbitrarily the position and orientation”. It is commonly chosen as a metric to evaluate the robot posture in the workspace to perform a given task. Based on the geometric Jacobian matrix ( J(q) or simply J ), which maps the joint velocities to the end-effector linear and angular velocities, the original manipulability formula measures the distance to singular postures, wstd =√det J JT.(6.48) The standard manipulability ( wstd ) can be rewritten as the product of the singular values of the Jacobian matrix found by Singular Value Decomposition (SVD). This analysis yields the singular vectors of the Jacobian matrix, which span the manipulability ellipsoids, a common graphical representation task space, Figure 6.24. As previously noted, anthropomorphic robotic manipulators are usually split into the upper arm Figure 6.24: Manipulability ellipsoid of a KUKA LBR iiwa R800 kinematic structure at θc= [0.0,15.0,0.0,−60.0,0.0,30.0,0.0] degrees. The shape of the ellipsoid represents the relation between the joint and end-effector velocity. It is more elongated along the axis where the same joint velocity translates to a higher end-effector velocity. and the wrist, respectively associated with the translational and rotational parts. In 1991, Yoshikawa (1991) revised the total manipulability formula (6.48) to consider the translational and rotational manipulability 115 measures. To represent the new manipulability formula for general serial-link manipulators, which we apply in our algorithm, two new representations are important. First, consider the Jacobian matrix, which can be parted in the translational (JP) and rotational component (JO), J= JP JO .(6.49) Second, the inverted Jacobian matrix of any manipulator that is determined from the right pseudo-inverse (J†), J†=JT(JJT)−1.(6.50) Now, the general case manipulability is determined as the product of the translational and rotational manipulabilities, w=wP,s wO,w.(6.51) where, wP,s =qdet JP(I−J† OJO)JT P(6.52) wO,w =qdet JOJT O.(6.53) The translational manipulability factor ( wP,s ) is determined as a “stricter” definition of the term, i.e. considering the rotational velocities input to be null (ωe= 0). The manipulability measure quantifies the proximity to singular postures but it does not account for mechanical joint limits (Vahrenkamp et al., 2012). Taking advantage of the method that maps these limits in the nullspace (refer to subsection 6.2.4) we were able to combine both the factors into a single metric, the modified manipulability (walt), walt =wg1−e−αψ−ψl m+ e−α(ψu m−ψ).(6.54) With the classifier metric defined, one can evaluate and grade each input “redundant pose”. Similar to other redundancy resolution strategies presented, the received poses are evaluated per configuration, that is, solutions are only compared when they belong to the same GC . For each pose evaluated, the algorithm determines the feasible arm angle ( ψ ) interval. A spacing parameter is used to transform the feasible arm angle interval (see Figure 6.19), into a discrete set of ψ values. The modified manipulability is calculated for each arm angle of each pose tested. These scores are then processed and for each GC , the pose and arm angle that have the highest score are returned along with the respective score. The global configurations GC that have a feasible solution are ordered according to their respective scores as well. The redundant poses may be evaluated for a specific or for all global configurations. Additionally, and as noted in subsection 6.2.5, arbitrary arm angle avoidance intervals may be added to further condition the feasible interval. 116 6.2.7 Statics Solver The Statics Solver is the object responsible for determining the relationship between forces / torques applied to the end-effector and forces applied to the revolute joints. At the moment, the solver is simply tasked with the computation of the external forces and torques exerted on the robot’s end-effector in task space. Siciliano et al. (2009), used the principle of virtual work to demonstrate this relationship in mechanical manipulators at elementary displacements (dq). The manipulator is at a state of static equilibrium when the elementary work ( dWτ ) performed by the joint torques ( τ ) equals the elementary work performed by forces (fe) and torques (µe) actuating at the end-effector (dWγ) with, dWτ=τTdq(6.55) and dWγ=fT edpe+µT edωedt, (6.56) where dpe stands for linear displacement and dωedt for angular displacement. According to the kinetostatics duality property, one can apply the geometric Jacobian matrix, ve= ˙pe ωe =J(q)˙q (6.57) to reciprocally map the forces/torques in Cartesian space (γe) to the calculate the joint torques, τ=JT(q)γe(6.58) Inverting the equation yields, γe T=J−1(q)τT.(6.59) Due to the redundant nature of the manipulator, the Jacobian matrix can be inverted using theright pseudo-inverse. The torques measured at each joint motor (τm) are a combination of, τm=τgrv +τdyn +τext +τerr (6.60) the torques originated from gravity ( τgrv ), the torques originated from joint accelerations ( τdyn ), the torques resulting from external forces ( τext ) and the torque disturbances caused by friction, measurement noise and modeling errors ( τerr ). Knowing that the Robot FRI component provides the current joint positions as well as the current external joint torques, one can immediately compute the current external forces and torques applied to the end-effector. 117 6.3 Joint Trajectory Controller The Joint Trajectory Controller is the component responsible for joint space trajectory planning (Figure 6.25). It builds on two libraries: the Reflexxes Type II (Kröger, 2010) (point-to-point - PTP Library) and a custom developed library to handle motion through a sequence of points (motion-through-points - MTP Library), each will be addressed below. This component generates smooth and time synchronous joint space trajectories within the specified velocity and acceleration limit values. The Joint Trajectory Controller component communicates with the proxy component that connects to the real or virtual robot and operates in a real-time. The component has three basic operation modes: i) the PTP motion, where the robot receives the reference joint coordinates and executes a joint space motion; ii) the MTP, where the robot receives a vector of joint coordinates as well as the time law and interpolates between the points to execute a controlled robot motion in Cartesian space; iii) the position online trajectory generation or OTG motion, where the component continuously receives reference joint coordinates (at a lower sampling frequency) and interpolates between them at a higher frequency until reaching the reference point. The component only outputs the “Next Joint Positions” whenever a joint motion is being generated. Joint Trajectory Controller - Motion params:   -Max Vel Traj Following: vector<double>   - Max Acc Traj Following: vector<double>   - Max Vel Online Traj Gen: vector<double>   - Max Acc Online Traj Gen: vector<double> - Operation Mode: enum MTP Library PTP Library Error Port Current Joint Positions Vector Joint Positions Next Joint Positions Interpolate Trajectory Current Joint Velocities <<Interface>> Interpolate Trajectory + SetPointToPoint(...): bool + SetMotionThroughPoints(...): bool + SetOTGPosition(...): bool + StopMotion(...): bool Target Joint Positions Figure 6.25: Joint Trajectory Controller component and interfaces. 6.3.1 Point-to-Point Library The Point-to-Point motion in joint space is generated using the open-source Reflexxes Type II library. The Type II algorithm means that the position progression is described by polynomials up to the second order. It also guarantees a time-optimal and a possibility to set a time and phase synchronous joint trajectory. A phase synchronous joint trajectory translates to the generation of a homothetic trajectory, i.e. one-dimensional straight lines in multi-dimensional space (task space). Intrinsic to the Type II algorithm is the concept of motion state. It is composed of the position ( Pi ), velocity (Vi) and acceleration (Ai) values of one or more DoF at a specific time (Ti), Mi= (Pi,Vi,Ai)(6.61) Adjacent to the concept of motion state, the kinematic motion constraints ( Bi ) that define the maximum velocity (Vmax) and maximum acceleration (Amax) constant values, are also specified. 118 Bi= (Vmax i,Amax i)(6.62) The algorithm is labeled memory-less due to the fact that it only generates the next motion state, i.e the motion state at Ti+1 . It does so by taking the input parameters i) the current motion state, ii) the target motion state, and iii) the kinematic motion constraints, Figure 6.26. Figure 6.26: Type II Joint Trajectory Generation Algorithm with the input parameters at the left side (green, yellow and red) and the output parameters at the right side (blue). The type II algorithm is structured in three distinct steps: i) the calculation of the synchronization time (tsync), ii) the synchronization of the DoF and iii) calculation of output values. In the first step and for each DoF, the algorithm determines the velocity profile that allows the actuator to move from the initial motion state ( Mini i ) to the target motion state ( Mtar i ). This profile is selected from a finite set of possible profiles, by following a decision tree with boolean conditions related to the input parameters . With the profile determined, the minimum trajectory time for the DoF ( tsync i ) as well as the quadratic polynomial coefficients that code the DoF position progression can be calculated by solving a system of nonlinear equations. In order to generate a time-synchronous motion with a multiple DoF system, the trajectory times for each DoF must match. In type II algorithms, there may be a single inoperative time interval, for t > tsync i , which specifies an interval in time that the joint trajectory cannot reach the target motion state with the current input parameters . Thus, the joint trajectory synchronous time ( tsync ) is, in fact, the minimum possible joint trajectory time for all DoF. The second step initiates with new motion profiles being calculated for the remaining DoF. Between the different types of motion profiles, there is an infinite number of solutions. To keep the algorithm deterministic, the motion profile with the minimum integral of the acceleration module ( Rtsync Ti|ai(t)|dt ) is selected. Once all DoF velocity profiles are set, one can determine the polynomials that code the quadratic position progression likewise to first step. 119 6.5 Task Supervisor Analogous to the central nervous system, the Task Supervisor component binds together the “Core” layer. It guarantees that each task is performed by the assigned components, that each component receives the required parameters so that the robot can perform the requested action. The component is also the “Core” layer connection to the outside world, providing and requiring interfaces to communicate with the “Application” layer or with other external components. The Task Supervisor is also the component accountable for the peripheral components’ states and error handling (Figure 6.31). From a central management position, this component takes part in most of the decision-making to guarantee that all required conditions are met prior to carrying out the action, and that faulty states are properly handled to avoid compromising the system or the user’s safety. Task Supervisor - Motion:   - Motion Type: enum   -Joint Trajectory: vector<vector<double>>   - Relative Velocity: double - Multiple Trajectories: bool - Online Trajectory Generation: bool - Control Hollow: bool - Error Codes:   dictionary<int, pair<log_level,string>> Current Joint Positions Generate Trajectory Compute Trajectory Current Posture Moving Interpolate Trajectory Error Port Connection Quality / Session State Operational Transformations <<Interface>> Task Space Trajectories + LinearMotion(...): bool + ArcMotion(...): bool + ComposedMotion(...): bool + PauseSegment(...): bool + AdjustElbow(...): bool <<Interface>> Joint Space Trajectories + JointMotion(...): bool + CoordinateJointMotion(...): bool + AdjustedCoordinateJointMotion(...): bool Task Space Trajectories Joint Space Trajectories Safe Trajectory Execution <<Interface>> Safe Trajectory Execution + SampleTrajectoryPath(...): bool + AnalyzeTrajectory(...): bool + ScoresTrajectory(...): bool + ComputeTrajectoryApproach(...): bool + InstantJointMotion(...): bool + InstantAdjustElbow(...): bool + InstantShadowReal(...): bool <<Interface>> Robot Management + StartMultipleTrajectory(...): bool + FinishMultipleTrajectory(...): bool + SetRelativeVelocity(...): bool + SetHollowRobotControl(...): bool + StopRobot(...): bool + RecoverRobot(...): bool + ShutdownRobot(...): bool Robot Management Figure 6.31: Task Supervisor component. 6.5.1 Action Interface Any action required of the controller involves a coordinated effort of two or more peer components. This action is characterized by the properties and attributes loaded that span globally to every instruction and the instruction’s parameters. To guarantee a reliable and stable operation a set of properties and attributes related to the joint physical limits, nullspace navigation, task space constraints, tool properties, etc, are statically defined and loaded during the configuration phase (refer to previous components). These properties impose a number of constraints that affect every action performed by the component. Let Action Interface be the combination of Joint Space Trajectories and Task Space Trajectories interfaces. These 126 interfaces were designed such that the caller has an explicit control over all the variables aside from those pre-defined in the property files. It is important to note that no trajectory can be initiated if the robot is in motion or has other motions queued8. Joint Space Trajectories refer to any robot action performed directly in the joint space, i.e. each robot joint moves synchronously along the shortest path from the starting to the reference point disregarding the robot body displacement in task space. The JointMotion method displaces the robot from the current joint positions to the specified joint positions, a PTP motion (refer to subsection 6.3.1). The user specifies the target joint positions as well as the relative velocity (a factor that scales the motion execution maximum velocity). If the motion is possible according to the joint physical limits, a request is forwarded directly to the Joint Trajectory Controller’s Interpolate Trajectory interface, Figure 6.25. The CoordinateJointMotion and AdjustedCoordinateJointMotion methods require a target frame to be reached by the robot’s end-effector. The caller also needs to specify the reference frame where the target frame is defined as well as the currently attached tool. The CoordinateJointMotion method requires the target arm angle as well as the global configuration along with the final frame. On the other hand, the arm angle and global configuration parameters for the AdjustedCoordinateJointMotion are calculated using the redundancy resolution method introduced in subsection 6.2.5. Opposed to the JointMotion method, these involve the services of the Operational Transformations interface (Figure 6.30) to retrieve the related reference and tool frames, as well as the Kinematics and Statics Module’s Kinematics interface to determine the correct set of joint positions. Then, if the motion is feasible, this set of joint positions is then forwarded to the Joint Trajectory Controller component. Task Space Trajectories permit the caller to specify the desired end-effector trajectory in task space. They are initially generated using the Cartesian Trajectory Controller’s Generate Trajectory interface (Figure 6.1), which computes a vector of end-effector frames discrete in space and their respective times, refer to section 6.1. After the task space trajectory is calculated, the vector of frames is forwarded to the Kinematics and Statics Modules’s Compute Trajectory interface. If feasible, each frame is converted into a set of joint coordinates, which is then sent to the Joint Trajectory Controller along with the trajectory way-point times. These vectors are the input variables to generate and execute a Motion-Through-Points (subsection 6.3.2). Across the different Task Space Trajectories methods, all require a frame of reference as well as the type of tool attached to its end-effector and, unless the robot is performing a multiple trajectory sequence, the requested motion always starts from the current robot pose. The interfaces for the LinearMotion, ArcMotion, and ComposedMotion mirror those of the Cartesian Trajectory Controller’s Generate Trajectory (subsection 6.1.1). If the robot is planning a multiple trajectory, the user may add a pause segment where, for a limited time interval, the robot remains fixed in its pose. The AdjustElbow method will be addressed in subsection 6.5.5. 8The Task Supervisor allows queuing more than one trajectory only if the multiple trajectory mode is enabled. 127 6.5.2 Robot Management The StartMultipleTrajectory and FinishMultipleTrajectory methods allow the component to enter into a multiple trajectory mode and accept one or more joint/task space trajectories. In multiple trajectory mode, the trajectories must share a common reference frame and tool type. If each trajectory is feasible, then the motion proceeds sequentially. Do not confuse with the ComposedMotion method that accepts a vector of poses and generates a linear interpolation path with parabolic blends at each way-point. The SetRelativeVelocity method, as the name implies, allows the caller to set the robot’s relative velocity scaling factor. If the controller is connected to the V-REP simulator, one may use the SetHollowRobotControl to preview the robotic manipulator motion or test its final posture. This command allows the user to control the pre-visualization ‘hollow’ robot using exactly the same control scheme used for the real platform. When the StopRobot method is called the command is forwarded to the Joint Trajectory Controller that halts the transmission of ‘Next Joint Positions’. In parallel, the queued trajectories are dropped and the multiple trajectory mode disabled if active. When the controller is in this stopped state, the robot is not allowed to move until a call is made to the method RecoverRobot. From this point, the controller resumes its normal operation. If the ShutdownRobot method is called instead, the robot halts its motion and the system terminates its operation. From a shutdown state, the robot cannot be recovered, requiring a complete boot. 6.5.3 Safe Trajectory Execution Whenever a coordinated joint motion or a task space trajectory are requested from the controller, the action is verified at the Kinematics and Statics Module for feasibility. If the movement falls within the subspace of possible and determined solutions, the system generates the joint space trajectory obeying the redundancy resolution scheme proposed in subsection 6.2.5. Otherwise, either the trajectory is feasible from another set of redundancy parameters and the controller prompts the user to alter the arm angle or global configuration; or the trajectory is simply not feasible from any configuration, Figure 6.32. The Safe Trajectory Execution interface extends this functionality by also providing methods to verify the trajectory feasibility prior to its execution. In stereotactic neurosurgery, it is common to have a list of pre-defined trajectories to guide medical instrumentation. Thus, the methods to sample, analyze, and score the trajectory in terms of the nullspace parameters allow the user to pre-position the robot in a configuration that guarantees the complete trajectory execution away from the mechanical limits and singularities. The SampleTrajectoryPath allows the user to sample the geometric task space path to be followed by the end-effector into frames separated in regular, pre-defined steps. When the AnalyzeTrajectory is called, the list of frames previously computed are sent to the Compute Trajectory interface from the Kinematics and Statics Module where the feasible redundancy intervals are calculated for each frame. The interval avoidance strategy is applied for the trajectory intervals and the initial/feasible arm angle intervals are calculated for all configurations (subsection 6.2.6 - Interval Avoidance Strategy). The ScoresTrajectory method takes as input the feasible arm angle intervals of each global configuration for the analyzed trajectory and delivers a score to the available nullspace intervals based on the distance to 128 Yes No New Trajectory Compute Feasible Intervals Feasible currentψ and current GC Execute Trajectory Feasible within current GC Change ψ Yes Feasible for another GC No Change GC Yes START END Impossible Trajectory No Figure 6.32: Decision process to guarantee a feasible trajectory. the limits and the proximity to the current pose (subsection 6.2.6 - Nullspace Scoring). The ComputeTrajectoryApproach method takes as input two points and a redundancy sampling step. The objective is to determine the best fitting full-defined pose (including the redundancy parameters GC and ψ ) that aligns the end-effector z-axis ( ze ) with the two input points vector. More about this functionality in the following subsection. Finally, the instant motion methods provide visual feedback of the desired poses so the caller can instantly preview the final robot pose in the simulation environment. Contrary to the hollow robot control mode, which emulates the movement of the real one, the instant motion methods instantly position the robot at the final location: the InstantJointMotion at the target joint positions, the InstantAdjustElbow at the specified nullspace coordinates keeping the end-effector pose and the InstantShadowReal to the pose of the real robot. 129 6.5.4 Trajectory Approach The Trajectory Approach feature was added to assist in the determination of the best pose to reach a given trajectory, in this case, defined by two points in task space. Since we have an unconstrained orientation variable, the manipulator becomes even more redundant with two non-defined task space variables, the nullspace. A new nullspace resolution strategy is proposed to address both degrees of redundancy, the undefined orientation coordinate and the intrinsic redundancy (GC and ψ). First, nullspace’s two-dimensional domain is sampled. Consider the points ( p1 and p2 ) to be position vectors in task space relative to the robot base reference frame ( {B} ), and the sampling step, ( κ ), a scalar to sample the redundancy orientation interval into a discrete set, Figure 6.33. The set of “redundant poses” Figure 6.33: Set of poses that align the end-effector frame ( {E} ) z-axis with the vector created from the two input points ( p1 and p2 ). The number of redundant poses n depend on the sampling step ( κ ). For clarity, only the zand x-axis of the test poses are represented in the picture. are forwarded to the Kinematics and Statics Module, Compute Trajectory interface and analyzed using the EvaluatePosesManipulability() method (refer to subsection 6.2.6.4). The subset of feasible nullspace solutions (see Figure 6.19) is determined for each pose. The solutions are grouped by global configuration branches. For each GC that contemplates feasible solutions, it is returned the pose and arm angle with the highest modified manipulability. To guarantee that robot can move along the two-point trajectory from the initial approach pose without driving into a joint limit or singularity, further analysis is conducted. With the linear trajectory defined with the two points ( p1 and p2 ) and the initial pose specified, the AnalyzeTrajectory and ScoresTrajectory methods are applied to determine first, if the trajectory is feasible from the initial pose, and second to score the feasible arm angle intervals. The output of the Trajectory Approach comprises, for each GC branch with feasible solutions, the highest modified manipulability pose and arm angle and a score vector (refer to subsection 6.2.6.2). 130 6.5.5 Nullspace transitions The possibility to adjust the robot arm angle and global configuration to guarantee the trajectory feasibility comprehends the transitions between the nullspace parameters, arm angle ( ψ ) and global configurations (GC), Figure 6.32, which involve two different sets of movements. Adjusting the arm angle means to move in the elbow position without changing the end-effector pose or the global configuration. Unlike global configuration changes, the movement to change the arm angle can be entirely performed within the nullspace, if the nullspace path falls within single feasible interval. The motion is generated at the Cartesian Trajectory Controller by calling the GenNullspaceTrajectory method. While the other task space trajectory methods generate a vector of poses and times, the nullspace trajectory generates a vector of arm angle positions and the respective times since the current frame is maintained throughout the movement. A velocity profile similar to the one that describes the task space path is parametrized and associated with the “nullspace path” It is then solved the inverse kinematics for each pose and arm angle pair, and the resulting joint space coordinates sent to the Joint Trajectory Controller as with any other trajectory. If the redundancy parameter transition involves a global configuration change or an arm angle shift between two separated feasible intervals within the same configuration, the robotic manipulator is initially moved to an intermediate posture before reaching the final values. Generating a simple point-to-point motion in joint space to change the global configuration would potentially lead to ample and unpredictable movements, whereas shifting between feasible intervals involves passing near a joint limit or singularity. To cope with this situation, we separated such adjustments into three sequential sub-movements: 1. zeroing motion of pivot joints; 2. point-to-point motion of hinge joints to their final position; 3. point-to-point motion of pivot joints to their final position. The robot starts by moving to a ‘twisted zero’ position by rotating just the pivot joints. At this point, the robot’s structure is straight and aligned normal to its base. In this position, the hinge movement causes the minimum displacement in the robot structure. The final point-to-point motion of the pivot joints causes the robot to finally reach the target global configuration with the same end-effector pose. This sequence guarantees that the transition between global configurations is predictable and relatively contained in the workspace. 6.5.6 Motion Queuing and Error Handler The Task Supervisor is the component responsible for handling robot motion execution, which involves a combined effort from other components in the ‘Core’ layer. Additionally, the component handles external requests and error codes reported from peer components. To guarantee timely responses and deterministic behavior, the component operates in real-time. The interface between the Task Supervisor and peer components can be divided into properties and attributes, ports and operations. Setting properties and attributes as well as communicating through 131 ports are real-time and thread-safe as long as the data transport is properly set up. Alternatively, calling and sending operations may compromise the real-time requirement since it implies memory allocation to collect the return values. Most of the memory allocation takes place during the configuration step, and the remaining variables are handled by the deployer’s real-time memory allocator. On the other hand, calling operations blocks the Task Supervisor own thread of execution, which may cause it to fail the real-time constraint. Some of the operations requested by the Task Supervisor take a finite but undetermined amount of time considering the variable computational cycles that depend, for example, on the type and length of the trajectory generated. As previously explained, the process of generating a robot movement involves a sequential execution of different components methods. However, the synchronization cannot be guaranteed by sequentially calling these methods, due to the risk of violating the real-time constraint. This situation is solved by implementing a finite-state-machine where each state relates to the different phases of motion generation, and by sending/collecting operations instead of making a blocking call. If no motion is being requested the Task Supervisor can be in the monitoring or in the motion ready state. From its motion ready state and depending on the requested action it can transition to: i) an online trajectory generation mode which requires a continuous stream of robot poses or joint values, ii) a task space trajectory that involves the generation of the Cartesian trajectory, computing and interpolating the joint trajectory, and iii) a joint space trajectory execution (moving state), Figure 6.34. ONLINE TRAJECTORY GENERATION MOVING Recover MONITORING Initiate/Finish Motion Start OTG Error Request Trajectory MOTION READY Trajectory Frames CARTESIAN TRAJECTORY GENERATION Trajectory Joint Positions JOINT TRAJECTORY COMPUTING Interpolation JOINT TRAJECTORY INTERPOLATION Figure 6.34: Task Supervisor motion state machine. Motion requests are only accepted if the Task Supervisor is in the motion ready state. When a new motion is accepted, instead of issuing calls to peer components the Task Superviser sends a method that is executed by the ExecutionEngine of the receiving component. Once per update cycle, the Task Supervisor component checks if the process is concluded to then collect the result and transition to the next state. 132 In case the robot is performing a multiple trajectory, the movements are processed in serial mode until the joint trajectories for each movement are determined. Once the multiple trajectory mode is terminated, the queued joint trajectories are sequentially executed by the Joint Trajectory Controller component. The component is also responsible for handling error codes and faulty behavior. The error codes received from peer components include 3 parameters, a log level, an error unique code, and an error message. The “error log level” falls under one of the categories: i) Information, ii) Warning, iii) Error and iv) Critical. The information level includes messages to notify the user of normal functioning events, successful process results, or correct connection with external devices/components. The warning level messages notify the user of events that do not compromise the robot operation but that require user-consent. Examples of warning error codes include events that require the user to adjust the arm pose to execute a trajectory, or to alert the user to create a work frame transformation prior to using it as a reference frame. The error level comprises messages related to exceptions or to errors occurred in peer components. Examples of error level messages include errors related to 3rd party libraries (KDL or Reflexxes), mismatch between expected and received parameters, mechanical limits violations, motions requested outside the manipulator’s workspace, etc. The critical level messages are reserved for hardware non-recoverable errors and communication failure within and outside the “Core layer” components. Each error message is uniquely identified by a key code so that the system can properly react to each specific event or exception. Moreover, the Task Supervisor component is configurable to report different levels of error messages. For example, if the log level report is defined as warning level only warning and higher level messages are reported. 6.6 Fast Research Interface The Fast Research Interface (FRI) components are the architecture components that interface with the robot controller hardware or the virtual robot controller. The FRI components were implemented to guarantee a transparent and seamless transition between controlling the real robotic platform or the virtual model (V-REP simulator). It is also possible to simultaneously control both platforms, to have the virtual robot acting as a pre-visualization tool to plan the real robot movements. 6.6.1 Robot FRI As aforementioned in subsection 5.6.1, the FRI communication channel is explored to drive the robotic arm in real-time. Operating in a client-server typology, the client is typically a C++ application built on top on the provided SDK to communicate with the robot application running in the server side. Instead of a stand-alone application, we developed an OROCOS component around the FRI client to smooth the hardware integration (Figure 6.35). In the configuration step, the connection IP addresses and ports of the local host and remote controller matching the server side configuration are marshaled from a property file along with data relative to the robotic joint limits. During the configuration step, the client 133 Robot FRI - Motion params:   -Max Vel: vector<double>   - Max Acc: vector<double> - Connection params:   - Local IP: string   - Local Port: int   - Remote IP: string   - Remote Port: int KUKA FRI Error Port Current Joint Velocities Next Joint Positions Current Joint Positions Connection Quality / Session State Current Joint Ext. Torques Figure 6.35: Robot FRI component. object is instantiated and the connection established. In the component’s update cycle, the client’s step method is triggered to update both the measured joint data, the commanded joint data as well as other state variables. These variables were modified to have an atomic property and avoid possible concurrent access issues. Unlike the “next joint positions” port from the joint trajectory controller component, which only posts information whenever a new trajectory is being generated, the FRI module requires new commanded joint position data at each iteration. To handle this requirement, the component stores and continuously sends the last valid set of commanded joint positions to the server until a new trajectory is initialized. The “Connection Quality” and “Session State” are two of the state variables read in each step (refer to subsection 5.6.1). These flags are monitored by the task supervisor component to avoid sending motion commands when the robot is not receptive. The current joint positions, connection quality / session state and external joint torques are directly forwarded to the output ports. The current joint velocities, however, are not directly available in the FRI. A 5-point numerical differentiation formula was used to determine the derivative of the joint position as a function of time, i.e. the current joint velocity. Considering we have a continuous feed of joint positions at a regular time step and are interested in computing the derivative at the current time, we apply the formula to the end-point using the four preceding joint positions. Let hbe the time step, f0(x0) = 1 12h[25f(x0−4h)−48f(x0−3h)+36f(x0−2h)−16f(x0−h)+3f(x0)]+ h4 5f(5)(ξ) (6.82) where ξ∈[x0−4h, x0]. The truncation error term (h4 5f(5)(ξ)) is considered 0 for h= 0.005. Figure 6.36: Representation of the last joint position (x0) and the 4 preceding values. A template library was also implemented to function as a circular buffer to store the last 5 joint positions, used as a parameter to calculate the joint velocity. The error code output port continuously updates the task supervisor component about the component’s operation. If the connection to the server node is not successful, or the target joint positions are not reachable for the specified joint velocity limit (time step is fixed) an error command is issued. 134 6.6.2 V-REP FRI One of the advantages of developing a high-level control architecture to move the robotic manipulator whose interfaces are mostly limited to the transmission of joint space variables is the possibility to test the solution in a virtual platform. The V-REP FRI component is the peer to the Robot FRI component that creates the bridge between the control architecture and the V-REP Plugin (refer to subsection 5.7.2). V-REP FRI - Motion params:   -Max Vel: vector<double>   - Max Acc: vector<double> Error Port Current Joint Velocities Next Joint Positions Current Joint Positions End-effector Position <<Interface>> Surgery Trajectories + AddTrajectory(...): bool + RemoveTrajectory(...): bool + ShowTrajectory(...): bool + SetSurgeryReference(...): bool <<Interface>> End-effector Change + GetCurrentEndEffector(...): bool + AttachEndEffector(...): bool + MoveEndEffectorTool(...): bool Surgery Trajectories End-effector Change Figure 6.37: V-REP FRI component. Dissimilar from the Robot FRI component that communicates with the KRC hardware through a Ethernet physical connection in a local network, the V-REP FRI connects to the V-REP plugin that hosts an OROCOS component using: i) inter-process communication (mqueue) if the simulation runs in the same machine as the control architecture or ii) distributed communication (CORBA middleware) if a remote computer in the local network is running the simulation. The other two differences to the peer component in terms of robot control relate to the lack of an external joint torque measurement from the virtual robot as well as the non-existing ‘Connection Quality / Session State’ port. In Figure 6.37, only the interfaces with the control architecture are depicted. The interface with the VREP plugin component mirrors 9 the current joint positions, velocities and next joint positions ports. Internally, the component receives the ‘Next Joint Positions’ from the joint trajectory controller and continuously forwards either the updated target joint positions or the last valid values to the plugin. Conversely, the component receives the current joint positions and velocities from the virtual robot and forwards this information to the control architecture. The component also includes the interface for the ‘Hollow Robot’. Parallel to the interface that controls the robotic arm, the V-REP FRI component includes two additional interfaces to manage the surgery trajectories and the end-effector. The surgery trajectories allow the user to add, remove or show a particular working surgical trajectory while hiding the rest. The End-effector change interface enables the user to check which end-effector is attached to the virtual robot, to attach a new one, to move the tool (trepan, electrode, endoscope,...) and to set the surgery reference frame. 9input ports are output ports and vice-versa 135