EFFICIENT ALGORITHMS FOR ARTICULATED BRANCHING MECHANISMS: DYNAMIC MODELING, CONTROL, AND SIMULATION

Transcription

1 EFFICIENT ALGORITHMS FOR ARTICULATED BRANCHING MECHANISMS: DYNAMIC MODELING, CONTROL, AND SIMULATION A DISSERTATION SUBMITTED TO THE DEPARTMENT OF COMPUTER SCIENCE AND THE COMMITTEE ON GRADUATE STUDIES OF STANFORD UNIVERSITY IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY By Kyong-Sok Chang March 2000

2 c Copyright 2000 by Kyong-Sok Chang All Rights Reserved ii

3 I certify that I have read this dissertation and that in my opinion it is fully adequate, in scope and quality, as a dissertation for the degree of Doctor of Philosophy. Oussama Khatib Department of Computer Science (Principal Adviser) I certify that I have read this dissertation and that in my opinion it is fully adequate, in scope and quality, as a dissertation for the degree of Doctor of Philosophy. Jean-Claude Latombe Department of Computer Science I certify that I have read this dissertation and that in my opinion it is fully adequate, in scope and quality, as a dissertation for the degree of Doctor of Philosophy. Bernard Roth Department of Mechanical Engineering Approved for the University Committee on Graduate Studies: iii

4 Abstract With advances in computational and mechanical hardware technology, a growing body of interest has emerged in the area of real-time dynamic control and simulation of branching mechanisms systems of tree-like topology such as a humanoid robot. Application of such interest requires fast algorithms to provide intuitive, systematic, and efficient ways to model, control, and simulate the dynamics of these complex systems. This thesis presents efficient recursive algorithms, using the operational space formulation, to model and solve, at the task level, the dynamics problems of highly redundant articulated branching mechanisms with multiple operational points. A modified spatial notation is introduced to model complex robot kinematics and dynamics in an intuitive and systematic way. Using this notation, efficient recursive algorithms are developed for branching mechanisms in order to control and simulate task/posture behavior dynamics, closed-chain dynamics, and contact dynamics. A basic underlying framework is developed to provide dynamic control of intuitive task-level commands and posture behavior. The application of these algorithms results in a significant increase in the interactivity and usability of the dynamic control and simulation of complex branching mechanisms. The computational complexity of these algorithms is analyzed and compared with existing methods. Experimental and real-time dynamic simulation results are presented to demonstrate the effectiveness of the proposed algorithms. iv

5 Acknowledgements I have benefited from the advice, friendship, and support of quite a number of people throughout my years at Stanford and my work on this thesis. First and foremost, my advisor Oussama Khatib has provided invaluable intellectual and financial support, without which this thesis may never have reached completion. I also thank him for inspiring my appreciation of good red wine; dinner has never been the same. Kenneth Powell s guidance and encouragement as my undergraduate advisor provided me with a solid engineering background and inspiration, even though I decided to jump ship to computer science rather than stay in aerospace engineering. I must thank Jean-Claude Latombe and Bernard Roth for their comments and assistance both as readers of this thesis and as mentors for me during the past several years. I would also like to thank Carlo Tomasi and Stephen Rock for their participation as members of my defense committee. I owe a great deal to the friendship, intelligence, and patience of my companions and colleagues both within and outside of the Stanford Computer Science Robotics Laboratory: Keerthisri Anagamanna, Alan Bowling, Herman Bruyninckx, Junghoo Cho, Eugene Jhong, Jaewoo Jung, Yotto Koga, Oscar Madrigal, Jutta McCormick, Sean Quinlan, Luis Sentis, Machiel Van der Loos, Gu-Yeon Wei, David Williams, Tomoharu Yamaguchi, Kazuhito Yokoi, and David Zhu. In particular, many thanks to Oliver Brock, Bob Holmberg, and Diego Ruspini for their comments and support during the preparation of this thesis. I would like to thank my family for their unlimited patience and persistence. I will finally be able to speak to them without answering the question Are you DONE yet? Last but not least, I thank my wife, Jennifer, for her endless patience, understanding, and encouragement. I look forward to spending more time with her and our new baby. v

6 Contents Abstract Acknowledgements iv v 1 Introduction Motivation Joint Space vs. Operational Space Branching Robot Dynamics Contributions Overview of the Thesis Operational Space Formulation Background Task Behavior Posture Behavior Dynamic Consistency Applications Spatial Notation and Quantities Efficient Modeling Basic Spatial Notation Spatial Differentiation in Rotating Coordinates Spatial Velocity and Acceleration Spatial Articulated Quantities vi

7 4 Joint Space Dynamics Control : Recursive Newton-Euler Method Simulation : Articulated-body Method Operational Space Inertia Matrix Background : Serial-chain Mechanisms Inertia Propagation Method Force Propagation Method Recursive Algorithm for Branching Mechanisms Analysis of Operational Space Inertia Matrix Derivation of Recursive Algorithm Computational Complexity Operational Space Dynamics Optimized Dynamic Control Structure Algorithm Jacobian Matrix Bias Acceleration Vector Operational Space Inertia Matrix Null Space Computed Command Vector Computational Complexity Simulation Results Experimental Results Closed-Chain Dynamics Introduction Augmented Object Model Single Branching Robot Multiple Branching Robots Algorithm for Dynamic Control Operational Space Control Structure Optimized Dynamic Control Structure vii

8 7.4 Algorithm for Dynamic Simulation Simulation Results Contact Dynamics Basic Concepts Operational Space Inertia Matrices of Links Effective Mass Spatial Impulse Efficient Recursive Algorithm Impulse-based Dynamic Simulation System Integration Hardware/Software Architecture Robot User Interface Real-time Implementation Concluding Remarks Future Work and Conclusion Future Work Robot Dynamics Behavior Control Conclusion A Singularity Control 99 A.1 Introduction A.2 Kinematic Singularities A.3 Control Strategy at Singularities A.4 Experimental Results A.5 Conclusion Bibliography 105 viii

9 List of Tables 4.1 Recursive Newton-Euler method Articulated-body method Inertia Propagation method Force Propagation method Algorithm for operational space inertia matrix Algorithm for operational space inverse dynamics Algorithm for dynamic control of simple closed-chain Algorithm for dynamic simulation of simple closed-chain Algorithm for operational space inertia matrices of all links Impulse Propagation method ix

10 List of Figures 1.1 Branching mechanisms Joint space vs. operational space A branching robot with a tree-like topology C-3PO A simplified humanoid robot with its tree-like topology Task/posture behavior End-effector parameters for a branching mechanism Posture behavior Null space motion with Null space motion with Basic notation Links, joints, and frames Outward recursion: spatial velocity and acceleration Force and acceleration propagators Forces on link Recursive Newton-Euler method Articulated-body method: apparent one-joint robot An articulated-body with link as its handle Articulated-body inertia: base step Articulated-body inertia: recursion step Articulated-body method x

11 5.1 Inertia propagation method Force propagation method Block diagonal matrices Force/acceleration propagation via nearest common ancestor Block off-diagonal matrices Recursion processes Jacobian matrix Bias acceleration vector Motion sequence task/posture behavior control Plots task/posture behavior control SAMM: Stanford Assistant Mobile Manipulator Task behavior: moving end-effector only End-effector behavior Task behavior: moving end-effector and base Posture behavior: moving end-effector and base End-effector behavior Base behavior Cooperation by multiple branching robots Dynamic coupling effect for fixed-base serial-chain mechanisms Dynamic coupling effect for a branching mechanism Augmented object model: concept Dynamics of an augmented object A branching robot holding an object Simple closed-chain mechanism Augmented object model: kinetic energy Motion sequence putting a box on the floor Plot putting a box on the floor Operational space inertia matrix of a point Basic concept xi

12 8.3 Base step Base step: proof Recursion step: first part Recursion step: second part Impulse Propagation method: recursion processes Overall hardware/software architecture Control/simulation servo loop flow chart PUMA 560 robotic system and internet-based user interface Graphical user interface tools: disks, planes, and circular belts A.1 Three basic singular configurations in PUMA A.2 Elbow lock and wrist lock: experimental response A.3 Wrist lock: experimental response xii

13 Chapter 1 Introduction The dynamics of a robotic mechanism play a central role in both its control and simulation. Thus, as the degrees of freedom of a robotic mechanism increase and the tasks performed by it become more complex, the need for a better understanding of its dynamic behavior becomes more significant. The work presented in this thesis is part of an ongoing effort to describe the dynamic behavior of a robotic mechanism in an accurate and simple form while taking into account the computational efficiency. 1.1 Motivation Branching (tree-like) mechanisms represent a large class of practical systems in robotics such as dextrous manipulators with multiple fingers, mobile bases with multiple manipulators, and humanoid robots shown in Figure 1.1. Applications of such systems require fast algorithms to provide intuitive, systematic, and efficient ways to model, control, and simulate the dynamics of these complex systems. In order to efficiently solve complex robot dynamics problems, it is essential to choose a dynamic modeling that naturally renders computationally efficient algorithms for various dynamics problems. The problem to be solved in the control of a robotic mechanism is known as the inverse dynamics problem and the solution to this problem requires the calculation of the correct forces that will produce the desired motion (acceleration) of the robotic mechanism. In simulation, the problem to be solved is called the forward (direct) 1

14 CHAPTER 1. INTRODUCTION 2 Figure 1.1: Branching mechanisms dynamics problem and the solution to this problem requires determination of the motion (acceleration) of the robotic mechanism in response to the applied forces. Since the equations of motion describe the correct behavior of robotic mechanisms, in principle, solving the branching robot dynamics problems presents no difficulty a branching robotic mechanism is just a system of rigid bodies with constraints, and the equations of motion of such systems have been known for a long time. The real problem is the practical one of formulating the robot dynamics such that it provides a systematic and uniform way to solve various branching robot dynamics problems such as task/posture behavior, closedchain, and contact dynamics, as well as lead to computationally efficient algorithms. These practical issues provide the main motivation for this thesis. 1.2 Joint Space vs. Operational Space Two main formulations represent the dynamics of robotic mechanisms: the joint space dynamics formulation [67] and the operational space dynamics formulation [30]. Although

15 CHAPTER 1. INTRODUCTION 3 these two different formulations ultimately describe the dynamics of the same robotic mechanism, each emphasizes different aspects of robot dynamics. The joint space dynamics formulation describes the dynamics of joints and the most common schemes for robot dynamic control and simulation as shown on the left in Figure 1.2. The operational space dynamics formulation is a newer approach describing the dynamics of the tip (endeffector) of a manipulator with its task specification defined directly in its workspace as shown on the right in Figure 1.2. f τ 1 τ 2 Figure 1.2: Joint space vs. operational space Note that dealing with dynamics is essential for achieving higher performance. In free motion, dynamic effects increase with the range of motion, speed, and acceleration at which a robot is operating. In contact operations, they also increase with the rigidity of the colliding objects. Furthermore, when controlling end-effector motion with simultaneous control of contact forces in the orthogonal subspace, inertial coupling can significantly affect performance. These effects must be taken into account to achieve higher performance. The difficulty with joint space control techniques lies in the discrepancy between the space where robot tasks are specified and the space in which the control is taking place. By its very nature, joint space control calls for transformations whereby joint space descriptions are obtained from the robot task specifications. The joint space task transformation problem is exacerbated for mechanisms with redundancy. The typical approach in the case of redundancy involves the use of pseudo or generalized inverses to solve an underconstrained of linear equations, while optimizing some given criterion [17, 20]. The limitations of joint space control techniques, especially in constrained motion tasks, have motivated alternative approaches for dealing with task level dynamics. The operational space formulation, which falls within this line of research, has been driven by the

16 CHAPTER 1. INTRODUCTION 4 need to develop mathematical models for the description, analysis, and control of robot dynamics with respect to task behavior. In this framework, the control of redundant manipulators relies on two basic models: a task level dynamic model obtained by projecting the robot dynamics onto the operational space [30], and the dynamically consistent force/torque relationship that provides decoupled control of joint motions in the null space associated with the redundant mechanism [32]. This decomposition provides the foundation for the algorithms presented in this thesis. 1.3 Branching Robot Dynamics There have been significant efforts to efficiently compute the operational space inverse dynamics of a serial-chain robotic mechanism with a single operational point. Kreutz- Delgado, Jain, and Rodriguez [36] have developed an recursive method to compute the operational space inverse dynamics based on the Spatial Operator Algebra [52] using the articulated-body inertias [13]. Lilly and Orin [42] developed an efficient recursive method for the computation of the operational space inertia matrix without requiring the extra computation of articulated-body inertias. However, these methods do not provide dynamics in the null space associated with a redundant mechanism. Recently, Russakow and Khatib [59] developed the (extended) operational space formulation that accommodates the operational space dynamics of branching robotic mechanisms with multiple operational points. This formulation is an approach for the dynamic modeling and control of a complex branching (tree-like) redundant mechanism (Figure 1.3) at its task or end-effector level. It is particularly useful for dealing with simultaneous tasks involving multiple end-effectors since its basic structure provides dynamic decoupling among the end-effectors tasks and the complex internal dynamics in their associated null space. As an example, a humanoid robot can be controlled by directly specifying tasks involving its feet, hands, and head while using the associated null space to achieve some desired posture behavior such as balancing. In addition, this formulation naturally renders closed-chain dynamics as object level dynamics and contact dynamics as contact-point level dynamics without altering the underlying branching robot kinematics and dynamics. However, in order for this formulation to be usable in interactive or real-time control

17 CHAPTER 1. INTRODUCTION 5 base root h h i e i j e j i ei j ej Figure 1.3: A branching robot with a tree-like topology. In its corresponding tree structure, each link becomes a node and each joint becomes an edge of the tree. of complex -link mechanisms, the complexity of the existing symbolic method [59] is not acceptable since is a small constant compared to in practice. As an example, C-3PO in Figure 1.4 has 20 to 200 links ( ) while less than 5 operational points ( ) will be sufficient for control. This complexity comes from the explicit inversion operation of the joint space inertia matrix of size required for the computation of the operational space inertia matrix and the dynamically consistent generalized inverse of the Jacobian matrix [59]. In addition, deriving explicit symbolic equations of motion for each different mechanism requires excessively large set-up costs if it is feasible at all. Figure 1.4: C-3PO : = , = 1-5 Therefore, in order to achieve interactive or real-time dynamic control/simulation performance, it is necessary to develop more efficient recursive algorithms for branching robot

18 CHAPTER 1. INTRODUCTION 6 dynamics. This argument similarly applies to closed-chain dynamics and contact dynamics involving branching mechanisms since existing algorithms require altering of the underlying kinematic structure and rely on the explicit operations involving the joint space inertia matrices and its inverse. 1.4 Contributions This thesis proposes efficient and general recursive algorithms, using the operational space formulation, to model and solve, at the task level, the dynamics problems of highly redundant articulated -link branching mechanisms with operational points. These algorithms provide intuitive, systematic, and efficient ways to model, control, and simulate task/posture dynamics, closed-chain dynamics, and contact dynamics of these complex systems. Since the most (computationally) expensive terms are the ones involving explicit inversion operations of the joint space inertia matrix, the proposed algorithms are derived by completely eliminating any explicit computation of the joint space inertia matrix and its inverse, achieving real-time efficiency. Application of these algorithms results in a significant increase in the interactivity and usability of dynamic control and simulation of complex branching mechanisms. The following list briefly describes the major contributions of this thesis. Dynamic Modeling In order to model and analyze complex articulated branching mechanisms in a more concise and uniform manner, a modified spatial notation is developed. This notation combines traditional scaler or vector notations and conventional spatial notations. One of the major benefits of this notation is that all inertia matrices are symmetric. Since any algorithm computing dynamic quantities requires extensive manipulation of inertia matrices, this symmetry of inertia matrices enables the implementation of the algorithms to be highly optimized, resulting in a significant increase in overall efficiency. Optimized Control Structure A computationally optimized operational space control structure is developed to eliminate the explicit computation of the dynamically consistent generalized inverse of the Jacobian matrix by manipulating the existing

19 CHAPTER 1. INTRODUCTION 7 symbolic method with dynamically accounted operational space and null space command vectors. Operational Space Inertia Matrix Since the optimized control structure still requires explicit computation of the operational space inertia matrix, an efficient recursive method is derived to produce the operational space inertia matrix without any explicit computation of the joint space inertia matrix and its inverse. Operational Space Inverse Dynamics Using the optimized control structure, the efficient algorithm for the computation of the operational space inertia matrix, and the modified spatial notation, an efficient recursive algorithm for computing the operational space inverse dynamics of branching robots is developed. This algorithm completely eliminates any matrix multiplication operations among matrices of size and, achieving the overall complexity of. Closed-chain Dynamics The augmented object model for branching robots is developed. This model formulates closed-chain dynamics as an object level dynamics with grasping constraint forces using the operational space formulation without modifying underlying kinematic and dynamic structures such as the Jacobian matrix. Therefore, this model renders an algorithm to solve closed-chain dynamics problems involving branching mechanisms. Contact Dynamics An efficient recursive algorithm to compute the operational space inertia matrix of any point in the branching robot is developed. This means that the effective mass of any point of a branching robot in any direction can be computed in linear time, achieving an algorithm to compute the impulse at the contact point. This is a significant speed-up for real-time impulse-based dynamic simulation. Task/Posture Behavior Control A basic underlying framework to accommodate high-level behavior control in both task and null space is presented. Applications of this behavior control include balancing, walking, carrying, mobile manipulator coordination, and singularity control (see Appendix A). This framework provides an intuitive and systematic way to model and control dynamically consistent task/posture

20 CHAPTER 1. INTRODUCTION 8 behavior of complex branching mechanisms. Note that since can be considered as a small constant in practice, we obtain the linear running time of for these algorithms. Therefore, these algorithms perform significantly better than the existing symbolic method,, as the number of links increases. 1.5 Overview of the Thesis The greater part of this thesis deals with the formulation and derivation of efficient algorithms, using the operational space framework, to model and solve the dynamics problems of highly redundant -link branching (tree-like) robotic mechanisms with multiple ( ) operational points. The desire for real-time performance is emphasized throughout this work. The following is a brief outline of the rest of the chapters in the thesis. Chapter 2 provides background material describing the operational space formulation for branching robotic mechanisms. The importance of the decoupled control structure of this formulation and its applications for branching mechanisms are discussed. In Chapter 3, a modified spatial notation is developed in order to model and analyze complex articulated mechanisms in a more concise and uniform manner. Chapter 4 provides important results from previous work on joint space inverse and forward dynamics of branching robots in the form of two existing recursive algorithms using the modified spatial notation. The concepts and intermediate results from these algorithms are essential for the new algorithms developed in later chapters. Chapter 5 presents an efficient recursive algorithm for computing the operational space inertia matrix for branching mechanisms without any explicit computation of the joint space inertia matrix and its inverse. In Chapter 6, a computationally optimized operational control structure is developed to eliminate the explicit computation of the joint space inertia matrix or its inverse. Using this optimized control structure, the algorithm for the computation of the operational space inertia matrix, and the modified spatial notation, an efficient and general recursive algorithm for the computation of the operational space inverse dynamics for branching robotic mechanisms is developed and its complexity is proven.

21 CHAPTER 1. INTRODUCTION 9 Chapter 7 extends the previous augmented object model for non-branching mechanisms to accommodate branching mechanisms without altering the kinematic and dynamic structures of the underlying branching mechanisms, resulting in an efficient algorithm to control and simulate simple closed-chain mechanisms. In Chapter 8, An efficient recursive algorithm to compute the operational space inertia matrix of any point in the branching robot is developed. Using this algorithm, the effective mass of any point of a branching robot in any direction can be found in linear time, achieving an algorithm to compute the impulse at the contact point. In Chapter 9, overall hardware/software architecture is presented to illustrate the current configuration of a real-time dynamic control/simulation environment. Issues involving user interface, control/simulation servo loop, and optimization techniques used to implement the proposed algorithms in a real-time environment are further discussed. Chapter 10 concludes this thesis with a discussion of future research and a summary of contributions. Note that throughout this thesis, real-time simulation results with a basic humanoid redundant robotic mechanism with and, as shown in Figure 1.5, are presented to illustrate the efficiency of the proposed algorithms. root calf thigh hip chest upperarml neck upperarmr lowerarml head lowerarmr handl eyel eyer handr Figure 1.5: A simplified humanoid robot with its tree-like topology

22 Chapter 2 Operational Space Formulation The operational space formulation [30, 59] provides dynamic modeling and control of a complex branching redundant mechanism directly at its task or end-effector level. This formulation is particularly useful for dealing with simultaneous tasks involving multiple end-effectors since its basic structure, the dynamically consistent force/torque decomposition, provides dynamic decoupling among the end-effectors tasks (task behavior) and the complex internal dynamics in their associated null space (posture behavior). The following sections introduces the operational space formulation for branching robotic mechanisms in terms of task/posture behavior and explains the dynamically consistent force/torque decomposition in detail. In addition, some of the applications of this formulation are discussed. 2.1 Background The operational space formulation projects the joint space dynamics of mechanisms into the operational space and its associated null space. The general joint space dynamic equations of motion for an -link -degree-of-freedom open-chain robotic mechanism with operational points may be written in the following form [67]: (2.1) 10

23 CHAPTER 2. OPERATIONAL SPACE FORMULATION 11 where is the total control torque vector, is the joint acceleration vector, is the joint space inertia matrix, is the joint space Coriolis and centrifugal force vector, and is the joint space gravitational force vector. Note that cannot be greater than since each link can have maximum of one operational point, and that since each link has a multiple-degree-of-freedom joint which can have any number of degrees of freedom up to six. Then, the size of is. Also, the relative motion of two free bodies in space can be considered as the action of a six-degree-of-freedom joint between the bodies. The generalized torque/force relationship [30, 32, 59] provides the decomposition of the total control torque, in Equation (2.1) into two dynamically decoupled control torque vectors: the torque corresponding to the task behavior command vector and the torque that only affects posture behavior in the null space: (2.2) Notice that this decomposition provides a natural framework for modeling and control of the task/posture behavior of complex branching mechanisms. In Figure 2.1, the task behavior is to keep the position and orientation of both hands constant and the posture behavior is to rock the torso back and forth.!"#"$&% ' $(% )*'+,&$% (-/.0) 1 23%,&4"'56*!"/#-$% ' $&(7(-4"..8)*1/ Figure 2.1: Task/posture behavior

24 CHAPTER 2. OPERATIONAL SPACE FORMULATION Task Behavior in Equation (2.2) is the torque corresponding to the computed task behavior command vector,, acting in the operational space: where is a 3 (2.3) vertically concatenated vector of the each of end-effectors and is the Jacobian matrix of each of end-effectors:. and spatial force vector of vertically concatenated matrix of the. (2.4) Figure 2.2 shows these spatial forces and Jacobian matrices associated with end-effectors. q k ( ae, f, ) 1 e J 1 e1 {e 1 } {e i } ( ae, f, ) i e J i ei k ( ae, fe, Je ) m {e m } m m Figure 2.2: End-effector parameters for a branching mechanism Since dynamic control of the task behavior in the operational space is desired, can be computed using the operational space (task behavior) command vector for a linearized unit-mass system,, and the dynamics obtained by projecting the joint space dynamics into its operational space: (2.5)

25 CHAPTER 2. OPERATIONAL SPACE FORMULATION 13 where the operational space inertia matrix,, is an symmetric positive definite matrix [30, 59]: - - (2.6) and and are the operational space gravitational force and Coriolis and centrifugal force vectors, respectively and defined as: and The bias acceleration vector,, is the product of the absolute derivative of the Jacobian matrix and the joint velocity vector, : (2.7) and is the dynamically consistent generalized inverse of the Jacobian matrix (2.4) and has been proven to be unique [30, 59]: - (2.8) Note that is a each of end-effectors shown in Figure 2.2: vertically concatenated vector of the spatial acceleration vector of. (2.9) Notice that the complexity of computing using Equation (2.6) is since the two matrix inversion operations of and - require and respectively, while the two matrix multiplication operations require.

26 CHAPTER 2. OPERATIONAL SPACE FORMULATION Posture Behavior in Equation (2.2) is the torque that only affects posture behavior in the null space given any arbitrary null space (posture behavior) command vector, : where is the dynamically consistent null space projection matrix that maps appropriate control torque: (2.10) to the (2.11) where is an identity matrix. Dynamic consistency is the essential property for task behavior to maintain its responsiveness and to be dynamically decoupled from posture behavior since it guarantees not to produce any coupling acceleration in the operational space given any. In Figure 2.3, the robot was commanded to keep the position and orientation of both hands constant (task behavior) while rocking its torso back and forth in the null space (posture behavior). Notice that dynamic consistency enables task behavior and posture behavior to be specified independently of each other, providing an intuitive control of complex systems. Next section explores this important concept in further detail. Figure 2.3: Posture behavior

27 CHAPTER 2. OPERATIONAL SPACE FORMULATION Dynamic Consistency In order to achieve dynamically consistent behavior of a redundant mechanism, it is necessary to decouple the motion in the null space from the motion in the operational space. In other words, the selection of null space control torques from the null space should not generate any acceleration at the end-effectors in the operational space. This can be achieved using the dynamically consistent relationship (2.2) between operational space forces and null space joint torques for redundant mechanisms [32]. This relationship provides a decomposition of joint torques into two dynamically decoupled control vectors: joint torques corresponding to forces acting at the end-effectors, in Equation (2.3); and joint torques that only affect null space joint motions, & in Equation (2.10). is the operational space control forces acting on the end-effectors; is the projection onto the null space; and is the joint control torques for desired motions in the null space. Using this decomposition, the end-effectors can be controlled by operational forces, while motions in the null space (posture behavior) can be independently controlled by joint torques that are guaranteed not to alter the end-effectors dynamic behavior (task behavior). Simulation Example The impact of the dynamically consistent control decomposition is illustrated using the 3Rplanar manipulator shown in Figure 2.4 and Figure 2.5. This manipulator is treated as a redundant mechanism with respect to the task of positioning the end-effector. The goal here is to maintain the end-effector position, while letting the manipulator move in the null space. The end-effector position is controlled by operational forces. An oscillatory motion in the null space is produced by the application, in the null space, of the negative gradient of an attractive potential,, without any dissipative forces so that. Two different generalized inverses are used to construct the projection of onto the null space: the Moore-Penrose or pseudo inverse, -, and the dynamically consistent generalized inverse, from Equation (2.8) The simulation results are shown in Figure 2.4 and Figure 2.5.

28 CHAPTER 2. OPERATIONAL SPACE FORMULATION 16 1 Null space with pseudo inverse 0.8 Position (m) x : solid line y : dashed line Time (sec) Figure 2.4: Null space motion with 1 Null space with dynamically consistent inverse 0.8 Position (m) x : solid line y : dashed line Time (sec) Figure 2.5: Null space motion with As expected, with the dynamically consistent inverse the motion in the null space does not affect the end-effector position in Figure 2.5, while large coupling forces are produced at the end-effector when the pseudo inverse is used in Figure 2.4. Doty [12] has discussed extensively the difficulty with pseudo inverses and observed the need for dynamically weighted generalized inverses such as in Equation (2.8). 2.5 Applications The dynamically consistent force/torque decomposition (2.2) provided by the operational space formulation is extremely useful for dealing with simultaneous tasks involving multiple end-effectors of complex branching redundant mechanisms. As an example, a humanoid robot can be controlled by directly specifying tasks evolving its feet, hands, and

29 CHAPTER 2. OPERATIONAL SPACE FORMULATION 17 head while using the associated null space to achieve some desired self-posture behavior, e.g. balancing. In addition, this formulation naturally renders closed-chain dynamics as object level dynamics and contact dynamics as contact-point level dynamics without altering the underlying branching robot kinematics and dynamics. The following explains these applications further. Task/Posture Behavior Dynamics The dynamically consistent force/torque decomposition of operational space formulation naturally provides task/posture behavior dynamics in a dynamically decoupled matter resulting in an intuitive specification of behavior control. Closed-chain Dynamics Using this formulation, simple closed-chain systems such as a humanoid robot carrying a box with two hands, can be modeled directly at the object (box) level with constraint forces at the grasping points (hands). This is done through an extension of augmented object model. This extension provides efficient dynamic modeling, control, and simulation for simple closed-chain structures without altering the kinematic and dynamic structures of the underlying branching mechanisms. Contact Dynamics This formulation renders contact dynamics as contact-point level dynamics. The operational space inertia matrix can be computed for any operational point in a branching mechanism. This matrix represents the inertial effect at that point. Therefore, finding this matrix at the contact point leads to the effective mass of the point along the contact direction. With this effective mass, it is trivial to compute the correct impulse.

30 Chapter 3 Spatial Notation and Quantities This chapter presents a modified spatial notation, its derived quantities, and notational conventions used throughout this thesis. Spatial notation has been widely used in the modeling of kinematics and dynamics of complex robotic mechanisms [13, 52, 36, 40, 42, 47]. In spatial notation, each quantity incorporates the appropriate linear and angular components and results in a concise form ( vector or matrix). Since the size of each quantity is constant in this notation, the actual implementation can be highly optimized. 3.1 Efficient Modeling The modified spatial notation and quantities developed in this chapter combine various versions of existing spatial notations and conventional vector notations [16, 10, 34] in order to utilize the results from various researchers in a unified way. The major difference between the notation developed in this chapter and the existing ones is that linear components always occupy upper or upper-left corners and angular components always occupy lower or lower-right corners of vectors or matrices. Also, note that one of the benefits of this notation is that all inertia matrices are symmetric. Since any algorithms computing dynamic quantities heavily involve the operations manipulating inertia matrices, this symmetry of inertia matrices enables the actual implementation of the algorithms to be significantly optimized, resulting a higher overall efficiency. 18

31 CHAPTER 3. SPATIAL NOTATION AND QUANTITIES Basic Spatial Notation A spatial velocity,, a spatial acceleration,, and a spatial force,, of link are defined as:,, where,,,,, and, are linear velocity, angular velocity, linear acceleration, angular acceleration, force, and moment vectors expressed in frame, respectively. Also, the spatial inertia matrix of link in frame,, is a symmetric positive definite matrix and defined as: (3.1) where is a identity matrix, is the mass of link, and is the inertia tensor matrix of link in frame. The origin of frame is located at the center of mass of link shown in Figure 3.1. a i v i f i S i {i} % $(-, i m i.&$(" {c i } I ci i The general joint model,, is a Figure 3.1: Basic notation matrix with full column rank,, when joint has degrees of freedom ( "! ) [13, 40]. Its columns (unit vectors) provides the basis for the motion space of joint. Notice that this matrix is constant since it is expressed in its own frame. For example, if joint is a prismatic joint along# -axis and joint$ is a spherical

32 CHAPTER 3. SPATIAL NOTATION AND QUANTITIES 20 joint, their corresponding general joint models are: and (3.2) Note that, in Figure 3.1, the origin of each link frame is located at the joint and any variable without the reference frame number (front superscript) is expressed in its own frame. Also, if link is a leaf (outermost) link, end-effector frame is located at the tip (operational point) of link shown in Figure 1.3 and Figure 3.2. These conventions will be assumed throughout this thesis. S h % $(-, h ri {h} h.&$(" h S i %*$(", {i} i.&$(" {c i } i {e i } Figure 3.2: Links, joints, and frames The spatial rotation matrix,, and the spatial transformation matrix, and transforms a spatial quantity from frame to frame, respectively: where and, rotates (3.3) is the rotation matrix which transforms a quantity expressed in frame to the same quantity expressed in frame and is the cross-product operator associated with

33 CHAPTER 3. SPATIAL NOTATION AND QUANTITIES 21, the position vector from the origin of frame to the origin of frame expressed in frame shown in Figure 3.2. The cross-product operator associated with a, is an skew-symmetric matrix defined as: vector, (3.4) For example, the spatial transformations of spatial velocities and forces between frames and are: and (3.5) Also, by spatially transforming (3.1) from frame to frame, the spatial inertia matrix of link in frame can be computed as: (3.6). where is a symmetric positive definite matrix since Equation (3.6) encapsulates the spatial counterpart of the similarity transformation and the parallel axis theorem [16, 10] for Note that while the inverse of the spatial rotation matrix is just its transpose, i.e., -, the inverse of the spatial transformation matrix is not the same as its transpose: - (3.7) 3.3 Spatial Differentiation in Rotating Coordinates A simple way to obtain the absolute time derivative of a spatial vector in a rotating frame is to rotate the spatial vector to the inertial (absolute) coordinate system (frame ), differentiate it component-wise, and rotate the derivative back to the rotating frame. Then, using and to denote absolute and apparent (component-wise) differentiations respectively, the

34 CHAPTER 3. SPATIAL NOTATION AND QUANTITIES 22 absolute derivative of a spatial vector is given by: (3.8) As in the case of the rotation matrix, [16, 10], the apparent derivative of the spatial rotation matrix (3.3) can be derived in terms of the relative spatial angular velocity between frames and : where is the spatial angular velocity of link : (3.9) and is the spatial analogue of the cross-product operator (3.4). The spatial cross-product operator associated with a spatial vector is defined as: Then, from Equations (3.8) and (3.9), the absolute derivative of a spatial vector be written as: can (3.10) 3.4 Spatial Velocity and Acceleration Since represents the motion space of joint and is the joint velocity measured from its parent link to link, the relative spatial velocity of link with respect to link. is Then, the absolute spatial velocity of link can be recursively computed in terms of the spatial velocity of its parent link and its joint velocity shown in Figure 3.3. This can be

35 CHAPTER 3. SPATIAL NOTATION AND QUANTITIES 23 written as: (3.11) root v a h h % $&(", h h v a i i Si % $(-, q i q i i i leaf Figure 3.3: Outward recursion: spatial velocity and acceleration The spatial acceleration is just the absolute derivative of its corresponding spatial velocity. Note that this acceleration is equivalent to the conventional acceleration [16, 10] and differs from the Featherstone s [13]. Using Equation (3.10), the spatial acceleration is given by: (3.12) Now, the apparent derivative of the spatial transformation matrix (3.3) can be derived in terms of the relative spatial velocity between frames rotation matrix (3.9): and as in the case of the spatial (3.13) Then, from Equations (3.11), (3.12), and (3.13), the spatial acceleration of a chained link can be recursively computed as shown in Figure 3.3, in terms of the spatial acceleration of its parent link, its joint acceleration, and the cross-product of velocity vectors: (3.14) (3.15)

36 CHAPTER 3. SPATIAL NOTATION AND QUANTITIES 24 Notice that the spatial velocity (3.11) and acceleration (3.14) can be expressed in the inertial coordinate system (frame ) by just rotating them to frame matrix (3.3): and using the spatial rotation 3.5 Spatial Articulated Quantities This section presents some of the spatial articulated quantities essential for deriving the algorithms in later chapters. A spatial articulated quantity, a matrix, transforms spatial vectors across actuated joint structures in a dynamically consistent manner and is a nonlinear function of the articulated-body inertia matrix [40]. The articulated-body inertia matrix of a link, introduced by Featherstone [13], relates the spatial force and acceleration of the link, taking into account the dynamics of the rest of the articulated body [13, 52, 40]. This matrix has been used to develop the Articulated-body method, a recursive algorithm to solve the joint space forward dynamics problems for branching robots [13, 47]. The Articulated-body method is presented in section 4.2. The force propagator,, propagates a spatial force from link to its parent link across the actuated joint in a dynamically consistent manner [40]: (3.16) where is the resulting propagated spatial force of link force of link,, is propagated across joint. Note that the recursion. The force propagator is defined as [40]: when the propagated spatial at the beginning of (3.17) where is a identity matrix. is the generalized inverse of weighted by the

37 CHAPTER 3. SPATIAL NOTATION AND QUANTITIES 25 corresponding inertia matrices: - (3.18) (3.19) where is the articulated-body inertia matrix of link (see Equation (4.10) in section 4.2) and is the full rank matrix projecting onto the motion space of joint with degrees of freedom. Notice that is the dynamically consistent generalized inverse of similar to in Equation (2.8). Force propagation is physically valid only if the spatial force is propagated in the inward (tip-to-base) direction shown in Figure 3.4. * f h base h i L * f i h * a h S i h T i L i * a i tip Figure 3.4: Force and acceleration propagators Notice that the force propagator (3.17) has the same dynamic property as the dynamically consistent null space projection matrix, in Equation (2.11), for redundant robotic systems [30, 34]. Both quantities guarantee that the resulting (propagated or projected) quantity does not produce any dynamic coupling effect in their corresponding motion (operational) space. Similarly, the acceleration propagator, shown to be equivalent to the transpose of the force propagator ( ) [40], propagates a spatial acceleration of link to its child link across the actuated joint in a dynamically consistent manner [13, 40]: (3.20) where is the resulting propagated spatial acceleration of link when the propagated spatial acceleration of link,, is propagated across joint. Acceleration propagation is

38 CHAPTER 3. SPATIAL NOTATION AND QUANTITIES 26 physically valid only if the spatial acceleration is propagated in the outward (base-to-tip) direction as shown in Figure 3.4. Note that at the end of the recursion. A matrix, defines the relationship between the propagated spatial acceleration of link and the propagated spatial force of the same link at the joint [52, 36, 40]: (3.21) Finally, the, of a single open-chain mechanism defined as [30]: operational space inertia matrix, - (3.22) can be related to using Equations (3.3), (3.5), (3.21), and (3.22): - (3.23) where end-effector frame is at the tip of leaf link.

39 Chapter 4 Joint Space Dynamics In the joint space formulation, since the joint space inertia matrix is an matrix, the explicit calculations of the joint space inertia matrix or its inverse will cost a minimum of. Therefore, it is necessary to avoid any explicit calculation of these quantities in order to achieve complexity. This is the major motivation for the two recursive algorithms described in the following sections. The recursive Newton-Euler method and the Articulated-body method will be presented to solve joint space inverse and forward dynamics problems, respectively. 4.1 Control : Recursive Newton-Euler Method The recursive Newton-Euler method is the most efficient general method currently known to solve joint space inverse dynamics problems for. The partially recursive version of this algorithm was originally developed for multi-body satellite dynamics [23] and further expanded to the fully recursive one later [44]. Using the force balance approach, the equations of motion (Newton s and Euler s equations) are applied to each link and the resulting equations are combined with constraint equations from the joints in such a way as to give the joint forces in terms of the joint accelerations. Computing with given in Equation (2.1) is the classical joint space inverse dynamics problem. This can be solved by the recursive Newton-Euler method [44, 13]. This section presents the derivation of the recursive Newton-Euler method using the 27

40 CHAPTER 4. JOINT SPACE DYNAMICS 28 modified spatial notation presented in Chapter 3. Using Equation (3.10), the net force acting on link in frame, link s rate of change of momentum: is given by the (4.1) where from Equations (3.5) and (3.12), (4.2) is constant and the spatial. From Equation (3.5), (4.1), and (4.2), the spatial net force in frame can be written as: Note that can be treated as a constant matrix since velocity (3.11) already accounts for the changes in (4.3) (4.4) where is the spatial bias force which must applied to produce zero spatial acceleration. Equation (4.3) combines Newton s and Euler s equations for linear and angular motion of a rigid body [13]. joint h f h Σ f net i f h link h f ext h Figure 4.1: Forces on link Then, the spatial total force on link (Figure 4.1) can be arranged to give a recursive equation as: (4.5)

41 CHAPTER 4. JOINT SPACE DYNAMICS 29 where represents the index of each child link of link and is the spatial external force acting on link. Note that the gravitational force can be included in this manner: (4.6) (4.7) where is the gravitational acceleration vector in frame and defined as: (4.8) Finally, the joint forces are the components of the spatial total force that do work in the motion space of the joint: (4.9) τ h f h (2) v h a h (1) f h %*$(", a h.&$(" h h f i.&$(" % $&(", i i Figure 4.2: Recursive Newton-Euler method Figure 4.2 illustrates the two (one outward and one inward) recursions required and Table 4.1 summarizes the equations for the recursive Newton-Euler method developed in this section.

42 = CHAPTER 4. JOINT SPACE DYNAMICS Given: Table 4.1: Recursive Newton-Euler method 2. Outward Recursion: Compute the spatial net force and gravitational force: "! # $% & & ' & (*) + -,. / 102, +, 3 3. Inward Recursion: Compute the spatial total force and joint force:! >? ! ;: < 4.2 Simulation : Articulated-body Method Computing with given in Equation (2.1) is the joint space forward dynamics problem. This section briefly presents the Articulated-body method, a recursive algorithm to solve the joint space forward dynamics problems for branching robots, introduced by Featherstone [13]. The detailed derivation of this method can be found in [13, 47]. The Articulated-body method [13] is the most efficient general method for solving joint A@ space forward dynamics problem for. This method solves forward dynamics problems by the constraint propagation approach calculating recursion coefficients that propagate motion and force constraints along the mechanism allowing the problem to be solved directly. The basic idea is to regard the robot as consisting of a base member (whose motion is known), a single joint, and a single moving link which is in fact an articulated-body representing the rest of the robot. An articulated body is a collection of rigid bodies connected by joints. Notice that all interactions between an articulated-body and the rest of

43 CHAPTER 4. JOINT SPACE DYNAMICS 31 the system (rest of rigid-body) occurs through a single link, the handle, of the articulatedbody. Figure 4.3 shows this apparent one-joint robot: the base member (rest of rigid-body), a single joint of the handle, and the articulated-body. f i '5,0% '5$ "$ - *%.&$(" i handle % $&(", i articulated-body Figure 4.3: Articulated-body method: apparent one-joint robot An articulated-body inertia gives the linear relationship between a force applied to the handle (by the rest of the system) and the acceleration, taking into account the dynamics of the articulated-body shown in Figure 4.4: where is the applied force, is the handle s acceleration, is the articulated-body inertia, and (see Equation (4.4)) is its associated spatial bias force which must be applied to the handle to give it zero acceleration [13]. fi a i % $(-, A I i.$&(" i i articulated-body Figure 4.4: An articulated-body with link as its handle Then, the forward dynamics problem for this one-joint robot is easily solved once the

44 CHAPTER 4. JOINT SPACE DYNAMICS 32 apparent inertia (articulated-body inertia) of the moving link is known. Having found the acceleration of the first joint, the articulated-body itself can be treated as a robot and the same process applied to obtain the acceleration of the next joint, and so on. Therefore, the Articulated-body method consists of the calculation of a series of articulated-body inertias which are used to solve the forward dynamics one joint at a time. Using Equations (3.6) and (3.17), the articulated-body inertia matrix of link, be written as:, can (4.10) where represents the index of each child link of link. This recursive equation shows that the articulated-body inertia of a link is the sum of its own spatial inertia and the dynamically consistent projection of the articulated-body inertia of each child link. Figure 4.5 and Figure 4.6 illustrate the base and the recursion steps of Equation (4.10) in the inward direction, respectively. h i leaf I leaf leaf = Figure 4.5: Articulated-body inertia: base step articulated-body I = I A leaf leaf articulated-body h i h i h i I h + A Ii = A h = I Figure 4.6: Articulated-body inertia: recursion step I h + i h i LI A h i i L T Similarly, using equations (3.3), (3.15), (3.17), (3.18), (4.4), (4.6), (4.9), and (4.10), the

45 CHAPTER 4. JOINT SPACE DYNAMICS 33 articulated-body bias force vector of link,, can be written as: (4.11) where represents the index of each child link of link. Once and are computed, using equations (3.3), (3.14), (3.15), (3.18), (3.19), (4.9), and (4.11), the joint acceleration can be computed by: - where link (4.12) is the parent link of link. Figure 4.7 shows the three recursions required for the Articulated-body method. ai A p i q i A I i (2) v i (3) '5,8% '5$"$ - % f i.&$(" handle i (1) % $&(", i articulated-body Figure 4.7: Articulated-body method The overall algorithm is summarized in Table 4.2. Since each step in Table 4.2 costs, the overall running time of the Articulated-body method is.

46 CHAPTER 4. JOINT SPACE DYNAMICS Given: = Table 4.2: Articulated-body method 2. Outward Recursion: Compute the spatial transformation matrix, velocity, and velocity product terms: > #! > < # 3. Inward Recursion: Compute the articulated-body inertia matrix and bias force: * :.! : > = 87 9! ;: 4. Outward Recursion: Compute the joint acceleration and the spatial acceleration: - 9 =! > :! > > 2 %

47 Chapter 5 Operational Space Inertia Matrix The operational space inertia matrix is an important quantity in branching robot dynamics since task/posture behavior dynamics, closed-chain dynamics, and contact dynamics problems can be solved efficiently using the properties of this matrix. In addition, since this matrix requires the explicit inversion operation of the joint space inertia matrix of size, this matrix is one of the most computationally expensive terms. Therefore, it is an essential requirement for real-time applications to compute this matrix as fast as possible. As background material, this chapter rewrites, using the modified spatial notation presented in Chapter 3, two existing efficient recursive algorithms to compute the operational space inertia matrix for serial-chain mechanisms with a single operational point. These algorithms are presented since their basic concepts and intermediate results play significant roles in the derivation of algorithms for branching mechanisms. For the computation of the operational space inertia matrix of branching mechanisms with multiple operational points, an efficient recursive algorithm is derived from basic analysis of the physical properties of and the relationships among accelerations, forces, and inertia matrices, using the modified spatial notation and quantities. The proposed algorithm is shown to be of overall complexity. 35

48 CHAPTER 5. OPERATIONAL SPACE INERTIA MATRIX Background : Serial-chain Mechanisms For serial-chain mechanisms, in the operational space dynamics formulation, both inverse and forward dynamics problems become computing either the operational space inertia matrix or its inverse, since the size of the operational space inertia matrix is always constant ( ) and inversion of a constant size matrix requires. In the following subsections, two recursive algorithms, the Inertia Propagation method and the Force Propagation method, will be presented to compute the operational space inertia matrix Inertia Propagation Method The Inertia Propagation method [40] is the most efficient method known for computing the operational space inertia matrix for. This algorithm is applicable to serial-chain mechanisms with known inertial properties at the base, including redundant mechanisms. The joints of the chain may be arbitrary and may have multiple degrees of freedom. The computational structure of this algorithm is a single recursion beginning at the base of the manipulator and progressing out to the last link. In the case of a floating base, this algorithm produces the exact solution using the actual inertia matrix of the base as the initial condition. In the case of a fixed base, the initial condition is based on the big base concept treating the earth as a big base link floating in space. By setting the magnitude of the spatial link inertia of this big base link sufficiently large relative to ones of the remaining links of the chain, this algorithm produces numerically very accurate results for the operational space inertia matrix at the tip of the chain [40]. Having found the operational space inertia matrix of the first link, the recursion may proceed to obtain the operational space inertia matrix of the next link, and so on. This can be written as: (5.1) where is the operational space inertia matrix of link, taking into account the dynamics of all of its ancestor links, i.e., all links in the path from link to the root link. The definition

49 CHAPTER 5. OPERATIONAL SPACE INERTIA MATRIX 37 of the propagator, in Equation (3.17) in the opposite direction (baseto-tip):, is similar to - (5.2) where is a identity matrix. is the generalized inverse of weighted by the corresponding inertia matrices: - (5.3) (5.4) - - (5.5) where, similar to in Equation (3.19), is the full rank matrix projecting onto the motion space of joint with degrees of freedom. Also, similar to in Equation (3.18), is the dynamically consistent generalized inverse of. Note that for a serial-chain with an unconstrained base, this method is basically equivalent to the method for computing the articulated-body inertias (4.10). The Inertia propagation method produces the operational space inertia matrix at the end-effector using a single recursion in the outward (base-to-tip) direction shown in Figure 5.1. Table 5.1 summarizes the Inertia Propagation method presented in this section. ~ A = I leaf ~ I = I A root root h a h h ~ A I h f i i i leaf Figure 5.1: Inertia propagation method

50 CHAPTER 5. OPERATIONAL SPACE INERTIA MATRIX Given: Table 5.1: Inertia Propagation method - 6! > > 7 < > - > < > > < $% - - < < 2. Outward Recursion: Compute the operational space inertia matrix for each link: 3. Spatial Transformation: Compute the operational space inertia matrix at the end-effector: - - < Force Propagation Method The spatial operator algebra has been developed to provide a high-level framework for describing the dynamic and kinematic behavior of complex robotic systems while keeping the complexity involved in analyzing such systems to manageable proportions [52]. The analysis of robotic mechanisms has shown that certain linear operators are always present in the equations describing the dynamic and kinematic behavior of robotic mechanisms. The elements of the spatial operator algebra are these linear operators whose domain and range spaces consist of velocities, accelerations, and forces. These operators are called spatial operators since they show how forces, velocities, and accelerations propagate through space from one rigid body to the next. Many important spatial operators encountered in multi-body dynamics belong to a class that can be factored as the product of a causal operator (upper block triangular matrix), a memoryless operator (block diagonal matrix), and an anti-causal operator (lower block triangular matrix). The implicit effect of spatial operators is equivalent to a tip-to-base or base-to-tip spatial recursion along the span of a manipulator. Thus, the spatial operator algebra provides a powerful analysis tool allowing one to manipulate the equations of motion

51 CHAPTER 5. OPERATIONAL SPACE INERTIA MATRIX 39 in new ways previously not available. This is particularly true given the existence of important identities and inversions relating the spatial operators. However, the use of certain mathematical techniques in the manipulation of the spatial operators, though computationally sound, may make the physical interpretation of the results somewhat difficult. Using the spatial operator algebra, an recursive algorithm for the calculation of the inversion of the operational space inertia matrix at the tip of a robotic mechanism is derived as an application of factorization, identities, and inversion capabilities of the spatial operators in the spatial operator algebra [36]. This algorithm is called the Force Propagation method [40] and defined as [52, 36, 40]: - (5.6) where link is the only child link of its parent link. Then, at any leaf link, the operational space inertia matrix at it tip can be found from Equation (3.23): - (5.7) Figure 5.2 illustrates the two (one inward and one outward) recursions required and Table 5.2 summarizes the equations for the Force Propagation method developed in this section. 1 leaf = leaf (2) Ω = 0 6 root * f i h S i % $&(", * a i i i Ω i h A i L I i (1) leaf f = * leaf f leaf * a leaf = a leaf Figure 5.2: Force propagation method

52 CHAPTER 5. OPERATIONAL SPACE INERTIA MATRIX Given: Table 5.2: Force Propagation method 2. Inward Recursion: Compute the force propagators: 3. Outward Recursion: > - >! > > 02 > -> * 3 > >? < ' 4. Spatial Transformation: Compute the inverse of the operational space inertia matrix at the end-effector: - < Matrix Inversion: Compute the operational space inertia matrix at the end-effector: 9 - : Recursive Algorithm for Branching Mechanisms This section develops an efficient algorithm to recursively compute the operational space inertia matrix,, for branching robotic mechanisms without any explicit computation of -. An efficient recursive algorithm is derived from the basic analysis of the physical properties of and the relationships among accelerations, forces, and inertia matrices. The proposed algorithm has the complexity of where and are the numbers of links and operational points in the system, respectively. Since is a function of configuration only (2.6), can be assumed for the analysis of without loss of generality.

53 - CHAPTER 5. OPERATIONAL SPACE INERTIA MATRIX Analysis of Operational Space Inertia Matrix As in the case of a single operational point (3.22), to the accelerations at the end-effectors: - relates the forces at the end-effectors - (5.8) Also, since - is symmetric, it can be expressed in terms of its block matrix components as: (5.9) the From Equations (5.8), (2.9), and (5.9), the additive property of the coupling effect on where end-effector (of leaf link ) can be written as: is the coupling acceleration on the - (5.10) (5.11) end-effector by the force of the$ endeffector. This additive property of the coupling effect shows that the resulting acceleration of an end-effector is not only dependent on its own force, but also on the forces of all other end-effectors in the system. Notice that when the$ end-effector produces the only non-zero force in the system, the coupling effect on the end-effector by the force of the$ end-effector can be isolated. This can be written, from Equations (5.10) and (5.11), as: - (5.12) $ when and for all. Then, similar to Equation (3.21), a matrix, defines the relationship between

54 CHAPTER 5. OPERATIONAL SPACE INERTIA MATRIX 42 the propagated spatial acceleration of link and the propagated spatial force of the link$ at the corresponding joints. This relationship can be written as: (5.13) Also, similar to Equation (3.23), the block inverse operational space inertia matrix, -, can be related to using Equations (3.3), (6.12), (3.5), (5.12), and (5.13): - (5.14) where end-effector frames and are at the tips of leaf links and$. Note that this relationship is necessary since the inertial properties are desired at the tips instead of at the joints Derivation of Recursive Algorithm This subsection will develop a recursive algorithm by separately analyzing the inertial effects of the block diagonal matrices, -, and of the block off-diagonal matrices, - $, of - in Equation (5.9). Block Diagonal Matrix Each block diagonal matrix, only leaf link with an end-effector. With this physical insight, -, is the inertial quantity that would occur if link is the - can be computed using a trivial extension of the Force Propagation Method (5.6), an recursive algorithm to compute the inverse operational space inertia matrix of just a single open-chain mechanism [52, 36, 40]. Using the relationships from (3.21) and (5.13), the Force Propagation Method (5.6) can be extended immediately for a branching robot by replacing with. This extension enables the outward recursion to pass through all children instead of a single child: - (5.15)

55 CHAPTER 5. OPERATIONAL SPACE INERTIA MATRIX 43 where is the parent link of link. Note that this recursion starts from the root link and ends at the leaf links with end-effectors shown in Figure 5.3. e 1,e 1 1 {e 1 } 2 e 2,e 2 {e 2 } h h,h i i,i 3 {e 3 } e 3,e 3 Figure 5.3: Block diagonal matrices Then, the block diagonal matrices, links using Equation (5.14). -, can be computed by transforming of leaf Block Off-diagonal Matrix $ The block off-diagonal matrices, -, may be regarded as cross-coupling inertial quantities that are a measure of the inertia coupling to the end-effector from the force of the$ end-effector via the nearest common ancestor of leaf links and$. The nearest common ancestor of links and$ is the first common link in two paths; one from link to the root link and the other from link$ to the root link. For example, in Figure 1.3 and in Figure 5.4, link is the nearest common ancestor of leaf links and$. From this physical property of block off-diagonal matrices, - can be conceptually viewed as an inertial quantity which propagates the spatial forces from the$ end-effector to link (the nearest common ancestor of leaf links and$ ) and then propagates the resulting spatial accelerations of link to the end-effector. With this conceptual view, a recursive algorithm will be developed to compute - by finding the propagation of the spatial force from the$ end-effector to link and the propagation of the resulting spatial acceleration from link to the end-effector. Then, -

56 CHAPTER 5. OPERATIONAL SPACE INERTIA MATRIX 44 can be computed by relating the resulting spatial acceleration of link to the propagated spatial force from the$ end-effector. Figure 5.4 illustrates this process. i a = * i h i L {e i } * T * a leaf = a leaf a * h a = * h h h, h f * h f = Lf * h * f leaf = f leaf j h j * * j {e j } Figure 5.4: Force/acceleration propagation via nearest common ancestor First, using Equations (3.5), (3.16), and (3.17), the force at the$ end-effector can be propagated to any of its ancestors: (5.16) (5.17) where link is an ancestor link of link$, link is the descendent link of link in the path from link to link$ and link is the parent link of link. propagation of the spatial force from link$ to link [40]. results a compound Similarly, using (6.12), (3.20), (3.17), and (5.17), the propagated spatial acceleration of link can be propagated to the end-effector of any of its descendant leaf link : (5.18)

57 CHAPTER 5. OPERATIONAL SPACE INERTIA MATRIX 45 Now, combining Equations (5.13), (5.16), and (5.18), and Then, from Equations (5.12), (5.13), (5.14), and (5.19), matrices can be derived: can be related as: (5.19) for the block off-diagonal (5.20) where is the nearest common ancestor of leaf links and $. A recursive version of Equation (5.20) is presented in Table 5.3 (step 4). Note that this recursion starts from and ends at the leaf links with end-effectors shown in Figure 5.5. e 1,e 3 1 {e 1 } {e 2 } e 1,e 2 2 h k e 2,e 3 3 {e 3 } Figure 5.5: Block off-diagonal matrices As for the block diagonal matrices, $ the block off-diagonal matrices, -, can be computed by transforming of leaf links and$ to the corresponding tips using Equation (5.14). Finally, the operational space inertia matrix,, can be computed by inverting - in Equation (5.9). Table 5.3 summarizes the recursive algorithm developed in this section. Note that although most processing occurs along the path from the root link to the leaf links with end-effectors, the effects of the other links enter through the articulated-body inertias (see Equation (4.10) in section 4.2) of the links in the path.

58 CHAPTER 5. OPERATIONAL SPACE INERTIA MATRIX 46 Figure 5.6 illustrates the recursion processes of the proposed algorithm for the branching robot shown in Figure 1.3. Arrows indicate the direction of the recursion and there is no computation required among the nodes connected by dotted lines. Table 5.3: Algorithm for operational space inertia matrix 1. Outward Recursion: Compute the spatial transformation matrices: Inward Recursion: Compute the force propagators:! > > Outward Recursion: Compute the block diagonal matrices starting with. : < < > -> 4. Outward Recursion: Compute the block off-diagonal matrices with nearest common ancestor of links and : return if else if <. 5. Spatial Transformation: Compute - - from otherwise of leaf links with end-effectors: < % 6. Matrix Inversion: Compute the extended operational space inertia matrix,, by inverting - (5.9) Computational Complexity For the computation of the operational inertia matrix, in section 5.2.2, each step in Table 5.3 can be analyzed. Steps 1 and 2 can be computed in since there are links in the

59 CHAPTER 5. OPERATIONAL SPACE INERTIA MATRIX 47 root root root root h h h h i ei j ej (a) step 1 i ei j ej (b) step 2 i ei j ej (c) step 3 i ei j ej (d) step 4 Figure 5.6: Recursion processes of steps in Table 5.3: (a) outward recursion for, (b) inward recursion for, (c) outward recursion for, and (d) outward recursion for $. links, all can be computed in. In step 4, since there are at most links to propagate to system. Also, since step 3 requires one outward recursion involving at most compute all $ from each of end-effector, all can be computed in. Spatial transformations of block matrices cost in step 5. In step 6, an inversion of - of size requires. Therefore, can be computed in.

60 Chapter 6 Operational Space Dynamics This chapter describes an efficient recursive algorithm for the computation of the operational space inverse dynamics of an -link branching redundant robotic mechanism with operational points. In order to control highly redundant articulated branching mechanisms efficiently, a computationally optimized operational space control structure is developed. The proposed algorithm is derived using this control structure and the modified spatial notation presented in Chapter 3. This algorithm achieves the overall complexity of while the existing symbolic method has the complexity of. Since is a small constant in practice, as the number of links increases, this algorithm performs significantly better,, than the existing symbolic method,. Therefore, the application of the proposed algorithm results in a significant increase in the interactivity and usability of dynamic control of complex branching mechanisms. Real-time dynamic simulation and experimental results are presented to illustrate the efficiency and effectiveness of the proposed algorithm for intuitive task/posture behavior control. 6.1 Optimized Dynamic Control Structure The most (computationally) expensive terms in the operational space control structure (2.2) are the ones involving the explicit inversion operation of the joint space inertia matrix, of size, since it requires. Therefore, eliminating the explicit usage of - is 48

61 CHAPTER 6. OPERATIONAL SPACE DYNAMICS 49 an essential requirement for the real-time efficiency. In this section, a new computationally optimized operational space control structure is developed by completely eliminating the explicit computation of the joint space inertia matrix, its inverse, and the dynamically consistent generalized inverse of the Jacobian matrix. This elimination can be achieved by manipulating the existing symbolic method (2.6) with the combination of the dynamically accounted task/posture behavior command vectors, resulting in a dynamic control structure optimized for the computational requirement. As in the case of the dynamic control in the operational space, since dynamic control in the null space is desired, the posture behavior command vector, should account for the dynamic effects by inertial, Coriolis, centrifugal, and gravitational forces: (6.1) where is the null space (posture behavior) command vector for a linearized unit-mass system. A similar null space command vector had been applied in order to optimize the control structure in the case for the traditional operational space formulation with a single operational space point [28]. Combining Equations (2.2), (2.5), (2.11), (2.7), and (6.1), the total control torque can be decomposed into two command torques: one with the terms related to and the other with the terms related to : Note that calculating (6.2) (6.3) in Equation (6.1) is the classical joint space inverse dynamics problem that can be solved by the recursive Newton-Euler method [44, 13, 10] presented in section 4.1. Then, in this control structure, any explicit usage of or - has been eliminated by the algorithm to produce without the explicit computation of - presented in section 5.2. Also, notice that the total control torque, in Equation (6.2), can be computed without any matrix multiplication operations among matrices of size and, achieving the overall complexity of.

62 CHAPTER 6. OPERATIONAL SPACE DYNAMICS 50 This control structure reveals physical insight about the dynamic consistency of the null space projection matrix: the extra accelerations in the extended operational space generated by the dynamic coupling effect of the computed null space command vector, (6.1), are being offset by in Equation (6.3). It should be emphasized that the posture behavior command torque,, is not guaranteed to be achieved since not all null space motion is possible without affecting the motion of the operational space points. Instead, this control structure provides the closest solution to achieve without generating any coupling acceleration effect in its extended operational space while minimizing the instantaneous kinetic energy in the system. This behavior is an example of the dynamic consistency presented in previous chapters. Given the task and posture behavior command vectors, and, the unknown quantities in Equations (6.2) and (6.3) are,,, and. The next section presents efficient recursive algorithms to compute these quantities. Note that this control structure also completely eliminates any matrix multiplication operations among matrices of size and in the proposed algorithm, achieving the overall complexity of. 6.2 Algorithm This section derives the proposed algorithm by developing and combining algorithms for the computations of,,, and. Using the computationally optimized control structure (6.2) presented in the previous section and the modified spatial notation presented in Chapter 3, the proposed algorithm is shown to be of overall complexity Jacobian Matrix The Jacobian matrix,, relates the velocity of link : joint velocity vector,, to the spatial (6.4)

63 CHAPTER 6. OPERATIONAL SPACE DYNAMICS 51 Using Equation (3.7), the recursive equation (3.11) of the spatial velocity can be converted to a summation form: - (6.5) - where is the indices of the links in the path from link to the root link. Then, from Equations (6.4) and (6.5), can be written as: if is an ancestor of otherwise (6.6) (6.7) Now, since the spatial velocity of the operational point associated with the leaf link is: its associated Jacobian matrix can be written as: (6.8) This single outward recursion process for the computation of the Jacobian matrix associated with the end-effector (operational point) is shown in Figure 6.1. {e 1 } J ei {e i } Figure 6.1: Jacobian matrix {e m }

64 CHAPTER 6. OPERATIONAL SPACE DYNAMICS Bias Acceleration Vector Using Equations (3.12), (2.7), and (6.4), the be given as: bias acceleration vector, of link can (6.9) Then, one way to compute is to find the absolute derivative of the Jacobian matrix and then multiply it by the joint velocity vector. Using Equations (3.10), (6.6), and (6.7), the absolute derivative of the Jacobian matrix of link can be derived as: if is an ancestor of otherwise and from Equation (6.8), the absolute derivative of the Jacobian matrix of the operational point associated with leaf link can be written as: Note that this is not the most efficient way to compute. A more efficient method is to compute recursively without using the absolute derivative of the Jacobian matrix explicitly. The recursive equation of the spatial acceleration (3.14) can be converted to a summation form: where (6.10) represents the indices of the links in the path from link to the root link. Then, from Equations (6.6), (6.7), (6.9) and (6.10), can be written as:

65 CHAPTER 6. OPERATIONAL SPACE DYNAMICS 53 where represents the indices of the links in the path from link to the root link. The corresponding recursive equation is: (6.11) is: Now, since the spatial acceleration of the operational point associated with the leaf link (6.12) its associated can be written as: This single outward recursion process for the computation of all bias acceleration vectors is shown in Figure 6.2. h e1 {e 1 } h ei {e i } h em {e m } Figure 6.2: Bias acceleration vector Operational Space Inertia Matrix An efficient recursive algorithm for the computation of the operational space inertia matrix is derived in section 5.2. This algorithm eliminates the explicit computations of the joint space inertia matrix and its inverse. Table 5.3 summarizes this algorithm.

66 CHAPTER 6. OPERATIONAL SPACE DYNAMICS Null Space Computed Command Vector Computing in Equation (6.1) is the classical joint space inverse dynamics problem that can be solved by the recursive Newton-Euler method [44, 13], the most efficient general method currently known for calculating joint space inverse dynamics. This method is derived in section 4.1 using the modified spatial notation presented in Chapter 3. Table 4.1 summarizes this algorithm Computational Complexity This section discusses the computational complexity of the algorithms presented in section 6.2 for an -link redundant branching mechanism with operational points. The Jacobian matrix in section 6.2.1, can be computed in since each of requires one inward recursion involving at most links and the complexity of a recursion step between a child and its parent links is. In section 6.2.2, since all can be computed in using the recursive equation (6.11) which requires only one outward recursion, can be computed in. Then, and can be computed in. The complexity for the computation of the operational inertia matrix,, is shown to be in section Also, since the recursive Newton-Euler method in section 4.1 takes, in equation (6.1) can be computed in. Therefore, using the algorithms presented in this section,,,, and can be computed in. Then, the total control torque vector, in Equation (2.2) requires computational complexity using the optimized dynamic control structure (6.2) presented in section 6.1. Table 6.1 shows the overall recursive algorithm combining the algorithms presented in section 6.2. The running time of the overall recursive algorithm is. Note that since can be considered as a small constant,, for any realistic robotic mechanism in practice, this algorithm achieves the linear running time of. Thus, as the number of links in a mechanism increases, the proposed algorithm performs significantly better than the existing symbolic method [59] which still requires inversion operations for - of size in Equations (2.6) and (2.8).

67 CHAPTER 6. OPERATIONAL SPACE DYNAMICS 55 Table 6.1: Algorithm for operational space inverse dynamics 1. Given: 2. Outward Recursion: < ' 3. Task/Posture Command Vectors: 4. Inward Recursion:? Outward Recursion: < < 6. Outward Recursion: 7. Spatial Transformation: - 8. Inward Recursion: 9. Outward Recursion: <. < % ' ' ' 10. LU Decomposition and Back-substitution: -!! ' 11. Total Control Torque Vector: ' ' 6.3 Simulation Results Using the proposed algorithm, it took less than 1.2 msec to perform the computation of the operational space dynamics for the branching robotic mechanism in Figure 1.5 using a PC with a 266 MHz Pentium II running under the QNX real-time operating system. This branching robot has an operational point at each of its 2 end-effectors and 24 links connected by one-degree-of-freedom joints. This result implies that the proposed algorithm enables highly redundant articulated robotic mechanisms such as a humanoid mechanism with multiple operational points to be controlled directly at the task/posture level with a high servo rate in a low-cost hardware environment while providing dynamic decoupling among the end-effectors task behavior and the complex internal posture behavior dynamics in the associated null space, resulting in intuitive task/posture behavior specifications for users.

68 CHAPTER 6. OPERATIONAL SPACE DYNAMICS 56 In order to show the effectiveness of the proposed algorithm, this robot has been controlled under the operational space formulation using the proposed algorithm. In the realtime dynamic simulation environment developed using the optimized robotics library [7], a servo rate of 300 Hz has been achieved for dynamic control and simulation of this robot with the setup above. This dynamic simulation used the optimized version of the Articulated-body method presented in section 4.2. Figure 6.3 shows the motion sequence that occurred when this robot was commanded to touch the floor with its left end-effector while maintaining the position and orientation of its right end-effector. In addition, the robot was advised to keep its posture the same as the initial configuration. Notice that the robot had to adjust its advised posture behavior in the null space without producing any coupling acceleration at both end-effectors in order not to violate the task behavior. This was done automatically without any additional commands. The top graph in Figure 6.4 shows the cubic spline motion of the left end-effector and the bottom graph in Figure 6.4 shows the constant position of the right end-effector. Figure 6.3: Motion sequence task/posture behavior control

69 CHAPTER 6. OPERATIONAL SPACE DYNAMICS left end effector (m) x xd y yd z zd time (sec) 1.5 right end effector (m) x xd y yd z zd time (sec) Figure 6.4: Plots task/posture behavior control in Figure Experimental Results This section presents experimental results with SAMM (Stanford Assistant Mobile Manipulator), a nine-degree-of-freedom redundant real robot, shown in Figure 6.5. This robot has an operational point at the end-effector and consists of a three-degree-of-freedom base and a six-degree-of-freedom manipulator; the base is a Nomadic XR4000, a holonomic mobile base with 4 powered caster modules [21], and the manipulator is a PUMA 560. Using the proposed algorithm, this robot has been controlled under the operational space formulation. In the real-time dynamic simulation/control environment developed using the optimized robotics library [7], a servo rate of 1000 Hz has been achieved for dynamic control and simulation of this robot using a PC with a 450 MHz Pentium II running under the QNX real-time operating system. Results of two experimental tasks are presented. The first case presents the results from the motion of the end-effector while the base remains fixed (task behavior only). The second case presents the results from the same motion of the end-effector while the base is

70 CHAPTER 6. OPERATIONAL SPACE DYNAMICS 58 Figure 6.5: SAMM: Stanford Assistant Mobile Manipulator moving in the null space (task/posture behavior). Finally, the results from both cases are compared and discussed. Case 1: Task behavior only Figure 6.6 shows the data gathered when this robot was commanded to follow a circle with its end-effector (task behavior) in the# - plane while its base remains fixed y (m) 1.55 actual desired z (m) Figure 6.6: Task behavior: moving end-effector only

71 CHAPTER 6. OPERATIONAL SPACE DYNAMICS 59 In Figure 6.7, this task behavior is plotted with respect to time resulting in sinusoidal graphs. The small errors ( 4%) are mainly due to friction and gear backlash in the system. 0.1 z (m) z zd time (sec) 1.7 y (m) y yd error (m) x 10 3 time (sec) (z zd) (y yd) time (sec) Figure 6.7: End-effector behavior Case 2: Task/posture behavior Figure 6.8 shows the data gathered when this robot was commanded to follow the same circle in case 1 with its end-effector (task behavior) in the# - plane. In addition, the robot was commanded to follow a circle with its base (posture behavior) in the - plane. This posture behavior is shown in Figure 6.9. As expected, the motion of the base in the null space does not affect the motion of the end-effector in the operational space. In Figures 6.10 and 6.11, the task and posture behavior is plotted with respect to time resulting in sinusoidal graphs. As in case 1, the small errors in task/posture behavior ( 4%) are mainly due to friction and gear backlash in the system.

72 CHAPTER 6. OPERATIONAL SPACE DYNAMICS y (m) 1.55 actual desired z (m) Figure 6.8: Task behavior: moving end-effector and base x (m) actual desired z (m) Figure 6.9: Posture behavior: moving end-effector and base

73 CHAPTER 6. OPERATIONAL SPACE DYNAMICS z (m) z zd time (sec) 1.7 y (m) y yd error (m) x 10 3 time (sec) (z zd) (y yd) time (sec) Figure 6.10: End-effector behavior 0.05 z (m) 0 z zd x (m) 0.05 error (m) time (sec) x xd x 10 3 time (sec) (z zd) (x xd) time (sec) Figure 6.11: Base behavior

74 CHAPTER 6. OPERATIONAL SPACE DYNAMICS 62 Discussion The experimental results presented in this section indicate that the efficiency of the proposed algorithms enable redundant robotic mechanisms such as SAMM to be controlled directly at the task/posture level with a high servo rate in a low-cost hardware environment. Notice that compared to the results from case 1, the results from case 2 show that the combined task/posture behavior does not degrade the performance of the task behavior nor the performance of the posture behavior. This illustrates one of the major benefits of the operational space formulation: dynamic decoupling between the end-effector s task behavior and the complex internal posture behavior dynamics in the associated null space while providing intuitive task/posture behavior specifications for users.

75 Chapter 7 Closed-Chain Dynamics The augmented object model for branching robots is developed. This model formulates the closed-chain dynamics as an object level dynamics with grasping constraint forces using the operational space formulation without modifying underlying kinematic and dynamic structures such as the Jacobian matrix. Also, this model provides the basis for effective cooperation between multiple robots. This chapter discusses the augmented object model in the context of mechanisms involving multiple branches such as humanoid robots and parallel mechanisms. This model renders efficient algorithms to solve closedchain dynamics problems involving branching mechanisms. Real-time dynamic simulation results are presented to illustrate the effectiveness of the proposed approach. 7.1 Introduction Cooperative manipulation is an important capability for extending the domain of robotic applications. It allows multiple robots to work together, resulting in a significant increase in their overall effective workload and workspace. Various cooperation strategies have been proposed [70, 19, 64, 65, 1, 26]. The earlier work on cooperative manipulation by multiple fixed-base serial-chain manipulators established the augmented object model describing the dynamics at the level of the manipulated object [31], and the virtual linkage model [68] characterizing internal forces. This chapter presents the extension and integration of these two basic models in order to 63

76 CHAPTER 7. CLOSED-CHAIN DYNAMICS 64 accommodate the closed-chain dynamics of systems involving branching mechanisms (Figure 7.1) while preserving overall performance. The discussion in the subsequent sections focuses on the augmented object model as dynamic modeling of cooperative manipulation among multiple robotic platforms involving multiple branches and parallel mechanisms. This model, in conjunction with the virtual linkage model, naturally renders a cooperative control structure consistent with the operational space formulation. This control structure provides the effective object level control of the closed-chain dynamics and the object internal forces. Figure 7.1: Cooperation by multiple branching robots Section 7.2 develops the augmented object model as dynamic modeling of closed-chain dynamics involving branching robots. In section 7.3 and section 7.4, efficient algorithms are developed for dynamic control and simulation of closed-chain systems using the augmented object model and the virtual linkage model. Finally, real-time simulation results with a 24-degree-of-freedom humanoid robot are presented. 7.2 Augmented Object Model The earlier augmented object model [31] provides a description of the dynamics at the operational point for a multi-manipulator system, where each manipulator has a stationary base fixed in a common inertial frame and a dynamic coupling effect only through the object shown in Figure 7.2. This subsection presents the augmented object model to accommodate branching systems, where all of the manipulators in the systems are cross-coupled not

77 CHAPTER 7. CLOSED-CHAIN DYNAMICS 65 only through the object but also through their common links shown in Figure 7.3. Figure 7.2: Dynamic coupling effect for fixed-base serial-chain mechanisms Figure 7.3: Dynamic coupling effect for a branching mechanism The augmented object model expresses the dynamics of a closed-chain system with an object and a branching mechanism as the dynamics of an augmented object taking into account the dynamics of the rest of the system shown in Figure 7.4. Then, under this aug object Figure 7.4: Augmented object model: concept model, the equations of motion of a closed-chain system can be expressed as the equations of motion of the augmented object shown in Figure 7.5. This can be written as: (7.1) where is the operational space coordinates of the object.,, and are the kinetic energy matrix (operational space inertia matrix), the centrifugal and Coriolis force

78 CHAPTER 7. CLOSED-CHAIN DYNAMICS 66 vector, and the gravity vector associated with the system of an object and branching robots, respectively. The generalized operational force vector,, is the resultant of the forces produced by all end-effectors at the operational point. f x obj Figure 7.5: Dynamics of an augmented object The simplicity of the equations associated with this model is the result of an additive property that allows us to obtain the system equations of motion from the dynamics of the individual branching mechanisms Single Branching Robot This section develops the augmented object model from Equation (7.1) for branching robots (Figure 7.6). This model formulates the closed-chain dynamics as object level dynamics with grasping constraint forces using the operational space formulation without modifying underlying kinematic and dynamic structures such as the Jacobian matrix. Therefore, this model renders an object level operational space formulation to solve the dynamics of closed-chain systems involving a branching mechanism. Figure 7.6: A branching robot holding an object

79 CHAPTER 7. CLOSED-CHAIN DYNAMICS 67 The kinetic energy matrix associated with a closed-chain system consisting of an object and a branching robot with end-effectors can be written as: (7.2) where and are the kinetic energy matrices associated with the object and the openchain branching robot, respectively. represents the connectivity of end-effectors to the common object. This matrix is the vertically concatenated matrix of identity matrices for a simple closed-chain system: (7.3) A simple closed-chain system is a closed-chain system in which the removal of the object breaks all closed loops in the system as shown in Figure 7.7. Many different robotic systems can be modeled as simple closed-chain mechanisms including a humanoid carrying a box with its arms, a dextrous hand using its fingers to manipulate an object [53], multi-legged vehicles, and mobile bases with multiple wheels [22]. Figure 7.7: Simple closed-chain mechanism Equation (7.2) results from the evaluation of the total kinetic energy,, of the closedchain system expressed with respect to the operational space velocities shown in Figure 7.8: (7.4) where is the operational space velocity vector of end-effectors of a branching robot

80 CHAPTER 7. CLOSED-CHAIN DYNAMICS 68 Λ obj Λ e = x obj x obj x C e x obj Figure 7.8: Augmented object model: kinetic energy and can be written as: (7.5) since the object and all end-effectors are expressed at the same operational point with the rigid grasp assumption. Here is a short proof of Equation (7.4): Proof: using Equation (7.5), The centrifugal and Coriolis vector of the closed-chain system,, has the additive property: (7.6) where and are the vectors of centrifugal and Coriolis forces associated with the object and the open-chain branching robot, respectively. Similarly, the gravity vector of the

81 CHAPTER 7. CLOSED-CHAIN DYNAMICS 69 closed-chain system,, can be written as: (7.7) where and are the gravity vectors associated with the object and the open-chain branching robot, respectively Multiple Branching Robots When branching robots grasp the same common object (Figure 7.1), Equation (7.1) still holds and the operational space dynamics of the system simply become the sum of the operational space dynamics of the object and each branching robot, all expressed at the same operational point. Since the rigid grasp is assumed, the operational space velocity of the end-effectors of branching robot can be written as: (7.8) where represents the connectivity of the end-effectors of the branching robot to the common object for a simple closed-chain. Then, the augmented object model for the closed-chain dynamics of the system consisting of a common object and multiple ( ) branching robots can be written as: where (7.9) (7.10) (7.11),, and represent the operational space dynamic components of the branching robot. As in the case of a single branching robot in the previous section, the kinetic energy matrix in Equation (7.9) results from the evaluation of the total kinetic energy of the

82 CHAPTER 7. CLOSED-CHAIN DYNAMICS 70 closed-chain system expressed with respect to the operational space velocities. 7.3 Algorithm for Dynamic Control This section presents dynamic control for closed-chain mechanisms under the operational space formulation using the augmented object model developed in the previous sections as well as the virtual linkage model [68]. The virtual linkage model characterizes internal forces, while the augmented object model describes the system s closed-chain dynamics as object level dynamics. Object level manipulation requires accurate control of internal forces. The virtual linkage model was proposed as a model of object internal forces associated with multi-grasp manipulation [68]. In this model, grasp points are connected by a closed, non-intersecting set of virtual links. Compared to other methods used to characterize internal forces, the virtual linkage model has the advantage of providing a physical representation of internal forces and moments. This allows control of non-zero internal forces to have a physically meaningful effect on the manipulated object Operational Space Control Structure The dynamically consistent generalized inverse of the Jacobian matrix, in Equation (2.8) has been proven to be load independent [14]. This immediately provides an important result; the dynamically consistent null space mapping matrix for the closed-chain system is the same as the one for the open-chain system, in Equation (2.11). Therefore, the closed-chain system can be controlled with the same control structure from Equations (2.2), (2.3), and (2.10) for the open-chain system: except the operational space control force,, should reflect the augmented object model and the virtual linkage model: - (7.12)

83 CHAPTER 7. CLOSED-CHAIN DYNAMICS 71 where is the resultant force at the operational point, e.g., in Equation (7.1). is the internal force and is the grasp description matrix. The grasp description matrix contains a model of the internal force representation as well as the relationship between applied grasp forces and object resultant forces [69, 60]. Thus this matrix is central to the control scheme employed by the virtual linkage model. For the cooperative operations among multiple robots, a hybrid strategy of the centralized and decentralized control architecture [35] provides an effective implementation consistent with the augmented object and virtual linkage models, while preserving the overall performance of the system. The control structure explained in this section is not the most efficient way to compute the control torque. The next section develops a computationally optimized control structure and an efficient algorithm Optimized Dynamic Control Structure As in the case of open-chain mechanisms, a computationally optimized dynamic control structure for closed-chain mechanisms can be developed from Equations (6.2), (6.3), and (6.1): except should reflect the closed-chain dynamics. Since all operational space Coriolis, centrifugal, and gravitational forces associated with the open-chain system are compensated in this structure, should take into account the dynamics of the object and the coupling effects among the end-effectors and the object. Using Equations (6.3), (7.1), (7.2), (7.6), (7.7), and (7.12), this can be written as: - (7.13) (7.14)

84 CHAPTER 7. CLOSED-CHAIN DYNAMICS 72 Table 7.1 summarizes the efficient algorithm developed in this section for operational space dynamic control of simple closed-chain mechanisms. in step 7 can be computed in since - and can be computed in using the efficient recursive method presented in section 5.2. Step 8 takes since the recursive Newton-Euler method presented in section 4.1 solves the joint space inverse dynamics of an open-chain system in. Therefore, the overall complexity of the algorithm presented in this section to solve the inverse dynamics of a simple closed-chain mechanism involving branching mechanisms is. Table 7.1: Algorithm for dynamic control of simple closed-chain 1. Given: States of the branching mechanism (, ) and the object ( 2. Object task/posture behavior command vectors:,, ) 3. Dynamics of the object: 4. Parameters of the branching mechanism: 5. Kinetic energy matrix of the augmented object: 6. Resultant force: - ( 7. Compute: /! 8. Recursive Newton-Euler method: & 9. Total control torque vector: 7.4 Algorithm for Dynamic Simulation Using Equation (2.1), the dynamic equations of motion of a closed-chain mechanism can be written as: (7.15)

85 CHAPTER 7. CLOSED-CHAIN DYNAMICS 73 where is the tip constraint force exerted on the object. Notice that since the only unknown is in Equation (7.15), if is known, the closed-chain system can be treated as an openchain mechanism with additional forces applied to its end-effectors. Therefore, the main goal of the following derivation is to find efficiently. Since simulation requires the computation of the joint acceleration given the torque, Equation (7.15) can be rewritten in terms of the joint acceleration: (7.16) where is the joint acceleration of the system if it is an open-chain system without the constraint force, i.e., removal of the object. Equation (7.16) shows the partition of the closed-chain joint acceleration into the difference of two terms: one for the open-chain joint acceleration and the other for the constraint joint acceleration. Notice that can be solved in using the Articulated-body method presented in section 4.2. Similarly, the operational space acceleration can be partitioned into the difference of two terms: one for the open-chain operational space acceleration and the other for the constraint operational space acceleration. From Equations (6.4), (3.12), and (6.9), the operational space acceleration of the closed-chain system can be written as: (7.17) Then, using Equations (7.16), (7.17), and (2.6), the operational space acceleration can be partitioned as: (7.18) where is the operational space acceleration of the system if it is an open-chain system without the constraint force, i.e., removal of the object. Notice that is the spatial

86 CHAPTER 7. CLOSED-CHAIN DYNAMICS 74 accelerations of the end-effectors and can be computed in by solving the recursive method in Equation (3.14) using the modified spatial notation presented in Chapter 3. Now, since the connectivity matrix, in Equation (7.3) is a constant matrix, the absolute derivative of Equation (7.5) is: (7.19) Then, from Equations (7.18) and (7.19), the tip constraint force, can be written in terms of the operational space acceleration and the object acceleration: (7.20) Notice that the only unknown in Equation (7.20) is. Therefore, if can be computed, the tip constraint,, can be solved and the joint acceleration,, for simulation can be computed. By realizing that the summation of the tip constraint force is the resultant force on the object, the dynamic equations of motion of the object can be written in terms of the tip constraint force and the object acceleration: (7.21) Then, by combining Equations (7.21), (7.20), and (7.2), the object acceleration can be computed with: - (7.22) Finally, using Equations (7.22), (7.20), and (7.16), the joint acceleration of the closedchain mechanism in Equation (7.15) can be computed and integrated forward for simulation. Table 7.2 summarizes the efficient algorithm developed in this section for dynamic simulation of simple closed-chain mechanisms. In Table 7.2, steps 2 and 6 take since the Articulated-body method presented in section 4.2 solves the joint space forward dynamics of a open-chain system in.

87 CHAPTER 7. CLOSED-CHAIN DYNAMICS 75 Step 3 also takes since it requires a single outward recursion. in step 4 can be computed in since - and can be computed in using the efficient recursive method presented in section 5.2. Therefore, the overall complexity of the algorithm presented in this section to solve the forward dynamics of a simple closed-chain mechanism involving branching mechanisms is. Table 7.2: Algorithm for dynamic simulation of simple closed-chain 1. Given: 2. Compute: the open-chain joint acceleration using the Articulated-body Method: & 3. Compute: the open-chain using the modified spatial notation: > > #! # 4. Compute: the object acceleration: -!! 5. Compute: the constraint force:! 6. Compute: the constraint joint acceleration using the Articulated-body Method: 7. Compute: the total joint acceleration and integrate for the next state:!

88 CHAPTER 7. CLOSED-CHAIN DYNAMICS Simulation Results Using the methods presented in previous sections, it took less than 1.5 msec using a PC with a 266 MHz Pentium II running under the QNX real-time operating system to perform the computation of the closed-chain dynamics under the operational space formulation for the humanoid robot in Figure 7.6. This branching robot has an operational point at each of its 2 end-effectors and 24 links connected by one-degree-of-freedom joints. Figure 7.9 shows the motion sequence that occurred when this robot was commanded to put the box on the floor while being advised to keep its posture the same as the initial configuration. Notice that the robot had to adjust its advised posture behavior in the null space without producing any coupling acceleration at both end-effectors in order not to violate the primary task behavior. This was done automatically without any additional commands. Figure 7.10 shows the cubic spline motion of the augmented object (box). 1.5 Figure 7.9: Motion sequence putting a box on the floor object coordinates (m) x xd y yd z zd time (sec) Figure 7.10: Plot putting a box on the floor in Figure 7.9

89 Chapter 8 Contact Dynamics The operational space dynamics formulates the contact dynamics as contact-point level dynamics since, in contact-point level dynamics, the contact point corresponds to the operational space point and the contact space corresponds to a subspace of the operational space. This chapter develops an efficient recursive algorithm to compute the operational space inertia matrix of any point in an articulated branching robot. Since the effective mass [33] of a contact point can be computed in from an operational space inertia matrix, this also provides an algorithm to compute the impulse at the contact point given the contact direction and velocity. Once the correct impulse at a contact point is determined, the corresponding instantaneous changes in joint velocity of each joint in the branching system can be computed in by the Impulse Propagation method [47]. Therefore, the proposed algorithm results in significant speed-up for real-time impulse-based dynamic simulation. 8.1 Basic Concepts The operational space inertia matrix of link, represents the inertial effect perceived at link, taking into account the combined effects of the dynamics of the whole system. Figure 8.1 shows two of these operational space inertia matrices; and represent the dynamics of the mechanism perceived at link and link, respectively. Note that these matrices describe the dynamics of the same mechanism with respect to different operational 77

90 CHAPTER 8. CONTACT DYNAMICS 78 points. leaf root h Λ h Λ i i leaf leaf Figure 8.1: Operational space inertia matrix of a point Operational Space Inertia Matrices of Links The main goal of this chapter is to develop an efficient algorithm to compute operational space inertia matrices of all links in an articulated -link branching mechanism. The basic concept used to develop this algorithm in the next section is that is the combined inertial effects of two trees: the subtree with link as its root and the rest of the tree shown in Figure 8.2. Note that the rest of the tree is the difference of two trees: the tree of the whole system and the subtree with link as its root Effective Mass Once the operational space inertia matrices are known, the effective mass at the contact point can be computed in its contact space. The effective mass at a point is a scalar quantity representing the inertial effect perceived at the point in a direction [33]. Given that a collision occurred at contact point in link, the effective mass of contact point,, along a contact direction,, can be computed as: - (8.1)

91 CHAPTER 8. CONTACT DYNAMICS 79 leaf subtree leaf root h i i leaf Figure 8.2: Basic concept where is the unit normal vector that is normal to the contact surface at contact point. is the operational space inertia matrix of contact point in link and can be computed using Equation (3.6): - - (8.2) Notice that is a constant spatial transformation matrix (see Equations (3.3) and (3.7)) since the rotation and translation of frame are constant with respect to frame Spatial Impulse In order to simulate the correct collision response, the correct impulse should be computed. Impulse is the instantaneous force causing an instantaneous change in velocity at the contact point in the contact direction [16]. By definition, the spatial impulse at contact point can be written as: (8.3)

92 CHAPTER 8. CONTACT DYNAMICS 80 where and are the spatial velocities of contact point instantaneously prior to and after the collision in the contact direction, respectively. The spatial velocity instantaneously prior to the collision in the contact direction can be written as: (8.4) where is the unit normal vector that is normal to the contact surface at the contact point. Using Equation (3.5), the spatial velocity of contact point in link can be computed as: (8.5) Given in Equation (8.4) and the coefficient of restitution,, the spatial velocity instantaneously after the collision in the contact direction,, can be computed as [16]: (8.6) Then, from Equations (8.4) and (8.6), the spatial impulse in Equation (8.3) can be be rewritten as: (8.7) Note that the spatial impulse in frame can be computed from the spatial impulse in frame using Equation (3.5): (8.8) where both frame and frame are in link.

93 CHAPTER 8. CONTACT DYNAMICS Efficient Recursive Algorithm A naive algorithm to compute the operational space inertia matrix of a contact point is to transform the link contains the contact point into the root link and compute the articulatedbody inertia matrix of this new root link. This is not the most efficient method since this naive algorithm requires extra cost to change frame assignment. In addition, it costs in order to compute the operational space inertia matrices of all links in the system since, for each of links, it takes operations to compute its articulated-body inertia matrix. The following algorithm eliminates the need for changing frame assignments and provides operational space inertia matrices of all links in the system in. This method produces the operational space inertia matrix at each link using two recursions: an inward recursion to compute and an outward recursion to compute. Assume that articulated-body inertia matrix,, of each link has been computed using Equation (4.10). Then, in the base step when, is the same as since, by definition, the articulated-body inertia matrix of the root link is equivalent to the operational space inertia matrix when the operational point is at the root link shown in Figure 8.3. This leaf leaf root = A root I root subtree leaf Figure 8.3: Base step is true since the articulated-body inertia matrix of the root link takes into account the inertia effects of all links in the system and this is the definition of the operational space inertia matrix with the operational point at the root link shown in Figure 8.4.

94 CHAPTER 8. CONTACT DYNAMICS 82 f leaf f leaf A I root a root articulated-body a root % )*'5,$%*(".8) %*$(", f = I A root a A I root = f = a Figure 8.4: Base step: proof Recursion step computes, given and, where link is a child link of link. This step is the combination of the Articulated-body method presented in section 4.2 and an extension of the Inertia Propagation method presented in section There are two major parts to be done in this step. The first part computes the operational space inertia matrix of link, taking into account only the rest of the tree. This is done by subtracting the inertial effect of the subtree with link as its root from the operational space inertia matrix of link shown in Figure 8.5. Then, the second part computes the operational space root h i leaf leaf leaf - i A I i subtree leaf leaf h h A h T ( LI L ) h i i i leaf = root h Figure 8.5: Recursion step: first part

95 CHAPTER 8. CONTACT DYNAMICS 83 inertia matrix of link,, by combining the articulated-body inertia matrix of link, which describes the inertial effect of the subtree and the dynamically consistent projection of the operational space inertia matrix of link, inertial effect of the rest of the tree. This is shown in Figure 8.6.,, which describes the root h '5,8%,!",'5 h A h T ( LI L ) h i i i leaf + i A I i subtree leaf leaf = root h leaf leaf i leaf i h A h T ( LI L ) K A h T I K h i = i + i h i i i i Figure 8.6: Recursion step: second part Therefore, having found the operational space inertia matrix of the first link, the recursion may proceed in the outward direction to obtain the operational space inertia matrix of the next link, and so on. This can be written as: where (8.9) is the operational space inertia matrix of link, taking into account the dynamics of the rest of the tree, i.e., all links in the system except the ones in the subtree with link as its root. The definition of the propagator,, is similar to in

96 CHAPTER 8. CONTACT DYNAMICS 84 Equation (3.17) in the opposite direction (base-to-tip) and - in Equation (5.2): (8.10) where is a identity matrix. is the generalized inverse of weighted by the corresponding inertia matrices: - (8.11) (8.12) - - (8.13) where, similar to in Equation (3.19) and in Equation (5.4), is the full onto the motion space of joint with degrees rank matrix projecting of freedom. Also, similar to in Equation (3.18) and in Equation (5.3), is the dynamically consistent generalized inverse of. Note that each step of the outward recursion is basically equivalent to the method for computing the articulated-body inertia (4.10) of link when link is the root link of the branching mechanism. Table 8.1 summarizes the algorithm developed in this section. 8.3 Impulse-based Dynamic Simulation This section presents the Impulse Propagation method [47] using the modified spatial notation presented in Chapter 3. The Impulse Propagation method is a hybrid impulsebased dynamic simulation method that combines the conventional constraint-based methods [3, 4] and the newer impulse-based methods [39, 45, 56] in order to take advantage of the strengths of both approaches. Given the correct impulse at the contact point, this method computes the instantaneous changes in velocities of all joints in an articulated branching system with complexity. Therefore, with the algorithm to compute the impulse at the contact point presented in the previous sections, this method renders the correct and fast collision response for dynamic simulation.

97 CHAPTER 8. CONTACT DYNAMICS Given: Table 8.1: Algorithm for operational space inertia matrices of all links 2. Inward Recursion: Compute the articulated-body inertia matrix: :.! > > 02 > -> 3 > >? 3. Outward Recursion: Compute the operational space inertia matrix of each link: 6! 7 9 : 3? - 6! > > %> 3 -! 7-3 > -> > 6 The main concept of the Impulse Propagation method is to propagate the articulatedbody impulse vector of the contact link back to the root link and then, to propagate the instantaneous changes in joint velocities via a single outward recursion. The definition of the articulated-body impulse vector of link,, is the integration of the articulated-body bias force of link, in Equation (4.11), over the collision period, : (8.14) Note that the collision period is an infinitesimal interval since the rigid-body assumption is used. The effects of the finite magnitude centrifugal, Coriolis, bias and joint forces can be

98 CHAPTER 8. CONTACT DYNAMICS 86 ignored since they vanish in the limit, : (8.15) Similarly, the effects of the external forces can be ignored in the limit except the impulse at link where the collision occurred: (8.16) where is the spatial impulse computed using Equations (8.7) and (8.8) when the collision occurred at contact point. Note that since the configuration of the whole system does not change during the collision, the terms depending only on configurations such as,, and stay constant during the collision. Using Equations (4.11) and (8.14) with the conditions (8.15) and (8.16) at the limit, the articulated-body impulse vector can be propagated from the contact link to the root link via a single inward recursion: where is the index of the link where the contact occurred. Note that (8.17) is constant during the collision since it is dependent on configuration only. This inward recursion process corresponds (1) shown in Figure 8.7. The quantity is the total change in joint velocity of link during the collision; it is the limit of infinite joint acceleration integrated over an infinitesimal interval: (8.18) Using Equations (4.12), (8.14), and (8.18) with the conditions (8.15) at the limit, the total change in joint velocity of link can be written as: - (8.19)

99 CHAPTER 8. CONTACT DYNAMICS 87 q i (2) v i h i y = A h h i Ly A i k (1) c y = A k k c Xy c Figure 8.7: Impulse Propagation method: recursion processes Similarly, the quantity is the total change in spatial velocity of link during the collision; it is the limit of infinite acceleration integrated over an infinitesimal interval: (8.20) Using Equation (3.14), (8.18), and (8.20) with the conditions (8.15) at the limit, the total change in spatial velocity of link can be written as: (8.21) Note that the instantaneous changes in velocities of all joints in an articulated branching system can be computed via a single outward recursion from the root link to the leaf links using Equations (8.19) and (8.21) at each recursion step. This outward recursion process corresponds (2) shown in Figure 8.7. Table 8.2 summarizes the Impulse Propagation method. The complexity of this method is since each step in Table 8.2 takes operations. Recently, many efficient collision detection algorithms [50, 51] have been developed based on fast distance calculation algorithms [15, 43]. Once correct collision is determined using one of these collision detection algorithms, the correct spatial impulse, can be computed by the algorithm presented in the previous section. Then, by propagating

100 CHAPTER 8. CONTACT DYNAMICS Given: Table 8.2: Impulse Propagation method 2. Inward Recursion: Compute the articulated-body impulse vector:! Outward Recursion: Compute the instantaneous changes in the joint velocity and the spatial velocity:! - >! > > 3 3? this spatial impulse appropriately, correct instantaneous changes in velocities of all joints,, in an articulated branching system can be computed using the Impulse Propagation method presented in this section. Finally, with appropriate changes in joint velocities, the configuration after the collision can be computed by integrating forward one time step for dynamic simulation. Therefore, the algorithms developed in this chapter result in a significant speed-up for real-time impulse-based dynamic simulation.

101 Chapter 9 System Integration This chapter describes the implementation and integration of the algorithms presented in this thesis into a complete robotic system. Overall hardware/software architecture is presented to illustrate the current configuration of a real-time dynamic control/simulation environment. Issues involving user interface, control/simulation servo loop, and optimization techniques used to implement the proposed algorithms in a real-time environment are further discussed. Finally, this chapter concludes with a remark about the implication of the capabilities of the current real-time environment that incorporates the ideas presented in this thesis. 9.1 Hardware/Software Architecture A prototype real-time dynamic control/simulation environment [7] has been developed to validate the ideas presented in this thesis. This environment is designed to incorporate and utilize the current technology available both in hardware and software in the industry. The overall hardware/software architecture of the current environment is shown in Figure 9.1. In order to make the user interface platform-independent, a robot user interface has been developed using VRML [18, 2] for graphics modeling/display and JAVA [24, 25] for implementing the front-end software. Through an Internet browser-based front-end user interface, users can communicate with the back-end control/simulation servo loop virtually from any platform. For real-time dynamic control/simulation, the back-end software is 89

102 CHAPTER 9. SYSTEM INTEGRATION 90 User 4 "7 8 * 791 ) :* * -6-2"7 Front-end &-,./ # $%5& %( )) * + Back-end 9 - : 9 mputer - Servo (C++) Drivers (C/C++) Controller 91 * 7 91 * > 2 Robot Proxy # $%'&%( ) ) * +! " Sensors # ; ) <* =*+ Figure 9.1: Overall hardware/software architecture implemented in C/C++ [27, 63] since the efficiency of C/C++ is the most crucial element for performance of the servo loop. An example of the control/simulation servo loop is shown in Figure 9.2. Notice that this message-based servo loop takes into account the discrepancy of the rates between the slower (? 30 Hz) user interface communication and the faster (? 1000 Hz) control/simulation servo loop. 9.2 Robot User Interface A robot user interface has been developed to take advantage of the capabilities of the proposed algorithms under the operational space framework. This interface provides basic capacities for users to control and simulate, in real-time, both a real robot and a graphical robot simultaneously. This interface promotes more intuitive and safer usage of robotic systems since the real-time dynamic simulation enables interactive feedback, validation, and verification of desired robot commands prior to actual executions. A PUMA 560 robotic system and its Internet browser-based robot user interface are shown in Figure 9.3.

103 CHAPTER 9. SYSTEM INTEGRATION 91 Start Finish Connected? yes robot 5 ' 3 N robot Disconnect no #"$ no yes! yes Report no no Empty(Q)? no % '& Message? yes yes ( *) + A*&O') )2 ;+, ( *) +,'-. / 0 1')2) 3+4 Insert(Q,Mesg) ( ')2+ #5 *64/ '78:9 2M A 2A N! 7 ( ') +,9 * ;< =!,!;?>@A)2!BC ;0 / ') Safe? no I2!J>@ ;-. KGLJ 0 /4!; yes? 4/, DE! FG2!;6H/ BC ;0 / ') Control/Simulation 03 Q Figure 9.2: Control/simulation servo loop flow chart (Q is a priority queue)

104 CHAPTER 9. SYSTEM INTEGRATION 92 Figure 9.3: PUMA 560 robotic system and internet-based user interface In this user interface environment, robot commands are given using text and/or graphical tools. Text based user interface is necessary for fine-tuning of desired robot commands. In order to cope with the discrepancy between the higher dimensional nature of the graphical modeling of robots and the lower dimensional input/output devices such as a monitor and a mouse, graphical user interface tools are developed to reduce the dimensions for users to manipulate at any given instance. In Figure 9.4, from the left, disks are used to specify one-degree-of-freedom joint angles, planes are used to specify translations, and circular belts are used to specify general orientations. Figure 9.4: Graphical user interface tools: disks, planes, and circular belts