experiments in the integration and control of an intelligent ... - CiteSeerX

EXPERIMENTS IN THE INTEGRATION AND CONTROL OF AN INTELLIGENT MANUFACTURING WORKCELL

A DISSERTATION SUBMITTED TO THE DEPARTMENT OF ELECTRICAL ENGINEERING AND THE COMMITTEE ON GRADUATE STUDIES OF STANFORD UNIVERSITY IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY

By Gerardo Pardo-Castellote June 1995

Copyright c 1995 by Gerardo Pardo-Castellote All Rights Reserved.

ii

I certify that I have read this dissertation and that in my opinion it is fully adequate, in scope and in quality, as a dissertation for the degree of Doctor of Philosophy.

Robert H. Cannon, Jr. Department of Aeronautics and Astronautics (Principal Adviser)


Gene F. Franklin Department of Electrical Engineering


Jean-Claude Latombe Department of Computer Science

Approved for the University Committee on Graduate Studies:

iii

Abstract This thesis comprises the experimental development of an intelligent, dual-arm robotic workcell. The system combines a high-level graphical user interface, an on-line motion planner, real-time vision, and an on-line simulator to control a dual-arm robotic system from the task level. The graphical user interface provides for high-level user direction of the task to be done. The motion planner generates complete on-line plans to carry out these directives, specifying both single and dual-armed motion and manipulation. Combined with the robot control and real-time vision, the system is capable of performing object acquisition from a moving conveyor belt as well as reacting to environmental changes on-line. The thesis covers in detail four main topics: 1. System design and interfaces. The system is based on a novel “interface-first” design technique. This technique structures the complex command and data flow as combinations of three fundamental robotic interface components: the world-state interface, the robot-command interface, and the task-level-direction interface. 2. Network communication architecture. Complex distributed robotic systems require very complex data flow. A powerful new subscription-based, network-data-sharing system, was developed (and is being commercialized) that enables transparent connectivity. 3. Control system. The architecture and design of the hierarchical control system for the experimental dual-arm assembly workcell is described. 4. Path time-parameterization. A fast (linear-time, proximate-optimal) solution to the fundamental problem of time-parameterization of robot paths is presented. The design was verified experimentally in a dual-arm robotic workcell. Experimental results are presented showing the system performing complex, multi-step tasks autonomously, including dual-arm object acquisitions from a moving conveyor, object motion among obstacles, re-grasps, and hand-overs. All these tasks occur under task-level human supervision.

iv

To my parents

v

Acknowledgements I wish to thank my principal advisor, Professor Robert H. Cannon Jr., for his guidance, encouragement, and support during the course of the research. His exemplary personality and technical insight have made graduate research fruitful and enjoyable. I was extremely fortunate to have him as an advisor. I thank my associate advisor, Professor Gene Franklin for his perceptive comments and accommodating my last-minute needs. I am indebted to my associate advisor, Professor Jean-Claude Latombe for his professional and personal advice from the moment I arrived to Stanford. He first guided me during my Master in Computer Science and has since influenced and supported my career in uncountable occasions. I am further indebted Stan Schneider for convincing me to join the Aerospace Robotics Laboratory and being a constant source of friendship, advice (technical and otherwise), and encouragement. I have greatly benefited from the legacy of his work. The Aerospace Robotics Laboratory has always been a friendly and stimulating research environment. I thank Professors Robert Cannon and Steve Rock for creating and directing it, Jane Lintott for administering it with such skill, and all my fellow students for their good-natured attitude and help. Among them I would like to specially thank Larry Pfeffer for developing the experimental hardware and his extensive advice on the design of the control system; Denny Morse for his selfless and timely assistance on computer subjects; Marc Ullman for his lessons on real-time computers, and general advice in all subjects; Dave Meer and Steve Ims for their dedication to administering the lab computer system; Stef Ssonck, Kurt Zimmermann, Howard Wang, David Miles, and Jeff Russakow for their patient review of parts of this thesis and constructive comments. I extend my thanks to the ARL technical staff. Gad Shelef for his help in the design of the modifications to the manipulator hardware, and Godwin Zhang for his efforts and able assistance in maintaining the electronics of the workcell. Financial support for this research was provided by a Fellowship form the Spanish Ministry of Education and ARPA contract N00014-92-J-1809. Finally, I would like to thank my parents for implanting in me the desire to learn, stressing and contributing to my education, giving me freedom to conduct my life from and early age, and always being respectful, understanding, and supportive. I really could not have asked for anything more. It is to them that I dedicate this thesis.

vi

Contents Abstract

iv

Acknowledgements

vi

List of Tables

xii

List of Figures

xiii

1 Introduction 1.1 1.2 1.3 1.4

1.5 1.6

:::::: Research Objectives : Research Issues : : : Summary of Results : Motivation

: : : :

: : : :

: : : :

: : : :

: : : :

: : : :

: : : :

: : : :

: : : :

2.2

2.3

: : : :

: : : :

: : : :

1.4.1

System Capabilities and Demonstrations

1.4.2

Contributions

:::::::::: Related Research Activities : : : : : : : 1.5.1 Supporting Research Activities : Reader’s Guide : : : : : : : : : : : : : :

2 Experimental Dual-Arm Robotic Workcell 2.1

: : : :

:::::::::::::: Manipulator Mechanism and Sensors : 2.2.1 Kinematics : : : : : : : : : : : 2.2.2 Joint-Torque Sensors : : : : : : 2.2.3 Joint Encoders : : : : : : : : : Workspace Objects : : : : : : : : : : : Introduction

vii

: : : : : :

: : : :

: : : : : :

: : : :

: : : : : :

: : : :

: : : : : :

: : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

: : : : : : : : : : : : : : :

1 1 3 4 7 7 9 12 13 15 16 16 16 17 20 20 22

2.4 2.5 2.6

:::::::::::::::::::::::::::::::::::::: Vision System : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : Computer System : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : :

Conveyor

3 Architecture, Interfaces and System Operation 3.1 3.2 3.3 3.4

3.5

3.6

3.7

:::::::::::::::::::::: Approaches to System Design : : : : : : : : : : : : : : : : Modular Interfaces : : : : : : : : : : : : : : : : : : : : : : Information Interfaces for the Manufacturing Workcell : : : 3.4.1 The World-State Interface : : : : : : : : : : : : : : 3.4.2 The System-Command Interface : : : : : : : : : : : 3.4.3 The Task-Specification Interface : : : : : : : : : : : 3.4.4 Custom Interfaces for the Manufacturing Workcell : System Architecture : : : : : : : : : : : : : : : : : : : : : 3.5.1 The Graphical User Interface : : : : : : : : : : : : 3.5.2 The Simulator : : : : : : : : : : : : : : : : : : : : 3.5.3 The World Modeler : : : : : : : : : : : : : : : : : 3.5.4 The Hierarchical Control System : : : : : : : : : : 3.5.5 The Planning Subsystem : : : : : : : : : : : : : : : Experimental System Operation : : : : : : : : : : : : : : : 3.6.1 Operational Description : : : : : : : : : : : : : : : 3.6.2 Experimental Tasks : : : : : : : : : : : : : : : : : Summary and Conclusions : : : : : : : : : : : : : : : : : : Literature Review

4.2

4.3

:::::: Literature Review: Communications in Distributed Systems : 4.2.1 The applications’ view of the information exchange : 4.2.2 Implementation aspects : : : : : : : : : : : : : : : 4.2.3 Review of Related Approaches : : : : : : : : : : : The NDDS Communications Model : : : : : : : : : : : : : 4.3.1 Producer Characteristics : : : : : : : : : : : : : : : 4.3.2 Consumer Characteristics : : : : : : : : : : : : : : 4.3.3 One-time Queries : : : : : : : : : : : : : : : : : : The Role of Communications Within the System

viii

24 25 28

: : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : :

4 Communications in Distributed Robotic Systems 4.1

23

29 34 37 38 38 39 41 42 43 44 45 48 49 49 51 51 52 77 78

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

80 82 83 85 86 90 92 93 94

:::::::: Implementation : : : : : : : : : : : : 4.4.1 Architectural Overview : : : : 4.4.2 Data Management Overview : : Experimental Results: NDDS : : : : : 4.5.1 Experimental Context : : : : : 4.5.2 Latency of the Updates : : : : 4.5.3 Maximum update rates : : : : : Summary : : : : : : : : : : : : : : : : 4.3.4

4.4

4.5

4.6

Reliable Updates

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

5 World Modeling 5.1 5.2 5.3

5.4

5.5

6.2 6.3 6.4 6.5

6.6 6.7

: : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : :

: : : : : : : : :

:::::: Literature Review: Hierarchical Robotic Control Systems : Control System Hierarchy : : : : : : : : : : : : : : : : : Joint-Control Layer : : : : : : : : : : : : : : : : : : : : Arm-Control Layer : : : : : : : : : : : : : : : : : : : : : 6.5.1 Arm Controller : : : : : : : : : : : : : : : : : : 6.5.2 Arm Controller Performance : : : : : : : : : : : : Object-Control Layer : : : : : : : : : : : : : : : : : : : Strategic-Control Layer : : : : : : : : : : : : : : : : : : 6.7.1 The Finite-State Machine Programming Model : : Control System for the Manufacturing Workcell

ix

: : : : : : :

: : : : : : : : :

: : : : : : : : : :

: : : : : : :

: : : : : : : : :

: : : : : : : : : :

: : : : : : :

: : : : : : : : :

: : : : : : : : : :

: : : : : : :

: : : : : : : : :

: : : : : : : : :

: Literature Review: World Modeling for Robotics : Architecture of the World Modeler : : : : : : : : : 5.3.1 The Object-Based Layer : : : : : : : : : : 5.3.2 The Sensor Processing/Integration Layer : 5.3.3 Characteristics of the Architecture : : : : : Sensor Integration : : : : : : : : : : : : : : : : : 5.4.1 Integration of Kinematics and Vision for Robot-Arm Position : 5.4.2 Robot State and Vision for Grasped-Object Position : : : : : : 5.4.3 Conveyor Displacement and Vision for Object Position : : : : Summary : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : World Modeling for the Manufacturing Workcell

6 Hierarchical Control System 6.1

: : : : : : : : :

: : : : : : : : : :

: : : : : : :

: : : : : : : : : :

: : : : : : : : : :

: : : : : : : : : : : : : : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : : : : : : : : : : : : : :

95 96 96 98 98 100 101 105 111 113 114 116 119 121 122 122 124 125 131 132 136 137 137 138 139 141 147 147 151 151 157 159

: 6.7.3 Picking an Object from the Conveyor Belt : : : : 6.7.4 Experimental Pick Operations : : : : : : : : : : Summary : : : : : : : : : : : : : : : : : : : : : : : : : 6.7.2

6.8

Strategic Control of the Workcell Using FSMs

7 On-Line Trajectory Generation

: : : :

: : : :

: : : :

: : : :

: : : :

: : : :

: : : :

: : : :

: : : :

: : : :

: : : :

: : : :

: : : :

:::::::::::::: ::::::::::::::::::::: :::::::::::::::::::::

160 168 170 178 179

7.1

The Role of Trajectory Generation within the Workcell

179

7.2

Literature Review: Trajectory Generation

182

7.3

Problem Formulation and Known Results 7.3.1

7.4 7.5

7.6

7.7

Minimum travel time with limits on actuator torque, acceleration and velocity186

::::: Formulation of the Proximate-Optimal Problem : : : : : : : The Proximate-Optimal Time-Parameterization Algorithm : 7.5.1 Continuous-Domain version : : : : : : : : : : : : : 7.5.2 Algorithm correctness : : : : : : : : : : : : : : : : 7.5.3 Discrete Implementation : : : : : : : : : : : : : : : 7.5.4 Complexity of the algorithm : : : : : : : : : : : : : 7.5.5 Complexity of other approaches : : : : : : : : : : : 7.3.2

Case of velocity-independent torque limits

8.2

: : : : : : : :

: : : : : : : :

: : : : : : : :

: : : : : : : :

: : : : : : : :

: : : : : : : :

7.5.6

Configuration-independent limits on velocities and accelerations

7.5.7

Modification of Ongoing Trajectories

::: Experimental Results: Time Parameterization : : : 7.6.1 Straight-line trajectories for a point mass : 7.6.2 Trajectories for the workcell manipulators : 7.6.3 Modifications to on-going trajectories : : : Summary and Conclusions : : : : : : : : : : : : :

8 Conclusions 8.1

185

: : : : : :

: : : : : :

: : : : : :

:::::::::::::::: Suggestions for Future Research : : : : : : : : : : : : : 8.2.1 Extensions to the Architecture and Interfaces : : 8.2.2 Enhancements to the Subsystems : : : : : : : : 8.2.3 Higher Degree of Planning Integration : : : : : : 8.2.4 Enhancements to the Communications Layer. : : 8.2.5 Possible Follow-on Experiments. : : : : : : : : Summary and Conclusions

x

: : : : : : : : : : : : :

: : : : : : : : : : : : :

: : : : : : : : : : : : :

: : : : : : : : : : : : :

: : : : : : : : : : : : :

: : : : : : : : : : : : :

: : : : : : : : : : : : :

: : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : : : : : :

190 190 194 194 195 197 199 200 207 208 212 213 215 218 225 226 226 232 232 233 235 236 237

8.3

Concluding Remarks

::::::::::::::::::::::::::::::::

A Calibration

237 240

::::::::::::::::::::::::::::: Arm Base Location Calibration : : : : : : : : : : : : : : : : : : : : : : : : : : Inertial Properties Measurement : : : : : : : : : : : : : : : : : : : : : : : : : :

A.1 Vision System Calibration

240

A.2

240

A.3

242

B Joint Control Parameters

244

C Arm Control Parameters

246

D Manipulator Equations of Motion

248

E Canonical Paths for Time-Parameterization

251

F Worst-Case Paths for Time-Parameterization

253

G Matrix Plotting Utilities

258

G.1 Overview : G.2 G.3

::::::::::::::::::::::::::::::::::::: Application Interface : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : Selected Interface Documentation : : : : : : : : : : : : : : : : : : : : : : : : :

H Selected Manual Pages

258 260 261 263

xi

List of Tables

3.4

::::::::: Actuator characteristics : : : : : : : : : : Encoder types and resolution : : : : : : : : Characteristics of the workspace objects : : Real-Time Computer Components : : : : : Characteristics of the information flow : : : The world state interface : : : : : : : : : : System command interface : : : : : : : : : Task-specification interface : : : : : : : :

4.1

Functional interface to produce, consume and query data.

2.1 2.2 2.3 2.4 2.5 3.1 3.2 3.3

Arm kinematic parameters

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

: : : : : : : : :

: : : : : : : : : : : : : :

: : : : : : : : : : : : : :

: : : : : : : : : : : : : :

: : : : : : : : : : : : : :

: : : : : : : : : : : : : :

: : : : : : : : : : : : : :

: : : : : : : : : : : : : :

: : : : : : : : : : : : : :

: : : : : : : : : : : : : :

: : : : : : : : : : : : : :

6.3 7.1

Related approaches to the Decoupled-Optimal-Time-Parameterization Problem

6.1 6.2

World-Model private interface

:::::::::::::::::::: Estimator parameters for the right shoulder motor plant. : : : : : Controller and parameters for the right shoulder motor plant. : : Parameters for the arm-level controllers. : : : : : : : : : : : : : Meaning of symbols in dynamic equations for the arms. : : : : :

A.1 Calibrated arm parameters B.1 B.2 C.1 D.1

: : : :

: : : : : : : : : : : : : :

:::::::::::: Summary of control layers and their interfaces. : : : Notation for arm-impedance controller derivation. : : Modes of Operation : : : : : : : : : : : : : : : : :

5.1

: : : :

: : : : : : : : :

xii

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : : : :

21 21 22 24 26 38 39 40 42 91 129 142 148 164 186 242 244 245 246 250

List of Figures

3.8

: : : : : : : : : : : : : : Commanding and Monitoring the Workcell with the several User Interfaces : Concept of Safe-Under-Time-Delay (SUTD) planning : : : : : : : : : : : :

3.9

Use of configuration-time space for safe-under-time-delay (SUTD) planning

1.1 1.2 2.1 2.2 2.3 2.4 2.5 2.6 3.1 3.2 3.3 3.4 3.5 3.6 3.7

:::::::::::::: Experimental System : : : : : : : : : : : : : Experimental Dual-Arm Robotic Workcell : : Z and Yaw degrees of freedom : : : : : : : : Arm Schematic : : : : : : : : : : : : : : : :

: : : : : Schematic of last Z and Yaw degrees of freedom and actuation : The different objects the robot interacts with : : : : : : : : : : : Hardware Architecture : : : : : : : : : : : : : : : : : : : : : : Contrasted Design Techniques : : : : : : : : : : : : : : : : : : Modular Interface Design : : : : : : : : : : : : : : : : : : : : System Architecture : : : : : : : : : : : : : : : : : : : : : : : Commanding and Monitoring the Workcell with the GUI (1-6) : Commanding and Monitoring the Workcell with the GUI (7-12) :

: : : : : : : : : : : : : Commanding and Monitoring the Workcell with the GUI (13-18). : Research Objective

:::::::: Typical system operation : : : : : : : Animation of capture and delivery : : Animation of hand-over operation : :

3.11 3.12 3.13 3.14 3.15

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : :

: : : : : : : : : : : : : :

: : : : :

: : : : : : : : : : : : : :

: : : : :

: : : : : : : : : : : : : :

: : : : :

: : : : : : : : : : : : : :

: : : : Photographs during hand-over operation : Animation of multi-object-placing sequence with static objects (stages 1-8) :

3.10 System Configurations

: : : :

: : : : :

: : : : :

3.16 Animation of multi-object-placing sequence with static objects (stages 9-14)

: : : : : : : : : : : : : : : : : : : : : : : :

: : : : : : : : : : : : : : : : : : : : : : : :

3.17 Photographs during multi-object-placing sequence with static objects (stages 1-6) xiii

: : : : : : : : : : : : : : : : : : : : : : : :

4 7 17 18 18 19 23 27 35 37 43 46 47 55 56 57 58 59 60 61 62 63 64 65 66

3.18 Photographs during static assembly sequence (stages 7-10)

67

3.19

:::::::::::: Photographs during static assembly sequence (stages 11-14) : : : : : : : : : : : :

68

3.20 Animation of multi-object-placing sequence with objects delivered on the conveyor

70

3.21 Photographs during multi-object-placing sequence with with objects delivered on conveyor

::::::::::::::::::::::::::::::::::::::

71

3.22 Animation of multi-object-placing sequence with different conveyor position (stages

::::::::::::::::::::::::::::::::::::::::

72

3.23 Animation of assembly sequence with conveyor across the workspace (stages 7-14)

73

3.24 Detailed strobe images of an multi-part-placement operation (1-15)

74

1-6)

3.25 3.26 4.1 4.2 4.3 4.4 4.5 4.6 4.7 4.8 4.9 4.10 4.11

:: Detailed strobe images of an multi-part-placement operation (16-30) : Detailed strobe images of an multi-part-placement operation (31-45 : : Architecture of the Two-Armed robotic workcell. : : : : : : : : : : : Multiple producer conflict resolution. : : : : : : : : : : : : : : : : : Consumer notification rate. : : : : : : : : : : : : : : : : : : : : : : One-Time Query Parameters. : : : : : : : : : : : : : : : : : : : : : Communication between NDDS nodes. : : : : : : : : : : : : : : : : Latency overhead of NDDS as a function of item size. : : : : : : : : : Latency overhead as a function of the number of items : : : : : : : :

: : : : : : : : : :

: : : : : : : : : :

: : : : : : : : : : Latency overhead as a function of the number of items for two consumers : Latency overhead as a function of the number of items for four consumers : Maximum update rate as a function of the number of subscribers. : : : : : : Influence of number of items requested on the maximum update rate. : : : :

: : : : : : : : : : : : : :

: : : : : : : : : : : : : :

4.12 Influence of computer performance and update size on the maximum update rate. 4.13 Maximum update rate for two clients as a function of the item sizes. 4.14 Maximum update rate for four clients as a function of the item sizes.. 5.1 5.2 5.3 5.4 5.5 5.6 5.7 5.8

Roles of the World Modeling Subsystem :

:::::::::::: Architecture of the World Modeling Subsystem : : : : : : : : : Arms and rigid-objects are represented using software objects. : Comparison of WM architecture with purely hierarchical WM : On-Line estimator of initial angular offsets : : : : : : : : : : : Performance of On-Line angular-offset estimator : : : : : : : : Adaptation to power-up configuration : : : : : : : : : : : : : : Sensor Fusion for the position of grasped objects : : : : : : : : xiv

: : : : : : : :

: : : : : : : :

: : : : : : : : : :

: : : : : : : : : :

: : : : : : : : : :

: : : : : : : : : :

: : : : : : : : : :

: : : : : : : : : :

: : : : : : : : : : : : : : : : : : : : : : : : :

75 76 80 93 94 95 97 101 103 104 105 106 108 109 110 111 115 119 121 123 127 128 130 131

6.2

:::::::::: Tracking performance of objects on the conveyor at 5 cm/sec : : : : : Tracking performance of objects on the conveyor at 10 and 20 cm/s : : Hierarchical Control Subsystem : : : : : : : : : : : : : : : : : : : : Hierarchical Control Dataflow : : : : : : : : : : : : : : : : : : : : :

6.3

Experimental frequency response for the motor-drive-train-link plant of the right arm.144

6.4

Closed-loop roots for the motor-drive-train-link plant and controller of the right

5.9 5.10 5.11 6.1

Sensor integration scheme for objects on a conveyor

6.11 6.12 6.13 6.14 6.15 6.16 6.17 6.18 6.19 6.20 6.21 6.22 6.23 6.24 6.25 6.26 6.27

: : : :

::::::::::::::::::::::::::::::::::: Arm-control layer dataflow. : : : : : : : : : : : : : : : : : : : : : : : : Experimental tracking performance: straight-line path : : : : : : : : : : : Experimental tracking performance: path through kinematic singularity. : Animation of trajectory through kinematic singularity. : : : : : : : : : : : Comparison of cooperative object control with coordinated object control : Dataflow of the object-impedance controller : : : : : : : : : : : : : : : : Experimental tracking performance of the object-impedance controller : : Animation of trajectory followed by the object under cooperative control : ControlShell.4.x’s Finite-State Machine model : : : : : : : : : : : : : : Architecture of the strategic-control layer : : : : : : : : : : : : : : : : : Sources of arm and object trajectories : : : : : : : : : : : : : : : : : : : Transition Graph for the Arm Finite-State Machines : : : : : : : : : : : : Transition Graph for Capture Subchain : : : : : : : : : : : : : : : : : : Transition Graph for Release Subchain : : : : : : : : : : : : : : : : : : Capture Strategy : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : Experimental single-arm capture of moving object : : : : : : : : : : : : : Experimental dual-arm capture of moving object : : : : : : : : : : : : : Experimental simultaneous multiple-object capture : : : : : : : : : : : : Animation of experimental data of simultaneous multiple-object capture : troller.

6.10

: : : :

xv

: : : :

: : : : : : : : : : : : : : : : : : : :

: : : :

: : : : :

: : : : : : : : : : : : : : : : : : : :

: : : :

: : : : :

Merging a cartesian-space computed-torque controller with a joint-space PID con-

6.9

: : : :

: : : : :

6.8

6.6

: : : :

: : : : :

6.7

6.5

: : : :

: : : : :

:::::::::::::::::::::::::::: Joint-control layer dataflow. : : : : : : : : : : : : : : : : : : Joint-torque step response of left shoulder subsystem. : : : : : Block diagram for the computed-torque impedance controller. : shoulder.

: : : :

: : : : :

: : : : : : : : : : : : : : : : : : : :

: : : :

: : : : : : : : : : : : : : : : : : : :

133 134 135 139 140

145 145 146 149 149 152 153 154 155 156 157 158 159 160 161 163 166 167 168 169 171 172 173 174

6.28 Photographic sequence of system picking two objects from the conveyor simultaneously :

6.29

:::::::::::::::::::::::::::::::::::::: Photographic sequence of system picking two objects from the conveyor : : : : :

175 176

6.30 Photographic sequence of system using two arms to pick the same object from the

::::::::::::::::::::::::::: The Time-Parameterization Process : : : : : : : : : : : : : Optimal time-parameterization constraints in phase-space : : Illustration of conditions of Theorem 1 : : : : : : : : : : :

7.5

: : : : Integration trajectory in phase space using proximate-optimal constraints : Steps of proximate-optimal algorithm : : : : : : : : : : : : : : : : : : :

7.6

Running time versus number of via points for different number of degrees of freedom200

7.7

Sketch of Shin’s direct-integration algorithm (from [164], Figure 6)

conveyor

7.1 7.2 7.3 7.4

7.8 7.9 7.10 7.11 7.12 7.13 7.14 7.15 7.16 7.17 7.18 7.19 7.20 7.21 7.22 A.1 A.2 C.1 D.1 D.2 E.1

: : : :

: : : :

: : : :

: : : :

: : : :

: : : :

: : : : : :

: : : : : :

: : : : : :

: : : : : :

:::::::: Worst-case scenario for direct-integration algorithms. : : : : : : : : : : : : : : : Modified direct-integration algorithm that avoids worst-case complexity problem : Comparison of approaches to time-parameterization : : : : : : : : : : : : : : : : Patching of an ongoing trajectory : : : : : : : : : : : : : : : : : : : : : : : : : Trajectory Modification Algorithm : : : : : : : : : : : : : : : : : : : : : : : : : Example of Trajectory Modification Algorithm : : : : : : : : : : : : : : : : : : Time-parameterization of straight-line path with acceleration limits : : : : : : : : Time-parameterization of straight-line path with velocity and acceleration limits : Time-parameterization of a joint-space straight-line path : : : : : : : : : : : : : Time-parameterization of a cartesian straight-line path : : : : : : : : : : : : : : Time-parameterization of a dual-arm path : : : : : : : : : : : : : : : : : : : : : Initial path and two subsequent modifications : : : : : : : : : : : : : : : : : : : Time-parameterization of initial path : : : : : : : : : : : : : : : : : : : : : : : : Results after first modification to on-going trajectory : : : : : : : : : : : : : : : Results after second modification to on-going trajectory : : : : : : : : : : : : : : Vision system calibration : : : : : : : : : : : : : : : : : : : : : : : : : : : : : Kinematic calibration : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : Step Response of Inverse-Dynamic Arm Controller : : : : : : : : : : : : : : : : Arm Schematic : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : Manipulator Coordinates : : : : : : : : : : : : : : : : : : : : : : : : : : : : : : Filtered random walks of different length for 2 degrees of freedom : : : : : : : : xvi

177 180 189 192 193 196 202 203 205 206 209 211 212 213 215 216 217 220 221 222 223 224 241 242 247 248 249 252

F.1

Two worst-case paths of different lengths. Each paths is composed of 2N circular

:::::::::::::::::::::::::::::::::::::::: Phase space direct integration and detail. : : : : : : : : : : : : : : : : : : : : : : A typical session using the matrixPlotServer : : : : : : : : : : : : : : : :

arcs. F.2 G.1

xvii

254 256 259

Chapter 1

Introduction This dissertation describes the Integrated Dual-Arm Manufacturing Workcell project conducted at the Stanford Aerospace Robotics Laboratory (ARL) during 1991-1994. This project encompasses the experimental development of an intelligent, dual-arm robotic workcell that includes on-line planning as an integral part of the architecture. The system combines a high-level graphical user interface, a hierarchical control system, an on-line motion planner, a real-time vision system, and an on-line simulator to control and monitor the workcell.

1.1 Motivation Technical progress has an ever increased need for automation whereby human labor is promoted to a higher, creative level of non-repetitive tasks performed in a human-friendly environment. In this context, robots provide two important capabilities: (1) they can move and manipulate the environment (manufacturing, materials handling) and (2) they can gather information passively (inspection) or actively (sample collection). In this manner, robots can extend the frontier of tasks humans can comfortably achieve. The current range of tasks subject to robotic automation ranges from manufacturing (assembly, packaging, welding, spray-painting), operations in hazardous environments (waste-remediation, radioactive or toxic environments), and activities in human-unfriendly environments (underwater sample collection, space exploration, mining). The long-term goal is the development of systems capable of operating autonomously (or semi-autonomously) in unstructured environments. These systems would accept task descriptions from human supervisors and perform these tasks on their own, while handling all the specific details 1

CHAPTER 1. INTRODUCTION

2

themselves. These systems should adapt to the surrounding circumstances to accommodate changes in both external and internal conditions, partial failures, and other (unexpected) events. To achieve these ambitious goals, robotic systems must incorporate sophisticated control, sensing, and planning capabilities. On-line planning is required to avoid the need to pre-program in excruciating detail all the required tasks. Tediousness aside, important details may be simply unknown in advance and therefore cannot be preprogrammed. Sophisticated sensing is essential if the system is to exhibit some degree of autonomy in a non-perfectly-structured environment. Control is fundamental for the robust operation of any mechanical system interacting with its environment. Unfortunately, systems of such sophistication will be complex. Equally important for their success is the requirement for these systems to be developed economically and operated with ease. Given the difficulty of systems integration, its economical development requires a solid architecture and methodology, powerful interfaces, and flexible communication1. Ease of operation requires a high-level human interface for both task specification/command and system monitoring. In the context of manufacturing workcells, the natural evolution will take robotics beyond the current sensor-poor robotic systems operating in perfectly structured environments (painting, spot-welding, pick-and-place from feeders), where robots perform repetitive tasks, towards a new generation of sensor-rich robots capable of achieving a variety of tasks (contact assemblies, surface finishing etc.) in a flexible manner, i.e., without the need for complicated fixturing and scheduling. Within the manufacturing context, “reacting to the environment” will extend beyond avoidance of unexpected objects (e.g. humans or other robots) towards adaptation to changing manufacturing needs (e.g. be able to change the mixture/rate of production and even to reconfigure to build new parts). The economic benefits of an intelligent workcell would be significant. Currently all tasks in the workcell must be carefully preprogrammed. This requires a perfectly-structured environment where everything is known in advance. Structured environments depend on a variety of fixturing devices and must be carefully scheduled. All this adds significant costs for the additional equipment and time required to arrange the operation. For products with a short shelf life, time-to-market and timeto-volume are becoming more critical than steady-state production rate. Complete preprogramming of the workcell causes design and manufacturing to proceed serially for a given product. Once the design is complete, careful workcell pre-programming, fixture selection and fabrication setup are required before production can begin. Only at this point may some flaws become apparent, which 1

Many of these systems are distributed due to size/weight/power requirements, or the need to control/monitor them from remote (safe) locations. Hence the need to communicate among the different subsystems.


3

may require redesign. A workcell incorporating sophisticated planning capabilities can operate without much of the fixture and scheduling burden, so production can begin as soon as the design is complete, and perhaps more importantly, design improvements can be incorporated as they occur, without the need for reprogramming. To help advance the underlying theory and technologies necessary to develop such systems, the Stanford Aerospace Robotics Laboratory (ARL) has undertaken focused experimental study of the problems associated with developing intelligent manufacturing workcells.

1.2 Research Objectives The objective of this research is to study the architecture and interfaces of distributed robotic systems that incorporate planning as a fundamental, integral part, and to demonstrate these ideas experimentally in a system capable of semi-autonomous capture and (cooperative) placement of multiple (moving) objects under high-level user direction, even in the unforeseen presence of workspace objects. Moreover, the system must function without any a´ priori fixturing or scheduling. The experimental concept is depicted in Figure 1.1. Specifically, the goal of this project is to develop a complete experimental system–integrating a two-armed robot, a conveyor, vision sensing, planning and human interaction–that meets the following requirements:

High-Level human interface. Complete tasks are specified only in terms of the desired assemblies and their locations (not of the steps, sequences, paths, via points etc. required to achieve them).

Semi-autonomous operation in a dynamic environment. The system must be capable of autonomous tracking and acquisition of parts from a moving conveyor.

Two-handed manipulation. The system must be able to use both manipulators cooperatively whenever appropriate.

Moreover, the system must:

Operate in a distributed environment where the command, control, and planning stations are distributed among computers in a network; and it must do so reliably, allowing for alternative subsystems to replace/interact with the original ones at any time.


4

Figure 1.1: Research Objective A manufacturing workcell is directed by a user using very-high-level task commands to make an assembly. The parts required for the assembly will appear on a conveyor. The system automatically acquires the parts and performs the assembly. All intermediate steps: required paths, trajectories, tracking and acquisition of parts, manipulation, etc., are performed by the system autonomously under high-level human supervision.

Allow multiple concurrent operating modes in a transparent fashion. For instance, have multiple users interact/monitor the operation of the system.

Operate in a semi-structured environment with no fixtures nor a´ priori scheduling.

Ensure safety by combining on-line planning and real-time collision checking.

Incorporate and define “open” interfaces between the major subsystems in such a manner that the system can easily be expanded, and new capabilities can be added. In particular it must interact with on-line planners in an expandable manner, allowing for the incorporation of diverse planning methodologies.

1.3 Research Issues The above objectives require advancing the current research in several areas:


5

System architecture, design, interfaces and integration.

System integration is a difficult prob-

lem. Often the integration effort surpasses that of developing the intermediate subsystems. While the number of subsystems grows linearly with system size, the number of possible interfaces increases quadratically (every subsystem can potentially interface with each of the remaining ones). To alleviate this problem, it is important to carefully select both architecture and interfaces. While substantial research has addressed the architectures required for specific robotic applications, similar research in the robotic subsystem interfaces has been limited. Integration of planning into robotic systems. The need for planning in many robotic systems is well established. Planning gives these systems higher degrees of autonomy and increased robustness as they can react to unexpected events. On-board planning permits utilization of high-level task commands instead of high-bandwidth teleoperation signals. Autonomy allows systems to operate in the presence of large time delays (such as in space exploration), low communication bandwidth (such as undersea vehicles), and allows the human to be more productive as a supervisor (as opposed to operator or low-level programmer). However, integrating planning into a control system with hard real-time deadlines remains a great challenge. Proof of this has been the emergence of more “reflective” approaches that trade off the ability to pursue and reason towards long-term goals with the capability to react quickly to environmental changes. The balancing between these approaches, and the specific interfaces between planning and robotic subsystems, is an active area of research. Automatic trajectory generation.

Most industrial robots still perform repetitive tasks (pick-

and-place, inspection, welding etc.). In most of these cases the spatial motion of the robots is determined by the application, while the trajectory itself (the motion as a function of time) is painfully hand-tuned by system integrators so that minimum cycle times can be achieved without exceeding the capabilities of the manipulators (e.g. motor torque limits). Substantial research has been performed, and many algorithms have been proposed to automatically generate trajectories from the spatial description of the path that minimize fairly general performance indices (e.g. minimum time or energy consumption), subject to the limits of the controlled system. Only recently has attention been focused on “on-line” algorithms that trade off overall optimality for algorithmic speed. Still, for a real-time system, a critical property of any algorithm is the ability to predict “á priori ” its running time. This aspect had not yet been addressed.


6

Control hierarchies for multiple-arm robotic workcells. The control of multiple-arm robotic workcells is a complex software problem. Hierarchies are one methodology to tackle these complex systems. A hierarchical approach allows the problem to be simplified and redefined in a layered manner where each layer performs a well-defined function and transforms (simplifies or abstracts) the problem for the layers above. The benefits of this abstraction process have to be balanced with the constraints imposed by the hierarchy; that is, higher levels are now limited in their actions by the layers below. While many solutions have been proposed, designing a hierarchical control system that simplifies the control problem without hampering or unnecessarily restricting the flexibility remains an unresolved problem. Information sharing (communication) in distributed robotic systems. The need to share data and event information is universal to all distributed2 systems. Distribution may be dictated by physical requirements such as weight/size/power budgets that prevent all the sensing/computation from being on-board. Distribution may also be desirable whenever the system functions as an extension/replacement for humans in hostile environments, and requires remote human direction and monitoring. Perhaps as fundamentally, many of these systems are distributed for “logical” reasons: As the sophistication of the system and the need to incorporate more complex sensing, reasoning and interfaces grows, so does the tendency to design such systems as a network of subsystem modules where each subsystem performs a well-defined function. This has resulted in the proliferation of custom “communication” schemes for specific applications (e.g telerobotics) and in ongoing research in distributed blackboard and other information-sharing architectures. Distributed robotic systems in general and robotic systems in particular have specific needs that are not addressed by traditional approaches (such as distributed databases, transaction systems, etc.). Well-established semantics, such as client/server, shared memory and message passing, do not exploit or support the specific characteristics of repetitive sensor-type information. More recently, publish/subscribe (dissemination-oriented) communication models have been researched. Prior to the work presented in this thesis, there was no communication protocol that exploited the nature of the information flow in distributed control applications while enabling transparent communication and complex, dynamically-reconfigurable, data flows. 2

The word distributed is used to mean that the different subsystems are logically or physically separate, containing their own computational resources and communicating over some link.


7

1.4 Summary of Results

Figure 1.2: Experimental System The experimental setup is composed of two SCARA-type manipulators, a conveyor, a set of manipulable objects, a number of non-graspable obstacles, and a vision system (not shown in the picture).

1.4.1

System Capabilities and Demonstrations

The ability to direct a dual-arm workcell (Figure 1.2) with very-high-level assembly-type commands has been experimentally demonstrated. Using a graphical interface, the operator can directly specify desired locations for several objects in the workspace without concern for: (1) current object positions, (2) how objects arrive to the workcell, (3) which arm or how many arms should grasp the objects, and (4) what intermediate motions are required. The system is essentially driven by the availability of parts (as they arrive on a conveyor or otherwise appear in the workspace of the arms). The user can supervise the system’s operation by changing its directives or issuing new commands.


8

The ability to plan collision-free trajectories for either (or both) arms to capture objects from a moving conveyor and deliver them among workspace objects has been demonstrated. These plans are computed on-line using the sensed position of all objects and arms in the workspace. The power of anonymous communication and interfaces has been illustrated in several experiments where multiple human interfaces operate concurrently and are used to command the system. The rest of the system did not have to be programmed specially, and was not even aware of these changes. The interfaces also allowed the author to develop a simulator that could be used to masquerade as the real hardware for illustration and debugging purposes. Transparent subscription-based network communication has also been demonstrated. Aside from allowing each subsystem module to tailor the nature and frequency of the information updates to its specific needs, it has made it possible to move (and replicate) the different subsystem modules among different computer systems3 while the system is operating. Transparent network communication allowed us to take advantage of the best computer hardware available for each function. Specifically, the planning subsystem was moved to take advantage of higher-performance computers, while the graphical interface was replicated to allow operators in different rooms to interact and take advantage of specific graphics hardware. The advantages are increased flexibility and robustness.

3 With the exception of the subsystem that controls the actual hardware which is tied to the real-time cage containing drivers for the motors and sensors.


9

To the author’s knowledge, this research has accomplished the first:

Experimental demonstration of dual-arm capture and multi-step delivery of moving objects among obstacles in the workspace using on-line planning.

Fully distributed, subscription-based data-sharing system that supports multiple producers and consumers, and incorporates a realistic model of time.

Separable time-parameterization algorithm with linear run-time complexity and predictable execution time (for a given manipulator as a function of the path length).

Experimental demonstration of anonymous, stateless robot interfaces, allowing simulator masquerading and collaborative multiple-user interaction.

Demonstration of modular, parametric interface design applied to robotic systems.

Robotic-system design using the “interfaces-first” technique.

1.4.2

Contributions

This research makes the following contributions to the fields of robotics, distributed control and software systems: 1. A complete system architecture integrating on-line planning into a distributed, dual-arm robotic workcell has been developed. A careful analysis of the interfaces and world modelling issues has been performed, specifically concerning the interaction of planning with a hard real-time control system in the presence of unpredictable planning and communication delays. The final system incorporated a variety of subsystems:

Human Interface. A graphical user interface containing a 3-dimensional rendering of the workspace allows the human to issue high-level task commands.

Planner. A combined task-planner and path-planner4 subsystem decomposes task commands into a sequence of safe, primitive “robot commands.”

Robot Workcell Controller and World Model. A hierarchical controller for the dualarm workcell and the supporting world model. The world model integrates data from different sensors, taking advantage of domain-specific knowledge given the status of the workcell.

4

These planners were developed at the Stanford Computer Science Robotics Laboratory.


10

Robot Workcell Simulator. A simulator capable of masquerading as the workcell while providing graphical simulation and world model functionality.

Software Communication Bus. A Software package that allows transparent distributed network connectivity with unlimited fan-in and fan-out, thus providing the illusion of a software bus.

The success of this design should provide a useful model for the development of similar systems. 2. A philosophy for designing robotic systems beginning with the interfaces (interfaces-first design) has been proposed, developed, and demonstrated in a complete operational system. Furthermore, the concept of anonymous interfaces has been introduced and successfully demonstrated. 3. A modular approach to developing system interfaces has been proposed and demonstrated. This technique structures the complex command and data flow as combinations of several primitive interface modules. These primitive interface modules have parameters that allow customization for use within a specific, larger interface. 4. Three fundamental robotic-interface components: world-model, robot-command, and tasklevel-direction have been developed. These interfaces can be combined to generate custom interfaces between all the subsystems. A detailed study of the nature of the dataflow in robotic systems has led to the selection of these interfaces to encapsulate three fundamentally different classes of information:

World-State Interface. Encapsulates periodic, sensor-type information, where only the most current data is required. Different clients need this information at different rates to accommodate their specific needs.

Robot-Command Interface. Command-type information that must be delivered reliably and in order. This information is non-periodic and indicates an action that must be taken immediately or discarded.

Task-Interface. Goal-type information. Also requires reliable ordered delivery, and is generated asynchronously. However, it does not indicate an explicit action but rather a “success condition,” and it persists until the goal is achieved or explicitly cancelled.


11

These interfaces alone are capable of describing the system geometry and kinematics necessary for the planner and human-interface subsystems, providing a step toward the development of generic (“common”) interfaces for robotic systems. They should serve as useful examples for similar systems. 5. A new subscription-based, network-data-sharing system (NDDS5 ) has been developed. This system allows data to be transparently accessed anywhere on a computer network. It introduces several new concepts not present in existing distributed communication packages:

Multiple anonymous producers and consumers. An unlimited number of producers can dynamically override each other. Any consumer can access data produced anywhere.

Realistic model of time. Allows explicit specification of custom update rates, deadlines and what-to-do if a deadline is missed. Data is time tagged and decisions can be made based on the time when data was produced/consumed.

Fully symmetric, stateless implementation. The implementation contains no central servers or privileged nodes. All communication nodes are symmetric and contain no state (other than time-decaying caches). This implementation is very robust to partial failures.

Complex distributed robotic systems require very complex data flow. NDDS orchestrates this data flow and provides a model of the communication that matches well the requirements of distributed control systems. NDDS has been demonstrated in the experimental system, and is now being utilised by most current projects at the Stanford Aerospace Robotics Laboratory [102, 195, 206]. This system has also become a commercial product [145]. 6. A fast (linear-time, proximate-optimal) solution to the problem of time-parameterization of robot paths has been obtained. This algorithm has the fundamental property that for a given manipulator dynamics, its running time is predictable, depending only on the length of the path. This allows planners to anticipate the minimum time required for a motion and thereby adjust the intercept position when a rendezvous with a moving object is required. Moreover, the algorithm provides for modification of on-going trajectories. This algorithm has been packaged in a general tool that allows specification of arbitrary robot dynamics. It is now being used in a variety of experiments [112, 104]. 5

NDDS stands for “Network Data-Delivery Service”.


12

7. Strategic algorithms have been developed to allow tracking and capture of moving objects by one or two arms, as well as simultaneous capture of multiple objects (one per arm). These algorithms tie planned trajectories with local intercept trajectories and merge direct sensordriven feedback (tracking) in some degrees of freedom with task-driven trajectories on other degrees of freedom. In addition, a scalable approach to the strategic control of a multi-arm workcell has been developed. This approach uses multiple instances of otherwise identical finite state machines to control each arm in the workcell. 8. A graphical human interface with full 3-dimensional perspective has been developed allowing facile specification and construction of task commands. The GUI is updated with direct sensor data (from a world model) and displays the status of both the workcell and the planning subsystem. 9. The concept of task-level control [189] has been extended to tasks where goal-driven planning is required and where multiple concurrent goals can be present at any time. 10. A complete control hierarchy ranging from joint-level local torque control to high-level arm and object control has been demonstrated. Specifically, a set of mixed control modes for intercepting, tracking and picking moving objects smoothly from a conveyor has been constructed.

1.5 Related Research Activities This section lists some of the efforts closely related to this research as well as supporting activities by other researchers from the Stanford Aerospace Robotics Laboratory. This is not intended as a literature review. The appropriate chapters will include detailed discussions and reviews of the related research. Previous research at the Stanford Aerospace Robotics Laboratory [155, 189] has demonstrated the ability to track and capture objects with multiple manipulators using vision feedback. Schneider [157] also demonstrated simple cooperative-arm vision-guided assembly. None of the previous work at ARL incorporated on-line planning. In the field of on-line planning for robotic workcells, Hormann, Rembold and Lueth [1, 67, 95] describe KAMRO: a workcell containing a pair of 6-DOF Puma 260 manipulators mounted on the ceiling of a 3 DOF omni-directional platform. Their system shares similar goals with the


13

author’s and incorporates on-line planning to achieve semi-autonomous operation. One of the major differences with the author’s scenario is that there were no moving objects in the workspace. Fu and Hsu [51] have developed a similar scenario dealing with two robots and multiple moving objects on a conveyor belt. In this work, they assume that only the last links of the arms may collide with each other and that the objects are fed slowly enough that the robots can always pick up objects from locations that do not deviate significantly from their nominal positions. The author’s approach is substantially less restrictive. Other authors have presented results on either tracking [12] and/or capturing objects using vision. In some of these cases [8] vision was used only to estimate the object trajectory, while the capture was essentially done open-loop. In others [27] a single arm is used, and its motion is restricted to always capture the object in the same plane.

1.5.1

Supporting Research Activities

A project of this magnitude would have been impossible were it not for the availability and assistance of an array of research results and tools provided by the ARL team. Except where otherwise stated the individuals mentioned below developed their work at the Stanford Aerospace Robotics Laboratory. Experimental Platform Larry Pfeffer [131] designed and constructed the experimental platform that was used for the experiment. Although a few enhancements were made (longer links, redesign of the last two axes of the robots), the main features, supporting electronics and computer hardware required few adjustments. Planning for Robotic Systems Planning is a fundamental ingredient in the overall system operation. The requirement that the system incorporate planning, while maintaining on-line performance has had profound implications. Both the planning portion and the rest of the system have been designed with these goals in mind. Tsai-Yen Li [91] and Yoshihito Koga [82] of the Stanford Computer Science Robotics Laboratory developed the planning subsystem as part of their Ph.D. research in a joint research program with the Stanford Aerospace Robotics Laboratory.


14

Real-Time Software Framework A software project of this magnitude would have been intractable without appropriate software tools and a solid design framework. As part of his Ph.D. research, Stan Schneider [154] developed the ControlShell software framework [160, 159, 158] to help the design of complex, interactive real-time systems. This framework supports systems that combine event-driven with data-flow computational models. Both aspects are essential requirements of a control system that must interact with its environment. ControlShell can be credited for the timely completion of this research. Dual-Arm Cooperative Control Cooperative control of objects with multiple manipulators is a fecund field. As part of his Ph.D. thesis at Stanford, Stan Schneider [154] developed the “Object-Impedance Control” concept which was later extended by Larry Pfeffer. In his PhD. thesis, Larry Pfeffer [131] also developed a system identification and control methodology to address the control of robotic arms with exaggerated joint flexibility. These methodologies were followed by the author to develop the lower layers of the control hierarchy, and this greatly simplified the lower-level control problem. Real-Time Vision System A robotic system that manipulates objects in its environment needs sophisticated sensing capabilities to detect, identify, and track the different objects. The requirement of tracking and capturing moving objects from a conveyor imposes constraints on the minimum bandwidth of the sensors used to track the object. Although this capability exists with off-the-shelf products, they are usually expensive and require substantial effort to be integrated into a closed-loop control system. As part of his Ph.D. research, Vince Chen [29] developed the Point Grabber II, an inexpensive board capable of tracking individual LED’s in a scene. Stan Schneider developed the VisionServer software to identify and track objects tagged with unique LED patterns. This hardware and accompanying software tools allowed easy integration of visual sensing into the overall system. Graphical User Interfaces for Robotic Systems The ability of a human operator to visualize and interact with the operational system using natural 3-dimensional views was a crucial part of the success and appeal of the experiment. As part of their Ph.D. work, Kurt Zimmerman and Howard Wang developed a graphical tool (GraPhigs) to facilitate building such interfaces from textual descriptions of geometry and kinematics of the manipulators


15

and other objects in the scene. This tool made it very easy to visualize the geometric models and create Graphical Human Interfaces and simulators.

1.6 Reader’s Guide This thesis is divided into eight chapters and nine appendices. Chapter 1 has provided an overview and motivation for this research. Chapter 2 presents the experimental dual-arm robotic workcell hardware (including the computer environment), Chapter 3 introduces the software system design methodology, the interfaces, system architecture, and complete-system operation. The next three chapters provide details on some fundamental elements of the architecture and subsystems: The communication system NDDS (now a general tool for distributed control applications) in Chapter 4, the world-modelling subsystem in Chapter 5, the hierarchical control system in Chapter 6, and the on-line trajectory generator in Chapter 7. Chapter 8 presents a summary of this project, draws conclusions, and provides suggestions for future research. Appendix A details the calibration procedures used. Appendix B and C document the control gains used. Appendix D contains the dynamic parameters of the manipulators. Appendix E presents the “canonical” via-point paths used for computational complexity measurements in Chapter 7. Appendix F presents a constructive proof for the worst-case complexity of “classical” time-parameterization algorithms described in Chapter 7. Appendix G documents the distributed matrixPlot package developed in support of this research. Finally, Appendix H presents detailed documentation of several software packages: the Network Data-Delivery Service (NDDS described in Chapter 4), the proximate-optimal via-point trajectory generator (see Chapter 7), and supporting libraries.

Chapter 2

Experimental Dual-Arm Robotic Workcell This chapter describes the dual-arm robotic workcell: manipulator hardware (an enhanced version of the one first built by Pfeffer [131]), workspace objects, conveyor, vision system and computer environment.

2.1 Introduction The experimental workcell shown in Figure 2.1 contains two robotic manipulators. Each manipulator has 4 degrees-of-freedom (DOF) and is of the SCARA configuration. These arms are placed facing each other in an attempt to balance the total “reachable” workspace (of either arm) with the volume where cooperative manipulation can be performed. The arms move above a task table containing a set of objects and obstacles as well as a small conveyor. The height of the table is such that the grippers, at their lowest position, can barely contact the table surface.

2.2 Manipulator Mechanism and Sensors The original manipulator mechanism was designed and built by Pfeffer (see [131] for a detailed description) to study cooperative manipulation in the presence of exaggerated joint flexibility. The new experiments required the redesign of the last two degrees of freedom (Z and yaw axis) and the extension of the shoulder and elbow links to increase the workspace of the manipulators. For

16

CHAPTER 2. EXPERIMENTAL DUAL-ARM ROBOTIC WORKCELL

17

Figure 2.1: Experimental Dual-Arm Robotic Workcell The experimental setup is composed of two SCARA-type manipulators, a conveyor, and a set objects.

completeness, this section will include the relevant details of the manipulator design as well as the modifications to the original design.

2.2.1

Kinematics

The SCARA1 configuration contains the first two links in the horizontal plane connected serially by revolute (vertical axis) shoulder and elbow joints. The third is a prismatic joint along the vertical (Z-axis) while the last is again a revolute joint with vertical axis. This configuration is standard for many commercial robots (for instance Adept, Panasonic, Sony, and IBM all manufacture SCARA-type manipulators), and is shown schematically in Figure 2.3. The arm kinematics are described using the Denavit-Hartenberg parameters [41, 38] in Table 2.1. The first two degrees of freedom (shoulder and elbow) are actuated from motors located in the robot base using cable drives. Exaggerated joint flexibility was introduced in the drivetrain using torsional springs. This flexibility was incorporated for earlier research [132]. Controlling it represents an important dimension of control-system robustness. 1

Selective Compliance Assembly Robot Arm


Spline Motor and Encoder

Screw Motor and Encoder

Elbow-Joint Encoder

Spline-Screw

LED

Force/Torque Sensor Pneumatic Gripper

Figure 2.2: Z and Yaw degrees of freedom

Shoulder Axis

Elbow Axis

Elbow Motor

Elbow torque sensors

Elbow Encoder Infrared LED for overhead vision system

Elbow cable drive Frame Attached to Base Plate Elbow Link

Shoulder Motor

Shoulder Encoder

Shoulder Link

Shoulder torque sensors

Spline-Screw (yaw and z links) Force/Torque Sensor Pneumatic Gripper

Figure 2.3: Arm Schematic

18


τ2 1:G2 θ2

screw-nut τ3 1:G3

θ3

spline-nut

θ = θ3 /G3 z = p (θ2 /G2 - θ3 /G3) z

θ

Figure 2.4: Schematic of last Z and Yaw degrees of freedom and actuation

19


20

The last two degrees of freedom (Z and Yaw) use a new screw-spline component manufactured by THK [94]. This mechanism contains both spline and screw grooves in the same vertical shaft. Two ball-bearing nuts each use one of the grooves to achieve both translational and rotational motion of the shaft. Both nuts are mounted on the elbow link using a custom-machined piece, and are driven using belts from two motors also located in the elbow link. This is illustrated in Figures 2.4 and 2.2. The actuators and gear ratios were selected so that the arm had similar performance in all degrees of freedom. The original design, based on our estimation of the speed of the planning subsystem, called for a 1 second move involving a 1 meter x-y displacement, 2 rotation angle and full 0.3

meter vertical displacement. Located at the arm base, the shoulder and elbow motors had no limits on their size/weight; they were over-designed in the initial experiment, and had no trouble meeting the aforementioned requirements. In the last two degrees of freedom, however, the actuators are mounted in the elbow link and therefore it was desirable to make them as light (and small) as possible to minimize the inertia of the arm. The limitations on available screws and gear-ratios, and the desire to be able to execute full up and down motions of 30 cm in times of the order of 1 second2 called for at least 50 oz-inch peak torque. These considerations motivated the use of high power-to-weight ratio rare-earth-magnet DC motors. Table 2.2 summarizes the characteristics of all the actuators and gear-ratios.

2.2.2

Joint-Torque Sensors

The shoulder and elbow joints contain joint-torque sensors built into the structure, so that the torque applied to each link can be directly measured. The use of joint-torque sensors allows closing fast torque loops on these degrees of freedom, so that joint dynamics and non-ideal characteristics of the actuators can be hidden from the upper control layers. The design of this sensor, its performance, and its frequency response are described in detail chapter 5 and in Appendix C of Pfeffer’s thesis [131].

2.2.3

Joint Encoders

Each manipulator contains six optical encoders, four mounted on the motor shafts (dual-shaft motors where used for this) and two at the shoulder and elbow links (outboard of the flexibility). The shoulder- and elbow-motor encoders were designed to allow cogging compensation and to have 2

This design requirement stems from the desire to balance the performance of all degrees of freedom in a typical pick-and-place operation. The first 2 degrees of freedom could move the arm across the workspace with good tracking in about 2 seconds.


i 1 2 3 4

Denavit-Hartenberg parameters i,1 ai , 1 di i 0 0 0 qsh 0 0.6096 m 0.12 m qel 0 0.6096 m z 0 0 0 0 qyaw

arm name right arm left arm

21

Limits min. -1.2 rad -2.5 rad 0.02 m -6.0 rad

max. 1.2 rad 2.5 rad 0.30 m 6.0 rad

Location of Arms in Workcell x y z -0.0831 -.9445 0.45 ,=2 rad -0.1057 0.76 0.45 +=2 rad

LED position 0.4750 m 0.4856 m

Table 2.1: Arm kinematic parameters Nominal Denavit-Hartenberg parameters for the manipulator arms are identical for the two arms. The notation used follows that of Craig [38]. The arm locations denote the position of the base frame (origin for Denavit-Hartenberg parameters) w.r.t. the global frame. The LED offset is its distance to the elbow axis. Both the location of the arms and LED offset were computed from vision information. The procedure collected encoder-angle and vision data at a grid of locations covering the complete workspace. A minimization was then performed to identify the location of the shoulder axis, encoder offsets, and LED location that would bring kinematics and vision to their closest correspondence over the whole workspace (see appendix A for details). This set f data is the one shown in the Table.

Actuator

Brand

Shoulder Motor Elbow Motor Z- Axis Motor Wrist Yaw Motor Conveyor Motor

Contraves ACR103 Contraves ACR103 ESCAP 35 NT2R 82 ESCAP 35 NT2R 82 CP JDH-2250-BX-1C

Peak/Continuous Torque 40.0/8.0 N-m 40.0/8.0 N-m 0.72/0.11 N-m 0.72/0.11 N-m 2.45/0.29 N-m

Torque constant 40.0 N-m/A 40.0 N-m/A 0.05 N-m/A 0.05 N-m/A 0.10 N-m/A

Gearing 9:1 3:1 0.06 m/rev 2.75:1 0.0118 m/rev

Table 2.2: Actuator characteristics The Contraves are AC brushless motors, the ESCAP are DC brush-commutated rare-earth permanentmagnet motors. CP stands for “Clifton Precision”. All motors are driven by transconmutance amplifiers (voltage inputs command current forced through the motor windings), and hence act as torque sources.

fairly low resolution. The highest-resolution encoders are located at the shoulder and elbow joints. Together with the encoders on the last 2 DOF they allow determination of the tool position with respect to the robot base using rigid-link kinematics (the drivetrain flexibility of the last 2 DOF can be neglected). Table 2.3 summarizes the characteristics of the different encoders. The encoder


22

signals were accessed using quadrature-decoding interface boards developed by Uhlik [188]. These boards provide both incremental position and absolute velocity measurements. The high-resolution shoulder and elbow encoders provide a velocity signal with low noise. Encoder resolution in the last two DOF was limited by speed limitations in the interface circuitry, and proved sufficient for all tasks under consideration. Location Shoulder Joint Elbow Joint Z axis Motor Wrist Yaw Motor Shoulder Motor Elbow Motor

Brand Cannon M-1 Cannon R-1 L HP HEDS 5500 HP HEDS 5500 BEI E203 BEI E203

Lines on disk 50000 81000 96 512 2048 2048

Equivalent Resolution 0.03146 mrad 0.01939 mrad 0.01736 mm 1.116 mrad 0.7670 mrad 0.7670 mrad

Table 2.3: Encoder types and resolution The number of lines on disk indicates the number of pulses in a complete revolution. Since they produce quadrature counts we get four times the number of counts per revolution, with the corresponding increase in angular resolution. The equivalent resolution takes into account the gear-ratio of the Z and wrist motors.

Pneumatic grippers were selected for their simplicity. These grippers were built by Zebra Robotics and are actuated by a pneumatic cylinder of 0.25 in2 (1.61 cm2 ) section. Shop air is used to pressurize up to 40 psi to achieve a grasping force of 10 lbs (44.5 N).

2.3 Workspace Objects The workcell manipulators manipulate and interact with a number of objects, some of which can be grasped and manipulated while others are “pure obstacles” in the sense that they obstruct the motion of the other objects but cannot be manipulated by the robots. Each object is marked with a unique LED pattern (three LEDs, with unique distances). This unique pattern allows the vision system to identify and track the objects. As we will discuss in chapter 5, there is nothing special about these objects other than they have been tagged with LED’s and described to the world modeler. These objects are shown in Figure 2.5, their characteristics are summarized in Table 2.4.


23

WhiteBox Conveyor

PinkBox Cove

Corner Block ClearBox

DarkBox

Figure 2.5: The different objects the robot interacts with The workcell interact with 7 objects: Block, Cove, Corner, DarkBox, ClearBox, WhiteBox, and PinkBox. Of these only Block, Cove, and Corner can be grasped by the robot. Due to its size and shape, both arms must be used to manipulate Cove.

2.4 Conveyor A small conveyor shown in Figure 2.5 is used to deliver objects to the workspace. The conveyor can be placed anywhere in the workspace, and is also tagged with a unique 3-LED pattern so that the system can know its location. The conveyor is actuated with a Clifton precision motor (see Table 2.2). The motor has an encoder which is used to close a velocity loop. The gear-ratio was chosen to allow a maximum conveyor speed of 0.5 m/s. Due to friction, the minimum conveyor speed that can be regulated is about 0.01 m/s.


name block

shape prism: base 7.4x5.8 cm, height 17.5cm

corner

L-shaped: main body 166.4 cm, side 9.63.2 cm, height 15.5cm

cove

U-shaped: main stretch 333.2 cm, sides 163.2 cm, height 15.5cm

bar

cylinder 43.2cm long, 2.7cm diameter

PinkBox WhiteBox ClearBox DarkBox

prism: prism: prism: prism:

base 18.611.2 cm, height 10cm base 31.611.2 cm, height 10cm base 38.215.1 cm, height 10.2 base 38.215.1 cm, height 10.2

purpose Light object graspable with a single arm heavier convex object graspable with a single arm Large convex object. Grasp requires 2 arms Long thin object. Grasp requires 2 arms Pure obstacle Pure obstacle Pure obstacle Pure obstacle

24

mass 0.53 Kg 0.69 Kg

1.05 Kg

0.45 Kg N/A N/A N/A N/A

Table 2.4: Characteristics of the workspace objects

2.5 Vision System An overhead vision system allows tracking of the objects and robot manipulators. This system consists of an overhead camera (Pulnix 440S CCD), a VME-based vision board and a generalpurpose VME-based processor dedicated to processing the vision data. The camera operates in non-interlaced mode, generating 60 frames per second. It is equipped with an internal electronic shutter which limits the frame exposure to 1/1000th of a second, eliminating blurring due to the object motion. The camera is mounted 2.83 m above the task table, and uses a Cosmo C-6 6mm lens to cover a field of 3m x 2.25m at the table level. An infrared filter is mounted on the lens so that the camera only “sees” the LEDs. The VME-based vision board Point Grabber II, was designed and built by Chen [29]. It provides a list of pixel coordinates and intensity values corresponding to the points in the image field above a certain (programmable) threshold. It is controlled through the VME backplane from a dedicated processor board (Motorola MVME-162, 68040-based singleboard computer). This dedicated vision processor runs the VisionServer software developed by Schneider [154]. The VisionServer software tracks objects in the horizontal plane using their


25

known unique LED patterns and height, and sends the updated position to any clients at the frame rate3 . The software can also track individually-described points once they are initially located.

2.6 Computer System The computer environment combines dedicated single-board computers running the VxWorks realtime operating system with several general-purpose unix workstations. This gives the best of both worlds: an expandable, low overhead computer system for the real-time tasks, combined with the flexibility and tool-richness of Unix-based workstations. Figure 2.6 shows the system schematically. The dedicated real-time computer system consists of VME-bus single board computers, the vision board and a number of I/O boards. The unix environment used anywhere from 1 to 3 Unix workstations (Sun Sparc-Station IPX, Sun SparcStation 10-51 and DEC Alpha). All development was performed on unix workstations, including cross-compilations for the real-time targets. All computers are connected in an Ethernet local area network which is also used by the real-time processors to load the executable code and access the file system (using NFS). This architecture was originally conceived by Schneider and Ullman [154, 189] and has been used with success in a number of other experiments at the Stanford Aerospace Robotics Laboratory. A detailed list of all the boards connected to the VME backplane is presented in Table 2.5.

3 This software uses TCP/IP network communications provided with the VxWorks operating system to send its updates to processors in the same backplane or anywhere on the network


Slot 1 2 3 4 5 6-8 9 13 14 15 16

Component Single Board Computer Single Board Computer Single Board Computer Single Board Computer Interface Module Interface Modules Vision Board Analog Input Analog Output Analog Output Digital I/O

Model MVME 167-1 MVME 167-1 MVME 167-1 MVME 167 MVME 712 MVME 712 ARL-PG II XVME-566 XVME-505 VMI-4116 MVME-340

26

Description 33 MHz 68040 CPU, 4 MB RAM 33 MHz 68040 CPU, 4 MB RAM 33 MHz 68040 CPU, 4 MB RAM 25 MHz 68040 CPU, 4 MB RAM Serial, Ethernet I/0 for board 1 Serial I/0 for boards 2-4 Point Grabber with FIFO’s 16 Channel differential 12 bit A/D 4 Channel, 12 bit D/A 8 Channel, 16 bit D/A programmable multiple-channel digital I/O

Table 2.5: Real-Time Computer Components The real-time computer components are connected on a VMEbus. Four single-board computers are used for real-time control and sensor processing. The remainder of the bus is occupied by interface and I/O boards.


27

INTERNET DEC Alpha 3100

DEC Alpha 3100

LAN SparcStation IPX

SparcStation 10

3-D graphics SparcStation 2

68040 Single Board Computer

VME Bus 68040 Single Board Computer

Vision Board

A/D

D/A

Digital I/O

Camera

Figure 2.6: Hardware Architecture The computer system combines a number of Unix workstations and a VME-based real-time computer system distributed over a network. The real-time computer combines four single-board computers, a vision board and both analog and digital I/O boards.

Chapter 3

Architecture, Interfaces and System Operation “It is common sense to take a method and try it. If it fails, admit it frankly and try another. But above all, try something.” Franklin D. Roosevelt – Speech at Oglethorpe University, May 22, 1932 “The greatest leverage in system architecting is at the interfaces.” Eberhardt Rechtin – Systems Architecting [146] “The greatest dangers are also at the interfaces.” Arthur Raymond – Speech at USC, 1989 [146] To be manageable, large systems need to be built around a solid architecture or framework. The advantages of modular system design are well established: simplicity (a large system is broken into smaller, simpler components which can be understood and developed in relative isolation), maintainability (failures can be isolated into the originating module(s) where they can be more easily addressed), reliability (redundancy can be introduced with replicated/redundant modules). However, modularity does not come for free. The design of the module interfaces and the interconnection of the different modules (i.e. the systems integration) becomes a large part of the overall development effort. This chapter presents the system architecture and interfaces of the Robotic Workcell and the new concepts and methodologies introduced in this research to address the integration problem: 1. The “interfaces-first” system-design technique—similar to the approach taken in large software projects. It differs from the more classical “subsystems-first” approach in that the 28

CHAPTER 3. ARCHITECTURE, INTERFACES AND SYSTEM OPERATION

29

interfaces are developed for the information flow in the system (as opposed to developing interfaces for the subsystems themselves). The subsystems then become users and providers of that information to collaborate towards the system goal. 2. Introduction of modularity at the interface level. The interfaces themselves are designed to be composed of multiple primitive modules which can be connected in a variety of ways to create new “custom” interfaces for each of the subsystem modules. 3. Anonymous interfaces where information is exchanged without the subsystem knowledge of the identity of the source/destination or even the number of subsystems involved in the exchange, creating in effect the illusion of a common information backplane. 4. Development of specific interface semantics appropriate for integrating remote on-line planning subsystems (with unpredictable planning and communication delays). In particular the use of strategic-level commands to interface the planner and control subsystems. 5. Stateless interfaces and subsystems that are robust to temporary disconnection, live-insertion or re-initialization of any subsystem and many-to-many interaction modes.

3.1 Literature Review Many authors have looked at the architecture of robotic systems and tried to formalize “general approaches to the design of such systems”. A detailed description of all the proposed architectures would take far too much space. For this reason, we limit the discussion to architectures that have been implemented and verified in experimental robotic systems. The National Institute of Standards and Technology (NIST) has defined a reference model for Robotic System Architectures. RCS [139, 6, 7], and its telerobotics counterpart, NASREM [4, 5], build systems by interconnecting generic controller modules. Each module incorporates three functions: Sensory Processing (SP), Behavior Generation (BG) and World Modeling (WM), in effect creating three interconnected hierarchies. All modules communicate through global sharedmemory. Modules execute periodically at fixed rates. The lower levels of the hierarchy take care of servo-level control and sensing issues, while the highest levels perform mission planning, highlevel user interfaces, etc. RCS’s approach is, by design, a strict hierarchy in the sense that each controller-module is restricted by the design to command a set of subordinates and receive status reports from them. The hierarchical design of RCS results on a tree-like topology which, according


30

RCS’s designers, allows the assignment of responsibility and more predictive behavior than an agent-like architecture. RCS also assumes that global-shared memory semantics can be achieved in a distributed environment, and prohibits the use of interrupt (event) driven programming techniques in favor of periodic execution and polling (which the authors claim is more predictable). Butler and Jones [134, 26] have developed an architecture called MICA (Modular Integrated Control Architecture). Their approach differs from RCS in that it provides support for both asynchronous (event-driven) interactions as well as synchronous (periodic) execution. MICA modules are interconnected with two mechanisms: HELIX [74, 73] (a distributed blackboard for synchronous-data exchange) and a dedicated event-network. Events are fixed-format messages, and are addressed to specific modules. Whenever an event cannot be handled locally it is broadcasted to all other modules. MICA is not a strict hierarchy like RCS, as any interconnection topology is allowed. MICA has been used to control the mobile robot HERMIES-III [134]. Hierarchical approaches use each layer to simplify and abstract the problem for the layer above, allowing the higher layers to be progressively more isolated from the actual hardware. The hierarchical approach structures and simplifies the design of the system, and allows the higher layers to be portable to different hardware. Hierarchical approaches are routinely used in control systems in the form of successive loop closures [50]. Hierarchical approaches have also been used to incorporate planning and control in a “command hierarchy” ranging from high-level (mission-level) tasks, down to servo commands. For instance Chatila, Herrb, Fleury and Noreils [119, 49, 118, 120] propose a three-level hierarchy composed of planner, control and functional layer. The planner level uses petri-nets to coordinate among multiple robots and to break tasks into smaller operations for a single robot. The control layer (a misnomer in my opinion) breaks down the commands into primitives, schedules them with the functional layer, and supervises their progress. Lastly the functional layer is responsible for sensor processing and feedback control. This architecture has been used to control the HILARE mobile robots. Hierarchies are not the only model for interconnecting subsystems. For instance Zanichelli, Caselli, Natali and Omicini [204] propose an Agent-Based architecture where all agents communicate though a central white-board-like “world-model”. The robot itself is just another agent. This is proposed in contrast with “layered” approaches or behavior-based architectures. They seem to advocate the “world” to keep models on behalf of the agents, but also allow the agents to do their own modelling. Communications are performed in a language similar to distributed Prolog containing primitives similar to those of LINDA[3]. Agent-based architectures are normally large-grained1 , 1

That is, each agent is quite complex by itself.


31

each subsystem (agent) can itself be developed as a hierarchy and incorporate its own models. A fundamental difference with the hierarchical approach is that the agent approach treats other agents as peers, and any interconnection topology is allowed. Autonomous mobile robots can be very complex systems as they integrate planning, sensing, and locomotion, and often operate in unstructured environments. Their complexity has motivated the development of many control architectures. Hinkel, Knieriemen, and Puttkamer have introduced an architecture composed of four subsystems (locomotion, sensor system, man-machine interface, and control system) to control the mobile robot MOBOT-III [63]. These subsystems are arranged as two parallel hierarchies: the command hierarchy (man-machine interface, control and locomotion subsystems), and the sensor processing hierarchy (sensors, combiner, map-building). The sensor information is processed synchronously, while the commands are issued asynchronously (presumably any synchronous feedback loop is handled locally by the lowest locomotion layer). Although the architecture is quite specific to their application, it reinforces the idea that command hierarchies are more naturally described as asynchronous or event-driven. Arkin proposes an autonomous robotic architecture (AuRA) [13], composed of planning, cartographic subsystem (world-modeling), motor subsystem (control), and homeostatic control (internal communications/broadcasts and change in control modes). AuRA introduces the idea of schemas: artificial potential fields instantiated by the planner to guide the behavior of the control subsystem (e.g. move-to-goal, avoid-static-obstacle, stay-on-path) without committing the controller to specific motions. Multiple schemas can be concurrently active, and their control commands are summed. This architecture bridges planning and reaction by providing mechanisms that allow the planner to “program” the reactive behavior of the system. Other authors have concentrated their work in the higher layers of the command hierarchy, and have studied ways of plan generation, task sequencing and decomposition. Bonasso and Slack [21] propose a three-layer hierarchy for planning: deliberation, sequencing/scheduling and skills. The Task Control Architecture developed by Simmons [171, 169] uses task-trees and exception mechanisms to sequence and monitor the operation of a robotic system, and has been used to control several autonomous robots [170, 85]. Nakamura and Xu’s Intelligent Multi-sensorbased Robot Controller [115] approach is based in the concept of skills (operations that the robot is capable of performing). Each skill contains a description of how to achieve in terms of primitive actions and other skills. This relationship between skills constitutes an and-or graph that is used in the planning process. Production rules are used by other architectures, including Soar developed by Rosenbloom, Laird, Newell and McCarl [148].


32

Modular decomposition and interconnection topology are only two aspects of the architecture of a system; a related, critical aspect is the interfaces between the modules. That is, what are the syntax (format) and semantics (meaning) of the information exchange. The interface issue is not unrelated to module development, because the interfaces can characterize the modules as far as the remaining subsystems are concerned. Although in principle the interface is abstract and does not imply any specific implementation, in practice, interface definition severely constrains the possible module implementation and vice versa. Object-Oriented Programming (OOP) provides facilities well suited for interfaces-first design and the construction of modular interfaces. Abstract primitive interfaces can be encapsulated in individual classes, and methods and multiple inheritance can be used to build a dedicated interface from the primitive elements. However, uses of OOP in robotic applications have used the objects to model individual subsystems not the information between them, yielding a subsystems-first approach. For instance, RIPE [109]2 defines a hierarchy of objects to describe the different physical elements in the system. Their architecture has four layers (task-level programming, supervisory control, real-time control and device driver). Within the object hierarchy, the object class definitions become interface specifications for the subsystems, but no abstract interfaces are provided for accessing sensor or world-modeling information, nor for task specification. The interface between planning (task or path planning) and the control system is particularly critical because of the fundamental impedance mismatch between the two subsystems. Planning is asynchronous, event-driven, and can potentially take an unbounded amount of time. Plans tend to be geometrical in nature. On the other hand, a control system is a periodic sampled-data system, requiring a continuously available reference state (reference-state trajectory). External events (such as an unexpected obstacle, or the motion of an object on a conveyor) require real-time responses (i.e. with definite, bounded response times) which cannot be guaranteed by deliberative planners. As a result, some authors advocate abandoning planning in favor of “reactive” approaches, such as Khatib’s potential fields [80], which have no deliberative phase, while others propose a plannercontroller interface that incorporates reactive information. For instance, Payton, Bihari, Rosenblatt and Keirsey [127, 128] propose an interface where the output of the planner is a potential field that can be used to guide the robot. Quinlan and Khatib [137, 138] have proposed solving the mismatch by introducing an on-line algorithm that can continuously deform the path originally created by the planner as if it were a rubber band subject to repulsive forces by obstacles in the environment. All these methods attempt to achieve the correct balance between local reactive behavior (fast but 2

RIPE stands for Robot-Independent Programming Environment, and was developed at Sandia National Laboratories.


33

sometimes misleading or without purpose) and deliberative goal-oriented planning (slower and computationally unpredictable but with a global goal-oriented perspective). The KAMRO workcell [1, 67, 95] combines an on-line planner (FATE) and a real-time control subsystem. The input to the FATE planner is a petri-net describing the assembly task. FATE interprets the net and sends elementary commands such as (TRANSFER, FINEMOTION, GRASP) to the real-time controller. FATE monitors progress and has facilities for error analysis and recovery. FATE plays a role analogous to the planner-subsystem in our architecture. A fundamental difference of our system is that the commands generated by the planner are strategic commands not elementary operations. These commands are further decomposed into elementary operations by a finite-state machine facility within our control subsystem. In this manner, our system performs monitoring and error-recovery both within the control and planner subsystems. This extra layer allows our system to deal with moving objects and obstacles which KAMRO cannot handle. NIST and NASA’s JPL have embarked in an ambitious effort to define interfaces for all telerobotic applications. The Unified Telerobotics Architecture Project (UTAP) [99] defines an architecture composed of modules of 20 different types (operator-input-device, object-modeling, taskdescription-simulation, task-program-sequencer, sensor-control, robot/axis-servo-control, etc.), and specifies the interfaces to each one of the modules. Modules accept similar messages to set or get parameters and to control their operation (set, get, start, stop, post etc.), but the parameters differ among modules. Modules are connected on a strict hierarchy, and messages can only be exchanged between a parent and its children with the exception of the Object-Knowledge module (similar to a world modeler) which can exchange messages with all modules. The interface has provisions for requesting modules to post periodic updates at a given rate (but only a single rate/destination can be requested). Their philosophy differs from the one presented here in that interface has state (modules assume that previous messages have been received and processed), message exchange is not anonymous (sender must specify receiver), and messages are strictly hierarchical. Moreover, in UTAP there is a fixed, pre-established number of modules of each module-type (a single instance for most types). In several respects however, UTAP’s interfaces are more powerful than ours since UTAP has language constructs which allow the creation of macros and message groups. Previous work at the Stanford Aerospace Robotics Laboratory has addressed some of these interface issues. Schneider [154, 157] used strategic-level commands to interface a dual-arm robotic system with a graphical interface, but did not address the interfaces to planning subsystems. Ullman also used strategic-level commands to direct a dual-arm free-floating vehicle from a graphical


34

interface [189], and made the command-interface stateless, but his system was mostly self-contained and did not incorporate planning. Several experimental multi-arm robotic workcells have been described in the literature. Fu [51] has developed a scenario similar to ours which deals with two robots and multiple moving objects on a conveyor belt. In this work, the assumption is made that only the last links of the arms may collide with each other, and that the parts are fed slowly enough that the robots can always pick up the objects from locations that do not deviate too much from their nominal positions. Our system does not have these restrictions.

3.2 Approaches to System Design Modular system design is a well-established methodology. This approach breaks a large system into smaller, well-defined modules with specified functionality, which then can be developed and tested independently from the others. While modularity facilitates the development of the individual components, the need to develop the interfaces (“glue”) between components makes the complexity of the overall system much greater than the sum of its parts. By careful selection of the functional boundaries, the resulting subsystems can be mostly independent, and the total system complexity can approach the ideal limit of the sum of its parts. Subsystems-First.

Traditional modular system design (subsystems first) follows the cycle illus-

trated on the left side of Figure 3.1. Following the analysis of the complete system, functional units are identified, and the system is broken into subsystems across these functional boundaries. These subsystems are further defined by their interfaces to the outside world, which specify their observable behavior. Once the subsystems have been developed and individually tested, custom interfaces (glue logic) are developed to interconnect the various subsystems. This cycle results in a complete system that can now be tested against the design specifications. This process repeats itself on each design iteration. Once the subsystems are designed, the overall system is integrated by developing appropriate interconnections between the subsystems (essentially building glue-logic that matches the different interfaces). This subsystems-first technique requires the scope and functionality of the overall system to be known in advance. It emphasizes subsystems rather than their interfaces. It is therefore most appropriate for “sparsely-interconnected” one-of-a-kind systems that, once designed, are unlikely


35

Figure 3.1: Contrasted Design Techniques Subsystems-first design, on the left, concentrates on developing functional units. Information flow between the units requires custom interfaces. Interfaces-first design on the right starts with the information flow. Functional units are then tailored to the interfaces developed. Interfaces-first design is more appropriate when subsystems are highly interconnected. In highly-interconnected systems, the number of interfaces increases quadratically with the number of subsystems.

to be expanded with the addition of new subsystems. For example, a monolithic electronic board (say a graphics board) is designed as a unit from the onset, and the different functional units (CPU, video memory, graphic accelerator hardware, etc.) are designed with their own interfaces, and require custom glue-logic to be connected to each other within the board. While the subsystems-first technique works well for systems in which subsystems have a low degree of interconnectivity, there are other systems whose full scope is not known in advance and/or that require a much higher degree of interconnectivity. This second class of systems is hard to design with the subsystems-first approach. Interfaces-First.

Interfaces-first design [125] is a methodology more appropriate for open-ended

systems with a high degree of subsystem interconnectivity. In interfaces-first design, the types


36

of information flow between the subsystems (whatever they turn out to be) are identified from the outset, and interfaces for the information (not the subsystems) are designed first (informationinterfaces). Later, subsystems are designed that use these interfaces to communicate and interact. This design is more typical of large software projects or computer systems. For example the busses in different computer families (e.g. PC bus, VME bus etc.) are designed first, and the different boards that interact through these busses are designed later to conform to the interface specification. In fact, new boards are designed at a later stage with functionality that was not initially anticipated. The interfaces-first design cycle, shown down the right side of Figure 3.1, is most appropriate for “heavily interconnected” systems and those that are “open-ended” in the sense that new pieces will have to be incorporated at a later stage. The interfaces-first approach may seem less intuitive at first. After all, does not the information exchanged depend on what the subsystems are? The extent of this dependency varies depending on the characteristics of the system under development. If the overall function and requirements of the system are well known in advance (and unlikely to change much), and if the system is loosely connected (i.e. each subsystem will be connected to one or two other subsystems), the subsystemsfirst approach is probably more intuitive and simpler. However, many systems do not belong to this category; we may not be able to know in advance the full scope, and we may want to leave the door open for enlargement by future (not necessarily well-characterized) subsystems. For instance, in the design of a computer window system, the designers do not know (or expect to know) all the applications that will be run on top. However, one thing they do know is that these applications will need to exchange data by cut-and-paste-type operations. The window-system designers define interfaces to this information (e.g. text, graphics, voice, video) and specify its format. Current applications (word processors, databases, spreadsheets etc) and future ones use these interfaces to exchange information. The “interfaces-first” methodology is better suited for strongly inter-connected systems and “open” systems that may expand in the future. Examples of this approach may be found in computer-bus design (SCSI, IDE, VME), desktop window system design (Macintosh, Windows), intra-tool protocols (ToolTalk), etc. Although presented as “alternatives,” the two approaches are the extremes of a continuum of design methods where the adequate mix depends on the characteristics of the system to be designed. Traditionally, robotic systems have been designed with the subsystems-first technique, which has made extension and development of the system difficult due to their limited interconnectivity and the considerable amounts of custom interfacing required. To the author’s knowledge this is the


37

first time the “interfaces-first” approach is recognized as an alternative design approach and applied to a robotic system.

3.3 Modular Interfaces

Primitive Information Interfaces

Custom Subsystem Interfaces

Figure 3.2: Modular Interface Design First, the fundamental categories of information flow are identified, and information interfaces are designed for each (on left). Subsystem interfaces are then built from combinations of these primitive information information interfaces.

Design of the information interfaces is not a trivial task for robotic systems, as the information/command flow is much less structured (i.e. higher level) and varied than that modeled with a bus-type interface. For example, all the devices connected to a SCSI bus are expected to be able to operate at (almost) the same speed and accept the same type of commands. This is a poor model when the “devices” include things like graphical interfaces, teleoperator-input devices, planning subsystems, and sensor subsystems. Each of these subsystems requires fundamentally different types of information, and is able to process it at significantly different speeds. The author proposes to address this problem with a “modular” approach to the design of the information interfaces themselves. This method first identifies the fundamental properties of the information flow. The information is then divided into categories based on the characteristics of the information-flow itself (bandwidth, persistence, idempotency). Next, primitive interfaces are


38

designed for each type of information flow. These primitive information interfaces can then be combined to create custom “compound” subsystem interfaces. Figure 3.2 illustrates this process.

3.4 Information Interfaces for the Manufacturing Workcell In the context of a robotics workcell, we must examine world-state information flow, command flow, control signals, etc., identify parameters that characterize the information, and build primitive interfaces around each information category. This section examines the system-architecture design for the smart workcell project using the interfaces-first technique. interface world-state interface systemcommand interface taskspecification interface

nature of the information periodic, idempotent, unreliable, last-is-best, persistent until the next update asynchronous, reliable, in-order delivery (exactly once), persistent until completed

selectable parameters update rate, specific information desired priority (to resolve conflicting requests)

asynchronous, persistent until cancelled, reliable but not necessarily in order

priority

Table 3.1: Characteristics of the information flow Three types of information flow can be distinguished in this application: (1) world state, (2) system commands, (3) and high-level task descriptions. Table 3.1 summarizes the three primitive interfaces and selectable parameters for these interfaces.

3.4.1

The World-State Interface

World-state information includes data such as object positions and velocities (on and off the conveyor), arm locations, joint angles, and force signals. By its very nature this information is either static or periodic, and requires continuous updating because it corresponds to physical quantities that change over time. In addition, the information is persistent until it is invalidated by a new update of the same information. World-state information also has idempotent semantics—getting two identical updates of the same information (say the position of an object) causes no side-effect, and is logically equivalent to a single update. Moreover, the natural semantics are last-is-best, i.e., we are always interested in the most recent value, even at the expense of missing intermediate values. The information


39

content and frequency of each specific instance of the interface must be customizable, as different subsystems require different subsets of the overall world-state information at widely different rates. For instance, a slow graphical interface animating the robot position may need new joint angles at only 10 Hz; a control or estimator subsystem will need updates at a much higher rate.

location grasps properties shape

Object-state information object location in the global reference frame. possible grasp locations within the object mass, inertia, limits on acceleration etc. shape (collision avoidance, graphics)

location joint val. limits kinematics status trajectory

Robot-state information robot-base location within workcell. values of the joint coordinates (pos, vel, acc) kinematic (joint) and torque limits Denavit-Hartenberg parameters moving, grasping an object etc. via-point and timing of the current arm trajectory Table 3.2: The world state interface

The specific information encapsulated within the world-state interface is summarized in Table 3.2.

3.4.2

The System-Command Interface

System-command information is quite different from world-state information. First, commands are asynchronous and “hold their value” only until their completion (even if no new command is received). Commands are not idempotent, and must be delivered reliably exactly once and in the right order. For instance, if we are sending a trajectory to the robot, sending the trajectory twice will cause the robot to execute two motions. Similarly, missing an intermediate command (such as closing a gripper before lifting an object), is not acceptable. In this thesis, no attempt has been made to create a comprehensive command language to cover a wide range of situations. Rather, the goal has been to investigate the specific issues that arise when a remote planner interacts with a workcell operating in a dynamic environment. These issues have been addressed experimentally within the context of the objectives described in Chapter 1.


move object

move and release move arms

move and grasp

40

Robot commands through the command interface Move an object that is being grasped. This command will provide a viapoint collision-free path for the object. The arm(s) is controlled using object impedance control [155]. This command also provides earliest-time constraints for the time of traversal of each via-point Same as above with the addition of a specified release location (must be close to the end of the trajectory) Move one or both arms. The arm(s) to move cannot be holding an object. Specifies a via-point collision-free path both in operational and joint-space so that the robot controller is free to use different control schemes (and kinematic ambiguities can be resolved). This command also contains earliest-time constraints on each via point. Same as above with the addition of the specification of the object(s) to be grasped and the corresponding arm(s), and grasp(s) location(s) for each arm involved (any combination of arm/object, including both arms on the same object is allowed). This command also contains earliest-time constraints on each via point. Table 3.3: System command interface

The specific commands that can be sent through the command interface and respective definitions are summarized in Table 3.3. Note two important aspects of the semantics of these commands: First they are strategic-level commands, and second, they allow description of timing constraints to be met by the workcell. Strategic commands.

The system commands are strategic-level, i.e. they specify multi-step

actions without completely constraining the details and timing of these actions. Strategic commands require further processing, and must be decomposed into primitive actions by the strategic-level controller within the control subsystem (see Chapter 6). For instance, the command to move and capture an object only specifies a geometric path (and timing constraints) for the arm to approach the object, as well as the required grasp transforms. However, the details of the interception (velocity and acceleration matching, closing the gripper etc.) are left to the control subsystem. Similarly, the move-and-release command specifies a path to approach the release location and the desired final location for the object; fine motion in the release is left again to the control subsystem. Commands of this type could easily be extended to handle interaction with other objects and actual assemblies, because the (control-system specific) details requiring high-bandwidth feedback,


41

assembly strategies, force control, etc. are left to the control system. The development of these types of primitives is the subject of other work at the Stanford Aerospace Robotics Laboratory [150, 152]. Description of timing constraints.

Interacting with a dynamic environment requires the planning

subsystem to account for and communicate timing information to the control subsystem. For this research, a simple interface has been chosen. Each via-point3 sent by the planner, is tagged with an “earliest-time” stamp. It is the planner’s responsibility to ensure that any trajectory that meets this constraint is safe (see Section 3.5.5). The control subsystem will generate a trajectory that, aside from satisfying the dynamic constraints of the system, also satisfies the imposed “earliest-time” constraints. This is achieved in practice by delaying initiation of the trajectory until all constraints are met. This approach makes the system robust to unpredictable communication delays, which occur when the planner executes at a remote location over the internet, and also accounts for the fact that the planner executes on a non-real-time computer system. However, it is clear that this approach has limitations. In particular, it restricts the set of viable plans to those whose execution can be safely delayed, and also allows useless motions to proceed (e.g. the arm goes to a position on the conveyor to grasp an object well past the time when the object was there). One can contemplate adding a “latest-time” constraint to each via point. In fact this ability already exists in the system, but the current planner is not able to take advantage of it. Clearly much research remains to be performed in this area.

3.4.3

The Task-Specification Interface

High-level task commands fall somewhat between the extremes represented by world-state and system-command information. Task commands are asynchronous like the system commands. However, tasks are in a sense self-contained (i.e. specify a goal or desired system state) as opposed to specifying an action. As a result, tasks may be persistent. For instance, a task may specify to repeatedly pick objects from a conveyor, immerse them in a solution, and then place them on a second conveyor. This task does not end until explicitly cancelled. Multiple concurrent tasks may be active at the same time and compete for the system’s resources. Tasks may also be built as logical combinations of smaller subtasks (e.g. put object O1 at location L1 and object O2 at location L2). Table 3.4 summarizes the task interface. 3

Via-points (a.k.a. through-points or way-points) specify intermediate configurations (poses) for either the manipulators or the objects being manipulated. Via points are discussed in detail in Chapter 7.


place objects

make assembly

42

Task commands through the task interface Place a set of objects at their specified destination. The objects may not even be in the workspace. The system must get the objects however it can, place them at their goal, and monitor to make sure they remain there until the command is cancelled. Similar to place objects, except the object goals are now in contact with each other. Table 3.4: Task-specification interface

3.4.4

Custom Interfaces for the Manufacturing Workcell

The three primitive interfaces previously discussed constitute the information building blocks (modules) from which custom interfaces can be built to connect the different subsystems. These interfaces are anonymous (no assumptions are made regarding the number or the specific subsystems that will be connected through them). Instead, the primitive interfaces provide a mechanism for subsystems (whatever they are) to share and communicate information. Since the primitive interfaces encapsulate all the information required to monitor and direct the robotic workcell, they can be combined to provide all the information and interaction modes required by any subsystem. Anonymous interfaces have several advantages: they allow the system to be easily extended and modified, because the subsystems are not hard-linked to each other. They also provide for subsystem replication (hot-standby, multiple monitoring stations) without additional effort. However, anonymity does have costs, mostly concerning safety and security. Since the interfaces are anonymous, there is no mechanism to specify (or restrict) who is allowed to access which information. Moreover, a negligent (or malicious) subsystem can corrupt the whole system by providing incorrect data. These tradeoffs must be carefully evaluated for each application. The different subsystems in the workcell use the three primitive interfaces to develop custom interfaces for each of the subsystems. For instance the graphical user interface (GUI) (see Section 3.5.1), uses both the world-state and the task-specification interfaces. The GUI customizes the world-state interface by selecting the specific information required for graphical rendering, and subscribing to the rendering information at a rate slow enough to be handled by the three-dimensional graphics program. The GUI also uses the task-specification interface to describe the high-level user requests to any subsystem interested in this information (the planner in our case). The combination and customization of these two primitive interfaces constitutes a unique interface tailored to the


43

needs of the GUI. The remaining subsystems create their corresponding interfaces in an analogous manner. On occasion, a new subsystem may be developed that requires different information, or a new sensor added that provides some other type of information. This could be handled by either augmenting one of the primitive interfaces or, if the characteristics of the information do not fit within any of the existing interfaces, a new primitive interface can be created. However, this addition should have minimal impact on the existing interfaces.

3.5 System Architecture

Network Data Delivery Service

Robot System 3 4

User-Interface

2

Task Planner Path Planner

Simulated Robot

Figure 3.3: System Architecture Four subsystems collaborate to operate the workcell. Each subsystem uses a combination of the primitive information interfaces to connect to the information flow. The interfaces are represented by connection icons: world-state information (triangle icon), task-specifications (trapezoid icon), and system commands (semi-circle icon). All the interfaces are built on top of a “software bus” called the Network Data Delivery Service (NDDS) [see Chapter 4].

The overall system architecture is illustrated in Figure 3.3. The system has been broken into four subsystems: (1) Human interface, (2) robot-control system, (3) task and path planners, and (4) robot simulator. The human interface receives world-state information and provides a graphical representation of the scene to the user. During operation, the high-level task is also specified using the human interface and sent to the planners. The task planner/path planner receives continuous updates from the robot and task requests from the user, and produces robot commands to implement the requested tasks. The robot controller executes these commands and processes the sensor signals to create and maintain a world model. The simulator can masquerade as the robot control system during development or, by running it concurrently with the robot, can be used to compare the


44

accuracy of the workcell models (kinematics, dynamics and state transitions) with the behavior of the actual system. The three primitive interfaces described in the previous section are used to communicate between the four subsystems. For instance, the human-interface uses the world-state interface to obtain the kinematics and position of the arms and workspace objects in order to display them to the user. Tasks are sent to the planners through the task-specification interface. The planners also use the world-state interface at a lower bandwidth than the human interface. The actual implementation of the interfaces over a computer network is a significant undertaking in itself, and is the topic of Chapter 4. The fundamental challenge is to provide efficient and transparent network connectivity consistent with the characteristics of the information encapsulated by the interfaces. The resulting outcome is similar to a “software bus,” where the three primitive interfaces provide the means for accessing the bus. This architecture allows functionally identical subsystems to be replicated and new subsystems to be added to create different configurations. The different subsystems are described in more detail in the following sections.

3.5.1

The Graphical User Interface

The purpose of the user interface is to provide an intuitive, easy-to-use mechanism for the user to command and monitor the workcell. Simple graphical user interfaces for robotic systems have been developed by previous researchers at the Stanford Aerospace Robotics Laboratory [154, 189], while ongoing research in collaboration with NASA Ames Research Center has explored the use of Virtual Reality [45, 179]. Researchers at other institutions have also addressed the human-robot interface issues [28, 64]. This work differs from graphical robot simulation-and-programming environments such as SILMA’s CimStation [37], DENEB’s IGRIP [16] and Tecnomatrix’s RoboCAD [197], which are mostly used for off-line programming and are not directly connected with the running robotic systems. However, these programming systems could potentially be used as the graphical front to the user interface. This approach is being pursued by other researchers [193]. The fundamental difference in our approach is that the graphical user interface (GUI) is employed at the task-specification level, that is, it is used to tell the workcell what the user wants done, not how to do it. For instance, with our interface, the human specifies the goal locations for a number of objects which may not even be in the workcell at the time. The remaining subsystems (planner, world modeler, controller...) will determine how to achieve the task4 and collaborate to achieve the 4

etc.

How to achieve a task will depend on the current state of the workcell, the arrival of the needed parts on the conveyor,


45

final goal. The user interface also serves as a monitoring device that allows the user to observe the workcell in action. Figures 3.4, 3.5, and 3.6 show the user commanding and observing the workcell through the graphical interface. It is important to note that these are not simulations or off-line animations, but rather displays of live data arriving from the physical system during operation. The GUI uses 3-D rendering to display the current state of the workcell from any view angle, which can be changed at the user’s discretion. Rendering is performed using the GraPhigs package (Zimmermann [207]) which is built on top of PHIGS [18]. In addition it incorporates logic that allows the user to select any number of objects and move their “ghost” images around the screen, and in this way, build an interactive specification of the task. Once the user is satisfied with the specification, he or she issues the command by clicking on the command button within the GUI. The fundamental tenets of this interaction paradigm were formulated by Schneider [154]. The new features of the GUI developed in this research, are the ability to specify tasks involving multiple objects, and the possibility (due to its stateless nature and anonymous interfaces) of running multiple copies simultaneously, as illustrated in Figure 3.7. This GUI has been utilized in many experiments to run the system remotely, where the human uses it as the only means of interaction with the system. The GUI uses the world-state interface to get (slow) updates from the system (at a rate that our 3-D graphics can handle, currently 5 Hz) and the task-specification interface to issue the user-constructed tasks. This architecture enables the use of multiple copies of the interface simultaneously. Multiple interfaces has allowed several users to monitor and collaborate with each other [see Section 3.6.1].

3.5.2

The Simulator

The goal of the simulator is to allow efficient testing and debugging of the other subsystem modules without using the actual robots5. Subsystems communicate using the anonymous interfaces already described; therefore, none of the remaining modules is aware it is interacting with a simulator rather than the actual robot. The purpose of the simulator is not to develop the control system, and therefore there is only need to simulate robot kinematics, not dynamics. In fact, the simulator uses the rigid-robot dynamics to generate the same reference trajectories that would be generated by the real robot6, and then assumes that the robot exactly tracks the reference trajectory. 5

Aside from risking damage, using the robots is more time consuming than using the simulator, and can only be done by one person at a time. 6 These trajectories use the dynamics of the robot to ensure that the manipulators can track the trajectory without exceeding any actuator limits. The algorithm used to generate the trajectories is described in Chapter 7.


46

1

User Selects object

4

Right arm grasps object

2 User drags ghost to mark goal Real Ghost

5

Right arm is moving object

3

Right arm moves towards object

6

Right arm releases object for regrasping

Figure 3.4: Commanding and Monitoring the Workcell with the GUI (1-6) The above pictures illustrate the use of the GUI for task-specification and system monitoring. Note that the pictures represent a projection of a 3-D view, and the manipulators move above the objects in the workspace. The above sequence continues in Figures 3.5 and 3.6. First the user selects the “block object” (1) with the mouse. Selection of an object creates a ghost image which the user drags anywhere in the workspace to indicate the goal position for the part (2). Once this simple task has been defined, the user clicks the “command” button and the system figures out a plan and starts moving the right arm towards the object (3). The object is grasped in (4). Since the goal cannot be reached with the right arm, a hand-over operation is automatically performed. The hand-over consists of leaving the object at an intermediate location (5) and (6), and grasping it with the other arm (continued in Figure 3.5).


47

7

Ghost

Right arm moves away. Left arm moves towards object.

10

Left arm moves object.

Ghost

8

Ghost

Left arm approaches object.

11

Object approaches goal.

9

Ghost

Left arm grasps object.

12

Object placed at goal location.

Figure 3.5: Commanding and Monitoring the Workcell with the GUI (7-12) The hand-over operation (continued from Figure 3.4) completes in pictures 7 through 12.

The simulator has been used extensively to debug the planner subsystem. This not only provided safe development (simulated robot collisions are not quite as damaging as real ones), but also created a repeatable environment where identical situations could be easily recreated (this is hard in the real world because the “situation” involves not only the exact location of parts in the workcell but also the timing of the arrival of the different parts on the conveyor). The simulator graphical front-end


48

is very similar to that of the GUI, except the user is now allowed to modify the physical location of the objects, as well as change the location of the robot bases, conveyors, and arm configurations. These values may be saved and retrieved from configuration files so that situations can be easily reproduced. The simulator subsystem was also extremely useful in the redesign of the workcell, as it allowed one to prototype manipulator location and kinematic parameters (link length), and to see the effects of these changes on the range of achievable tasks. This feature alone is one of the main functions of several graphical robot-simulation packages such as SILMA and DENEB—of course these systems are more general and have many other capabilities. Lastly, the simulator has also been successfully used to explore the possibility of subsystem masquerading and live system reconfiguration. In this experiment, the robot-subsystem and simulator are both run concurrently. The design of the anonymous interfaces allows both to receive commands and export their world-state information. However, the interfaces (using the mechanisms provided by NDDS, described in Chapter 4) allow the world-state information from the robot-subsystem to take precedence while the robot is operational, so that the real system functions normally, and the operation of the simulator can be compared with that of the real system. The simulator can also be put in a mode in which it constantly resets its state with that received from the real system. If the real robot goes off-line (purposely or due to a failure or communications breakdown), the simulated data will replace the real data throughout the rest of the system (masquerading as the robot system). The benefits of this type of interaction have been demonstrated in telerobotic applications by the ROTEX experiment [64]7. Our system could use a similar strategy to replicate the planning subsystem and allow multiple planners to compete with each other for the best plan.

3.5.3

The World Modeler

World modeling: The integration, interpretation, and representation of the sensory information using both a´ priori knowledge and other information exchanged by the different subsystems is a sophisticated process. The philosophy and implementation of this subsystem is presented in Chapter 5. The world modeler communicates with the rest of the system through the world-state interface. 7

In the ROTEX experiment a graphical simulator was used to display the state of a robot in orbit and to interpolate its state between updates from the remote system. The state of the simulator was corrected every time an update from the remote robot was received. This approach allowed a user on earth to teleoperate a robot in orbit despite the long communication delay.


3.5.4

49

The Hierarchical Control System

This subsystem receives strategic-level commands and controls the workcell in response to these commands. The control of the workcell is quite sophisticated in that both arms must be managed and controlled in a variety of modes (independent trajectories for each arm, independent object motions, cooperative object motions, etc.). Furthermore, since the commands to the workcell are strategic-level commands, the control subsystem must be responsible for dividing them into elementary primitives, sequencing them and monitoring their progress. The control subsystem is explained in Chapter 6.

3.5.5

The Planning Subsystem

The planning subsystem is responsible for interpreting user task specifications, decomposing them into primitive subtasks (or strategic commands), and issuing them to the robot system. These plans include safe (collision-free) via-point paths for the arms and objects. The planning subsystem must also monitor the progress of the workcell so that the individual subtasks can be scheduled. Planning is a notoriously hard problem, in this case exacerbated by the need to interact with a dynamic environment (objects approaching on the conveyor, planning for one arm while the other is already moving, etc.). The planning subsystem has been developed by Li from the Stanford Computer Science Robotics Laboratory as part of his PhD research [91, 92]. The interface to the planning subsystem is built by combining the world-state, system command, and task-specification interfaces. The characteristics of these primitive interfaces have several implications on the planner implementation:

The planner uses the world-state interface to receive periodic updates of the state of the world (e.g. position of different objects, arm configuration) as well as notification of significant events (arms grasping an object, appearance of a new object in the workcell). This type of interface promotes the development of an event-driven planning subsystem.

The planner commands the workcell using the system-command interface. This interface is stateless, so there is no explicit acknowledgment of individual commands. Therefore, the planner must use the world-state information as its only means to infer the status and progress of the system. As a consequence, the planner must also be quasi-stateless, using pre-computed plans only as hints on how to achieve the remaining tasks. I must always evaluate the next step based on the current (sensed) state of the workcell. This increases the planner complexity


50

but, in exchange, provides robustness to unexpected events (they simply represent a sudden change in workcell state). This approach also allows the planner to be (re)started at any time.

There are unpredictable communication and execution delays on each command issued (some may be rejected unilaterally by the robot-subsystem). As a result, the planner groups individual plans into “transactions” that are delayed and executed as a unit. For example a coordinated two-arm motion cannot be sent as two individual single-arm commands, since the time synchronization among the two-arm motions would be lost, resulting in a potential collision.

These constraints have resulted in the restriction of the generated plans to those satisfying the constraint of safety-under-time-delay (SUTD). This constraint was introduced by the author in previous work [126]. It basically means that each individual plan transaction (strategic command) must be safe (assuming no unexpected events) under an arbitrary time delay. For instance, if an arm is already in motion, a plan that moves the other arm temporarily inside the region that will be swept by the already-moving arm can be safe, provided the motion of the second arm is coordinated in time so that the second arm leaves the swept region before the already-moving arm moves through that region. However, this plan will not be SUTD, because if the motion of the second arm is delayed, the arm may not leave the swept region in time to avoid a collision with the first arm. Figure 3.8 illustrates this concept. The value of SUTD planning relies on the fact that these plans can be computed as easily as normal plans. The technique to compute SUTD modifies the configuration-space obstacles in

configuration-time space (CT -space) [87] by projecting future CT -space obstacles back in time as illustrated in Figure 3.9. For any given time, this technique removes the region that will be swept by moving obstacles in their future motion from the present free space. Details of this technique can be found in Li’s thesis [91]. As described in Section 3.4.2, the system-command interface incorporates these ideas by allowing every via-point path to be tagged with an “earliest-time” stamp (generated from the SUTD plan). So long as the controller ensures that the trajectory meets all the earliest-time constraints, the plan will be safe. The strategic control system will ensure that these constraints are met, or else the command will not be executed.


51

3.6 Experimental System Operation This section illustrates the complete system in operation, and presents experimental data on the system accomplishing several tasks. Section 3.6.1 describes some typical operational scenarios, while animations and photographic sequences of the system in operation are presented in Section 3.6.2

3.6.1

Operational Description

Information interfaces together with the software bus allow combinations of modules to be easily used. Figure 3.10 illustrates some of these configurations. In the first three cases, we have replicated modules mentioned earlier. The top diagram indicates several users collaborating to control the robot or monitor its activities. The second diagram depicts the use of multiple simulators to simulate different aspects of the system, such as for command previewing. The third diagram shows the use of combined multiple planning strategies, each of which may be more appropriate for certain tasks. Finally, the last diagram indicates that modular interfaces allow us to build new interfaces such as one for a teleoperation subsystem. In our operations, we have not experimented with multiple concurrently-active planners nor with teleoperation. Nothing in the architecture prevents us from doing so (since the remaining subsystems do not know where the data is coming from or going to), but the planners themselves require enhancement to be aware that they are not the only possible masters8. The actual arm trajectory is available to the planner through the world-state interface; however, the planner is not using this information. Similarly, a teleoperation subsystem would need to be developed (a significant endeavor by itself) and the system-command interface extended to allow specifying motion in ways more appropriate for teleoperation. For instance, we may want the reference state for an object to be directly tied to that of a hand-controller, or we may want to tie the reference velocities to a force-input device. Any of these mechanisms would require simple extensions to the system interface. Figure 3.11 illustrates the command flow and resulting actions of the workcell during a typical task. This hypothetical task consists of placing two objects at some desired location. Actual experimental tasks are included in the following section. The task is surrogate to an assembly situation, except there is no contact between the objects. Although the system is able to manipulate objects in contact with their environment, neither fine-motion planning nor contact-specification 8

For instance, assumptions currently made such as: “if you sent a command to move an arm, and the arm starts moving, then it must be moving along the specified trajectory,” should be avoided.


52

commands have been addressed by the current research. These difficult and important issues are being addressed by other researchers in the ARL [152], (and elsewhere), and are promising areas for future research.

3.6.2

Experimental Tasks

The workcell can perform many operations semi-autonomously. These operations involve capture and delivery of objects in the workcell. The tasks specified by the user may require multiple steps due to the need to regrasp objects (to attain a more advantageous arm configuration by changing handedness), transfer objects from one arm to another (to deliver objects to areas accessible by only one arm), capture moving objects, etc. In this section we present experimental data for several representative tasks. The data set is presented in two formats: graphical animations and photographic sequences. Graphical animations were generated by a drawing program using experimental data collected during system operation. The workcell is represented using the top view, and all objects are drawn to scale. The complete operation is divided into several segments, each corresponding to an individual system command from the planner, and each segment is animated by representing the position of the arms and objects at 0.8 sec intervals. Each segment is illustrated in two views (see Figure 3.12): The figures on the left include the robot arms while the ones on the right show only the objects. In this manner, pure arm motions are easily distinguished (no objects move) while the motion of the objects around each other can be observed unobstructed by the arms. These graphical animations contain a lot of operational information. They illustrate the system-command sequence and the location of every object in the workcell at small time increments. However they do not provide useful control-type information (tracking errors, reference velocities and accelerations, etc.). This data is presented in Chapter 6. Photographic sequences

have been generated by digitizing video taken during the operation of

the workcell. Photographic sequences are used in combination with the corresponding graphical animations to illustrate certain operations as in Figures 3.13, 3.14. Detailed photographic sequences are also used to document complete tasks of the workcell as in Figure 3.24 thru 3.26. The pictures in the photographic sequences have not been taken at fixed time intervals (this would require far too many images). Instead, they have been chosen at a granularity such that the complete operation of the system can be inferred. In these pictures, paper cues have been placed on the table to represent


53

the user-requested goal location for the different parts. These cues are visual aids included only to enhance the illustrative nature of the photographs, and they are not used by the system. To conserve space, most operations are illustrated as either graphical animation or photographic sequences, not both. The workcell operates completely on-line without fixtures, a´ priori schedules, plans or assembly sequences. As a result, the same task can (and will) be executed differently depending on the exact timing of the arrival of the parts, and on CPU usage. This makes the workcell adaptive to the manufacturing environment, and opens interesting possibilities of part-driven workcell operation9 . These facts have been illustrated by repeatedly commanding the same tasks, and observing several different strategies used by the workcell. Operation of the workcell will be documented in an incremental fashion, beginning with simple tasks with static objects and concluding with more sophisticated tasks where objects are fed on a conveyor. Maneuvers in a static workspace A static workspace, one in which objects are not moving, is less challenging than a dynamic one because the timing of the operations need not be synchronized with anything external to the system. As a consequence, a plan taking longer to be generated, or a trajectory not being parameterized quickly results only in slower system operation, and no “deadlines” are missed. More importantly, there is no need for the planner to exchange timing information with the robot through the systemcommand interface, or indeed to consider the time domain at all. These operations are challenging nonetheless in that they are carried out interactively by the user, and it is important that the system appears responsive. Figure 3.13 illustrates the result of the user requesting two objects to be placed at different locations. The first object can be immediately grasped and delivered by the right arm, but the second (square object) requires a “hand-over” operation. This occurs because the current location of the second object is reachable by only one arm, while the destination is reachable only by the other arm. The system automatically recognizes this situation and commands the right arm to grasp the object, place it at an intermediate location where it can be grasped by the left arm, and then commands the left arm to grasp and deliver the object to its intended destination. It is important to 9

This may also make the manufacturing process less predictable. Compensating for these effects with the incorporation of learning and/or user-specified hints are all areas of promising research.


54

note that these decisions and coordinated maneuvers occur without user intervention. Figures 3.14 contains photographic images of the system during each one of the six stages. Figures 3.15, 3.16 illustrate a more complicated scenario which may arise during assembly operations. The user has specified the “assembled” configuration consisting of the relative locations of the three objects shown at the end of Figure 3.19. Achieving this “assembly” involves moving all three objects10. The order in which these operations occur is important (some orderings may be impossible). The user does not need to be aware of any of these details and simply uses the GUI to instruct the system of the desired final configuration and to monitor system progress. Images from the actual assembly can be seen in Figures 3.17, 3.18, and 3.19. These images were taken by digitizing video recorded during the assembly operation.

10

Note that the large object requires cooperative dual-arm manipulation.


13

55

16

Ghost

User drags ghost image to indicate goal

Coordinated two-arm grasp

Conveyor

14

17

Goal position and orientation

Both arms cooperate to move object

15

Both arms move towards object

18

Object placed at goal location and orientation

Figure 3.6: Commanding and Monitoring the Workcell with the GUI (13-18). Continuation from Figures 3.4 and 3.6. In this sequence, the user commands to move the big “U-shaped” up towards the conveyor. The user selects the object and drags its ghost image back to a position and orientation near the conveyor (13,14). From there on the system proceeds automatically by first acquiring the object with both arms (15,16) and then delivering it to the requested position and orientation (17,18).


1

Conveyor

4

Both arms coordinate to pick object

Object detected on the conveyor

2

5

Both arms cooperate to move object

Object moves on the conveyor

3

Both arms move towards object

56

6

Object placed at goal location

Figure 3.7: Commanding and Monitoring the Workcell with the several User Interfaces This sequence illustrates the use of multiple GUIs to view the same live operation from different perspectives. The two GUIs are running on different computers (they are displayed on the same screen for documentation purposes only). Multiple GUIs could be used to monitor/command the workcell simultaneously from different locations. The GUIs need not be identical, and can be customized for each user.


57

position

Motion already in progress

time

position

Motion is safe under time delay

Motion is NOT safe under time delay

time

Figure 3.8: Concept of Safe-Under-Time-Delay (SUTD) planning These figures illustrate the concept of SUTD planning. The top figure animates the future motion of an object which is already moving. The bottom figure shows collision-free plans for two objects. The plan for the circular object is not SUTD because if the motion was slightly delayed it would collide with the square object. On the other hand, the plan for the hexagonal object is SUTD because it can be arbitrarily delayed, and the resulting motion will still be collision-free.


58

position

CT-space obstacle with respect to circular object

Motion already in progress

position

time

CT-space obstacle projected back in time

position

time

time

Figure 3.9: Use of configuration-time space for safe-under-time-delay (SUTD) planning Normal configuration-time space (CT -space) planning can be used to generate SUTD plans. First the normal CT -obstacle corresponding to the motions in progress is generated (top figure). The CT -obstacle is then projected backwards in time (bottom figure). Normal planning in CT -space using this modified obstacle yields SUTD plans.


59

Robot System 3 4

User-Interface

User-Interface

User-Interface

2


Robot System 3 4

User-Interface

2

Simulated Robot


Simulated Robot

Robot System 3 4

User-Interface

3 2

4


2


Robot System 3 4

User-Interface

2


Figure 3.10: System Configurations The anonymous interfaces allows replicated modules (top three diagrams) in the system. The modular information interfaces allow new subsystems to be added to the system (last line).


1

4

2

5

7

60

3

6

8

Figure 3.11: Typical system operation The user has specified the simple assembly of two objects depicted in the bottom-right (8) by dragging the iconic representations of the objects on a graphical interface (not shown). After this the remaining actions are autonomous (left to right, and top to bottom): (1) The planner constantly monitors the workcell using the world-state interface, and as soon as a needed object appears on the conveyor, a capture trajectory is planned and sent to the robot [move and grasp command]. This command specifies the top arm to be moved out of the way, and the bottom arm to grasp the object. (2) Once the object is grasped, the planner (which has detected the event through the world-state interface) plans a delivery trajectory and sends it to the controller [move and release command]. Since the arm cannot deliver the object to its goal location with its current handedness, the object is placed at an intermediate location where the arm can change handedness and regrasp it. (3) In the meantime, a new object has appeared on the conveyor, so the planner issues a move and grasp command for both arms (one for the conveyor object, the other for the just-released object). (4) While one arm picks the second object from the conveyor, the other delivers the first object to its final destination [move and release command]. (5) Once the second object is grasped, since the final destination is only reachable by the other arm, the arm grasping the object is commanded to place it at a location reachable by both arms [move and release]. (6) Next a move and grasp command moves the first arm away while the second picks the object, and finally, (7) a move and release command delivers the second object to its final destination (8).


61

1

2

Figure 3.12: Animation of capture and delivery Animation taken at 0.8 sec intervals of experimental data collected during the capture and delivery of an object. Each one of the two figures corresponds to the response to a single strategic command from the planner: (move-and-grasp) for the top figure and (move-and-release) for the bottom figure. Side-by-side figures represent the same time sequence, except in the right figures the arms are not shown, so that the object motions can be seen in detail. The complete operation starting at the time the object appears on the conveyor took 20 seconds.


1

2

3

1. Right Arm approaches and picks "Corner" object

2. Right Arm delivers Corner to destination

3. Right Arm approaches and picks "Block" object

4

5

6

62

4. Right Arm moves Block to intermediate location

5. Left Arm grasps Block

6. Left Arm delivers Block to destination

Figure 3.13: Animation of hand-over operation The workcell has been commanded to move the corner-shaped object inside the “U” shaped one, and the small square object (bottom-right on first figure) to the other side of the workspace. Since all the required objects are in the workspace, the system can proceed immediately. The total delay from user specification to the beginning of the motion of the robot was 0.7 sec (this includes communication from the GUI to the planner, planning time, communication from the planner to the robot, command processing and trajectory generation). Six system-commands are needed for this operation (one per figure above).


1

4

2

5

3

6

Figure 3.14: Photographs during hand-over operation Digitized video images from the sequence illustrated in Figure 3.13

63


1

2

3

4

1. Both arms approach object "Cove"

2. Both arms pick Cove

3. Arms deliver Cove to destination

4. Right Arm approaches Corner, Left arm moves away

5

6

7

8

64

5. Right Arm picks Corner

6. Arm moves Corner to intermediate location

7. Arm regrasps Corner to avoid joint limit

8. Arm starts delivering Corner to destination

Figure 3.15: Animation of multi-object-placing sequence with static objects (stages 1-8) This sequence automatically moves three objects to their new specified locations. This sequence continues in Figure 3.16. Representative pictures of the workcell during the operation are presented in Figures 3.17, 3.18, and 3.19.


9

10

11

9. Left Arm approaches Block while Right Arm is moving

10. Left Arm reaches and grasps Block

11. Right Arm moves out of the way

12

13

14

12. Left Arm delivers Block to intermediate location

13. Left Arm regrasps Block to avoid joint limit

14. Left Arm delivers Block to final destination

Figure 3.16: Animation of multi-object-placing sequence with static objects (stages 9-14) Conclusion of the sequence started in Figure 3.15.

65


1

4

2

5

3

6

66

Figure 3.17: Photographs during multi-object-placing sequence with static objects (stages 1-6) Snapshots taken during the same operation animated in Figures 3.15. Each picture contains the workcell at some point during the corresponding stage. The above six pictures are for the first six stages.


7

9

8

10

67

Figure 3.18: Photographs during static assembly sequence (stages 7-10) Snapshots taken during the same operation animated in Figures 3.15, Each picture contains the workcell at some point during the corresponding stage. The above pictures are for stages 7 through 10.


11

13

12

14

68

Figure 3.19: Photographs during static assembly sequence (stages 11-14) Snapshots taken during the same operation animated in Figures 3.15, Each picture contains the workcell at some point during the corresponding stage. The above pictures are for stages 11 through 14.


69

Maneuvers in a dynamic workspace The objects required for the assembly need not be present in the workcell at the time of task specification. The system remembers the task and will acquire the objects as they appear on the conveyor (in whatever order). Dynamic scenarios where the workcell interacts with moving objects are much more challenging than the static ones described in the previous section. In a dynamic environment, there are real-time constraints on the duration of certain operations. Time needs to be taken into account both by the planner and the strategic control system. The planner must incorporate the time “dimension” and certain “goals” become curves in configuration-time space (the “goal” of intercepting an object corresponds to a position that varies as a function of time). This results in an increase in problem dimensionality. Additionally, the planner must be aware of (and account for) the time consumed in its own planning process. In the context of this project, these issues have been successfully addressed by Li’s PhD research [91]. The complexity of the dynamic environment not only affects the the planning subsystem, the interfaces themselves must provide the means for expressing the time constraints (section 3.4.2), and the trajectory generation must also trade off compute time versus efficiency of the generated trajectory (see Chapter 7). Figure 3.20 animates data collected during a multi-object-placing operation (again an assemblytype scenario where objects are brought within close proximity but no contact is made). In this case, the required parts are not present in the workcell and will be delivered by the conveyor at some later time. As soon as any of the required parts appear on the conveyor, the system generates on-line plans to acquire and deliver the parts to their intended destination without further user intervention. Snapshot images during this operation are presented in Figure 3.21. The system can be reconfigured without any programming. As an example of this, the location of the assembly and conveyor has been completely changed during system operation, from the configuration in Figure 3.21. Since the planner and remaining subsystems have subscribed to the relevant information, they are notified immediately of this reconfiguration without having to explicitly query for any configuration changes. Again, the required parts are not initially present in the workspace and are delivered by the relocated conveyor. The successful operation of the system under these new circumstances is illustrated in Figures 3.22 and 3.23. As previously explained, because all planning and trajectory-generation occurs on-line, any changes in the order or timing of part arrival may cause the system to operate differently. For example, the detailed photographic sequence presented in Figures 3.24, 3.25 and 3.26 is taken during a different execution of the same user task-command (i.e. the placement of the objects is the same). However, the timing is different. In particular, the last part is fed on the conveyor shortly


1

2

3

4

1. Both arms approach and pick "Cove" from conveyor


3. Left Arm picks Corner from conveyor


5

6

7

8

70


6. Arm delivers Corner to destination

7. Right Arm picks Block from conveyor

8. Right Arm delivers Block to final destination

Figure 3.20: Animation of multi-object-placing sequence with objects delivered on the conveyor This sequence animates data collected during system operation. The user has specified the placement of three parts in the workcell. The parts are not present in the workcell initially, but as soon as they arrive on the conveyor, the workcell automatically retrieves them from the conveyor and delivers them to their destination. These animations are taken at 0.8 sec. intervals, the complete sequence operation takes 40 seconds from the time the first object is detected on the conveyor. Representative pictures of the workcell during the operation are presented in Figure 3.21.


1

5

2

6

3

7

4

8

71

Figure 3.21: Photographs during multi-object-placing sequence with with objects delivered on conveyor Snapshots taken during the same operation animated in Figure 3.20. Each picture shows the workcell at some point during the corresponding stages.


1

2

3

1. Both arms approach object "Cove"

2. Arms move Cove to intermediate location

3. Left Arm changes handedness to avoid joint limit

4

5

6

72


5. Left Arm approaches Corner, Right arm moves away

6. Left Arm picks Corner

Figure 3.22: Animation of multi-object-placing sequence with different conveyor position (stages 1-6) Sequence animating data collected during system operation. The user has specified a task involving the placement of three objects that are not currently present in the workspace. As soon as the first object is detected, the planner issues a command to pick the object (top-left figure). Note that the system knows that the object needs two arms to be manipulated. Due to joint limits in the left arm, a regrasp is required before it is delivered to its destination (stages 2,3,4). Stages 5,6 animate the capture of the second object. This sequence continues in Figure 3.23.

after the second one is fed, forcing the workcell to acquire it while it is still manipulating the second part. Otherwise the system risks being too late to grasp the last part. This sequence also illustrates the system ability to use both arms concurrently to increase throughput. This capability is enabled by proper design of both the planning and hierarchical control subsystems.


7

8

9

10



9. Arm delivers Corner to final destination

10. Right Arm approaches Block

11

12

13

14

73

11. Right Arm picks Block while Left Arm moves away

12. Right Arm delivers Block to intermediate location

13. Right Arm regrasps Block to avoid joint limit

14. Right Arm delivers Block to final destination

Figure 3.23: Animation of assembly sequence with conveyor across the workspace (stages 7-14) Continuation of Figure 3.22. Delivery of the second object also requires a regrasp due to joint limits (top images: stages 7, 8 and 9). Finally, stages 10 though 14 animate the capture and delivery of the third object (which also requires a regrasp).


1

6

11

2

7

12

3

8

13

4

9

14

5

10

15

74

Figure 3.24: Detailed strobe images of an multi-part-placement operation (1-15) This sequence documents the operation of the workcell that has been commanded the same multi-objectplacing task illustrated in Figures 3.22 and 3.23, however the pictures do not correspond to the same run. Since the timing of part arrival is different, the actual trajectories and command sequences are different. This sequence continues in Figures 3.25 and 3.26.


16

21

26

17

22

27

18

23

28

19

24

29

20

25

30

75

Figure 3.25: Detailed strobe images of an multi-part-placement operation (16-30) Continuation of sequence started in Figure 3.24. Notice how the workcell is capable of moving both arms independently to pick the third object from the conveyor while it regrasps the second object (last two rows of figures). This sequence continues in Figure 3.26.


32

37

42

33

38

43

34

39

44

35

40

45

36

41

46

Figure 3.26: Detailed strobe images of an multi-part-placement operation (31-45 Conclusion of the sequence started in Figure 3.24.

76


77

3.7 Summary and Conclusions This chapter has described the operation of the complete system as well as the architecture and interfaces that enabled its functionality. System operation has been described using graphical animations of experimental data collected from the system, as well as still images obtained from video sequences. The experimental results document complex, multi-step autonomous operations involving part-acquisitions from a moving conveyor, part regrasping, hand-overs, and dual-arm manipulation. A novel approach to complex robotic system design called interfaces-first design was also a major contribution made in this chapter. Interfaces-first design starts by identifying the fundamental types of information flow in the system, and then encapsulates that flow into primitive anonymous interfaces. Subsystem interfaces are then built from combinations of these primitive information interfaces. This approach results in expandable systems with facile interconnectivity. The characteristics and information content of the specific interfaces designed for the dual-arm workcell are described in detail along with the operational examples. Using these information-interfaces, the architecture of the workcell is built around five subsystems: (1) Graphical User Interface, (2) Planner, (3) Hierarchical Control System, (4) World Modeler, and (5) Simulator. Brief descriptions of the user interface and simulator subsystems have been presented in this chapter. The remaining subsystems are described in Chapters 6, 5, and in Li’s thesis [91]. The design methodology and architectural concepts presented in this chapter are not limited to the specific architecture of the workcell. Rather, the experiments performed provide a perspective on the kinds of complex multifaceted systems that may be developed using this approach.

Chapter 4

Communications in Distributed Robotic Systems Many control systems are naturally distributed. This is due to the fact that often they are composed of several physically distributed modules: sensor, command, control, monitoring, etc. To achieve a common task, these modules need to share timely information. Robotic systems are a prime example of such distributed control systems. The need to share information in a distributed environment is common to many other application environments such as databases, file systems, distributed computing and simulation, banking and transaction systems, etc. However, distributed control systems have unique needs not fully addressed by the available approaches. The same way an inadequate mathematical formalism impedes our ability to formulate problems and prevents us from developing clean solutions, an inappropriate information-sharing paradigm hampers our ability to develop powerful interfaces and efficient solutions to distributed robotic applications. This chapter describes the Network Data Delivery Service (NDDS) [124, 123]–a framework to communicate and distribute data tailored to distributed control applications–its role within the overall system and its relation to other research in the field. This communications layer was developed to support the information-interfaces described in Chapter 3. From its research origin, NDDS has evolved into a stable software package that has been released commercially [145]1 . In the remaining of the chapter, the following topics will be addressed: 1

Appendix H contains a selection of pages from the NDDS manual.

78

CHAPTER 4. COMMUNICATIONS IN DISTRIBUTED ROBOTIC SYSTEMS

79

1. Identification of the unique communication requirements of distributed control applications. Complex distributed robotic systems require sophisticated dataflow. The unique characteristics of the data exchanged in this context needs to be carefully identified in order to develop an approach specifically tailored to these applications. 2. Use of a publish/subscribe paradigm to model the information exchange. Periodic data exchange is quite common in distributed robotic systems (e.g. sensory updates). This data is commonly needed with minimum time delay. In this context, the publish/subscribe model provides a more efficient mechanism than client/server models because (1) a single subscription request replaces the continuous stream of client requests resulting in increased bandwidth and (2) the data exchange is initiated at the source of the data and can be synchronized with the availability of the data, hence arriving to its destination with minimum delay. The publish/subscribe paradigm is also more appropriate for event notification because it avoids the need for continuous event polling. 3. NDDS’s support for multiple anonymous producers and consumers. In distributed robotic systems there are often multiple sources of otherwise identical data. A communications system must provide mechanisms for the end-user application to specify how to resolve such conflicts. In this respect NDDS characterizes data with a strength (priority of the data) and persistence (period of time during which the data is valid). These parameters can be specified by the user application for each data-item. Similarly, support for the information interfaces described in Chapter 3 requires the system to send information to multiple subscribers simultaneously without additional programming effort. 4. NDDS’s realistic model of time. NDDS allows explicit specification of custom update rates, deadlines and the actions to take if a deadline is missed. Within NDDS all data is time-tagged and decisions are made based on the time when the data was generated, sent and/or received. 5. Distributed queries and reliable updates present significant challenges in an environment where any data item may be available from multiple sources and/or go to multiple destinations. NDDS provides mechanisms for the user to customize the semantics of this operations. For instance, the use of NDDS’s wait and deadline parameters provides return best, then first query semantics, which offer a continuum between the extremes of return first and return best.


80

6. Robust architecture. NDDS’s implementation is totally symmetric and quasi-stateless, absent of central servers or privileged nodes. All communicating peers are identical and use data-aging to decay the any cached state. All information regarding subscriptions and productions is refreshed periodically. This implementation is very robust to partial failures and communication dropouts. In addition to the above, Section 4.5 provides a characterization of NDDS’s performance relative to underlying (baseline) transport protocols such as UDP/IP and TCP/IP and a comparison of NDDS’s publish/subscribe model with the client/server model provided by RPC2 .

4.1 The Role of Communications Within the System

Network Data Delivery Service

Robot System 3 4

User-Interface

2


Simulated Robot

Figure 4.1: Architecture of the Two-Armed robotic workcell. The main subsystems communicate using three information interfaces (represented as connectors) that are built on top of NDDS.

Chapter 3 presented the system architecture and subsystem interfaces. In that chapter, the concepts of anonymous and customizable interfaces were also presented and their advantages described. The overall architecture is shown again in figure 4.1. Rather than building each information interface from scratch, it is much simpler to develop a communications layer that supports the data-flow used by the interfaces. This layer must be flexible enough to support the different categories of information-interfaces described in Chapter 3. The design goal for NDDS was to provide a natural model and mechanism for dealing with subsystem distribution in the context of real-time applications. No attempt is made to completely 2

Remote Procedure Calls.


81

hide fundamental issues arising from subsystem distribution such as timing, delays, multiple sources of data etc. Instead, NDDS strives to provide mechanisms to deal with these issues in a natural way so that interfaces built on top can exploit these mechanism while being sheltered from the complexities of network communications, data formatting, addressing, data localization, conflicting data sources, etc. The challenges therefore encompass design and implementation of the appropriate mechanisms.

Requirements of Distributed Robotic Systems As an application domain, distributed robotic applications have unique requirements that set them apart from other domains:

Data exchange is often time-critical. For control purposes data must transfer from source to destination with minimum delay.

Computations are often data-driven, that is, triggered by the arrival of new data. For instance, a collision-avoidance plan may need to be re-evaluated as soon as a new obstacle is detected.

A significant portion of the data flow is repetitive in nature. This is true of sensor readings and motor commands. For this type of data, data loss is often not critical. Moreover, sending data is an idempotent operation and new updates simply replace old values. Considerable overhead can be avoided using a data transfer paradigm that exploits these facts.

There are often multiple sources of (what may be considered) the same data item. For example, a robot command might be generated by a planner module as well as a tele-operation module. Similarly there can be many data consumers. A robot and a simulator are both sinks of “command-data.” The network of data producers and consumers may not be known in advance and may change dynamically.

Data requirements are ubiquitous and unpredictable. It is often difficult to know what data will be required by other modules. For instance, force-level measurements—normally used only by a low-level controller—may be required by a sophisticated high-level task planner in the future. The architecture should support these types of data flow. Thus, vital data should be accessible throughout the system.

Most data flow can and should be anonymous. Producers of the sensor readings can usually be unaware of who is reading them. Consumers may not care about the origin of the data they


82

use. Hiding this information increases modularity by allowing the data sources and sinks to change transparently. In addition, the communication system must be able to operate in readily available hardware and be portable across several architectures and operating systems (for instance a real-time platform for closed-loop control, a graphical workstation to provide a user interface, and compute engines to perform computation-intensive operations such as planning). As computers evolve, we want to maintain the freedom to select the most appropriate platform for each application. The Network Data Delivery Service has been developed to address these unique requirements. NDDS provides transparent network connectivity and data ubiquity to a set of processes possibly running on different computers. NDDS allows distributed processes to share data and event information without concern for the actual physical location and architecture of their peers3 . NDDS allows its “clients” to share data in two ways: subscriptions and one-time queries. NDDS uses the “publish/subscribe” model, supports multiple information sources (producers) and users (consumers). It provides clear semantics for multiple-producer conflict resolution, provides support for and guarantees multiple update rates (as specified by the consumers). NDDS’s implementation is nearly “stateless” and internally uses decaying state to ensure inherently robust communication. Aside from this research, several other projects are using NDDS including an underwater robotic vehicle [195], and a self contained, two-armed free-floating robot originally described in [189].

4.2 Literature Review: Communications in Distributed Systems When comparing the different approaches to distributed connectivity, it is necessary to examine both what is provided as well as how it is provided. The what refers to the functionality of the system and the resulting interaction model apparent to the user. The interaction model affects the assumptions the user can make about the reliability, ordering and coherence of the information as seen by the different entities that participate in the exchange. The how refers to the implementation aspects, that is the architecture and support mechanisms. Buried within the how is the where. Computer systems are highly layered; the operating system, high-level languages, tool-sets and mediators shelter the applications from the actual computer and communications hardware. Distribution-hiding can be performed on any of the layers. As a general rule, the lower the layer, the more efficient and 3 NDDS is being used to communicate between Sun, HP, SGI and DEC workstations as well as VME-based real-time processors running the VxWorks operating system.


83

transparent the communications can be. Unfortunately this comes at the expense of flexibility and portability.

4.2.1

The applications’ view of the information exchange

Independent of the actual implementation, the communications framework provides a model of the information exchange to the applications that communicate using the framework. The success of a specific model depends on how natural and efficient it is for a given application. While no model can be appropriate for all applications, several have had widespread acceptance: The Shared Memory model

presents the illusion that there is a global (shared) memory where

data shared by different applications is stored. Communication occurs by writing into and reading from the global memory. In the shared memory abstraction any changes must be seen seen simultaneously and consistently by all the peers. This is a powerful and familiar model because it allows the application to treat the system as if it was not distributed. Algorithms and programs developed for non-distributed systems can therefore run in a distributed environment without changes. The attractiveness of this model has generated substantial research [88, 117, 89]. Simulation of a true physical shared memory on a distributed system can only be achieved by the operating system. Several research operating systems: Amoeba [53, 183], the V kernel [31], provide these facilities. The problem is that this model cannot be implemented efficiently without special hardware support [198]. As a result the model has been relaxed by increasing the granularity of the shared elements. In the shared data-object model, shared data is encapsulated in objects or other structures and accessed through operations on those objects. This abstraction can be provided by the operating system as in Clouds [46, 40, 11], Chorus [149], by a language and run-time system as in Linda [3], Eden [53, 9], and by dedicated environments such as Distributed Blackboards [61, 122]. Despite its familiarity, the shared memory model is not a natural paradigm for many applications. It is difficult to develop event-driven applications, notice changes in data and synchronize or wait for those changes. Constructs such as semaphores provide some of this functionality, but short of continuous polling, there is no mechanism for ensuring that no changes have occurred to specific data. In addition, the requirement that all participants maintain a consistent view of memory makes memory updates expensive and unpredictable in their delay (remote copies may have to be invalidated). This is inappropriate for real-time applications.


84

Request/Response model. In this model information exchange occurs by issuing requests and waiting for corresponding responses. Examples of such exchanges are the client/server model and the remote invocation model. The client/server model is asymmetric and intended for master/slave type requests. A client (master) issues a request and waits for the response from the server. The server (slave) is normally idle waiting for requests from the clients. This model is very popular and has been used to implement file systems [178], window systems [44], and distributed databases among other things. Suites of protocols and supporting software have been developed, notably Remote Procedure Calls (RPC) [17, 107]. However the client/server model has several drawbacks. For example, the client blocks while waiting for the response precluding the client from issuing concurrent requests or even taking advantage of the time lapse until the response arrives. Also, two messages are required to receive data increasing the latency and diminishing throughput of the information flow, especially when communication delays are significant. It is also difficult for a server to notify a client of events (which may be useful to provide partial answers/results or status/progress information). These shortcoming have been recognized and addressed by augmenting the basic client/server model with the introduction of Asynchronous RPC [10] and related mechanisms such as Futures [192], Promises [93], Upcalls [33], Maybe-RPC [203], Sun’s Batched-RPC [107]. The remote invocation model extends object-oriented programming to a distributed environment. Objects interact by exchanging messages unaware of their physical location. In this exchange, the object-to-object relationship is symmetrical (the sender and receiver roles are specific to each invocation). This abstraction can be supported by the operating system as in Sun’s DOE4 [181], Amoeba [183], Chorus [149], or at the language level as in Emerald [140, 76]. To locate an object or a service, a run-time system (or the operating system) must maintain a directory of the available services, objects and/or interfaces. To communicate between different machines and applications from multiple vendors, standard object interfaces, data-representations, and invocation methods must be used. Several standards such as Sun’s RPC and XDR5 [107, 108] and the Object Management Group’s CORBA6 [121] are in use. Despite the power, buried within the client/server model, is the concept of peer-to-peer exchanges where one-request is followed by its response. For this reason, the client/server model 4

Distributed Objects Everywhere, an initiative of Sun Microsystems. XDR stands for eXternal Data Representation and is a computer-independent format to represent data. XDR was originally developed by Sun Microsystems but has since become the industry standard. 6 Common Object Request Broker Architecture. 5


85

is inappropriate for applications where most of the data flow is one-way, one-to-many and where computation is data-driven. Publish/Subscribe model. In this model, information providers publish (advertise) the information they can provide. Consumers subscribe to the information they require and receive updates from the providers at the discretion of the sender. The consumer only needs to contact the producer if its information needs change. This model offers significant advantages in situations where data transferred corresponds essentially to time-changing values of an otherwise continuous signal (such as sensed data or control signals). A single subscription replaces a continuous stream of requests. Moreover the data is transferred with minimum delay since the exchange is one way and synchronous with the availability of new data. The publish/subscribe model is also notification-based. This is very beneficial when a system needs to monitor a great number of perhaps infrequent events. In a client/server or implicit model, the monitoring process must continuously poll for possible changes. One-to-many communications are easily supported because the sender decides when to send data and can take advantage of multi-cast and broadcast mechanisms. Publish/subscribe models have recently been used for financial applications [175], command and control systems [19, 48, 201, 177] and desktop tool communications [180, 181].

4.2.2

Implementation aspects

In general, all layers from the operating-system up to the application are involved in supporting the application’s data exchanges. However, the decision as to which layer takes responsibility for hiding the distributed aspects from the layers above has deep consequences: Operating Systems

provide the basic communication services used by the applications to com-

municate among distributed computers. Distributed Operating Systems such as the ones described in [183, 113, 53], can hide the distribution from the applications and provide any of the information exchange models described before. For example Chorus [149, 71] and Clouds [46, 11] provide distributed virtual memory, while Amoeba [183], Argus [173], the V kernel [31], OSF7 DCE8 [153], Sun’s DOE [181], provide transparent access to remote objects and services. To date no distributed OS supports the publish/subscribe model directly. The use of the OS to hide the distribution forces 7 8

Open Software Foundation. A consortium of several computer vendors. Distributed Computing Environment.


86

all the applications to be running the same OS. This is inappropriate in situations where different operating systems must be intermixed9. Computer Languages

provide another level of interaction with a computer system. Language

constructs and run-time systems can be used to provide distributed connectivity [14, 32]. Languages such as DOWL [2], Emerald [90, 140], and Linda [3] offer the advantage of special constructs to make interaction between distributed applications natural (language constructs can be tailored to the specific communications model). The language approach is more portable and flexible than the OS approach because it can be implemented on top of multiple operating systems. One disadvantage is the requirement of porting all applications to use the same language. This makes it unsuitable for many applications such as the one in this thesis because none of the aforementioned languages has achieved widespread use and the corresponding development tools (compilers, debuggers etc.) do not exist for many computer architectures. To the author’s knowledge, none of the language approaches support the publish/subscribe model. Mediators and Run-Time Systems.

Run-Time systems which may contain active mediators

(brokers, agents, daemons) can be used to provide yet another layer on top of OS and languages. This approach is the most flexible and portable since a variety of operating systems and languages can be supported. Typically the applications are linked to interface libraries that use mediators and run-time systems to facilitate information exchange. This is the approach followed by NDDS and all systems that support the publish/subscribe model including TCX [48], TelRIP [201, 202], the Teknekron Software Bus [175], SPLICE [19], and CRONUS [15]. This is also the approach used by most implementations of the CORBA specification.

4.2.3

Review of Related Approaches

The following review will be restricted to approaches that use mediators and run-time systems to support the communications. There are two reasons for this restriction: First these are the most portable approaches, the only ones that can run on a variety of hardware and software platforms10. Second, these are the approaches most closely related to NDDS. 9

For instance, the experiments in this thesis require the flexibility of a Unix workstation for the planning and human interface subsystems, and also require the predictable performance of a Real-Time Operating System running on dedicated processors to control the workcell hardware. 10 This requirement is imposed by the robotic workcell experiment


87

The Common Object Request Broker Architecture (CORBA) is a specification [121] developed by the Object Management Group (OMG)11. CORBA provides a mechanism for languageindependent definition of objects and network-transparent communications between objects. The main objective is to interconnect objects supplied by different vendors. It specifies both a language in which objects can be described in terms of their interfaces (IDL12), and a general architecture and set of facilities for accessing the objects that must be provided. CORBA does not specify any implementation and therefore a wide variety are possible (e.g. library based, agent based, use of a central server, implementation as part of the operating system, etc.). CORBA differs from NDDS in that the paradigm for object interaction is request/response (not producer/consumer)—CORBA requires specification of the (unique) object to which a message is sent. Unlike NDDS, CORBA does not provide facilities for multicast nor multiple producers and consumers. Also, CORBA does not specify any mechanism for “notifications” which are critical for event-driven systems. Still, it would be possible to develop a CORBA compliant implementation providing some of NDDS’s facilities. For example Sun’s DOE [181] event management facility provides notifications and ORBIX13 [69, 70] offers support for multiple operating systems. To date no CORBA implementation provides real-time facilities similar to those of NDDS. POLYLITH

was developed at the University of Maryland [136] as a tool to support and configure

applications composed of modules running in a distributed system. Like CORBA, POLYLITH defines only the interfaces (using a custom language called MIL) between different modules. The actual implementation is not specified and can use a variety of communications mechanisms. POLYLITH is very different in philosophy to NDDS, it is primarily a tool to configure static communication channels and distribute modules among the available processors; In POLYLITH the user must specify the topology of the module connections and processor assignments in a configuration file prior to running the system. It offers no support for subscriptions, custom update rates, nor multiple producers. CRONUS is an environment for distributed programming developed by BBN Systems and Technologies [15]. CRONUS allows transparent access (creation and invocation) of objects in a distributed environment. The goals of the system are similar to those of CORBA. CRONUS has been 11

The OMG is an industry consortium with the participation of several computer vendors such as Sun, HP, IBM and DEC 12 Interface Definition Language 13 A CORBA compliant product of IONA technologies.


88

used primarily to provide access to remote databases. CRONUS provides facilities for describing objects (using their own interface specification language from which stub code is automatically generated), replicating objects, and authenticating requests. The interaction model is request/response, but the use of “Futures” and “Future-sets” [192] allows multiple outstanding requests and out-oforder responses. Routines can be attached to Futures to provide a functionality similar to NDDS’s notifications. CRONUS goals differ from NDDS. The emphasis is in allowing secure remote access to services (hence the request/response model, and authentication facilities), not in providing data distribution. An invocation is logically directed to a single server (multiple services are supported only for reliability and availability). When more than one server is present, they are expected to perform identically and a voting mechanism is used to resolve conflicts. The Data Manager (DM). Was developed at MBARI14 [103] to provide connectivity between applications running on top of the VxWorks operating system . Applications using the DM must declare themselves as either producers or consumers of data items. A data item is a named block of memory. Data items may have at most two producers (a regular one and an extraordinary one) and any number of consumers. Consumers may specify the update rate and a semaphore to be signalled when the update arrives. A data connection facility is provided allowing ’dynamic aliasing’ of data items (a data item becomes the source for another one, when the first item is updated the consumers of the second item are also updated). Actual communications occurs using TCP/IP and UDP/IP15. The DM is very similar in its philosophy to NDDS. It has, however, several limitations overcome by NDDS. Data items are simply a continuous memory buffer. This offers no data-type support, type checking nor external (machine independent) data representation; resulting in additional work for the user and increased error risk. “Regular” and “extraordinary” producers offer very limited support for multiple producers: only two producers are allowed. Moreover, each producer needs to be aware of its respective role. For instance, if the “extraordinary” producer fails, there is no mechanism to provide an alternative producer for the data. The DM has no support for consumer deadlines, queries nor reliable updates. The DM is only available for the VxWorks operating system. 14

Monterey Bay Aquarium Research Institute. TCP is the Transmission Control Protocol, UDP is the User Datagram Protocol, and IP is the Internet Protocol. TCP and UDP are transport level protocols. IP is a network-layer protocol. These protocols are the ones used by the INTERNET, and are supported by most operating systems. 15


The Task Control Architecture (TCA)

89

developed at CMU16 [171] has a communications layer

called TCX [169, 48] that provides distributed connectivity using the publish/subscribe model. This architecture runs on a variety of Unix and VxWorks platforms and uses a central-server responsible for setting up all communications. TCX uses TCP/IP for communications. The communications layer was originally developed to support TCA, a task-decomposition planner that has been used to control several robotic vehicles [85, 170]. In TCX, all applications maintain static connections with the central server where they register their interests and productions. Producers name the consumers explicitly but consumers can receive from any producer. A queue of messages is maintained by the receiver. There is only a single producer and consumer of each item. TCX supports typed data using self-describing types that are used to perform serialization and deserialization at each end. TCX differs from NDDS in its use of a connection-oriented protocol (TCP/IP) that establishes and maintains connections, the existence of a central server, the need for producers to explicitly name consumers and, the lack of support for multiple producers, consumers, and multiple update rates. The Teknekron Software Bus (TIB)

from Teknekron Software Systems Inc. [175] was developed

primarily to facilitate data dissemination for financial market applications. TIB is based on the publish/subscribe model and uses a hierarchical naming scheme so that related information may be grouped (subscriptions can be made to any data in the group). Data updates are self-describing using a custom data representation and sent using reliable datagrams. The distribution scheme is based upon “Active Repositories” that get periodic updates from producers and relay the information to all subscribers. These repositories can be placed as gateways to dedicated nets in order to increase fan-out and bandwidth of the dataflow. TIB differs from NDDS in its support for hierarchical naming and use of intermediate agents (the repositories) to increase fan-out. It has been optimized for throughput to many (thousands) of consumers, not for latency with is increased by the intermediate repositories. TIB has no support for best-efforts delivery, multiple update rates, deadlines or multiple producers. TelRIP17

is an architecture developed at Rice University [54, 201] to communicate and share data

in distributed telerobotic applications. TelRIP uses a fully distributed architecture without central nodes. Each network node contains a dedicated daemon process to serve as a gateway. These daemons are connected to each other using TCP/IP and route all the messages between applications. 16 17

Carnegie Mellon University. Tele-Robotic Interconnection Protocol a.k.a. "Inter Agents".


90

This approach yields a very scalable architecture but increases communication delays. TelRIP does not support deadlines, custom consumer rates, best efforts delivery or multiple producers. SPLICE18

was developed at Hollandse Signaalapparaten [19] to provide the communications

backbone for command and control applications. In SPLICE applications refer to data using unique identifiers. Data is typed and is declared using SPLICE’s own Pascal-like language. Updates are self-described (the type of the data is sent along). The actual communications are handled by SPLICE agents tied to each application. SPLICE provides a mechanism that allows specific fields within a data-type to be designated as keys; consumers can choose to treat updates that differ in their keys as completely different items (stored and retrieved independently) or just different updates of the same item (only one the latest copy is stored). SPLICE also contains a mechanism similar to NDDS’s for arbitrating among different producers of the same data. Each producer specifies the quality and persistence of its updates. SPLICE is quite similar to NDDS in its philosophy and goals but has significant implementation differences: SPLICE’s key-based hierarchical naming is more flexible than NDDS’s flat-name scheme, but otherwise, the actual data instances (identifiers) produced and consumed by each SPLICE application must be known and specified at compile time using SPLICE’s language. SPLICE offers no support for multiple update rates or deadlines and consumers must poll for updates (where NDDS uses notifications)—a more restrictive and inefficient mechanism. Others.

Many other researchers have recognized the need for a network communications layer

allowing both data-transfer and event-signalling, many of these approaches have been tailored to specific applications or embedded within an architectural model.

Examples of this are

APHRODITE [185] and MICA [134].

4.3 The NDDS Communications Model The NDDS system builds on the model of information producers (sources) and consumers (sinks). Producers register a set of data instances to be produced, unaware of prospective consumers and “produce” the data at their own discretion. Consumers “subscribe” to updates of any data instances required without concern for who is producing them. In this sense the NDDS is a “subscriptionbased” model. The use of subscriptions drastically reduces the overhead required by a client-server 18

Subscription Paradigm for the Logical Interconnection of Concurrent Engines.


91

architecture. Occasional subscription requests, at low bandwidth, replace numerous high-bandwidth client requests. Latency is also reduced, as the outgoing request message time is eliminated. Function NddsProducerCreate NddsProducerAddProduction

NddsProducerSample

NddsConsumerCreate NddsConsumerAddSubscription

NddsConsumerPoll

NddsInstanceQuery

Action Create and specify producer parameters Add an item to the list of productions published by the producer Take a snap-shot of all the items produced by the producer. For immediate producers also send updates to all consumers. Create and specify consumer parameters Add a subscription to a consumer. Specify a call-back routine to be called when updates arrive Poll the consumer for updates. Will result on call-back routines being called when applicable. Required only of polled consumers. Issue a one-time query.

Table 4.1: Functional interface to produce, consume and query data. Producing data involves three phases: Creating (declaring) a producer, declaring the instances the producer will publish (produce) and sampling the producer. Receiving data updates involves two (possibly three) phases: Creating (declaring) a consumer, declaring the instances that the consumer subscribes to along with the action to be taken when an update arrives and optionally (polled consumers only), polling for updates.

NDDS identifies data instances by name (their NDDS name). The scope of this name extends to all tasks sharing data through NDDS. Two instances with the same NDDS name are viewed by NDDS as different updates of the same data instance and are otherwise indistinguishable to the client. If two data instances must be distinguished by any NDDS client, they must be given a different NDDS name. NDDS requires all data instances to be of a known type. NDDS has some built in types (such as strings and arrays) but most data flow consists of user-defined types. Creating a new NDDS type


92

involves binding a new type-name with the functions that will allow NDDS to manipulate instances of that type. NDDS provides a tool nddsgen that can be uses to automatically generate code for user-defined types from the type-specification given in the XDR language (see Appendix H for details). NDDS treats producers and consumers symmetrically. Each node maintains the information required to establish communications. Producers inform prospective consumers of the data they produce. Consumers use this information to either subscribe to data or issue one-time queries. Table 4.1 lists the steps involved in becoming a producer or consumer of data.

4.3.1

Producer Characteristics

A producer can be compared to a multi-channel Sample-and-Hold. It is associated with a set of object instances (similar to the signal channels) that are sampled synchronously. Sampling a producer takes a snapshot of the values of each data item the producer has associated with it. Depending on the type of producer, the data is either immediately distributed or sent at a later time by a helper agent. NDDS supports three types of producers: asynchronous, signalled, and synchronous: Asynchronous producers send the updates immediately after the producer is sampled (in the context of the task calling NddsProducerSample()). This achieves minimal latency but slows down the sampling task which may be unacceptable for certain critical real-time tasks. Signalled producers differ in that the task sampling the producer does not perform the network transaction, instead it signals a slave task to do the network transaction on its behalf. This achieves almost the same latency as the asynchronous producers case but doesn’t slow down the producer task. Synchronous producers are similar to signalled producers in that the network transactions are performed by a daemon in behalf of the producing task, but there is no signalling of the daemon. Instead the daemon collects all the updates together and sends them synchronously at a certain rate (provided there is something to be sent). Synchronous productions can potentially achieve higher bandwidth than the other types, because the daemon updates from different producers can be grouped together into single messages. A producer is characterized by three parameters: production rate, strength and persistence. The strength and persistence parameters are used to resolve multiple-producer conflicts. Their meaning


Accept only updates of higher strength

93

Accept any update

time

last update

persistence Figure 4.2: Multiple producer conflict resolution.

NDDS resolves the multiple-producer conflict by characterizing each producer with two properties: the producer’s strength and its persistence. When a data update is received for some object instance, it is accepted if either the producer’s strength is greater (or equal) to that of the producer of the last update for that instance or, the time elapsed since the last update was received exceeds the persistence of the producer of the last update. In essence the strength is similar to a priority and the persistence is the duration for which the priority is valid.

is illustrated in Figure 4.2. A producer’s data is used while it is the strongest source that has not exceeded its persistence. Typically, a producer that will generate data updates every period T will

set its persistence to some value Tp where Tp

> T . Thus, while that producer is functional, it will

take precedence over any producers of less strength. Should the producer stop distributing its data (willingly or due to a failure), other producers will take over after Tp elapses. This mechanism establishes an inherently robust, quasi-stateless communications channel between the strongest producer of an instance and all the consumers of that instance.

4.3.2

Consumer Characteristics

Consumers are notification based. They subscribe to a set of instances (identified by their NDDS name) by providing call-back functions for each instance to which they subscribe. When a data update arrives, the call-back function of every consumer is called with the data-item as a parameter. Two consumer models are currently supported: immediate and polled. An immediate consumer will be called back as soon as the data update arrives. A polled consumer will not be called back until it itself “polls” for updates. Consumers are characterized by two parameters, the minimum separation and the deadline (see Figure 4.3). These parameters are used to regulate consumer update rates. Consumers are guaranteed updates no sooner than the minimum separation time and no later than the deadline.


94

Consumer always notified

time

Consumer never notified

Consumer notified if new updates

minimum separation last notification

deadline Figure 4.3: Consumer notification rate.

The NDDS characterizes consumers requests for periodic updates with two properties: the consumer’s minimum separation time and its deadline. Once the consumer is called with an update for an object instance, it is guaranteed not to be notified again of the same instance for at least the minimum separation time. The deadline is a the maximum time the consumer is willing to wait for a new update. Even if new updates have not arrived, the call-back routine will be called when the deadline expires.

Typically the minimum separation protects the consumer against producers that are too fast whereas the deadline provides a guaranteed call-back time which can be used to take appropriate action (the expiration of the deadline typically indicates lack of producers or communications failure).

4.3.3

One-time Queries

A client task may issue one-time queries for specific NDDS data instances. Queries are blocking calls. Aside from specifying the name and type of the NDDS data instance, a query contains two parameters: the wait and deadline illustrated in Figure 4.4. These parameters regulate the tradeoff between returning as soon as data becomes available and waiting for “better” data19. The use of these parameters makes the latency of this call predictable, allowing its use from real-time application code. Typically the wait is set to be long enough to account for communication delays from all producers to the consumer. The deadline provides a guaranteed call-back time in case no responses arrive. Setting a wait time to zero causes the first response to be accepted (accept first). In other words, these two parameters provide accept best, then first semantics to distributed queries. This semantics offers a continuum between the accept first and accept best semantics common to other systems. 19

i.e. data from a stronger producer.


Return best update (if any) Never return (blocked)

95

Return failure

Returns any update received

time

wait query issued

deadline

Figure 4.4: One-Time Query Parameters. A one-time query specifies two parameters: the wait time and the deadline. A query will block for at least wait time. During the wait, data arriving from different producers is received and only the one from the highest strength producers saved. At the end of the wait time the query returns if any data has been received. Otherwise, query remains blocked until either an update comes or the deadline expires (whichever comes first).

4.3.4

Reliable Updates

By default NDDS provides unreliable, unacknowledged data updates from producers to consumers. While this gives the most throughput with minimum overhead, it may result in occasional loss of updates or out-of-order arrival. For sensory-type data, having the latest data as soon as it becomes available is usually more important than occasionally missing an update. Other kinds of information, such as commands, often present in distributed control systems, would be better served with a reliable protocol. NDDS supports reliable updates. A producer may specify any one of its productions to be delivered reliably. Reliable updates are grouped together in special packets that are individually acknowledged. The producer is notified if the update is not acknowledged by at least one consumer after a specified deadline and/or if another reliable production is attempted before the previous one was acknowledged. This guarantees in-order update delivery at the expense of reduced update bandwidth. A window size larger than one may be specified to compensate for longer communication delays so that multiple reliable updates can be sent before any acknowledgement is received.


96

4.4 Implementation NDDS is symmetrically distributed, that is, there are no “special” or “privileged” nodes or name servers. All NDDS nodes are functionally identical and each node maintains its own copy of the NDDS database and contains the helper processes necessary to implement the communication model described above. NDDS uses UDP/IP [135, 34] as a means of communication. To allow communications between computers with different data formats the External Data Representation (XDR) [108] is used.

4.4.1

Architectural Overview

An NDDS node is composed of one or more NDDS client processes (each with its respective NDDS Server Daemon ) a copy of the NDDS database and three daemon (helper) processes that maintain the database and implement the NDDS communication model described above. See Figure 4.5. The user task becomes an NDDS client by linking to the NDDS library. Each NDDS client process spawns a private NDDS Server Daemon process that will assist in establishing the subscriptions and informing the peer nodes of the productions. There is at most one NDDS node per address space so in operating systems that support shared memory threads (for example VxWorks), several NDDS client processes may belong to the same node (sharing the same copy of the NDDS database and helper daemons20). The following is a functional description of the different daemons:

NDDS Forwarding Daemon (NFD). There is one NFD per network host. All the Request Receiver Daemons running on the host register with the NFD. Production notifications and subscription requests received by the NFD daemon are immediately forwarded to all the Request Receiver Daemon(s) running on the host.

NDDS Server Daemon (NSD). Each NDDS client (user-task) spawns its private NSD. The NSD is responsible for periodically informing all other NDDS nodes of both the subscription requests and the productions of the NDDS client.

Request Receiver Daemon (RRD). There is one RRD per NDDS node. The RRD is responsible for maintaining the remote subscriptions and productions in the NDDS database.

20

In operating systems that do not support shared memory threads, such as Unix, the helper daemons are not independent tasks but rather are installed as signal handlers.


NDDS Node 2

NDDS Node 1

NDDS Server Daemon

Subscription Request / Production Declaration

NDDS Client (User Process)

NDDS Client (User Process) NDDS Server Daemon

NDDS user-library

NDDS user-library

Poll Consumer/ Callback

Declare Productions Sample Produced Data

NDDS database

NDDS database Register Remote Producers/ Subscribers

Deposit Update Update Sender Daemon

97

Request Receiver Daemon

Update Receiver Daemon

Update Sender Daemon

Request Receiver Daemon

Distribute to all Request Receivers in Host

Periodic Data Updates Query (Request/Response)

Periodic Subscription Requests Periodic Producer Information

Update Receiver Daemon

NDDS Forwarding Daemon

Figure 4.5: Communication between NDDS nodes. A NDDS node is composed of one or more NDDS clients and three helper daemons that share a local copy of the NDDS database. Each NDDS client has a private NDDS Server Daemon that informs other NDDS nodes of the productions and subscriptions of the client. The three helper daemons are responsible for maintaining the NDDS database, sending updates to remote subscribers, receiving updates and servicing queries. There is one NDDS Forwarding Daemon per Network host.

Stale productions and subscription requests are aged and eventually dropped by the RRD. This daemon is also responsible for replying to one-time queries from other NDDS nodes.

Update Sender Daemon (USD). There is one USD per NDDS node. The USD is responsible for sending the updates of locally produced data items to the subscribers in other NDDS nodes. This daemon also ensures that the timing parameters requested by the consumer are met.


98

Update Receiver Daemon (URD). There is one URD per NDDS node. The URD is responsible for receiving updates for the local subscriptions of the nodes. The URD solves multiple-producer conflicts and, in the case of immediate consumers, executes the callback routine(s) installed for that data item. The URD also ensures that the timing parameters requested by each consumer in the node are met.

4.4.2

Data Management Overview

The NDDS database is replicated and maintained on each NDDS node by three helper daemons (the Request Receiver Daemon, Update Sender Daemon and Update Receiver Daemon). The database stores and cross references producers and the data they produce, consumers and the data they consume, remote productions, and subscriptions requested by the NDDS clients in both the local and remote NDDS nodes. Consistency between databases across different NDDS nodes is not necessary and requires no special effort. Temporary inconsistencies between databases may result in subscription requests (or queries) not reaching all the producers of a given data item and, as a consequence, different nodes may receive data from different producers. A similar situation may result from the data loss due to communication failure. At worst this will be a transient situation that arises only if there are multiple producers of the same data. All information about remote NDDS nodes is aged and is eventually erased unless it is refreshed. The NDDS Server Daemon associated with each NDDS client is responsible for the periodic refreshment of information that concerns that NDDS client . This mechanism is inherently robust to remote node failures, communication dropouts and network partitioning. Furthermore, it requires no special recovery mechanisms.

4.5 Experimental Results: NDDS NDDS is a high-level service that provides a layer above the transport and network layers. In the OSI reference model [62, 182] NDDS logically belongs in the session and presentation layers. NDDS uses the transport-level facilities provided by the operating system (UDP/IP sockets). As a higher-level protocol, NDDS provides many useful functions: subscription management, networkwide location-transparent address (name) space, multiple-producer conflict resolution, clustering of updates into messages to diminish network traffic, deadlines etc. These facilities add extra overhead over the cost of the raw transport-level message traffic, and therefore, NDDS will be slower than


99

the underlying UDP/IP layer. The significance of these comparisons is to provide metrics for both overhead with respect to the underlying transport layer and expected performance benefits when compared with other higher level protocols such as RPC. In this section, NDDS is compared with a group of four protocols: UDP/IP, TCP/IP, Sun’s RPC and Sun’s Batched-RPC. The four protocols in the group were selected because they are widely available and their performance is well known. This group contains the protocols used by NDDS and all the other approaches reviewed in Section 4.2.3. Therefore, comparing them with NDDS will provide two metrics: (1) the overhead introduced by NDDS and (2) the limits on the performance achievable by any high-level protocol that uses one of the transport-level protocols in the group. NDDS’s overhead stems from several facts:

Extra information transmitted with each update. Each individual message contains extra information indicating the number and identity of the different updates as well as the relevant parameters (strength, persistence) and timing information. This overhead is approximately 50 bytes per message and 50 additional bytes per individual-item update.

Computation to parse update messages and call the appropriate action routines for each update item.

Computation to provide update-rate guarantees and arbitrate among multiple-producers. These services require gathering and processing of several time stamps per update.

Extra data copies to support multiple asynchronous consumers and enable coalescing of updates directed to the same NDDS node.

Nonetheless, NDDS provides more than increased functionality at some performance cost, in cases where the characteristics of the data exchange match NDDS’s model, as is the case on distributed-control systems, this overhead is balanced by the benefits provided by NDDS’s optimizations. Two performance measurements are critical when sending updates within distributed control applications: (1) the time delay from data sending to its arrival at the other end (latency) and (2) the fastest update rate at which data can be sent (throughput). In control applications, latency is important because it contributes to the phase lag in measurements and control commands impacting stability. Update rate is important because it affects the rate at which control loops can be closed, impacting achievable performance.


4.5.1

100

Experimental Context

All times shown in this section correspond to measurements taken on a network of Sun workstations. Although NDDS was specifically designed (and is commonly used) to communicate with computers running networked real-time operating systems such as VxWorks [200], the data presented here does not involve any real-time nodes. These computers are not as widely available and therefore the comparative results would be more difficult to relate to a familiar environment. All workstations are connected by a 10 MB/s Ethernet LAN . The absolute times and rates are obviously dependent on the actual computer hardware (both speed of the computers and network). The real significance of the data arises when viewed as a comparison with the underlying transport delay. The relative performance is likely to remain more constant when faster hardware is used. Unless otherwise specified, the producer of all updates is a Sun Sparcstation-10-51 (SS-10) computer, the consumers are mostly SparcStation IPX computers. In the few experiments requiring more than four computers, SparcStation IPC (SS-IPC) computers were also used. The SS-10 is approximately 3 times faster than the SS-IPX21 , which itself is about twice as fast as the SS-IPC. All computers are running SunOS 4.3.1. In the plots in this section, the adjective “fast” is used to denote a SS-10 computer (or similar) while “slow” denotes and SS-IPX or SS-IPC. NDDS uses XDR to encode all data in a computer-independent fashion. The cost of XDR encoding varies widely across computer systems (e.g. it is essentially free if the internal machine representation matches XDR’s representation). To prevent this extra layer from obscuring the main issues, bulk data sent in all the messages is untyped (i.e. requires no encoding). However, NDDS sends some extra information with every update (about 50 bytes per message + 50 bytes per update) which is subject to XDR encoding. All the data taken in this section requires comparing time-stamps taken in different computers. To do this meaningfully, the respective clocks must be adequately synchronized. Although there are standard time-synchronization protocols available, notably NTP [110], measured accuracy is on the order of 5 msec, while sub-millisecond resolution is required. Moreover, even if the computers were

synchronized at a given point, the time drift between machines is so large that the sub-millisecond accuracy would be lost in a few seconds. A time-synchronization utility was developed to solve this problem that allows computation of inter-machine time offsets with accuracy on the order of 100 sec under programming control. All tests in this section use this utility to re-synchronize and measure clock drift with sufficient frequency to bound each measurement error down to acceptable 21

The SS-10 performance is 66 SPECint (136 MIPS), the SS-IPX performance is 21 SPECint (28 MIPS).


101

levels (typically 100-200 sec). These errors are accounted for in the error bars shown in the figures and represent the standard deviation of a large (typically 100) set of measurements.

4.5.2

Latency of the Updates

The data presented in this section will show that NDDS’s overhead pays off when either the receiving computer is at least as fast as the sending one, or else when several (in this case two or more) consumers want the data. End−to−end delay for updates of a single item

update delay (seconds)

0.015

0.01

10 M bps ethernet NDDS 0.005

0 0

RPC

TCP

UDP

1000

2000

3000 4000 5000 update size (bytes)

6000

7000

8000

Figure 4.6: Latency overhead of NDDS as a function of item size. This figure shows the total one-way delay for a single data item sent over the network from a single producer to a single consumer. The delay resulting from the raw communications bandwidth of the 10 Mbps Ethernet is shown hashed for reference. This figure shows that the overhead introduced by NDDS ranges from 1 ms for the smallest items to 2 ms for the largest ones. Overhead increases with packet size because of the extra copy performed by NDDS. This is a worst-case scenario for NDDS because it has been optimized for situations requiring multiple updates to several clients.

The first important metric for data exchange in distributed control applications is the latency or delay in the data from the time it is sent to the time it is received. For a given computer environment, this delay is a function of many parameters including data size, number of items sent, and number of hosts receiving (subscribing to) the updates. A detailed exploration of this four-dimensional space


102

is beyond the scope of this section. Instead, several two-dimensional cuts will be examined. In these cuts, two parameters are kept fixed and the delay is measured with respect to the remaining parameter. Description of the experiment and data presentation.

For all the measurements in this section

the following simplified model is used: There is a single producer and Nc consumers, all subscribing

to the same Ni items; each item is of size Nb bytes. The objective is to characterize the delay function

d = d(Nc; Ni; Nb).

The experiment consists of sending the Ni items from the producer to all Nc consumers. The

total delay from the time just before the first update is sent to a consumer, to the time after the last update arrives to that consumer, is measured (by time-stamping the items and correcting for clock

offsets). For example, for Ni = 100, the delay d(Ni) represents the time lapsed until the arrival of the 100th item when the total number of items is exactly 100. In general, this will not be the same as say, the delay for the 100th item in the sequence when 160 items are being sent. Each experiment is repeated many times (100 is a typical number) and the mean and standard deviation of the delay is shown as it varies with respect to the different parameters. Influence of update size. Figure 4.6 illustrates NDDS’s latency overhead for a single item sent from a producer to a consumer as a function of the size of the item. This delay represents the minimum latency or phase loss for a single data transaction and illustrates NDDS’s added overhead for a single item. In the case of RPC, the delay corresponds to that of the one-way message from the client to the server. Since only one-way communication of a single item is involved, all transport protocols compared have similar performance. In many ways, this is a worst-case scenario for NDDS, because it has been optimized to efficiently send multiple updates to several clients. The overhead incurred in support of this objective, is most damaging when there is a single data-item to be sent, because of the fixed costs incurred regardless of the number of updates. However, these measurements indicate that the overhead of NDDS is small, ranging from 1 ms for the smallest items to 2 ms for the largest ones. Overhead increases slightly with packet size because of the extra copy performed by NDDS. Effect of number of items requested

Ni .

Figure 4.7 illustrates NDDS overhead versus the

number of items requested by a single machine, for two different item sizes. In this example, the

producer sends Ni individual items to the consumer. The delay from the time the sender sends the


103

End−to−end delay for sequence of 500 byte updates

End−to−end delay for sequence of 100 byte updates 0.12

0.06

0.1

0.05

0.08

0.04 RPC

delay (seconds)

delay (seconds)

NDDS

NDDS

0.06 TCP UDP

0.04

RPC

0.03

Batched RPC

UDP

0.02

Batched RPC

0.01

0.02

0 0

20

40

60

80 100 number of items

120

140

160

0 0

10

20

30 number of items

40

50

60

Figure 4.7: Latency overhead as a function of the number of items This data represents total communications delay for a series of items sent from a single producer to a single consumer. This figure shows that NDDS’s overhead increases linearly with the number of items sent. This is a result of the fact that the receiver must perform a lookup to identify the appropriate consumer/action routine for each individual update. This is only noticeable when a fast producer sends to a slow consumer (in this case the sending computer is 3 times faster than the receiving one) and when there are few consumers of each update. This remarks are substantiated by Figures 4.8 and 4.9. RPC’s low performance is due to its client/server semantics requiring each individual update to be acknowledged before the next one can be sent.

first item until the time the last item is received at the consumer end is plotted as a function of the total number of items sent. The RPC, TCP, UDP and Batched RPC implementations send each item individually in rapid succession. Batched RPC is quite efficient because it pipelines individual messages without acknowledgment. After the last update is sent, a normal RPC is performed to flush the pipeline (to ensure the messages buffered in the pipeline are sent). UDP is also efficient because its messages are delivered unreliably and require no acknowledgment. In contrast, sending updates with the regular RPC requires acknowledgment of each individual update before the next may be sent, resulting in much lower performance. TCP has similar performance for the 100 byte updates but was not usable in the 500 byte case (i.e. its performance was so bad that it fell outside the range shown in the figure) because of the extra burden imposed by reliable delivery. The slope of the curves in Figure 4.7 represents the marginal cost of sending an extra item. NDDS’s higher slope is primarily due to the increased computation involved in processing more updates at the receiving end and is most significant for small item sizes. Each update must trigger a notification of the appropriate installed callback function that imposes extra burden per update. In all cases its performance is within a factor of two of the fastest underlying transport protocol. Especially


104

remarkable is the poor performance (large delays) obtained using RPC. RPC’s poor performance is due to its client/server semantics that require each update to be fully acknowledged before the next one can be sent, forcing sequential computation at the client and server sides.

delay (seconds)

End−to−end delay: 100 byte updates to machine 1 (slow, same LAN) 0.1

RPC

0.08 0.06

NDDS

0.04

UDP

0.02

Batched RPC

0 0

10

20

30

40 50 60 number of items

70

80

90

100

delay (seconds)

End−to−end delay: 100 byte updates to machine 2 (fast, across LAN bridge) 0.1 0.08 RPC

0.06

NDDS, UDP, Batched RPC

0.04 0.02 0 0

10

20

30


70

80

90

100

Figure 4.8: Latency overhead as a function of the number of items for two consumers Total communication delay for a series of items from a single producer to two consumers. This plot illustrates that NDDS’s overhead is only significant for consumers that are slow compared with the data-producers. The bottom plot corresponds to a computer of comparable performance to the sending machine for which there is no noticeable overhead.

Influence of the number of consumers Nc . Update delay as a function of number of consumers receiving the updates is illustrated in Figures 4.8 and 4.9. Figure 4.8 illustrates the case where two computes are receiving updates. Note that the data sent though NDDS has a higher delay arriving to the slow computer despite the fact that updates are sent first to the slow and then to the fast machine. This corroborates the statement that NDDS’s multiple-update-item overhead is only significant when the receiving computer is slow compared to the sending computer. Figure 4.9 illustrates another important point. Despite the fact that the last two receiving machines are slow, NDDS has no noticeable overhead over the underlying UDP transport layer.


100 byte items, mach. 1 (slow)

100 byte items, mach. 2 (fast) 0.1

0.08

delay (seconds)

delay (seconds)

0.1 RPC

0.06 NDDS Bat. RPC

0.04 0.02

0.08

RPC

0.06 Bat. RPC NDDS

0.04 0.02

UDP 0 0


UDP 0 0

60

100 byte items, mach. 3 (slow)

RPC

delay (seconds)

delay (seconds)

60

0.1

0.08 NDDS Bat. RPC

0.04 UDP

0.02 0 0

20 40 number of items 100 byte items, mach. 4 (slow)

0.1

0.06

105


60

0.08 0.06

RPC

NDDS UDP Bat. RPC

0.04 0.02 0 0


60

Figure 4.9: Latency overhead as a function of the number of items for four consumers Total communication delay for a series of items from a single producer to four consumers. This figure illustrates that even for “slow” consumers, NDDS’s overhead pays off when several consumers (in this case two or more) are subscribed to the same data.

This is not due to the sender becoming a bottleneck22, but rather to the fact that much of NDDS’s upfront overhead is caused by its attempted efficiency when multiple clients are involved (the normal case), and updates sent to each additional computer computer are quite efficient, resulting in delays similar to those of the raw protocols. Therefore, even in the presence of slow receivers, NDDS’s extra computation pays off when several (in this case two or more23) consumers subscribe to the same data.

4.5.3

Maximum update rates

This section compares regular RPC using the client/server model to NDDS (publish/subscribe model) in terms of the fastest achievable update rates. This comparison demonstrates the advantages 22

This is illustrated by the fact that the fast computer has the same delay as in Figure 4.8 where only two computers where receiving data. 23 The number of computers for which this tradeoff occurs will depend on the relative speed of the sending and receiving computers.


106

of the publish/subscribe paradigm over the client/server paradigm for data updates that need to be sent repeatedly. The previous section demonstrated that RPC’s performance degrades significantly with the

number of items requested by each computer Ni . This section will show that even for a very small number of requested items:

Ni = 2 to 10 (i.e.

the most favorable case for RPC), NDDS achieves

updates rates that are a factor of four or greater than RPC, irrespective of the number of hosts Nc

subscribing to the updates. The results also illustrate that the performance advantage of NDDS over RPC diminishes with packet size.

Fastest update rate: 10 items of 100 bytes

3

10

rate (Hz)

NDDS 2

10

RPC

1

10

0

2

4

6 8 10 number of hosts receiving updates

12

14

16

Figure 4.10: Maximum update rate as a function of the number of subscribers. This experiment illustrates that for small data sizes (100 bytes), the (sustainable) update rates achievable using NDDS are a factor of four higher than those achievable with RPC regardless of the number of subscribers Nc .

The dramatic advantage of the publish/subscribe model over client/server stems from several facts:

In the publish/subscribe model, data transactions are initiated by the sender and can therefore be synchronized with the availability of new data (assuming data is produced at a certain


107

periodic rate). At the receiving end, the phase-loss in the data will be exclusively due to the delay within the communications layer. In the client/server model, data transactions are initiated by the client polling for data asynchronously with the availability of new data. As a result, the expected phase-loss will be 1=2 of the sample period in addition to the communications delay. The experiment in this section compare just the additional delay.

Publish/subscribe also allows faster update rates than client/server polling because the information needs to flow only one-way (from producer to consumer); whereas in client/server, the client must first request the data which requires a message with the consequent delay. This is most significant when the underlying network is slow compared to the time taken to process the data.

Client/server exchange is serial. Servers wait for clients to issue requests, and clients remain blocked while their request is processed. While parallelism can be achieved at the server end with the use of multiple threads, the client still remains blocked every time a service is requested. This limitation is what prompted researchers to introduce extensions to the basic RPC protocol as described in Section 4.2.

Publish/subscribe is inherently one-to-many (producer to many consumers), this also allows the use of broadcast and multicast techniques that can be much more efficient in shared-media networks (such as Ethernet), especially as the number of consumers increases. Client/server is one-on-one.

The client/server relationship is asymmetric. It is difficult for a server to turn around in the middle of processing a request and become a client to the same process that issued the original service call24. Publish/subscribe is symmetrical in the sense that any task can be simultaneously a producer and a consumer of data.

Description of the experiment and data presentation.

This experiment assumes that a data

source contains information that several “users” need at the fastest possible rate. Using NDDS, the data source becomes the producer, and the “users” become consumers requesting the data as fast as it is made available. Using RPC, the data source becomes a RPC server and the “users” become RPC clients issuing requests as fast as they can. This experiment compares the update rate of these two methods as a function of the number of items requested by each “user” 24

This situation could easily lead to a deadlock.

Ni and the


108

total number of remote “user” tasks Nc . In each case, the “user” tasks record the rate at which all

Ni updates are received over a large number of iterations (typically 100) and compute the average,

standard-deviation and minimum (worst) of all the rates. This data is plotted in logarithmic scale (to account for the large ranges). The error bars indicate both standard deviation (top segment of the error bar) and the minimum (worst) rate (bottom segment of the error bar).

Fastest update rate: 100 byte items (same LAN)

3

10

100 byte items, mach. 1 (same LAN)

100 byte items, mach. 2 (across LAN bridge) 3 10

3

10

NDDS 2

RPC

1

10

1

10

2

NDDS

10

rate (Hz)

rate (Hz)

rate (Hz)

2

10

RPC

NDDS

10

1

10

RPC

0

0

10

20

30

40


80

90

0

100

10

Fastest update rate: 100 byte items, machine 2 (across LAN bridge)

3

10

60

NDDS

RPC

1

10

0

1

2

RPC

0

0

10

20

30

40


80

90

100

10

0

60

3

NDDS

10

10


10

2

rate (Hz)

10

0


3

10

2

rate (Hz)

0



10

10

0

rate (Hz)

10

NDDS

10

1

10

RPC

0


10

60

0


60

Figure 4.11: Influence of number of items requested on the maximum update rate. This figure shows that, for small item sizes, NDDS outperforms client/server RPC by a factor of three to four independently of the number of items requested by each computer. This fact is illustrated in two scenarios: On the left only two computers receive updates; on the right, four computers receive updates. It is also noticeable that RPC is particularly bad when one of the computers is located across a LAN bridge (worst-case rates can be less than 1 Hz).

Overall performance

Figure 4.10 illustrates the main result of this section: The maximum update

rate achievable with NDDS is significantly higher (a factor of four in this case) than that achievable with RPC. In the experiment represented by the figure, the number of items requested by each computer remained constant (Ni

=

10), as did the size of each individual item (100 bytes). The

experiment allowed the number of computers subscribing to the data 15. For each sample in that range (Nc

=

Nc

to change from 1 to

1; 2; 4; 8; 16), an experiment was run to determine the

maximum update rate that could be consistently provided to all subscribers. Note that for one or two subscribers, NDDS’s performance is limited by the speed of the (slow, SS-IPX) receiving computers (which as we will see in Figure 4.12 limits this rate to around 200 Hz even for a single consumer). For larger numbers of subscribers, NDDS’s update rate is primarily limited by the sending machine.


109

Effect of number of items requested Ni . Assuming the computation overhead and transmission delay scale linearly with the number of items, maximum update rates are expected to be inversely proportional to the number of items. This trend is observed in Figure 4.11. This figure also confirms that the performance advantage of NDDS over RPC remains approximately constant independently of the number of items Ni. Notice also that RPC’s performance can become quite unpredictable when some of the participating computers are separated by a LAN bridge.

Fastest update rate: 2 items, slow machine in same LAN NDDS RPC

102

103 rate (Hz)

rate (Hz)

103

Fastest update rate: 10 items, slow machine in same LAN

NDDS

102 RPC

101 0 103

101 0

500

1000 1500 2000 2500 3000 3500 4000 4500 item size (bytes) Fastest update rate: 2 items, fast machine across LAN bridge

500

1000 1500 2000 2500 3000 3500 4000 4500 item size (bytes) Fastest update rate: 10 items, fast machine across LAN bridge 103

102

RPC

rate (Hz)

rate (Hz)

NDDS NDDS

102 RPC

101 0

500

1000 1500 2000 2500 3000 3500 4000 4500 item size (bytes)

101 0

500

1000 1500 2000 2500 3000 3500 4000 4500 item size (bytes)

Figure 4.12: Influence of computer performance and update size on the maximum update rate. This figure shows that the performance advantage of NDDS over RPC increases with the speed of the computers involved. This is because NDDS’s advantage is dues to the protocol (publish/subscribe) and is only countered by the extra computational overhead that diminishes with computer performance. In the top figure, the (slow) receiver is the bottleneck. This figure also illustrates that NDDS’s performance advantage over RPC diminishes with increasing item sizes, because at this point the producer becomes the bottleneck.

Influence of update size and computer performance.

NDDS’s performance edge over RPC is

due to the advantages of the publish/subscribe model over client/server for repetitive data updates. On the other hand, NDDS’s overhead over the raw transport delay diminishes with increased computer performance. As a result, the faster the computers involved, the more dramatic the difference between NDDS and RPC. Figure 4.12 corroborates this analysis. In both plots the same (fast) computer is used to send the updates. In the plot above, a slow computer receives updates while in the one below the receiving computer has the same speed as the sending one. For small update sizes, the maximum update rate achievable is 180 Hz for the slow receiver and 500 Hz for the fast receiver (this corresponds almost exactly with the speed difference between the two


110

machines). RPC’s performance on the other hand remains constant because it is limited by the protocol (client/server) more than by the computer speed.

Fastest update rate: 2 items, machine 1 (same LAN)

3

10

rate (Hz)

rate (Hz)

Fastest update rate: 10 items, machine 1 (same LAN)

3

10

NDDS 2

10

RPC

NDDS

2

10

RPC 1

10

1

0

500

1000

1500

2000 2500 item size (bytes)

3000

3500

10

4000

Fastest update rate: 2 items, machine 2 (across LAN bridge)

3

200

400

600

800

1000 1200 1400 item size (bytes)

1600

1800

2000

Fastest update rate: 10 items, machine 2 (across LAN bridge)

3

10

rate (Hz)

10

rate (Hz)

0

NDDS 2

10

RPC

NDDS

2

10

RPC 1

10

1

0

500

1000

1500

2000 2500 item size (bytes)

3000

3500

4000

10

0

200

400

600

800


1600

1800

2000

Figure 4.13: Maximum update rate for two clients as a function of the item sizes. This figure shows that NDDS provides a more predictable, fair service than RPC does. This is because the producer initiates the transactions, eliminating contention and ensuring all subscribers get a fair service. When RPC is used, clients essentially contend for service and some of them (e.g. the one separated by a LAN bridge) may receive unfair service. These observations are corroborated also by figure 4.14.

For larger update sizes, the advantage of NDDS over RPC diminishes as illustrated in Figures 4.13 and 4.14. This is because for the computers used in this experiment, the producer becomes the bottleneck for updates of 500 bytes or larger (this can be seen from the fact that the update rate changes significantly in going from 2 to 4 consumers). This problem will become less significant as computer speed increases and can be eliminated with the use of broadcast or multicast techniques, which are difficult with RPC. Notice also that NDDS provides a more predictable and fair service to the clients, all of which receive updates at the same time. This is because NDDS regulates the update rate to a value compatible with the producer and consumer characteristics (sample rate and minimum separation). In this example, the consumer has specified to receive all updates (zero minimum separation) and therefore, the rate is fixed by the common producer. NDDS avoids contention and provides orderly and fair delivery. On the other hand, when RPC is used, each client independently polls the server for updates This results in contention and one of the clients (the one separated from the LAN by a router) is systematically receiving updates at a lower rate.


2 items, machine 1 (same LAN)

2 items, machine 2 (across LAN bridge) 3 10

10 items, machine 2 (across LAN bridge) 3 10

RPC

2

10

RPC

rate (Hz)

10


NDDS rate (Hz)

rate (Hz)

NDDS 2

3

10

rate (Hz)

3

10

2

10

NDDS

2

10

NDDS

RPC 1

1


10

2000


3

0

3

10


3

1

500 item size (bytes)

10

1000


0

3

RPC

2

10

RPC

1

0


1000


10

2

10

NDDS

2

10

NDDS

RPC 1

10


NDDS rate (Hz)

10

0

10

NDDS rate (Hz)

10

2000

10

2

RPC

1


rate (Hz)

0

rate (Hz)

10

111

2000

10

0

RPC

1


2000

10

0

1


1000

10

0


1000

Figure 4.14: Maximum update rate for four clients as a function of the item sizes.. This figure shows that NDDS’s performance advantage over RPC diminishes for larger update sizes. This is because in this case the producer becomes the bottleneck and it is essentially doing the same function for NDDS and RPC. This figure also confirms that NDDS provides a more predictable and fair service to all subscribers than RPC does.

RPC does have some redeeming features: first, the semantics of remote procedure calls are familiar to programmers (due to their similarity to ordinary procedure calls), second it provides automatic flow control and regulation of the information exchange. For instance, a client can be programmed to request data as fast as it can process it, and this program will work unchanged regardless of the speeds of the computers in which they run, the speed of the network communications and even adapt to changes in computer load. This is because each request/response exchange in effect provides flow control keeping client and server in lock (not allowing one to run ahead of the other). This is hard to do with NDDS because the requested update rates and deadlines are set explicitly by the application.

4.6 Summary This chapter has presented NDDS, a communications system specifically developed to address the distinct needs of distributed robotic applications. NDDS allows programs distributed on a computer network to share data and event information unaware of the location of their peers. NDDS has several important features:

Publish/subscribe information-exchange model.


112

Support for multiple anonymous producers and consumers of any data item.

Realistic model of time, allowing consumer-specified custom update-rates and deadlines.

Clear semantics for multiple-producer arbitration.

Flexible semantics for distributed query.

Fully distributed symmetric implementation without central nodes or name servers.

Quasi-stateless implementation using decaying state at each node.

This chapter has also analyzed two aspects of NDDS’s performance: (1) overhead introduced by NDDS on top of the underlying transport protocols (UDP/IP, TCP/IP), and (2) maximum sustainable update rates compared with those using a standard client/server communications layer (RPC). The analysis shows that NDDS’s overhead becomes negligible when either the receiving computer is at least as fast as the sending one, or else when several (in this case two or more) consumers want the data. When compared with client/server RPC. NDDS achieves update rates a factor of four greater irrespective of the number of hosts subscribing to the updates.

Chapter 5

World Modeling This chapter describes the World-Modeling subsystem (WM). The WM is responsible for gathering, processing, and managing the raw information collected by the system sensors. In this task, the WM uses the available domain-specific knowledge to transform the information into a form suitable for use by the remaining subsystems1. The WM also serves as a centralized repository of all a´ priori known “configuration” data (e.g. object geometries, manipulator kinematics and mass properties). This a´ priori knowledge comes in many different forms: (1) it may be in the form of a database containing facts known to be immutable and cannot (perhaps due to cost) be sensed, (2) it may be used in model-based sensor integration algorithms, or (3) it may be embedded within the sensor processing algorithms themselves. World modeling is needed due to the impossibility of directly measuring all the quantities and states required for the operation of the various subsystems comprising the manufacturing workcell. This need is common to systems of significant complexity that need to interact with the external world. The term “complexity” is used to (vaguely) describe the fact that the system interacts with its environment in many different ways, and is capable of a wide variety of actions. In this context, “complex” systems are contrasted with systems that use few sensors, and have very limited ways to interact with the environment. Even if their mechanical or algorithmical complexity is great. For instance, a car engine is a mechanically complex system, but its interaction with the external world presents few variations (amount and mixture of the fuel, ignition, delivered torque). 1 The selection of such “common denominator” representation usable by the remaining subsystems is one of the difficult issues addressed by the WM.

113

CHAPTER 5. WORLD MODELING

114

5.1 World Modeling for the Manufacturing Workcell The manufacturing-workcell experiment requires world modeling to characterize both the robotic equipment and the different objects in the workspace. In particular, the different subsystems require world-model information to support their function as described below:

The Graphical User Interface (GUI) must render the robot, objects, and workspace for the user. It therefore needs to be provided with the position and shape of the objects in the workcell; and the geometry, kinematics, and kinematic state of the robot manipulators.

The Planner requires the kinematics and kinematic limits of the manipulators in order to generate paths. It requires object and manipulator geometric shape (from a solid-model perspective) to guarantee that the generated paths are safe. Sufficient information must also be provided to generate possible object grasps. Finally, the planner must be informed of the workcell status2.

The Control Subsystem must know manipulator and object states (positions, velocities, accelerations). It also requires other information such as contact forces during grasping, manipulation and assembly.

The Sensor Processing Subsystem uses domain-specific information to perform sensor integration/fusion. For this reason, this research encapsulates sensor processing within the world modeler3.

Objective: The design and development of a comprehensive world modeling system for robotic workcells could itself be the focus of a complete thesis. The prototype system described in this chapter is not so ambitious. Rather, it provides simple, solutions addressing several of the major issues arising in such systems. The design described here could be used as a first step in the development of the aforementioned comprehensive system. Philosophy:

The described requirements demand a world-model subsystem that can serve a variety

of roles as illustrated in Figure 5.1. First, it must provide information both in subscription and query 2

The word status is used to denote information about the logical state of the workcell: What is it doing? Are any manipulators holding objects? If so which is object and grasp transform(s)? Are any arms moving? If so along which trajectories? 3 This encapsulation will be discussed in more detail in Section 5.3


Query

115

Subscription

World Modeler

Sensor Processing & Integration

Processed Sensor Info

Controller

Sensors

Sensor Data

Sensor If.

Do

Action Request

Is it feasible?

Outcome Interrogation

Information Interface (Query & Subscription)

Figure 5.1: Roles of the World Modeling Subsystem The World Modeler serves a variety of purposes: It provides status and state information to any subsystem in a query or subscription mode, it answers questions about the feasibility of different actions, it can be commanded (driven) so that its state changes as a result of system actions, it processes (raw) sensor information (sensor integration/fusion), and transforms this information into a form suitable for use by the remaining subsystems.

mode about the state and properties of different system parts. Second, it must allow interrogation on the feasibility of different actions. Third, it must accept direction from the strategic-control layer so that the model can be kept up-to-date and consistent with the strategic actions. Fourth, it must embed sensor-processing within the dataflow of the control-loop to process and integrate sensor signals into a form usable by the controller. The approach to world modeling taken in this thesis combines a physical description and model of the system with a repository of sensory and state information, where minimal processing is performed, and then only to transform the information into a form which is either more intuitive, generic, or flexible.


116

Rationale: The motivation for limiting the degree of sensor processing performed by the world modeler stems from the need to keep it versatile. The reason being that the ultimate representation of the world model information within each subsystem (Planner, Controller, GUI) is likely to be quite different. For instance, different planning techniques already use completely different representations of the workspace and objects (discrete bitmaps, quad-trees, spheroids, distance maps). Similarly for the GUI, objects may be rendered in a variety of fashions and details. Hence, the most appropriate object representation format (boundary representations, CSG4 , generalized cylinders) will vary among different graphical interfaces. Therefore, rather than attempting to develop an all-encompassing world-modeling subsystem, which will be necessarily tied to every piece of the system; we advocate the alternative of minimally representing what is known and sensed about the physical system, making this information available, and letting the subsystems further develop specific models for their own use. In principle, the World Modeler (WM) could have been distributed among different computers. The characteristics of the system advise, however, to keep it centralized and colocated with the control subsystem. In this manner, the control subsystem (which requires the information at high bandwidth and with minimal delay) can access information locally. Consistency problems are also avoided.

5.2 Literature Review: World Modeling for Robotics The World Model provides a framework where domain-specific knowledge may be used to process sensor information and provide a uniform, simpler interface to the remaining subsystems in need of that information. It also provides a structure for reasoning about actions and consequences (a requirement strategic control and planning). As a result most experimental robotic systems of significant complexity that either incorporate planning, strategic control, or use sophisticated sensors incorporate some form of world modeling. For example the HILARE-2 robot [116], MOBOT-III [63], and AMBLER [85], all incorporate sophisticated world-modeling subsystems. Solid modeling is often an important aspect of World Modeling systems. This topic has been extensively addressed by mechanical CAD packages [78]. In the field of robotics, several authors have developed dedicated world modeling systems emphasizing the solid modeling aspect. These are mostly useful for graphical rendering, simulation, collision detection and fine motion planning. 4

Constructive Solid Geometry.


117

For instance Mirolo and Pagello [111] describe a modeling approach based on CSG 5 , with generalized cylinder primitives, that also incorporates surface representation for graphical rendering. Commercially available robotic-programming-and-simulation packages such as SILMA’s CimStation [37], DENEB’s IGRIP [16] and Tecnomatrix’s RoboCAD [197] also incorporate sophisticated solid modeling. These systems, however, are primarily used to simulate and program robots off-line and do not integrate sensor information, nor interact with the robots during operation. In the context of distributed robotic-system architectures, Oliveira, Camacho and Ramos [47] describe an architecture where different agents cooperate to achieve assembly-type tasks. They identify a separate “model + world descriptor” agent that provides information on the properties and state of the different objects in the workcell. However, this system is used only to aid in the generation of plans and programs. These programs are then down loaded to the workcell for execution. Their system is not used as part of the feedback loop. World-modeling systems designed specifically for robotic workcells have also been developed. Ravani [143] developed a general-purpose system (RWORLD) for robot programming and simulation that models the manipulators and objects in the workcell. RWORLD uses four primitives to describe the world: rigid objects, devices (mechanisms with more than one DOF, e.g.: robot manipulators, end-effectors, and conveyor belts), transformation frames, and sensors. The description of rigid bodies includes mass, inertia, features, center of mass, and geometrical description (CAD based). Manipulator devices are described in terms of their rigid links, joints, Tool Center Point (TCP), and acceptable commands. Models of the joint include limits (displacement, velocity, torque) and geometric shape. Link models include rigid-body properties. The system can model geometrical, spatial, kinematic, and dynamic aspects. RWORLD also supports aggregation of objects (e.g. when assemblies are made or when the robot grasps an object or an end-effector). RWORLD can compute the kinematics and dynamics of the manipulator devices from the model description. This system appears to have been used only in simulation. A similar approach emphasizing a unix-shell like interactive interface for modeling robotic systems called R-shell has been developed by Vuskovic et al. [191]. The RCS specification.

One of the most detailed specifications of the role and architecture

of world-modeling subsystems for robotic applications is the one provided by NIST’s RCS and NASREM reference models [139, 6, 7, 4]. This model has been used in several applications [5, 98]. 5

Constructive Solid Geometry. An approach that models solids as composed of primitive solid shapes that are combined using boolean (set) operations. Its main drawback is that surface information, necessary for graphical rendering, is hard to extract.


118

RCS advocates a vertical hierarchy of World-Model “components” where each layer operates in a periodic cycle at a rate that diminishes by roughly an order of magnitude with each higher level in the hierarchy. Each layer is restricted to communicate with the ones above and below in the hierarchy (it also communicates horizontally with peer modules in the planning and control hierarchies). While the strict hierarchy works well for lower levels in which the physical system and sensors are modeled, the higher levels (planners, strategic controllers) are often too unstructured to be restricted into accessing a single world-modeling layer. For instance, a strict hierarchy with no ramifications does not allow alternative implementations of similar functionality (i.e. there is only a single World Modeler at each level). Also, the periodic requirement is not natural for the higher layers, which are naturally event-driven. Nonetheless, there are several general tenets in the RCS model which have been adopted by the system developed in this thesis: (1) need for hypothetical6 as well as factual queries7 , (2) need for periodic updates, (3) need for a lower layer embedded within the dataflow of the control-loop. Sensor Integration.

The need to integrate information from multiple sensors is always present in

systems operating in not-completely-structured environments. Much theoretical work in this area is devoted to the definition of optimality criteria and the mathematical methods that can be used to combine information in an optimal fashion. A variety of such strategies have been presented in [100, 39, 57], including the classical statistical methods (Kalman filtering, Dempster-Shafer, DurrantWhyte) [43], and others such as Error Functions [186]. However, as noted by Thomopoulos [184], these rigorous signal processing methods, require the existence of a precise mathematical model that describes the generation of the data (including noise and certainty characteristics). As these mathematical models are not always available, more ad-hoc methods such as deciding, averaging, and guiding are also in use [56]. Moreover, the use of formal generic approaches alone is difficult in practice because the sensors may be hard to characterize in the ways required by these methods (e.g. noise, error, or certainty may be difficult to characterize or even define for certain sensors). In addition, some of these properties may depend on the task, not just the sensor. Also, it is often difficult to incorporate in the generic approaches important practical aspects such as: bandwidth of the various sensors (which may be very different), failure characteristics (sensor integration scheme must be fail-safe and degrade gracefully), asynchronous task-dependent information, and domain-specific procedural 6 7

Queries about the feasibility and/or outcome of actions. Queries about the current system state and characteristics.


119

knowledge. As a result, many systems that operate in the “field” use a combination of general and ad-hoc approaches.

5.3 Architecture of the World Modeler

Rest of the System

World Model Public Interface W.M. Private If.

Strategic Control

What? Where? Feasible? Take action

Arm Model

Object Model

status, grasp transforms

Conveyor Model

geometry geometry

status states properties geometry

SENSOR PROCESSING & INTEGRATION grasp Transforms Vision Kin

Arm grasped object

Sensors

obj state

object, arm state

(Vision, Encoders) Vision Conveyor

Object

WORLD MODEL Data-Flow Layer

Figure 5.2: Architecture of the World Modeling Subsystem The world-model subsystem contains a dataflow layer and an object-based layer. The dataflow layer processes and integrates information from multiple sensors. The object-based layer contains a software object for each physical entity (manipulator, conveyor, part), keeps status and state information and can respond to queries about the feasibility of different actions.


120

The implementation of the world modeler must be capable of supporting the roles described in Section 5.1; as well as providing the information required by the external interface described in Chapter 3. The external interface requires the World Modeler to provide three types of information: Configuration information. This is static information describing the immutable characteristics of the system: Rigid-body shapes and mass properties, arm kinematic parameters, visual fiducials, and base position and orientation for the fixed workcell arms all belong to this category. This information is typically stored in configuration files. The role of the WM is to access and cache this information and make it available through its interface to the remaining subsystems. In this manner, the WM hides the details of configuration-file parsing and description formats. Status (logical) information. The workcell transitions through a series of discrete states. These states represent distinct logical conditions that are meaningful irrespective of the implementation of the strategic workcell controller (i.e. they are not just “states” of a particular finite-state machine diagram or petri-net that controls the robot at the strategic level). These “abstract” states represent the activity of the workcell to the external world. Hence, the use of the word status to avoid confusion. Examples of status information include whether the manipulators are involved in motions, grasping objects, and which objects are being grasped. The WM interface uses a “bit + string vector” to represent the status of each of the robot arms in the workcell. Some of the entries in this vector were listed in the “status” entry of Table 3.2, Chapter 3. The World Modeler maintains this status and provides mechanisms for accessing and modifying if from the strategic-control layer. Sensor-derived information. Sensor information may be processed for a variety of reasons: pure signal processing (filtering, enhancing etc.), transformation into a more useful format for kinematic/dynamics, and integration/fusion with other sources. The prototype robotic workcell presents examples of all these motivations. These examples a described in detail in Section 5.4. To fulfill the above requirements, the world modeler subsystem has been designed integrating two layers with complementary computational models: object-based8 and dataflow as illustrated in Figure 5.2. 8

This layer is object-based, not object oriented because no inheritance hierarchy has been defined. This has not been a problem because we are using fairly simple models. At the time the world model was developed, there were no object-oriented language (e.g. C++) support for our real-time system. This situation has changed and the WM could benefit from using a true object-oriented approach.


5.3.1

121

The Object-Based Layer

Name: "left arm" Base location: 4x4 transform State of TCP: pos, vel, acc Desired state of TCP: " Joint state: pos, vel, acc Desired joint state: " Current trajectory: joint, end-point start time. Status: grasping/moving ... Grasped object: "obj. name" Grasp Tranform: 4x4 transf. Possible Grasps: grasp1, grasp2, etc. Shape: graphical model planner model Limits: max. velocity max. acceleration max. torque

Name: "bar" Location: 4x4 transform State: pos, vel, acc Desired state: Goal Location: 4x4 transform Possible Grasps: grasp1, grasp2, etc. Shape: solid model simplified plannar model Limits: max. velocity max. acceleration Properties: mass, inertia, etc.

Kinematics: Denavitt-Hartenberg param. Joint limits.

Figure 5.3: Arms and rigid-objects are represented using software objects.

This layer is built from three types of (software) objects: (1) arm-objects (manipulators), (2) part-objects (rigid objects that can be manipulated or need to be avoided), and (3) conveyorobjects (which can carry objects on top). There is one software object for each corresponding physical entity. For example, there are two arm-objects (one per arm), eight part-objects, and one conveyor-object. These software objects contain state information about the physical objects they model (see Figure 5.3), and communicate to the external world by exchanging messages through two interfaces: The public world-model interface described in Chapter 3 and a private interface described in Table 5.1. The private interface is used by the strategic-control system (see Section 6.7) to build and modify the model.


122

The private interface serves several functions:

Create models for new physical entities.

Interrogation of the world model regarding the feasibility of actions.

Direct and inform the world model of actions taken by the strategic control so that the model is kept up to date.

5.3.2

Query about the state and properties of the different system parts.

The Sensor Processing/Integration Layer

The sensor-processing layer is embedded within the dataflow of the control subsystem. Sensor integration is model-based and it is directed by the object-based layer. The results of the sensor processing/integration are immediately available to the the object-based layer. Details on this layer are presented in Section 5.4.

5.3.3

Characteristics of the Architecture

The architecture of the world modeler is unique in that it presents three radically different interfaces to the remaining system: (1) high-bandwidth dataflow interface, (2) (private) multiplicity1 master read/write interface, and (3) (public) multiplicity-N, read-only interface. As shown in Figure 5.4, this approach allows the WM subsystem to be used efficiently within a hierarchy and has several advantages over a purely hierarchical approach:

Interaction can occur with any other subsystem in the hierarchy, not just among subsystems at the same level, or at levels immediately above and below.

Replication and redundancy can be supported. Using the public read-only interface, multiple subsystems that logically occupy the same position in a hierarchy can coexist. This allows, for example, multiple GUIs or planners to use information from the WM. Moreover, this approach leaves the system open for expansion: new subsystems needing world modeling information do not require a dedicated position and interface within the hierarchy.

Fewer interfaces have to be defined. Compare this with defining interfaces at each level in the hierarchy.


123

World Model

Graphical User Interface

level 4

level 4

Task Planner

Graphical User Interface

Task Planner

Path Planner

level 2 Path Planner

Strategic Control

Sensors

level 1

Strategic Control

Sensors

level 0

Controller

World Model Sensors

sensor processing

Controller

Figure 5.4: Comparison of WM architecture with purely hierarchical WM The architecture of the WM (left) uses a dataflow interface to process sensor data right in the control loop, a private read/write interface, and a public read-only interface. The read/write interface can only be connected to a single subsystem which will be responsible for building and modifying the world modeler. The read-only interface can have any multiplicity and be connected to any number of subsystems. The hierarchical approach (right) uses dedicated interfaces between each pair of subsystems.

High-bandwidth feedback control can be achieved using the processed sensor data because the processing is sitting within the dataflow of the controller and is driven by the same periodic control loop. This same data is made available at lower bandwidths to other subsystems through the other interfaces.

The hierarchical approach, however, offers advantages too: It is more modular and allows each subsystem to have a fully tailored interface. Nonetheless, the fact that the WM interface is anonymous and customizable (using the parametric approach described in Chapter 3); achieves most of the benefits of dedicated interfaces, without the drawback of interface explosion. A sensor integration architecture must accommodate two functions: (1) low-level sensorfusion strategies (such as Kalman filtering, Shafer-Dempster, Durrant-Whyte) to process and combine the stream of information and (2) high level rule-based strategies that can adapt the lower-level strategies to different situations, and provide for error detection and recovery9 . The above architecture provides both mechanisms: The sensor-processing/fusion layer is embedded within the dataflow, and can be controlled by the object-based layer. The private interface allows 9 For instance when a sensor malfunctions the low-level sensor-fusion strategy may need to be radically changed. Similarly, when an object is grasped, the information on the arm location can be used to derive the object location.


124

an external strategic or rule-based subsystem to interact with the World Modeler. Using the private interface, the object properties can be accessed and set, and hypothetical scenarios can be evaluated. In addition, the public interface allows unrestricted read-only access to the information produced by the World Modeler. Although the architecture of the World Modeler subsystem has been tailored to the needs of the workcell, the following characteristics should be generally applicable to other intelligent robotic systems:

Need for embedding sensor processing within the dataflow. This is justified because the control loops need high bandwidth access to the sensory information processed by the world model.

Need for an interface to control the World Modeler. Changing environments, states and error conditions required rule-based strategies to direct the sensor-integration process.

Use of an object-oriented layer above the dataflow. The use of objects to represent different physical entities (manipulators, parts, conveyors, etc.) allows the model to be easily built from simple pieces, and the system to be expanded to include more entities as need arises.

5.4 Sensor Integration The terms sensor integration and sensor fusion refer to the use of more than one sensor to infer a particular property of the environment. Sensor fusion usually implies some form of analytical combination of the sensor signals (e.g. averaging), whereas sensor integration is used when different sensors are performing different tasks (for example guide each other or measure different properties). Integration sometimes is used as a more generic term that includes any mode of combining information from multiple sensors. The need for sensor integration typically arises because no single sensor can provide all the required information with the necessary accuracy and bandwidth. By using multiple, complementary, sensors ambiguities can be eliminated. By using multiple sensors to measure the same property, error can be reduced. In the dual-arm manufacturing workcell there are examples of each of the above needs:

The position of the end effector in the X-Y plane can be obtained from kinematics (using the joint encoders and the rigid-link kinematics of the manipulators) and vision (tracking an LED


125

mounted on the elbow link). Kinematics has higher bandwidth, less noise, and better accuracy. However, vision is the same sensor used to determine object positions. Therefore, the vision sensor provides better measurements of the relative distance between the manipulator and the objects, and would be the preferred sensor whenever interacting with objects in the workspace (e.g. tracking and picking them from the conveyor).

The position and orientation of an object can be determined by the vision system. Once the object is grasped, its position and orientation can also be determined from kinematics (i.e. given the grasp transform and the location of the manipulator end-effector). These measurements need to be combined for several reasons: First the vision often loses track of the object when it is grasped (the arm(s) grasping the object obstruct the view from the overhead camera). Second, the vision system is two dimensional and cannot measure the object height. Third, the kinematic measurements are available at higher bandwidth allowing higher performance control. Fourth, whenever possible, vision data must be used because vision provides better relative measurements of distances to other objects and measures object location directly10. Sensor-integration is complicated by the fact that it must perform reliably under many circumstances: one or two-handed grasp, with or without visual tracking.

The location and velocity of an object on a conveyor can be determined by the vision system. However, the object is usually lost from vision during the final track and grasp operation because the arm obstructs the camera view. Additional information on the conveyor displacement and velocity is available from an encoder mounted on the conveyor-motor shaft. This relative data must be combined with the available visual information to allow successful picking of objects from the moving conveyor.

The next three sections describe the approach used to solve these sensor-integration issues.

5.4.1

Integration of Kinematics and Vision for Robot-Arm Position

The objective is to generate an estimate of the arm configuration by merging the information provided by the visual tracking of the elbow-link-mounted LEDs with the information provided by arm-joint encoders. The encoders have the least noise and highest repeatability. From the encoder resolution described in Table 2.3 and the link-lengths (see Table 2.1) it can be seen that the X-Y location of the end-effector as derived from arm kinematics has a resolution of 60m. This 10

The kinematic approach relies on a good knowledge of the grasp transforms, and these may slip.


126

information is also available at the sample rate of the arm controller (100 Hz). For comparison, the vision system provides data at 60 Hz (with a 1/30 sec. delay), has a resolution of about 0.8mm

0.2mm11. Lens distortion contributes further to vision error. The vision system is calibrated using a 3rd order polynomial fit, but even after calibration, the absolute error may be as large as 4mm in some parts of the workspace (see Appendix A). Despite the higher resolution and bandwidth of the kinematic measurements, to interact with the workspace objects, capture them from the conveyor, and avoid inter-object collisions, relative measurements are critical. Moreover, vision provides an absolute measurement whereas the encoders have offsets that must be calibrated during each power-on. Approach.

Given the characteristics of the two sensors, what we desire is the resolution, band-

width, and noise characteristics of the kinematic-based measurement with the absolute values provided by the vision measurement. In other words, the high frequency behaviour of the kinematics and the low frequency behaviour of the vision. The key observation is that small difference between the endpoint coordinates measured from kinematics and vision can be mapped into an equivalent joint-angle offset. A first order approximation to this mapping is given by the manipulator jacobian:

dx = J ()d.

These small differences

(typically 3-4 mm, see Appendix A) will result in small angular offsets (under 0.01 rad) which have negligible effect on the value of the Jacobian, Mass Matrix, or any other quantity related to the manipulator dynamics. Therefore, we can–without side-effects–bring the kinematic and vision measurements to full agreement by adding corrective offsets to the encoder-measured joint angles. These offsets may be adaptively changed12 as the manipulator moves though the workspace using the equation d

=

J ,1 ()dx. Note that this approach also takes care of the initial offsets due to the

power-up configuration of the arms (the encoders measure relative angles from the initial power-up angles)13. The architecture used to implement this approach is shown in Figure 5.5. Performance.

The performance of the sensor integration scheme can be seen in Figure 5.6. In this

figure, the arm has been commanded on a straight-line slew and the vision and kinematic estimates for the x-coordinate of the endpoint are compared. Initially (steady state) the vision and kinematic 11 The resolution of the vision system is 0:1 pixels along the X axis and 0:02 pixels along the Y axis [154]. In interlaced mode, a pixel corresponds to an area of 8mm 9mm. 12 The vision-based correction is is first run through a low-pass butterworth filter with 3 Hz bandwidth so it only affects the low-frequency value of the position signal. 13 This capability was an important motivation for adapting the angular offsets instead of adding corrective terms in cartesian space directly.


θ raw

Joint Encoders

127

θ

Σ

(θ)

F kin

x

Forward Kinematics

-k

z

delay

TV θ

x

vis

Vision System

Σ

+

offset

∆x ∆θ

a(z) b(z)

J

-1

(θ)

Offset Estimator

EncodersHP EncoderLines

0x00e20000

Encoder2LeftJoint_ioMap

vision_T_LeftArmBase

Pos_joint_encoder

X

vel

Vel_joint_encoder

Y

encoderMVME340

ArmDHParameters

Encoder2relCoor_A

hpC num clo boa pos

TwoArmRobotEncoders

LeftArm_JointPosFilteredCorrection

Encoder2relCoor_B

2457600

ioM A

B

C out

LeftArmJointPos

0.4856

glo dHP off qRe x qRe

x_d J

GeneralLinCombination Encoder2LeftArmJointPos

LeftArm_J

J_T

LeftArm_J_T

qDy

LeftArmJointDynPos

qDy LeftArm_JointPosFilteredCorrection sin Encoder2relCoor_B

LeftArmKinEndPtPos LeftArmKinEndPtVel

x_a

LeftArmJointDynVel LeftArm_sin_cos_qDyn LeftArmKinVisTargetLoc

Encoder2relCoor_A scara4dofKin Encoder2LeftJoint_ioMap

LeftArmKinematics 100 ioM A X

B

C out LeftArmJointVel

Y

LeftArm_VisionDelay

x tim delay sam x_d

0.05

LowPassFiltNumCoefs

100

LowPassFiltDenCoefs

y sam b filter a x

GeneralLinCombination

Left_Arm_Jinv

Encoder2LeftArmJointVel

LeftArmKinVisTargetLocDelayed 0.05

0.62

JointPC_ioMap

0.035

win hei tim exp pos

LeftArmEndPtVisPos

vel

LeftArmEndPtVisVel

age

LeftArmEndPtVisAge

VisionPoint

LeftJointCorrectionFilter

LeftJPC_Factor

ioM fac A X

LeftArm_Jsingular

B

C dis out

Y

LeftArm_JointPosCorrection

GeneralLinScaledCombination LeftJointPosCorrection

LeftArmVisionPoint

Figure 5.5: On-Line estimator of initial angular offsets The (shoulder and elbow) joint angles produced by incremental joint encoders need to be combined with the initial angular offsets that correspond to the arm power-up location. In addition, these offsets are used to compensate for the (configuration-dependent, small) differences between kinematics and vision coordinates. The offset estimator maps the Cartesian error between kinematic and vision coordinates through the inverse Jacobian to obtain a corresponding joint-space error. This error is run through a filter (essentially an integrator followed by a low pass filter) to produce an estimate of the angular offsets. This process is illustrated in the top diagram. The diagram on the bottom shows the ControlShell implementation using generic software components.


128

Delay and noise of vision signal Arm Endpoint X coordinate (m)

Arm Endpoint X coordinate (m)

Tracking performance 0.4 0.2 0

vision

reference actual (on top)

-0.2 -0.4 -0.6 -0.8

0.5

vision

0.45 kinematics 0.4

0.35

-1 2.5

3

3.5

4

4.5

5

5.5

Joint Angle Offsets (rad)

2.8

3

Time (secs)

Estimation of joint offsets

Straight line path

elbow 0

-0.01

shoulder

-0.02

2.5

2.6

Time (secs)

0.01

2

2.4

Arm Endpoint Y coordinate (m)

2

3

3.5

4

4.5

5

5.5

Time (secs)

0

-0.5

actual reference

-1

-1.5

-1

-0.5

0

0.5

Arm Endpoint X coordinate (m)

Figure 5.6: Performance of On-Line angular-offset estimator The top-left plot shows both kinematic and vision coordinates during a straight-line slew (bottom-right plot). The vision signal is not suitable for high-performance control: it is quite noisy and has a delay of about 50 ms due to the processing overhead (detail in top-right plot). The on-line estimator adapts the values of the shoulder and elbow angular offsets (bottom-left plot) so that at the end of the trajectory the kinematic and vision coordinates match again.

measurements match (the adaptation has made it happen). During the slew, the vision measurements are delayed and noisy; the adaptation is active but too slow (due to the low-pass filter) to make a difference until the arm stops at a new location. At the new location, the offsets are adapted until the vision and kinematic measurements match again. The speed of the adaptation can be seen in the step response of Figure 5.7. This plot also illustrates the adaptation to the power-up configuration which has approximately 1 second time constant.


nature arm

object

conveyor

129

Model construction action Create and add a model for a manipulator arm. Provide its name. The configuration files are read to create the model, read the kinematics, DH parameters, base location in the workspace etc. Create and add a model for an object (a manipulable part or a pure obstacle). Looks up the configuration-database to get all the information on the object (geometry, grasps, visual fiducials etc.). Create and add a conveyor model. Provide its name. The configuration database is used to obtain the conveyor’s shape, height, and visual-fiducials.

subject extrapolation reachability graspability safety release height

Model feasibility inquiries checks Extrapolate future object/arm state. Can the arm reach a given position? Would the arm succeed in grasping an object? Check an arm trajectory for collisions. What would be the height of the object if it were released now?

subject arm status misc. info kinematics

Model control control action Modify grasping, catching, moving, tracking, picking, or lifting status Set arm/object trajectories and start time Transform via points using Forward Kinematics or Inverse Kinematics

topic status distances goals

Model query available information Get status on motion, picking. or grasping. Get object being grasped. Compute distance from arm to a location relative to the current object position. Get object goal destination, get current trajectory and time when it started. Table 5.1: World-Model private interface

In addition to the public world model interface described in Table 3.2, the private world-model interface provides a mechanism for the strategic layer to incrementally build the system model, access and modify the world model status, request services, and inquire about the feasibility of prospective actions.


130

Arm Endpoint coordinates (m)

Power-on convergence of kinematics to vision -0.2 Y coordinate

vision -0.4

X coordinate

vision -0.6 kinematics

-0.8 -1 kinematics -1.2 -1.4 2

3

4

5

6

7

8

Time (secs)

Power-on estimation of joint offsets

Joint Angle Offsets (rad)

0 -0.2

elbow

-0.4 -0.6 -0.8

shoulder

-1 -1.2 -1.4 2

3

4

5

6

7

8

Time (secs)

Figure 5.7: Adaptation to power-up configuration When the incremental encoders are powered-up, it becomes necessary to identify the angular offsets that correspond to the location of the arms. In this experiment the arms are held at an unusual location when the encoders are powered-up. As a result, the initial kinematic location is very different from the (true) vision location. As the angular offsets are identified (bottom plot) the kinematic coordinates converge to the vision coordinates (top plot).


5.4.2

131

Robot State and Vision for Grasped-Object Position "perigee" shm

ConveyotState

out

barWMBeingManipulated

CopyCSMatFromSharedMemory

2.0

GetConveyorState

0.0

0.1 "barWM"

0.038

"perigee" obj gra len wid hei tab con bod

shm

ConveyorDispl

out

con

bod

barWMConveyorState barWMConveyorAge

bod bod


WM_TrackConveyorObject

GetConveyorState BarSearchHeight

barWMTrackConveyorObject

"perigee" right_armWM_state

"barWM"

left_armWM_state vis bod sea vis VisionBody dat

barWMVisionState arm arm vis bod

BarDataAge

objKinStateFromLeftArmGrasp

barVisionBody left_armWM_T_obj 0

"perigee" shm

objKinStateFromRightArmGrasp

out left_armWM_state

right_armWM_T_obj "perigee"

0

right_armWM_T_obj

left_armWM_state

TwoArmGraspedBodyKinState

TwoArmGraspedBodyKinBodyManager 2

poi ena ena poi poi

RightEndPtState out right_armWM_state

CopyFromShmRightEndPtState

barWMManipBodyManager

obj_T_right_armW

arm bod gra arm bod gra kin bod KinBodyManager kin



LeftGraspObjKinState

CopyFromShmLeftEndPtState

shm

man

kin

left_armWM_T_obj

ena TwoPtKinState

arm

barWMState barWMManipValidity

ManipBodyManager

right_armWM_state

obj_T_left_armWM CopyCSMatFromSharedMemory

val

arm

2

poi ena ena poi poi

LeftEndPtState

vis

ena TwoPtKinState RightGraspObjKinState

Figure 5.8: Sensor Fusion for the position of grasped objects This dataflow diagram details the sensor fusion scheme. The grasp transforms are measured using the vision system after the grasp has been consolidated. If the object is not tracked by the vision, the transform is set to the desired grasp transform. The KinBodyManager component uses rigid body kinematics from each existing grasp to derive the kinematic estimate of the object position (an average of all the grasps is used). The ManipModymanager component merges the information with that of the vision.

The overhead vision system used to track objects cannot measure object height. In fact, object height must be provided to the vision tracking algorithm so that the objects can be accurately identified. In addition, the vision system often loses track of the objects once grasped because the arm obstructs the LEDs from the camera. If the grasp transforms are known, they can be used in combination with the known arm locations to provide a kinematic estimate of the object location. This kinematic estimate can be merged with the vision information to provide a measurement that incorporates object height and is robust to loss of visual tracking. The sensor integration scheme fulfills two concurrent roles: First it guides the vision sensor (by providing the height of the object).


132

Second it replaces the vision sensor whenever it loses track of the object. Notice that it would also be possible to enhance the estimate by merging its measurements with a less noisy and higher bandwidth estimate from the kinematics. The current implementation does not exploit this last fact. Instead, the integration of kinematic and vision estimates uses a simple heuristic: If the object is grasped by both arms, the kinematic measurement is used. In the case of a single-arm grasp, the vision is used if available (for all but height), else kinematics are used. In any case, the kinematically-derived object height is used to guide the vision system in its tracking. This strategy does have some failure modes, but the failure is graceful. For example if the grasp slips, the corresponding change in the grasp transforms results in an error in both the object and the reference-arm positions. However, the arms are under impedance control and this error will only result in certain forces/tensions being applied through the object14. The grasp may also be completely lost from one (or both) manipulators. In this situation, the controller still commands each manipulator to follow the trajectory as if the object was still grasped and a corrective action is only taken when the failure is discovered by the system (e.g. by seeing the object at a different location). These examples illustrate the need for these fairly low-level sensor-integration schemes to be combined with higher-level strategies (such as the strategy which watches for an object whose vision coordinates are far from the kinematic coordinates to determine whether the grasp has failed) to achieve robust operation.

5.4.3

Conveyor Displacement and Vision for Object Position

For the workcell to operate properly, it must accurately predict and/or sense the motion of the objects on the conveyor. As previously mentioned, the object is often lost during the capture maneuver because the arm obstructs the camera view of the object’s LEDs. This problem is solved by adding LEDs to the conveyor (so its location can be determined) and an incremental encoder to the motor driving the conveyor belt (so that belt displacement can be measured)15. The World Modeler uses a dedicated sensor-integration software component for each conveyor/object pair. The position and size of the conveyor and various objects is used by the component to determine whether a specific object is on top of that conveyor. If an object is on the conveyor, vision information is used whenever available, otherwise, the object’s position and velocity are estimated from the last vision update and the conveyor measurements. This estimation uses the measured conveyor orientation (determined by the vision system) and the relative displacement of the conveyor belt from the time 14 15

The magnitude of these forces will depend on the arm impedances estimation errors. The procedure described here can be applied to any number of conveyors present in the workcell.


133

right_armWM_state

left_armWM_state

conveyorWMVisionState ConveyorDispl

barWMBeingManipulated 2.0 barWMSearchHeight

"barWM"

0.038 0.1

"barWM" obj gra len wid hei tab con bod

"hohmann"

vis bod sea vis VisionBody dat

con barWMVisionState barWMDataAge

bod


arm arm vis bod vis

val

bod

arm

man

bod

arm

WM_TrackConveyorObject barWMVisionBody

barWMState barWMManipValidity

kin ManipBodyManager

barWMTrackConveyorObject barWMManipBodyManager

objKinStateFromLeftGrasp objKinStateFromRightGrasp TwoArmGraspedBodyKinState

Figure 5.9: Sensor integration scheme for objects on a conveyor A dedicated component exists for each object/conveyor pair. The component uses the vision-sensed position of the conveyor and object to determine whether the object is on the conveyor by modelling the area the conveyor occupies and figuring out whether the center of gravity of the object rests on this area. The vision measurements are used whenever available. When vision tracking is lost, the position of the object is extrapolated from the last visible position using the relative conveyor displacement and orientation.

visual tracking was lost. Object velocity is simply deduced from conveyor speed and orientation. This scheme is illustrated in Figure 5.9. Figures 5.10 and 5.11 illustrate the performance of this algorithm to track objects on a conveyor. In all cases the accumulated error is smaller than the accuracy required for a successful capture (about 1 cm).


134

Trajectory: estimated vs. vision

Estimation position error 0.01 location error (m)

1 0.8 0.6

Y coordinate (m)

0.4

0.005 0 −0.005

0.2

−0.01 −20

−10

0

0 time (s)

10

20

Velocities: estimated vs. vision 0.02

−0.2

0 velocity (m/s)

−0.4 −0.6 −0.8 −1

−0.02 −0.04 −0.06

0.4

0.6 0.8 1 X coordinate (m)

1.2

−0.08 −20

−10

0 time (s)

10

20

Figure 5.10: Tracking performance of objects on the conveyor at 5 cm/sec The above plots illustrate the performance of the sensor-integration algorithm for the nominal conveyor speed of 5 cm/s. At time t=0, the sensor-integration software was misled into believing that the vision had lost track of the object. In this manner, the difference between the actual vision state and the one derived from the conveyor sensors can be compared. The position discrepancy is on the order of 1 cm even after 0.5 m travel without vision information, this is the limit on the gripper tolerance for a successful capture. The plots on the right illustrate positions and velocities for the X and Y coordinates.


135



1 0.8 0.6

Y coordinate (m)

0.4

0.005 0 −0.005

0.2

−0.01 −10

0 time (s)

5

10

Velocities: estimated vs. vision 0

−0.2 velocity (m/s)

−0.4 −0.6 −0.8 −1

0.4


−0.05

−0.1

−0.15 −10

1.2


−5

0 time (s)

5

10


1 0.8 0.6 0.4 Y coordinate (m)

−5

0

0.005 0 −0.005

0.2

−0.01 −5

0

0 time (s)

5

Velocities: estimated vs. vision −0.05

−0.2 velocity (m/s)

−0.4 −0.6 −0.8 −1

0.4


1.2

−0.1

−0.15

−0.2 −5

0 time (s)

5

Figure 5.11: Tracking performance of objects on the conveyor at 10 and 20 cm/s The above plots illustrate the performance of the sensor-integration algorithm for faster conveyor speeds: 10 cm/s and 20 cm/s. The approach used is the same explained in Figure 5.10: the sensor-integration algorithm believes the vision has lost track of the object at time t=0. As in Figure 5.10, the position discrepancy is on the order of 1 cm after 0.5 m displacement. This is mostly due to the error in the (vision-measured) orientation of the conveyor.


136

5.5 Summary This chapter has described the World Model subsystem. The world model plays a variety of active and passive roles: (a) repository of information, (b) consultant regarding feasibility of different actions, (c) disseminator of information in response to subscriptions and (d) transformer of sensor information (conditioning, sensor integration/fusion). The architecture adopted is composed of an object-layer and a dataflow layer. The dataflow layer is responsible for sensor processing and is connected directly to the sensor and control subsystem. The object-based layer provides the remaining functionality through the use of software objects that correspond to the different entities (manipulators, conveyors, parts) in the workcell. The sensor-integration layer integrates vision and kinematic data for the manipulators and parts. It also merges the conveyor displacement with information from the vision system to allow robust part acquisition despite the potential loss of visual tracking. Experimental results show excellent performance. This approach is modular, generic, and scalable. Additional manipulators, parts and conveyors can be added with ease. Incorporating a dataflow layer within the world model brings the benefits of model-based sensor integration directly into the control loop. The feasibility checks and action interfaces allow direct interaction with a strategic-control layer that can inform the model of changes in the activity of the system.

Chapter 6

Hierarchical Control System This chapter describes the hierarchical control subsystem (controller). The ultimate objective of this research was not to advance the edge of performance of the control system but rather to develop a complete balanced system so that overall performance requirements of the experiment were met. While it is important to exercise rigorous control methodology when developing the control system, having a very-high-performance controller would be of little benefit unless the rest of the system were able to exploit the extra performance.

6.1 Control System for the Manufacturing Workcell The control subsystem receives strategic-level commands through the Robot Interface described in Chapter 3, and controls the manipulators in response to those commands. To this end, the control subsystem must not only provide stable control of the manipulators, but must also interact with the world modeler and use a certain level of “intelligence” to transition between control modes, perform all the autonomous steps and coordination required to pick up objects from the moving conveyor, and detect/report/recover from failure conditions. Safety is commonly a concern in robotic systems. In the Manufacturing Workcell, the size, inertia and peak velocities (on the order of 1 m/s) of the manipulators make safety mechanisms imperative to protect both humans and the equipment and its environment. Passive mechanisms such as an enclosed workspace and the ubiquitous “kill switch” can be effective to protect the human operators when used consistently. However, ensuring the safety of the manipulators themselves, even in the presence of failures in the planner/controller, requires special provisions in the design of the control subsystem. 137

CHAPTER 6. HIERARCHICAL CONTROL SYSTEM

138

6.2 Literature Review: Hierarchical Robotic Control Systems A number of successful experiments at the Aerospace Robotics Laboratory have used a hierarchical approach to control dual-arm robotic systems. This approach was pioneered in the laboratory by Schneider [154, 157]. Several extensions were made in subsequent experiments: Ullman [189] modified it for free-flying robotic systems, Meer [105, 106] extended it to handle manipulation of flexible objects, and Pfeffer [131, 132] added an extra lowest-level layer to handle manipulators with joint-flexibility and non-ideal actuators. The lowest levels of the dual-arm workcell control system are heavily based on Pfeffer’s methodology. This chapter spans a wide variety of topics: low-level control issues, cooperative manipulation, and strategic programming of robot behavior/actions. Each of these issues has been the focus of substantial research in the robotics community. A detailed review of this vast field is beyond the scope of this thesis. Rather, the summary provided here will be restricted to reviewing related hierarchical approaches. Specific references on each one of the layers are included within the corresponding sections of this chapter. Hierarchical control systems for robotics have been proposed by many authors [194, 4, 154, 24]. Some of the most detailed are RCS [139, 6, 7] and NASREM [4, 5] from NIST. These approaches have already been discussed in 3.1. Classical robot-control hierarchies are programmed using a specialized robot language [22]. Sequential languages are not well suited to control highly asynchronous event-driven systems [157]. While some languages provide asynchronous extensions such as LM’s “guarded-commands” [86], many authors have opted for the use of more declarative or synchronous languages [58] such as Esterel [172], or constructs such as Statecharts [59] and finite-state machines [157, 75, 141, 161]. For instance, the ROTEX experiment [64] used finite-state machine transitions to control the different stages of a remote ORU replacement. Habib et al. [55] present a control system for the Yamabico-9 mobile robot composed of strategic and motor layers. The strategic layer is programmed using ROBOL-0, which is essentially a language in which to program a FSM whose states (“action modes” in their terminology) represent modes in the operation of the system. State transitions occur when pre-programmed conditions (which depend in external stimuli and sensed values) are met. Transitions trigger execution of a corresponding transition function. This model is similar to the one provided by ControlShell.4.x except ControlShell’s allows the next-state to depend on the return values of the transition functions.


139

The KAMRO workcell [1] uses petri-nets to sequence through the stages of pre-computed assembly plans. Coste-Manière, Espiau, Simon and Rutten [35, 36] propose the use of a “tasklevel” language containing a rich set of event-handling constructs which allow the program to wait for conditions to occur, loop while certain conditions hold, or even watch in parallel for sets of conditions and bind reactions to them. The language provides synchronization mechanisms between parallel applications. Programs in this “task-level” language are compiled into Esterel and ultimately executed as FSM programs. On the other hand, ControlShell.4.x’s Finite-State Machines are much higher level than those generated by Esterel, and programming them directly with the aid of available graphical tools can be as simple as (and often more intuitive than) writing “task-level” programs.

6.3 Control System Hierarchy

Task Control (Get, Assemble)

Object Control (Object Motions and Forces)

End-Point Control (Manipulator Motion and Forces)

Joint Torque Control (Flexibility, Friction, Cogging)

Figure 6.1: Hierarchical Control Subsystem We use a four layer hierarchy to control the two-armed robot. At the lower joint level, we use joint-torque sensors to compensate for the non-idealities of the motor (cogging, non-linearity) and the joint dynamics induced by the joint flexibility. The next layer, the arm level control, can now assume ideal actuation (i.e. that the motors deliver the desired torque to the link itself) and use a Computed Torque approach to compensate for the non-linear arm dynamics. The third object layer is concerned with object behavior and assumes that the arms are virtual multi-dimensional actuators that apply forces and torques to the object. The top layer implements elementary tasks such as object acquisition and release, insertions etc.


140

Robot commands (paths, acquire, etc.)

Strategic Controller ..

x obj f obj Object Controller

Sensor Processing and World Modeling

Object state

..

x arm f arm Arm state

Arm Controller

τ joint Joint state

Joint Controller

τ motor sensors

actuators

Workcell

Figure 6.2: Hierarchical Control Dataflow The strategic level processes robot commands and generates object and arm trajectories. If an object is being manipulated, the object controller generates arm references (positions, velocities, accelerations, and applied forces) based on the object reference trajectory, or else the arm trajectories are used as references for the arm controller. The arm controller uses these references to generate reference joint torques. The joint-controller commands motor torques so that the reference torque is delivered to each link. Each one of these controllers closes a feedback loop.

The control system for the two-arm robotic workcell uses the four-layer hierarchy shown in Figure 6.1. The idea behind the layered approach is to simplify the control design by decomposing it into functional layers. Each layer closes a control loop transforming the response of the system from the one presented by the layer below to the one provided to the layer above. These layers and their interfaces are selected so that each layer presents a more idealized system to the layer above, transforming the actual system dynamics into some more-ideal pre-selected set of dynamics. This


141

approach lends itself naturally to a multi-processor implementation. In the workcell experiment, each layer was executed on a different processor communicating through shared memory. The closed-loop bandwidth of the innermost (joint-control) loop is about 40Hz (Section 6.4), the next (arm-control) loop has a closed-loop bandwidth of 3.3 Hz while the outer (object-control) loop has a closed-loop bandwidth of 1.1 Hz. The theoretical basis for such layered frequency-separation approach is based on the application of singular perturbation theory to manipulator dynamics [83, 81]. Figure 6.2 illustrates the dataflow of this control hierarchy, and Table 6.1 summarizes the interfaces between the different layers. Aside from simplifying the control design, the layered approach has the advantage of permitting individual layers to be modified without affecting the remaining layers (provided the interface does not change). Therefore, individual layers can be changed to implement different controllers, control policies, or even compensate for different plant/object dynamics. For example, if the joint flexibility in the workcell arms changed or was eliminated, only the joint-control layer would be affected. Similarly, if an object had some dynamics (e.g. a flexible or deformable object), only the object layer would be affected [106]. .

6.4 Joint-Control Layer The purpose of the Joint-Control layer is to compensate for joint flexibility and non-ideal motor characteristics. To this end, the manipulators were designed and built with integral joint-torque sensors on both shoulder and elbow joints [131]. The concept of using fast joint-torque loops to modify joint dynamics so that they exhibit (viewed from the layers above) near-ideal torque response, has been understood for quite some time [96, 129, 133]. In fact, some commercial manipulators, such as the Robotics Research arm, the Zebra Zero arm , and the newer Barrett Arm incorporate such joint-torque loops. In order to close a local torque loop around each joint, it is first necessary to identify the motor-drivetrain-joint subsystem that exists between the commanded torque input and the measured (delivered) torque output. This is a non-linear system (primarily due to motor friction and cogging). During his research, Pfeffer explored the use of cogging compensation to linearize the system prior to identification and control-design, and determined this extra step was unnecessary because once the linear torque loop was closed, the cogging compensation did little to add to its performance.


142

Layer Strategic Control

Responsibility Implement strategic commands: follow trajectories, pick up moving objects, move objects

Interface to layer below Robot interface described in Chapter 3

Object Control

Command arms so that the object behaves like a virtual impedance on each of its DOF

Object forces and accelerations

Arm Control

Change arm end-effector dynamics so that it behaves like a virtual Cartesianspace impedance

Endpoint force and acceleration

Joint-Torque

Compensate for joint flexibility and nonideal motor characteristics (cogging, friction). Make actuators appear to be ideal torque sources

Torques applied to each rigid link at the joint

Table 6.1: Summary of control layers and their interfaces. The control system for the manufacturing workcell consists of four layers. Each layer performs a well defined function and modifies the apparent system dynamics to simplify the task of the layer above.

Although this result may not hold in general, it was certainly the case for the Manufacturing Workcell. Identification of each of the four motor-drivetrain-joint subsystems (shoulder and elbow joints on each of the two arms) was performed by collecting frequency-response data with the aid of a Schlumberger Solartron model 1254 frequency response analyzer. This instrument generates a periodic analog signal1 to drive the system, and measures the steady-state (amplitude and phase) response of the system on as many as four analog signal outputs. The analyzer measures the amplitude and phase at the fundamental frequency (unless instructed otherwise). The frequency responses were taken with a real-time computer running a weak position-control loop on both joints in order to keep the manipulator close to its nominal position. In principle, the behavior of the flexible subsystem could change drastically as a function of arm configuration and payload. However, Pfeffer designed the actuators and (low) gear ratios in such 1

The Solartron is capable of generating periodic signals of several forms. For the identification, sinusoidal inputs were used exclusively.


143

way that the effective link inertias are much greater than the actuator inertias, effectively decoupling each actuator-joint subsystem. Pfeffer’s derivation is presented in [131], Chapter 4.

For the identification of the shoulder subsystem, the elbow was regulated at 90 relative shoulder

angle (position of maximum decoupling). For the identification of the elbow subsystem, the shoulder was mechanically kept fixed. Since the system is not linear, the frequency response varies depending on the amplitude of the driving sinusoid. Only frequency responses where the peak-to peak value is under 4.0 Nm are of interest, because this torque is never exceeded during operation (this is the limit that was chosen for safety considerations). Frequency sweeps with peak torques of 0.25 Nm, 0.5 Nm, 1.0 Nm, 2.0 Nm and 4.0 Nm were performed. The results were very similar for all except the smaller (0.25 Nm and 0.5 Nm) peak values. (This was to be expected since the cogging torque for the shoulder and elbow motors has been determined to be on the order of 0.5 Nm [131]). The system identification was performed on the frequency response to the 1.0 Nm peak-torque excitation, which appeared most representative of the joint subsystem response over the intended torque range. Results of the identification and model fitting are shown in Figure 6.3 for the right shoulder subsystem. A second-order model was fitted to the frequency response. The parameter-fitting technique used was developed by Pfeffer [130]. This fit identifies the poles and zeros of the

p

J (p), representing the mismatch between the magnitude of the experimental frequency response jHe (w)j and that of the model jHm (w; p)j.

second-order model, , that minimize a cost function,

J (p) =

frequencies X

k

log(jH (w )j) , log(jH (w ; p)j)2 e k m k

The use of only the magnitude of the frequency response, and the selection of this particular cost-function, have been justified by previous researchers [168, 130]. Once a model has been fit to the response of each joint subsystem, it is transformed into state-space form, discretized (using the zero-order-hold approximation) for the controller sampling rate (360 Hz), and transformed to balanced-realization form. These transformations produce a model of each subsystem in the discrete state-space form:

x[k + 1]

y [k]

= =

x[k] + , u[k] H x[k]

A predictive estimator and a state-space controller were designed for each subsystem using the LQE/LQG optimal estimator/controller formalism [50]. This approach results in the following control equations:

x[k + 1]

=

x[k] + , u[k] + Lp (y[k] , H [k]x[k])

(6.1)


144

Magnitude of measured Joint-Torque / Commanded Torque ( A = 1.0 Nm )

2

Amplitude ratio

10

1

10

0

10

-1

10

0

1

10

10 freq (Hz.)

2

10

Phase of measured Joint-Torque / Commanded Torque ( A = 1.0 Nm )

Phase (Hz.)

200

0

-200

-400 0 10

1

10 freq (Hz.)

2

10

Figure 6.3: Experimental frequency response for the motor-drive-train-link plant of the right arm. The frequency response was obtained using sine sweeps. A second-order model has been fit to the amplitude data. This model has proved to be an adequate approximation to the true plant dynamics in the frequency range of interest. The second-order model has a DC gain of 7.85, a resonant frequency of 11.4 Hz, and a damping ratio of 0.226

u[k + 1]

=

K (Nx ur , x[k + 1]) + Nu ur

(6.2)

The estimator and control parameters are summarized in Appendix B (tables B.2 and B.1). Figure 6.4 shows the Z-plane location of the the poles introduced by the controller and estimator as well as the closed-loop system roots for the right-shoulder subsystem. The experimental step-responses shown in Figure 6.6 confirm the expected rise-time, although the system has considerably more overshoot. This control-system is directly implemented as a set of ControlShell.4.x components, as illustrated in Figure 6.5.


right shoulder pole/zero locations 1 0.5

145

Loop Magnitude

2

10

1

10

0 0

10

-0.5

-1

-1 -1

0

1

10

0

2

10

10

Step Response

4

10

Loop Phase

1.5 -50

1

-100 0.5 -150 0

0.05

0.1

0.15

0

0.2

2

10

10

4

10

Figure 6.4: Closed-loop roots for the motor-drive-train-link plant and controller of the right shoulder. The controller was designed using the LQE/LQG state-space technique for a sample rate of 360 Hz. The location of the closed-loop poles represent an adequate compromise between performance and robustness. This controller has a gain margin of 2.7 and a phase margin of 46 degrees.

RA_Sh_CtrlNx RA_Sh_CtrlNu

RA_Sh_RefJT

RA_Sh_CtrlK RA_Sh_EstPhi

GblTorqueTweekGain

RA_Sh_SatLim

RA_Sh_EstGamma Nx Nu ref

RA_Sh_EstLp RA_Sh_EstH

u

Phi Gam Lp

H x_b

y

y_b

K Ktw u

RA_Sh_CtrlJT

lim inV

lim

RA_Sh_CommandedJT

x stateSpaceController

saturator

RA_Sh_Ctrl

RA_Sh_Sat

RA_Sh_EstState

RA_Sh_EstJT

RA_Sh_MeasuredJT predictiveEstimator RA_Sh_Est

Figure 6.5: Joint-control layer dataflow. Shown are the components required to control the shoulder and elbow motors of the right arm. Two main ControlShell.4.x components are used to control the torque at the robot joints. A state space controller component (stateSpaceController) implements a state-space control law: u = K (Nx r , x) + Nu r, where r is the reference torque (ref) and u is the commanded motor current. A state space predictive estimator (predictiveEstimator) estimates the system state: xk+1 = xk + u + Lp (y , H xk ). The saturator component is used to enforce software limits on the actuator torques.


146

Joint−torque step response away from contact (left shoulder)

10 closed−loop open−loop

Torque (N/m)

5

0

−5

−10 −0.1

−0.05

0

0.05

0.1

0.15 seconds

0.2

0.25

0.3

0.35

0.4

Joint−torque step response into contact (left shoulder)

0 closed−loop open−loop

−2 −4

Torque (N/m)

−6 −8 −10 −12 −14 −16 −18 −20 −0.1

−0.05

0

0.05

0.1

0.15 seconds

0.2

0.25

0.3

0.35

0.4

Figure 6.6: Joint-torque step response of left shoulder subsystem. This Figure compares the open-loop and closed-loop responses of the right shoulder subsystem. The closed-loop system has a bandwidth of 40 Hz and shows good steady-state tracking and fast, well-damped transient response.


147

6.5 Arm-Control Layer The control achieved at the joint-level allows the arm-control above it to treat the manipulator as if it had ideal motors and the commanded torques were delivered perfectly to the rigid links. Ultimately any arm controller must generate joint-torque commands based on the manipulator’s desired state; however, different controllers differ in two related but somewhat decoupled issues:

The error law or control policy: selection of the space in which errors are computed and specification of reference behavior when reacting to external disturbances. Typical choices include position and velocity errors in Cartesian or joint space, impedance relationships and hybrid position/force control schemes.

The implementation of the policy. The type of controller used to enforce the error law. Typical choices are inverse dynamics (also referred to as computed-torque) methods, kinematic controllers and independent-joint-space controllers.

Inverse dynamics controllers, which are the highest-performance control schemes, require a good kinematic and dynamic model of the manipulator. The following sections describe the control scheme used for the workcell manipulators.

6.5.1

Arm Controller

The arm controller used in the Manufacturing Workcell combines two controllers: a highperformance inverse-dynamics controller used for manipulator configurations away from kinematic singularities, and a low-performance (yet robust) joint-space PID controller used near singularities. These two controllers are blended in a transition region. The high-performance controller implements an impedance relationship at an operational point located at the center of the tool (gripper), and uses an inverse dynamics (computed-torque) approach to enforce this relationship. This particular choice of control law (policy) and implementation is usually referred as “arm impedance control” and was introduced by Hogan [65]. The advantages of arm-impedance control in the context of flexible drive-train robots and object manipulation have been described in previous research [131, 154]. The structure of the arm-impedance controller is described below for the simpler case of a SCARA manipulator. The general formulation can be found in [157, 131]. Table 6.2 summarizes the meaning of the different symbols used in the description.


Symbol Mimp x; x˙ ; x¨

Dimension 44 41

xref ; x˙ ref ; x¨ ref

41

Kp ; Kv fext q; q˙ ; q¨ M (q) B (q) C (q) g(q) J(q)

44 41 41 44 43 44 41 41 41

148

Meaning Desired virtual mass (diagonal matrix). Actual Cartesian position, velocity and acceleration of the operational point. Reference Cartesian position, velocity and acceleration of the operational point. Desired virtual stiffness and damping (diagonal). External force acting on the operational point. Vector of generalized coordinates and time derivatives. Configuration-dependent mass matrix. Configuration-dependent matrix of Coriolis term. Configuration-dependent matrix of centripetal terms. Configuration-dependent gravity vector. Jacobian relating joint-rates to operational-point velocities. Torque vector.

Table 6.2: Notation for arm-impedance controller derivation. The above sizes apply to a 4-DOF SCARA manipulator. The cartesian vector x contains the position and rotation about the vertical axis of the operational point.

First, the error law specifies that the manipulator operational point (normally the end-effector or tool) must obey the following impedance relationship:

Mimp x = Mimp xref + Kp (xref , x) + Kv (xref , x) + fext

(6.3)

Second, the actual equation of motions of the manipulator are:

M (q) q + B (q) [q; q] + C (q) [q2] + g(q) + Jt(q) fext =

(6.4)

The cartesian coordinates of the operational-point are related to the configuration-space coordinates through the manipulator Jacobian as:

x = J(q) q ) x = J(q) q + J(q) q ) q = J(q),1 (x , J(q) q)

(6.5)

The impedance relationship (6.3) can be enforced if actuator torques are chosen in (6.4) such that the resulting acceleration

q (equation (6.5)) results in operational-point accelerations x that

verify the impedance relationship. This scheme is illustrated in Figure 6.7. The kinematics and equations of motion for this type of SCARA manipulators have been derived by previous researchers [66]. These equations are included for completeness in Appendix D.


x ref . x .. ref x ref

Impedance Equation f

.. x cmd

Kinematic Transf.

.. q cmd

. qq

ext

f

ext

Equations of motion

149

τ ref

Joint Controller

Actuators

. qq

World

Sensor processing and World Modeling

Sensors

Figure 6.7: Block diagram for the computed-torque impedance controller. The arm controller enforces an impedance relationship at the arm operational point (OP). Given the cartesian error between the reference (desired) and actual states for the OP, the impedance relationship specifies the commanded acceleration x¨com for the OP (i.e. the acceleration that will satisfy the impedance relationship). This acceleration is transformed into the equivalent joint-accelerations q¨ cmd using the manipulator kinematics. Given q¨ cmd , the manipulator dynamics are used to determine the reference joint torques ref that will achieve this joint-acceleration. These joint torques become the reference command for the joint-control layer below.

| det(J)| Joint-Space trajectory

Joint-Space Error Law

..ref q pid

1 0

min max

| det(J)| Operational-Space trajectory Operational-Space Error Law

..ref q os

+

..ref q

Joint torque command Inverse-Dynamics Controller

1 0

min max

Figure 6.8: Merging a cartesian-space computed-torque controller with a joint-space PID controller. The strategic module is required to specify both the cartesian-space and the corresponding joint-space trajectories for any arm motion. These trajectories are run in parallel through a cartesian-space error law (see Figure 6.7) and a joint-space error law. The commanded joint accelerations q¨ from the two controllers are combined to generate the actual joint-acceleration command q¨ cmd for the inverse dynamics controller. The combination is a weighted sum with weights that depend on how close the manipulator is to a singularity as measured by the value of the determinant. The transition interval corresponds to relative elbow angles jqel j between 0.1 and 0.2 radian.

The disadvantage of this inverse-dynamics controller is that it cannot be used to transition the arm through a kinematic singularity (which occurs whenever the elbow is fully extended). At the kinematic singularity, the Jacobian matrix becomes singular, and its inverse cannot be computed as required in equation (6.5). The inverse-dynamics controller was modified so it would


150

be stable even at the singularity by using a Jacobian pseudo-inverse in equation (6.5) (i.e. whenever the determinant of the Jacobian is below a threshold, the pseudo-inverse is used instead of the Jacobian inverse). Although this modified controller works well in regulation, it cannot be used to follow reference trajectories that cross the singularity, because these trajectories cannot be specified exclusively in operational-space. In essence, the workcell manipulators are redundant: There are two arm configurations (“elbow in” and “elbow out”) that result on the same cartesian location of the operational point. Two different (joint-space) trajectories that start from the same initial state, go through the singularity, and come out with opposite elbow configurations are indistinguishable to the operational-space controller. It is important to be able to cross the singularity under control to take full advantage of the arm workspace. For this reason a hybrid approach was developed: Commanding an arm trajectory requires both the operational-space trajectory and the corresponding joint-space trajectory to be simultaneously specified. The controller monitors the proximity of the arm to a singularity (by evaluating the determinant of the Jacobian matrix). Away from the singularity the inverse-dynamics controller is used. Near the singularity, a pure joint-PID controller is used. In the transition region, a weighted combination of the reference commands from both controllers is used. In either case, the commanded joint-space accelerations are fed to the inverse-dynamics controller as illustrated in Figure 6.8. The parameters used for both controllers are summarized in Table C.1 (Appendix C). A word on the selection of impedances and virtual masses: Note that for

Kp was chosen (through an iterative process) to achieve small steady-state tracking errors and Kv adjusted to keep the system slightly underand

Kv =Mimp

fext = 0 only the ratios Kp =Mimp

are relevant. In view of this,

damped (for faster performance). The selected gains correspond to a closed-loop bandwidth of

fcl = 3:26Hz, and a damping ratio of cl = 0:6. When the arm is in contact with the environment,

fext and the operational point is commanded to respond with the same acceleration as a mass of value Mimp . For this reason, larger Mimp results in more stable, the force/torque sensors measure

yet “slower” contact behavior. The value finally chosen was a compromise for the required tasks. Gains for joint-space error law were selected such that they produced commanded accelerations of similar magnitude as the operational-space error law for small joint errors at the singularity boundary. In other words, boundary.

Kpid p

=

J(q) Kp =Mimp

and

Kpid v

=

J(q) Kv =Mimp

at this


6.5.2

151

Arm Controller Performance

Figure 6.9 illustrates the implementation of the arm-level controller in terms of reusable software components. The full inverse-dynamics computed-torque controller for both arms was run at 100 Hz on a dedicated processor board2 . A straight-line slew that pushes the edge of the controller’s tracking performance is illustrated in Figure 6.10. This slew moves the arm across its workspace (maintaining the same configuration) in 2.5 seconds, with extremely good tracking. This performance could not be achieved by the jointbased PID controller, as was demonstrated by previous research [131]. Figure C.1 in Appendix C illustrates the step response of the arm-control layer. Figures 6.11 and 6.12, illustrate control system performance when the arm traverses a kinematic singularity. The hybrid controller is able to control the arm motion smoothly through the transition. This thesis does not make the claim that the approach followed here to achieve stable performance through the singularity is valid in general. Merging a singularity-free (yet low-performance) controller with a high-performance controller in such a way that the singularity-free controller is used in proximity to the singularity is simple and intuitively appealing (in fact is an approach commonly followed by so called fuzzy controllers). However, there are a multitude of issues to be addressed more formally for this approach to be rigorous. It is not even clear under which circumstances a weighted (configuration-dependent) average of two controllers results in a stable controller. This investigation could be the topic of continuing research.

6.6 Object-Control Layer The purpose of the object-control layer is to allow the workcell to manipulate objects either singlehandedly or cooperatively using both manipulators. In this section, the term cooperative control is used in contrast with coordinated control. In either case an object reference trajectory is specified; but in coordinated control, that trajectory is transformed (through the grasp kinematics) into reference arm trajectories, and each individual arm is controlled from its own trajectory (regardless of what the other arm, or the object are doing). In cooperative control no individual-arm trajectories are computed. Rather, the arm commands are computed directly from the object trajectory and error. This closes another control loop on the object state. Figure 6.13 contrasts these approaches. True cooperative object control often requires force sensing at the arm end-effectors so that the interaction forces can be accurately controlled. 2

A VME-based single-processor computer containing a 33 MHz m68040 processor.

out

LeftEndPtStateRef

Figure 6.9: Arm-control layer dataflow.

C out

ioM A

B

C out

zero4x1

out

LeftArmJointDynStateRef

LA_Encoder2JointVel

LA_Joint_Vel

LA_Encoder2relCoor_B

LA_Encoder2relCoor_A


Y

X

LA_Joint_Pos

LA_Encoder2LeftJoint_ioMap

LA_Encoder2JointPos

perigee

shm

B

LA_CopyFromShmJointDynStateRef

acc

vel

pos

LA_EndPtMin

0.4871

x_a

sin

qDy

qDy

J_T

J

x_d

acc

vel

pos

LA_EndPt_PosRef

LA_DJoint_AccRef

LA_DJoint_VelRef

LA_DJoint_PosRef

LA_KinVisTargetLoc

LA_sin_cos

LA_DJoint_Vel

LA_DJoint_Pos

LA_J_T

LA_J

LA_EndPt_Vel

LA_DJoint_RefSplitState

SplitState

sta

LA_Kinematics

scara4dofKin

qRe

LA_EndPt_Pos

LA_DHParameters

glo dHP off qRe x

out

matrix2in1out

inp

Opc inp

CSMatSelectMax

LA_LimitEndPtRef

LA_Vision2Base

LA_EndPt_AccRef

LA_EndPt_VelRef

LA_EndPt_RawPosRef

LA_JointPosFilteredCorrection

LA_Encoder2relCoor_B

LA_Encoder2relCoor_A


Y

X

ioM A


Encoder_Vel

Encoder_Pos

Constant

any

LA_EndPtMinZOH

SplitState

sta

LA_Encoder2LeftJoint_ioMap

LA_CopyFromShmEndPtStateRef


shm

perigee

LA_CopyFromShmEndPtAppliedForce

LA_EndPtRefSplitState

LeftEndPtAppliedForceRef


out

perigee

shm 0.0 0.0 0.0 wrapYaw

des

des

vel

0.0

des

des

vel

x2

x1

x0

A y

A y

affineElemByElemMap

x2

x1

x0

LA_JointForcesToAcc

LA_DJoint_ForceCommand

LA_DJoint_1/Mimp

LA_EndPt_ForcesToAcc sin

J_T

J

qDy

qDy

Jdo

Jsi

det

Jin

dHP nor x_d qDy

0.4

LA_Jdot

LA_DJoint_AccCommandFromDJoint

LA_EndPt2DJointAcc

LA_detJ LA_Jsingular

LA_Jinv

LA_DJoint_AccCommandFromEndPt

LA_DHParameters

scara4dofCartToJointAcc

LA_EndPt_AccCommand

LA_EndPt_1/Mimp

affineElemByElemMap

LA_DJoint_MeasForce

any Constant

wrapYaw

0.0

LA_DJoint_MeasForceZOH

0.0

pidControl

LA_DJoint_Control

LA_EndPt_MeasForce

LA_EndPt_ForceCommand

Kp Kv Ki Ktw int int vel err pos out

0

LA_DJoint_Ki

LA_DJoint_Kv

LA_DJoint_Kp

LA_EndPt_Control

pidControl

Kp Kv Ki Ktw int int vel err pos out

0

LA_EndPt_Ki

LA_EndPt_Kv

LA_EndPt_Kp

tor

Constant LA_EndPt_AppliedForceZOH

LA_MergeEndPtAndJointControl

any

LA_EndPt_AppliedForce

LA_DJoint_AccCommand

MergeCTandPIDcontrol

det

tor

det tor

LA_Det_J_Interval

eom Jtr tor

app

q_d

q_d

q

LA_DerivativeFiltDenCoefs

y

b sam y

sta

MergeState

acc

vel

pos

sta

LA_DJoint_Acc

LA_JointDynMergeState

MergeState

acc

vel

pos

perigee

CopyCSMatToSharedMemory

shm inp

perigee shm inp

perigee shm inp

LA_CopyToShmJointDyn


LeftArmJointDynState

LA_CopyToShmEndPt


LeftEndPtState

LA_CopyToShmArmJointTorqueCommand

LeftArmRefJointTorque

LA_EndPtMergeState

LA_EndPt_Acc

100

b sam

filter

a

a

filter

LA_DiffJointVel

x

LA_DerivativeFiltNumCoefs

x

LA_DiffEndPtVel

LA_EOM

scara4dofEOM

LA_EOMParameters

CHAPTER 6. HIERARCHICAL CONTROL SYSTEM 152


153

Trajectory tracking performance: 2.5 second straight-line slew 0 reference trajectory actual trajectory

position along y (cm)

-20

-40

-60

-80

-100

-120 -120

-100

-80

-60

-40 -20 position along x (cm)

0

20

40

Figure 6.10: Experimental tracking performance: straight-line path Illustration of the tracking response of the right arm. The reference is a fifth-order polynomial trajectory for the arm endpoint, commanding it to follow a 1.75 m straight-line path in 2.5 sec. This trajectory requires accelerations of up to 1:2 m/s2 . The maximum tracking error is 1:4 cm.

There have been many approaches to cooperative object control [151, 79, 60, 205, 187, 142]. This experiment uses the Object Impedance Control (OIC) approach originally developed by Schneider [154, 155]. This approach draws from Hogan’s impedance control concept [65] and the work of Nakamura [114]. OIC was originally developed for fixed manipulators handling a rigid object. This work was later extended to manipulators on a mobile base [190, 189, 42] and manipulation of objects with internal dynamics [105, 106]. These researchers have demonstrated the utility of the OIC approach to cooperative-object manipulation. Detailed derivation of the OIC approach can be found in [155, 131]. This section will simply state the main concepts and relevant equations.


position tracking (x and y)

154

position tracking (rotation)

1

6

radians

meters

0.5 0

2

−0.5 −1

4

2

4 seconds

6

0

8

0.1

0.01

0.05

0 −0.01 −0.02

2

4 seconds

6

−0.1

8

0

radians/second

meters/second

1 0.5 0

2

4 seconds

6

2

4 seconds

6

8

velocity tracking (rotation) 0.5

−0.5

−0.5 −1 −1.5 −2

8

velocity tracking error (x and y)

2

4 seconds

6

8

velocity tracking error (rotation)

0.2

0.5

0.1

radians/second

meters/second

8

−0.05

velocity tracking (x and y)

0 −0.1 −0.2

6

0

1.5

−1

4 seconds

position tracking error (rotation)

0.02

radians

meters

position tracking error (x and y)

2

2

4 seconds

6

8

0

−0.5

2

4 seconds

6

8

Figure 6.11: Experimental tracking performance: path through kinematic singularity. Tracking performance on X, Y, yaw position and velocities. The above plots illustrate reference and actual trajectories (top line), the tracking error (second line), the reference and actual velocities (third line), and velocity tracking error (fourth line). The vertical lines indicate the region where the arm is at a kinematic singularity. In the center band between the innermost vertical lines, the arm is under pure joint-based control, in the adjacent bands, cartesian and joint control commands are averaged (see Figure 6.8), away from the singularity pure cartesian control is used.


155

Figure 6.12: Animation of trajectory through kinematic singularity. This figure animates the data collected during the trajectory shown in Figure 6.11.

The OIC is a model reference controller which enforces an impedance relationship on the state (position, velocity, and acceleration) of a certain point in the object (the object’s Remote Center of Compliance or RCC)3 :

Mimp x = Mimp xref + Kp (xref , x) + Kv (xref , x) + fext

(6.6)

The above equation can be interpreted as representing the equations of motion of a virtual object that is affected both by the forces applied to the object

fext and a virtual force that makes it behave

as if it were attached to the reference trajectory by a spring and dash-pot on each degree of freedom. The dataflow of the OIC is illustrated in Figure 6.14. First the RCC and a reference “state” for the RCC,

xref ; xref ; xref ) are specified.

(

Given the actual object state (

x; x), and the rigid-

body transformation to the RCC, the impedance relationship of equation (6.6) can be used to

x (the acceleration that the RCC should have to satisfy the impedance relationship). The acceleration of the RCC (x ) can now be transformed back to obtain the required object acceleration xcom. Since the actual EOM of the body and the grasp transforms are known, the required

determine

manipulator-end-effector forces and accelerations can be computed. These required end-effector 3 This point does not have to be physically in the body. It is sufficient that it remains fixed in any body-fixed reference frame. That is, it is related through a rigid-body transformation to the object’s position.


Coordinated Object Control

156

reference arm trajectory Arm Controller

Grasp Kinematic Transf.

arm state

reference object trajectory

reference arm trajectory

Grasp Kinematic Transf.

arm state command

Cooperative Object Control reference object trajectory

Arm Controller

Arm Controller

arm state

Object Controller

arm state command

Arm Controller

Manipulator

Sensors & World Modeling

Manipulator

Manipulator

Sensors & World Modeling

Manipulator

object state

Figure 6.13: Comparison of cooperative object control with coordinated object control In coordinated object control the reference trajectory for the object is merely transformed using the grasp kinematics into reference trajectories for each arm. Each arm is then controlled independently from its own trajectory. Cooperative object control generates no arm trajectories directly. Instead, arm commands are generated from the reference object trajectory and the error between the trajectory and the actual object state. Cooperative object control thus closes an additional, important control loop on the object state.

accelerations/torques become commands for the arm-level controller. In case there is redundancy solving the required manipulator forces on the object, the redundancy can be used to control the internal forces the object is subject to. The above OIC formulation is restricted to situations where the manipulator arms holding the object are not at a singular configuration. This restriction is enforced by the strategic-control layer. A nice feature of the OIC approach is that the EOM of the object decouple the arms so that, assuming each arm can compute locally its own required reference end-effector state and applied forces4 , each arm would only need to know about its own dynamics to control itself. This 4

This computation transforms the required object acceleration through the known grasp transforms and distributes the required force/torque on the object among the two arms.


157

LeftEndPtStateRef

barWMConveyorState

VC_T_bar

barWM5thOrderMaxAccel

BarVirtMass

Bar_T_VC BarTensionForce max act des

BarImpStiffness

BarMass

BarImpDamping BarImpIntEnable

obj_T_right_armWM 0

obj_T_left_armWM

2 right_armWMstatus

barWM5thOrderObjStateTraj barWMVipMaxVel

poi ena ena poi poi

obj_T_left_armWM

ena

left_armWM_T_obj

BarImpIntGain BarImpIntForceFract BarPosForceLim

left_armWMstatus

barWMVipMaxAccel

BarNegForceLim

TwoPtKinState

barWMState

arm gra obj arm gra obj Bod ten obj VC_ Vir Imp Imp Imp Imp Imp Imp Pos Neg Obj gra

barWMLeftArmTwoPtKin max max ini sta

BarImpIntVelThresh

right_armWM_T_obj

fifthOrderStateTrajectory

barWMDesState

Obj

gra ObjImpControl

Obj

barWMEndPtVIP 0 VIP_GenerateState

2

barObjImpControl

barWMExtForce

gra

LeftEndPtAppliedForceRef

gra

RightEndPtAppliedForceRef

obj_T_right_armWM

poi ena ena poi poi RightEndPtStateRef

ena

right_armWM_state

TwoPtKinState left_armWM_state barWMRightArmTwoPtKin conveyorWMVisionState ConveyorDispl 0.1 BarSearchHeight

"barWM"

2.0

0 .038

vis

"barWM" obj len wid hei tab con bod

"homann"

vis bod sea vis VisionBody dat

barWMState

vis

val

barWMManipValidity

bod

arm

man

bod

arm

con barWMVisionState BarDataAge

vis

arm arm vis bod

bod


man man WM_TrackGraspableBody barWM__NDDS

kin

WM_TrackConveyorObject


barVisionBody

ManipBodyManager barWMTrackConveyorObject barWMManipBodyManager

objKinStateFromLeftArmGrasp objKinStateFromRightArmGrasp

Figure 6.14: Dataflow of the object-impedance controller Implementation of the object-control layer using ControlShell.4.x components. This diagram illustrates the control loop for the “barWM” object. The reference trajectory barWMDesState can originate from a planned via-point trajectory (VIP Generate component), a locally generated intercept (fifthOrderStateTrajectory), or a tracking (WM TrackConveyorObject) trajectory. The ObjImpControl components combine the reference state, actual state, and (estimated) external force on the object to generate reference states and applied forces for each one of the manipulators. The ObjImpControl component uses the specific grasp transforms, mass properties, and virtual object behavior set by the strategic-layer (using the private world-model interface) to implement the desired impedance relationship.

feature makes the algorithm easily parallelizable and extensible to multiple independent robots that cooperate in the manipulation. The work of Dickson [42] addresses these issues. Figure 6.15 illustrates the tracking performance of the OIC for a cooperative object motion in free space animated in Figure 6.16. Comparisons with other object-level controllers and contact experiments have been documented extensively by Schneider [155, 154] and Pfeffer [131].

6.7 Strategic-Control Layer The underlying object and arm-control layers allow the strategic layer to command reference object and arm trajectories. The strategic layer performs the following five functions: (1) It generates all the required trajectories, (2) it keeps track of which objects are being grasped by which manipulator(s), (3) it sequences through the different steps required to achieve individual robot commands (defined


position tracking (x and y)

158

position tracking (rotation) 3

0

radians

meters

2.5 −0.2 −0.4

1.5

−0.6 −0.8

2

2

4 seconds

1

6

position tracking error (x and y)

2

4 seconds

6

position tracking error (rotation)

0.01 0.02 0.01

radians

meters

0.005 0

0 −0.01

−0.005 −0.02 −0.01

2

4 seconds

6

−0.03

velocity tracking (x and y)

radians/second

meters/second

0.4 0.2 0 2

4 seconds

1 0.5 0 −0.5

6

velocity tracking error (x and y) 0.2

0.02

0.1

0 −0.02

2

4 seconds

6

2

4 seconds

6

velocity tracking error (rotation)

0.04 radians/second

meters/second

6

1.5

0.6

−0.04

4 seconds

velocity tracking (rotation)

0.8

−0.2

2

0 −0.1 −0.2

2

4 seconds

6

Figure 6.15: Experimental tracking performance of the object-impedance controller Tracking performance on X, Y, and yaw position and velocities. The above plots illustrate reference and actual trajectories (top row), the tracking error (second row), the reference and actual velocities (third row), and velocity-tracking error (fourth row), for the object trajectory shown in Figure 6.16.


159

Figure 6.16: Animation of trajectory followed by the object under cooperative control This figure animates the data collected during the trajectory shown in Figure 6.15.

in Chapter 3), (4) it monitors the progress and safety of the system, and (5) it takes corrective actions when failures occur.

6.7.1

The Finite-State Machine Programming Model

The strategic control of the workcell uses the Finite-State Machine Engine available within the ControlShell.4.x programming system. This approach to strategic robot-control was introduced by Schneider [154, 157] and later expanded by Ullman [189]. In their work, they developed strategic controllers based on a finite-state-machine programming model (FSM) allowing individual arm motions, dual-arm capture of a single moving object, and cooperative manipulation of an object. The dual-arm workcell must perform the aforementioned tasks. Additionally, the dual-arm workcell needs to handle concurrent manipulation of multiple objects (e.g. one object per arm), mixed arm and object motion (e.g. an arm is manipulating an object while the other is moving), and in general be extensible to any number of arms/objects. These needs represent a significant departure from the previous work in that they require multiple FSMs to be active concurrently so that different parts of the total system (e.g. each arm) can operate independently. In other words, some operations of the robotic workcell require independent, yet coordinated, strategic control of each arm.


160

CurrentState Stimulus1 Stimulus2 TransitionFunction1 ReturnValue1

NextState1

ReturnValue2

TransitionFunction2 NextState2

ReturnValue3

ReturnValue1

NextState5 ReturnValue2

NextState3 NextState4

Figure 6.17: ControlShell.4.x’s Finite-State Machine model The arrival of a matching stimulus triggers the execution of the corresponding transition function whose return value determines the next state.

ControlShell.4.x’s FSM has been described in detail and compared with robot-programming techniques in [154, 157]. Here ControlShell.4.x’s FSM model is described briefly so that the subsequent state diagrams are meaningful. ControlShell.4.x’s FSM is an extension of the traditional FSM model: A state represents the internal configuration of the system and encodes the information needed to determine its future behavior. A state transition occurs as a response to an external stimulus. For each state and stimulus pair, there is an associated transition function. The arrival of valid stimulus triggers the execution of the corresponding transition function whose return value determines the next state. Stimuli that do not have an associated transition-routine are simply discarded (or handled by a pre-specified catch-all state). A stimulus is simply a message sent to the FSM and can be generated from anywhere in the system. In ControlShell.4.x, a FSM program is built using a graphical editor where the state, stimulus, and transition functions/return values are specified. The automatically-generated code is then linked to user-written code that implements the transition functions. ControlShell.4.x’s FSM facility contains other useful constructs to facilitate building reactive systems. The interested reader may find further details in [158, 159, 144].

6.7.2

Strategic Control of the Workcell Using FSMs

The strategic control of the workcell uses several concurrently-active FSMs as illustrated in Figure 6.18. The architecture consists of a Workcell Daemon and several FSMs (one per individual


161

arm). These FSMs communicate by sending stimuli to each other. The Workcell Daemon receives robot commands through the Robot Interface (Chapter 3), and sends stimulus to one (or more) of the individual-arm FSMs. The individual FSMs control the arms and synchronize with each other while informing the World Modeler of the significant events. The strategic layer affects the object and arm control layers by modifying their dataflow (usually achieved by changing configurations, that is, the active components in the system) and the parameters used by the different components. In this manner, the strategic level can change control modes, generate/start/stop trajectories, and modify controller parameters (e.g. grasp transforms, impedance gains). Robot Commands

Robot Interface

Strategic Control Layer

Workcell Daemon

Command Stimuli

Arm#1 Finite State Machine

Command Stimuli

Status Report

Arm#2 Finite State Machine

Coordination Stimuli

Modify Dataflow: Configurations, control-parameters, etc.

Object and Arm Control Layers

Figure 6.18: Architecture of the strategic-control layer The strategic-control of the workcell uses a Workcell Daemon and a set of State-machine programs (one per manipulator in the workcell). The Workcell Daemon interfaces to external subsystems though the Robot Interface. Commands arriving through this interface are processed and sent as stimulus to one (or more) of the arm-FSM. This approach scales to more than two arms by adding an extra arm-FSM for each new arm.


162

Extensions to ControlShell.4.x’s FSM Model In order to make the approach easily expandable to any number of manipulators in the workcell, we have pursued the concept of a FSM program as defining an abstract data type (ADT) (as opposed to an instance of the type) from which multiple FSM-instances can be created. This view interprets a FSM as defining a behavior specification in terms of how to respond (which transition functions to call) to different stimuli. An FSM-instance binds a specification to private data that can be accessed by the transition-functions (similar to what happens when a method is invoked in an object in OO programming languages). This view of FSM-programs as ADTs allows the strategic controller of the workcell to scale easily to any number of manipulators. New manipulators simply instance fresh copies of the armFSM ADT; each instance binding to different private data containing the specific information for that manipulator (kinematics, location, state etc.), which is obtained from the manipulator’s name by querying the World Modeler. This approach is quite powerful but it does not come for free: The Arm-FSM and transition-functions must be carefully designed so that the different FSM-instances can interact and cooperate as required, even though the specific names (or even the number) of the arm-FSM instances are not known in advance. An example will help clarify this point: when picking up a part from the conveyor with two arms, both arms must synchronize so that they close the grippers simultaneously, and once the object has been grasped, cooperate to move the object. For this to work, the individual arm-FSMs must query the world modeler at run-time to find out with which other arm-FSM they need to synchronize. A simple extension to ControlShell.4.x’s code-generation process provided this functionality. The next two sections describe the different control modes (or configurations) the workcell can operate in, introduces the workcell daemon and the individual arm FSMs in detail, and illustrates how the FSM strategic programs utilize the different control modes to capture and manipulate objects in the workspace. Reference Trajectories In the absence of events (external or internal), different strategic-level FSMs remain in the same state, and the operation of the workcell is determined by the dataflow layer. Thus, workcell operation is fully characterized by two complementary dataflow aspects: trajectory-generation (the source of reference states) and the controller used. Loosely speaking, trajectory generation specifies the desired behavior of the system (trajectories for the arms and object to follow) while the controller


163

is responsible for commanding the actuators so that the system tracks the desired behavior. In the previous sections the different controllers have been described in detail. This section focuses on the possible sources of reference trajectories.

Via Point Trajectory Generator

. .. arm1

{x x x} ref {q

arm1

(Independent)

. .. arm1 q q}ref

Fifth Order Trajectory Generator

. .. arm1

{x x x} ref {q

arm1

(local-arm)

. .. arm1 q q}ref . .. arm1

Trajectory Mixer

{x x x} ref {q

. .. part1

{x x x}

(mixed)

. .. arm1 q q}ref

. .. arm1

Conveyor Tracking

{x x x} ref

Kinematic Transf.

Via Point Trajectory Generator

{q

(offset)

. .. arm1 q q}ref

. .. part1

{x x x} ref

(manipulate)

part1

Fifth Order Trajectory Generator

. .. part1

{x x x} ref

(local-object)

part1

Figure 6.19: Sources of arm and object trajectories For each arm, there are four possible sources of arm reference states: a via-point trajectory, a two-point fifth-order trajectory, a kinematic transformation from the state of an object, and a mix of the previous two. The sources of object reference states are either a via-point trajectory or a fifth-order trajectory.

Figure 6.19 illustrates the different sources of reference trajectory states: Via-Point trajectories are splined trajectories that traverse a sequence of via points (a.k.a. way points or through points). Via-Point trajectories are used by the planner (or any other subsystem) to command motions along a specified path. The on-line generation of such trajectories is a research topic in itself and is described


164

in detail in Chapter 7. Fifth-Order trajectories result from interpolated fifth-order polynomials that match pre-specified initial and final states at specified times. Generation of fifth-order trajectories from the boundary conditions is a straight-forward computation. Fifth-Order trajectories are useful to generate short intercept motions that match expected object states so that moving objects can be captured. Trajectory mixes serve two purposes: (1) they allow different degrees of freedom to be controlled from different source trajectories5 and (2) (by using continuously varying mixing coefficients) allow smooth transitions between different trajectory sources. Workcell Configurations A configuration can be loosely defined as a state of the dataflow. The previous sections have described the different control modes for both arms and objects as well as the different trajectory sources. For the purposes of the strategic-control layer, a configuration (or operating mode) is defined by specifying both the control mode, and the source trajectories for each of the manipulators and grasped objects. Table 6.3 summarizes the possible configurations of each arm: Configuration

Object Traj. Source

Arm Traj. Source

independent local-arm offset mixed local-object manipulate

None None None None fifth order Via-point

Via Points fifth order slaved to object Mixed slaved to object slaved to object

Control Mode arm-impedance arm-impedance arm-impedance arm-impedance object-impedance object-impedance

Table 6.3: Modes of Operation These are the main operating modes for each arm in the workcell. These modes are used by the FSM in the strategic-control layer to change the behavior of the system as a reaction to events and external commands.

Independent Mode.

In this mode, the arm is controlled with the hybrid controller described in

Section 6.5.1, either regulating to a fixed location or following a Via-Point trajectory. This mode is employed to move the arms, except in the case when the arm is about to grasp an object. 5

This is important when an object is being captured since, the x,y position and orientation of the tool must match that of the object while the z value must be independently controlled to pick-up the object.


Local-Arm Mode.

165

In this mode, the arm follows a locally-generated fifth-order trajectory. This

mode is useful for small arm motions such as ones in the final stages of intercepting a moving object. During an interception, the final point in the fifth-order trajectory is the expected state of the object grasp location at the computed intercept time. This mode is only used once the arm is near the object’s grasp position. Offset Mode.

To track an object, the reference state of the arms is obtained directly from the

measured position of the object’s grasp location. The desired grasp state is obtained by transforming the current object state using the desired grasp transform. This corresponds to the use of the information right out of the kinematic transformation module in Figure 6.19. This mode is used regardless of whether the object is static or in motion. Maintaining the information on the correct object state is the responsibility of the the World Modeller. This mode can be used to track an object from above (by specifying a grasp transform with some vertical offset) and to keep the arm at a constant position with respect to an object while waiting for the grasp to consolidate or for other arms to grasp. Mixed Mode. To descend and pick an object, the reference arm state is generated by mixing two reference trajectories: The object’s grasp location is used as a reference for all but the Z degreeof-freedom. A locally-generated fifth-order trajectory for the Z degree-of-freedom moves the arm onto the object. Local-Object Mode.

In this mode, the object and the arms grasping it are under object-impedance

control, the reference state for the object comes from a locally-generated fifth-order trajectory. This mode is useful for moving the object short distances such as when lifting the object after a grasp, moving it down to release it, or (in the future) fine assembly maneuvers. Manipulate Mode.

In this mode, the object and grasping arms are also under object-impedance

control. The reference object state is fed from a via-point trajectory. This mode is useful for moving objects around the workspace when the specific path followed by the object is important (in practice always, unless the motion is very short). The via points are normally specified by the planner subsystem. Note that the case when several arms are manipulating a common object is handled by the object-impedance controller that knows about this fact6 and generates reference states for all grasping arms. 6

More precisely, it is programmed with this fact by the strategic layer.


166

The Workcell Daemon The Workcell Daemon is responsible for receiving robot-commands through the Robot Interface (see Chapter 3), process the commands, and coordinate the individual-arm FSMs. In order to be scalable to any number of arms in the workcell, the Workcell Daemon does not model what the individual arms are doing in detail. Instead, whenever a command arrives, it queries the World Modeler to decide if the command is acceptable given the the current state, and if so, where the command should be forwarded. For example, a command to move an object will be rejected unless that specific object is being grasped, and if so, it will setup and start the corresponding object trajectory. A command to move an arm will be simply forwarded to the specific arm FSM (which in turn may either move the arm or reject it altogether). Individual-Arm FSMs

"SET_GRIPPER_TO_IDLE"/ FSMSetGripperToIdle()

moving

"ARM_TRAJ_ENDS"/ FSMArmStopped()

CSFSM_IGNORE_RETCODE 0

0 "ROBOT_ONE_ARM_GRASP"/ FSMOneArmGrasp()

SUB grasping

idle

CHAIN

releaseObject

1

"ARM_TRAJ_BEGINS && Catch = FALSE"/ NULL()

releaseInit

"ROBOT_ONE_ARM_UNGRASP"/ FSMUngraspObject()

0

"ARM_TRAJ_BEGINS && Catch = TRUE"

1

"OBJ_TRAJ_BEGINS"/ FSMObjTrajBegins() 0

CSFSM_ALWAYS_STIM

release

1 1

captureInit

SUB

CHAIN

movingObject

"OBJ_TRAJ_ENDS"/ FSMObjTrajEnds()

armCapture

0

Figure 6.20: Transition Graph for the Arm Finite-State Machines Two Arm Finite-State Machines (FSMs) are used for the strategic control of the workcell. These FSMs have identical functionality but are bound each to the private data of the corresponding arm. The two FSMs allow the independent strategic control of each arm. These FSMs communicate with each other to allow synchronization during coordinated arm maneuvers (such as picking an object with two arms).

Figure 6.20 illustrates the FSM of each individual arm. As previously explained, this FSM diagram represents conceptually an abstract-data type from which we instance several FSMs by binding the transition diagram with its own private data. The arm FSM receives stimuli from the Workcell Daemon, the other FSM, and several event-generating components in the dataflow7 . If a valid 7

For example the trajectory-generation components generate events every time a trajectory is started or completed.


167

stimulus is received, it executes the corresponding transition routine that in turn modifies the dataflow and may send other stimuli. The FSM utilizes subchains (fine-state-machine subroutines) to perform capture and release operations. These subchains are illustrated in Figures 6.21 and 6.22.

CSFSM_ALWAYS_STIM/ FSMInitCapture()

captureInit

"TIMEOUT"/ NULL() "PERIODIC_TIMER"/ FSMUpdateVipTrajectory() approaching CSFSM_IGNORE_RETCODE "OBJECT_WITHIN_REACH"/ FSMSwitchTo5thOrderTraj() "PERIODIC_TIMER"/ FSMUpdate5thOrderTraj()

"TIMEOUT "/ NULL() fail

reaching

CSFSM_IGNORE_RETCODE

CSFSM_ALWAYS_STIM/ FSMCaptureFailed()

"TRACKING_TOLERANCE_MET"/ FSMSlaveFromObjPosition() "PERIODIC_TIMER"/ FSMCheckGraspTolerance()

captureFailed

"TIMEOUT"/ NULL() CSFSM_ALWAYS_STIM

tracking

CSFSM_IGNORE_RETCODE

"OBJECT_OUT_OF_REACH"/ NULL() "GRASP_TOL_MET && CompanionReady = TRUE"/ FSMFinalDescent()

SUBCHAIN END 0 "TIMEOUT"/ NULL() descending "OBJECT_OUT_OF_REACH"/ NULL() SUBCHAIN END 1

0 "ARM_5th_ORDER_TRAJ_END"/ FSMAttemptGrasp()

0 1 CSFSM_ALWAYS_STIM "TIMEOUT"/ NULL()

gripping

"TIMEOUT"/ NULL() 1

lifting

"ARM_5th_ORDER_TRAJ_END"/ FSMArmStopped()

captureOK

"CompanionGrasped = TRUE"/ FSMLiftObject()

Figure 6.21: Transition Graph for Capture Subchain Capturing an object from a moving conveyor requires a series of steps and control-mode transitions. The Capture subchain sequences each arm through these steps. Transitions are triggered by events such as the completion of a trajectory, a timeout, or the detection of an object within the grasping range.


CSFSM_ALWAYS_STIM/ FSMPlaceObject()

168

releaseInit

placing

"ARM_5th_ORDER_TRAJ_END"/ FSMOneArmUngrasp()

ungrasp

CSFSM_ALWAYS_STIM/ FSMRaiseArm()

raising "ARM_5th_ORDER_TRAJ_END"/ FSMRaiseDone()

armUp

CSFSM_ALWAYS_STIM

SUBCHAIN END

0

Figure 6.22: Transition Graph for Release Subchain The use of a FSM subprogram to release an object allows delicate placement of the object at a prespecified location under local, closed-loop control.

6.7.3

Picking an Object from the Conveyor Belt

This section describes a concrete example of the interaction of the Workcell Daemon, the arm FSMs, and the dataflow layer during the task of picking up an object from the conveyor. The maneuver is triggered by the arrival of a ROBOT ONE ARM MOVE N CATCH strategiccommand through the Robot Interface. This command includes, among other things, the name of the arm to be used, an approach trajectory, the name of the object to pick and the grasp transform (from object position to grasp position). The Robot Daemon uses the name of the arm to query the World Modeler to ensure that the arm is available and in the independent configuration. Then it retrieves the the Via-Point trajectory generator component for that arm and starts the requested trajectory. The Robot Daemon also sends the CaptureObject=YES stimulus to the corresponding arm FSM. When the via-point trajectory starts, a “start” routine (installed by the Workcell daemon) automatically sends a ARM TRAJ BEGINS stimulus to the arm FSM which immediately enters the Capture subchain.


1. approach

2. intercept

4. descend

5. grasp

169

3. track

6. lift

Figure 6.23: Capture Strategy Six discrete stages are required for the capture of a moving object. During approach, the arm follows a pre-planned via-point trajectory (1). Triggered by the arm proximity to the object, the reach (intercept) stage uses a locally generated intercept trajectory to move the arm towards the object while matching the object’s velocity (2). In the track stage the arm is regulated directly from the object’s position and velocity (3). Three more stages: descend, grasp, and lift complete the capture.

The Capture subchain.

The basic capture strategy is illustrated in Figure 6.23. To perform a cap-

ture, the Capture subchain transitions through a series of states as shown in Figure 6.21. The subchain immediately executes FSMInitCapture() which initializes a set of auxiliary “watchdogs” to send periodic stimuli and timeouts, and transitions to the approaching state. While in this state, the FSM receives PERIODIC TIMER stimuli that trigger the FSMUpdateVipTrajectory() transition function that refines the last points of the trajectory, based on the most current estimate of the object’s grasp position, at the intercept time. This function also checks whether the arm is within a certain tolerance of the current object grasp position, in which case an OBJECT WITHIN REACH stimulus is generated. The FSMSwitchTo5thOrderTraj() transition function changes the arm configuration to Local-Arm mode and starts a fifth-order intercept trajectory from the current arm state to the intercept state. This trajectory matches the expected positions, velocities, and accelerations at intercept time. While in the intercepting state, the trajectory is refined periodically (each time the PERIODIC TIMER stimulus arrives). The periodic FSMUpdate5thOrderTraj() transition function also checks whether the arm is within a certain tolerance of the current grasp position. If so, the TRACKING TOLERANCE MET stimulus is sent. The corresponding FSMSlaveFromObjPosition() transition function switches the


170

arms to the Offset Mode configuration, the offset being the desired grasp transform with the addition of a small offset in height so that the arm looms directly above the object. The grasp tolerance is checked periodically by the FSMCheckGraspTolerance() transition function. When met, the GRASP TOL MET stimulus is sent. The FSMFinalDescent() transition function switches to the Mixed Mode configuration and sets the appropriate descent trajectory and mix coefficients so that the arm is tracking in the

x; y; ) degrees of freedom while it descends

(

on the object. When the trajectory ends, the installed notify routine informs the FSM with the ARM 5th ORDER TRAJ END stimulus, and a grasp is attempted by FSMAttemptGrasp(). If successful, the arm transitions to the gripping state, the FSMLiftObject() function switches to the Local-Object configuration and initiates a lift trajectory. When the lift trajectory finishes, the FSMArmStopped() function switches the arm to Manipulate Mode (where it is slaved to the VIP-Trajectory generator for the object), and the Capture subchain returns leaving the arm FSM in the grasping state. Note the numerous timeouts and error conditions that may arise during the capture maneuver. Auxiliary processes check for inter-arm collisions, kinematic singularities, and object reachability. If errors are detected, the corresponding stimulus is sent and the capture is aborted. Also note that in the case where two arms must cooperate to capture the object, the corresponding FSM synchronize twice: once prior to the final descent and a second time before they lift the object. Each arm FSM is informed of the need for synchronization by the Workcell Daemon when the maneuver starts and they wait at the synchronization points for CompanionReady=TRUE messages from their peers.

6.7.4

Experimental Pick Operations

Figure 6.24 illustrates the value of several relevant signals during a single arm capture operation. Note the switch to the intercept state at time 4 sec. From there on the position, velocity, and acceleration (not shown) of arm tool matches that of the object8 . Also note the switch to the descend state at 5.2 seconds. Note the velocities are matched during the time the grasp is consolidating (t

=

5:6 s to t = 6:6 s)9

8 In fact the tool position, velocity, and acceleration matches that of the grasp location in the object rather than the object’s. However in this case, since the object has a single grasp location, the object frame has been chosen to coincide with the grasp location. 9 The grasp takes a long time to consolidate because the grippers are pneumatic and they are connected through a long, thin hose.

Position (meters)


171

idle

Capture trajectories

0.5

approach

0

-0.5 RightEndPtState_y_pos RightEndPtStateRef_y_pos cornerWMState_y_pos RightEndPtState_z_pos

-1 0

2

4

6

8

10

12

14

intercept

track

Velocities during capture descend Velocity (m/s)

0.6

RightEndPtState_y_vel cornerWMState_y_vel RightEndPtStateRef_z_pos RightEndPtStateRef_y_vel

grasp

0.4

lift

0.2

0

0

grasping 2

4

6

8

10

12

14

Time (secs) Figure 6.24: Experimental single-arm capture of moving object The object is picked from the conveyor at t = 6:5 secs. Trajectory generation switches from planner control to local intercept trajectory (intercept state) at t = 4 secs. Velocities are matched and remain matched during the descent and grasp phases (t = 5.2s to t = 6.8s) . At t = 9 secs, the object is moved along a planned path to its goal location.

Figure 6.25 illustrates the case when two arms must cooperate to grasp and manipulate the object. Notice the intercept trajectories (approximately t

s to t = 4:8 s) bringing each arm to the corresponding grasp location in the object (symmetrically at 15cm about the object center). The arms wait for each other until they are both ready to descend (t = 5 s). Also note that while the grasps are consolidating (t = 5:5 s to t = 6:5 s), the state of both arms’ grippers accurately track the position and velocity of the corresponding grasp locations within the object. At time t = 6:5 s =

4

both grasps are consolidated and the arms cooperate to lift the object. The individual-arm FSMs allow the arms to be independently commanded. The arms can be used to capture two different objects simultaneously as illustrated in Figures 6.26, 6.27, and 6.28.


Position (meters)

LeftEndPtState_y_pos RightEndPtState_y_pos barWMState_y_pos LeftEndPtState_z_pos RightEndPtState_z_pos

Capture trajectories

0.4

172

0.2

0

idle

idle

approach

approach

intercept

intercept

track

track

-0.2 0

2

4

6

8

10

12

synch

Velocities during capture barWMState_y_vel LeftEndPtStateRef_y_vel RightEndPtStateRef_y_vel LeftEndPtState_z_pos

Velocity (m/s)

0.24

0.12

descend

descend

grasp

grasp synch

0

lift

lift

grasping

grasping

-0.12 0

2

4

6

Time (secs)

8

10

12

Figure 6.25: Experimental dual-arm capture of moving object In response to a planner command, a single object is picked from the conveyor belt with two arms and subsequently delivered. The two arms synchronize at t = 5 secs, and then grasp and lift the object simultaneously. Again both arms match velocities with the object before the final descent and grasp is performed. The object is moved to the goal position at t = 7 seconds.

The sequence of pictures in Figure 6.29 illustrates the capture of two consecutive objects from the conveyor (using one hand per object). Figure 6.28 illustrates the use of both arms to simultaneously capture two different objects (one with each arm), while Figure 6.30 illustrates the use of two arms to cooperatively capture a single object.


Left Arm

Left Arm

Left Arm

0.4

−0.4 −0.6 arm corner

−0.8 −1 0

10 time (s)

arm corner 0.3

0.2

0.1 0

20

0.3 Z coordinate (m)

Y coordinate (m)

Right Arm

10 time (s)

Y coordinate (m)

arm block

−0.6 −0.8

10 time (s)

0.2

20

−0.3

20

0.35 arm block

−0.4 −0.5 −0.6 0

10 time (s) Right Arm

−0.2

−1 0

0.25

Right Arm

−0.2 −0.4

arm corner

0.15 0

20

Z coordinate (m)

X coordinate (m)

−0.2

X coordinate (m)

173

10 time (s)

20

0.3 0.25 0.2 0.15 0

arm block 10 time (s)

20

Figure 6.26: Experimental simultaneous multiple-object capture These plots illustrate the simultaneous capture of two objects, each by a different arm. Each arm proceeds independently under the strategic control of the corresponding arm-FSM. The discrepancy between the Z coordinate of the right arm and the (grasped) “block” object is simply due to grasp location being offset in Z with respect to the body frame.


1

3

2

4

Figure 6.27: Animation of experimental data of simultaneous multiple-object capture This figure animates the data presented in Figure 6.26.

174


175

1

6

11

2

7

12

3

8

13

4

9

14

5

10

15

Figure 6.28: Photographic sequence of system picking two objects from the conveyor simultaneously This sequence illustrates the use of two arms to simultaneously acquire two objects from the conveyor. This capability is enabled by the strategic control layer (by using a dedicated FSM program to supervise each arm), and can be extended to any number of arms in the workcell.


176

1

6

11

2

7

12

3

8

13

4

9

14

THIS FIGURE HAS BEEN LEFT INTENTIONALLY BLANK

5

10

15

Figure 6.29: Photographic sequence of system picking two objects from the conveyor This sequence of fourteen images has been obtained by digitizing video filmed during the operation of the system. The positions, velocities and accelerations of the manipulators are carefully controlled to match those of the object to be picked so that its motion is not disturbed suddenly. This delicate transition can be seen in the data (from a different capture) presented in Figure 6.24.


177

2

5

8

3

6

9

4

7

10

Figure 6.30: Photographic sequence of system using two arms to pick the same object from the conveyor Sequence illustrating the use of two arms cooperatively to pick the same object from the conveyor. In addition to precise tracking of the respective grasp position by each arm, both arms need to synchronize in their grasp and lift operation.


178

6.8 Summary This chapter has described the hierarchical-workcell control subsystem composed of four layers: Joint-Control, Arm-Control, Object-Control and Strategic Control. Each layer of the hierarchy addresses one control aspect and presents a simplified interface to the layer above. The jointcontrol layer uses joint-torque loops to compensate for joint flexibility, motor cogging, and other non ideal features. The joint-control layer allows the arm-control layer to treat the motors as ideal torque sources that apply their torque directly to the rigid links. The arm-layer uses an inverse dynamics (computed-torque) controller to implement an arm-endpoint-impedance controller that allows the object-control layer to command the arms by specifying applied endpoint forces and reference trajectories for the arm end effectors (joint trajectories must also be specified to go through kinematic singularities). The object-control layer uses an object-impedance controller to allow the strategic-control layer to control the motion of the object directly (by specifying object trajectories, impedances, and remote-center of compliance). Finally, the strategic-layer coordinates the motion of the two arms, reconfigures the dataflow, and modifies control parameters so that the workcell can be controlled by sending the commands specified in the Robot Interface (system-command interface of Chapter 3). In the process of developing the hierarchical control system, several contributions and enhancements to previous approaches have been presented, including: (1) configuration-based merging of operational-space and joint-space controllers to achieve stable transition through kinematic singularities, (2) scalable-strategic control layer for the workcell, allowing additional manipulators to be easily added, and (3) development of strategic programs for the workcell enabling concurrent–as well as cooperative–arm motions (e.g. picking up multiple objects from the conveyor simultaneously, picking objects from the conveyors using two arms cooperatively).

Chapter 7

On-Line Trajectory Generation This chapter presents a computationally-efficient algorithm to generate reference trajectories from a geometric description of the desired paths for the arm or objects in the workcell. This algorithm allows external subsystems to command motions in the workcell by specifying the path to be followed geometrically (e.g. by giving a sufficiently fine set of “through points”). These geometric paths will be converted into trajectories that follow the path as quickly as possible, within the dynamic constraints of the system (actuator torque limits, externally imposed object-acceleration limits etc.). Since the system must operate on-line, both the efficiency of the final trajectory, and that of the computational process itself must be placed on equal footing to evaluate system performance. Moreover, in a real-time environment, predictability of the computational delay of an algorithm is often more important than best-case or even average-case performance. The algorithm presented here is both computationally efficient and predictable. This is accomplished by introducing new proximate-optimal constraints (more restrictive than the dynamic constraints of the system) that may produce sub-optimal trajectories, but in return, the algorithm is guaranteed to execute in a time that is linear with respect to the length of the path. The linear scale factor depends on the specific dynamic equations of the system. Hence, this constant factor can be characterized a´ priori for a given system.

7.1 The Role of Trajectory Generation within the Workcell The high-level command of a robotic workcell from a planning subsystem requires a format for specifying the movements of the arms and objects in the workcell. Many of the planners in the literature (and the ones used in this research) can take into account the kinematics of the system, but 179

CHAPTER 7. ON-LINE TRAJECTORY GENERATION

180

2

TimeParameterization

y

3 1 4 time

2

y

y

3

2 3 1 4

1 4

x

time

Planner outputs sequence of spatial via-points

y

2 3 1 4 time

Figure 7.1: The Time-Parameterization Process Time parameterization transforms a geometric path (given for example as a sequence of spatial viapoints) into a reference trajectory that specifies positions, velocities, and accelerations as a function of time. Many possible trajectories can be created that traverse the same sequence of via-points. The three plots on the right illustrate three possible time-parameterizations traversing the same set of via-points. The time-parameterization process usually minimizes some performance index (such as total travel time) subject to the constraints of the system (e.g. velocities, accelerations, torque limits).

not its dynamics. The generated paths are geometric in nature. A geometric path specifies the path to be followed in space by giving, for instance, a sequence of via-points (a.k.a. through-points or waypoints) in a space that fully describes the motion of the arms (or the manipulated objects). These via-points can, for example, represent joint-configurations, or for a non-redundant manipulator, positions of the operational point in space. In any case, the control system must ultimately be provided with a reference trajectory to track. This trajectory contains not only positions, but also velocities and accelerations as a function of time. The process of transforming geometric paths into trajectories is known as time parameterization of the geometric path, and is depicted pictorially in Figure 7.1.


181

The need for path specification and automatic trajectory generation is not restricted to the command of robotic workcells. Rather it is present in many other robotic system such as mobile robots and underwater vehicles, whenever complete paths need to be specified to the system1. Time parameterization of an a´ priori -obtained geometric path is not the only way to obtain a reference trajectory for the workcell control system. Several other approaches are discussed in section 7.2. However, it is one of the most popular due to its flexibility, intuitive appeal, conceptual simplicity, and speed. The need for on-line operation of the workcell in a dynamic environment, combined with the fact that it must be commanded from a geometric planner, and operate in a not-completely-structured environment, introduces several requirements for the trajectory generation algorithm:

The algorithm must be computationally efficient and be able to generate trajectories that fully exploit (but do not exceed) system capabilities. Since the goal is to perform on-line planning and execution of tasks, we are willing to trade off strict time optimality (speed of the final trajectory) in exchange for a computationally more efficient algorithm.

The algorithm running time must be predictable for a given geometric path. This allows the different subsystems to anticipate and account for the computational delay in the timeparameterization process.

The algorithm must provide a mechanism for “patching” ongoing trajectories, i.e., modifying parts of the path that lie ahead of the current trajectory. This allows the system to adapt to changes (such as a better estimate of the intercept position of a moving object) that may be detected after the trajectory was initiated.

In addition, there are several practical requirements imposed by the architecture and system interfaces, particularly because subsystems may operate remotely:

The algorithm must accept geometric paths described as discrete sequences of via-points (through-points). Via-points sequences are a bare-bones, minimalist representation of the information that typical geometric planners are able to provide. Deciding on some ad-hoc continuous or even differentiable representation as an input to the algorithm would impose unnecessary constraints on the planning subsystem.

1

For example, a tele-operated system has no need for time-parameterization because the reference state is generated directly from the motions of the tele-manipulation mechanism.


182

The algorithm must produce a continuous description of the trajectory, from which position, velocities, and accelerations can be inferred at any sample rate. Using a continuous representation rather than a sequence of position, velocity, and acceleration states at some sample rate isolates the trajectory-generation algorithm from the control subsystem. In fact, the control subsystem may have sample rates which may be unknown to the subsystem performing trajectory-generation (or the controller may be be multi-rate). A continuous representation can also be described with fewer parameters (e.g. a few spline coefficients) rather than a much denser sequence of states at the controller sample rate. A compact description is advantageous when communicating with a controller over a link with limited bandwidth.

The trajectory must have limited jerk. Again, the trajectory generator may be used for a variety of systems, on some of which jerk could excite flexibility or other system dynamics2.

After providing a review of the related approaches in the next section, the rest of the chapter will describe a proximate-optimal trajectory-parameterization algorithm developed to address the above issues, and present experimental results to evaluate its computational efficiency (linear with respect to path length) and predictability. This algorithm is currently used for several other projects ranging from free-floating space robots [179] to under-water vehicles [104, 195], and has been made available to the robotics community as a ControlShell component [144].

7.2 Literature Review: Trajectory Generation Current industrial robots do not employ any automated means of time-parameterizing geometric paths. Rather, many robot programming languages (such as VAL-II) provide facilities for low-level trajectory definition and tuning based on acceleration profiles. With this approach, the burden of tuning those parameters is left to the applications programer or systems integrator. Typically, trajectory tuning is performed manually (by trial and error) in an attempt to find the fastest trajectory that can be achieved with reasonable tracking errors (which are related to motor saturation and controller bandwidth). This approach is both difficult and time-consuming, and only applicable when the programming is done off line (i.e. the paths are known ahead of time and the robot will execute them over and over). Given that trajectory tuning is in the critical path of the system integration, it is surprising that industrial practice does not incorporate some of the established research results described below. 2

In fact the arms in the workcell contain exaggerated joint flexibility that was designed for earlier research [131].


183

Given the perceived need for an automated method of generating reference trajectories for the system, it is not surprising that several different approaches have been proposed over the last decade. These approaches can be roughly classified into decoupled, coupled, reactive, and hybrid. Time Parameterization of a Geometric Path (Decoupled Approach)

The decoupled approach

breaks the overall problem in two parts: First a geometric3, collision free, path is obtained using a path planner or some other means. This geometric path can be described as a continuous function

q = f (s) with, f

: [0; sf ] 2 R ! C , where C is the configuration space of the robot. Given the geometric path, the next step is to obtain a of some parameter “s” (usually path length):

time parameterization by finding the time history s(t) that minimizes a pre-specified performance

index subject to the dynamic constraints. This latter stage does not change the geometric path, hence the name decoupled. The algorithms presented by Bobrow, Dubowsky and Gibson [72], Shin and McKay [164, 166, 165], Singh and Leu [174], as well as the one presented in this chapter all belong to this class. The decoupled approach is conceptually simple and well suited to interface with standard planning subsystems (which can generate the geometric paths). However, the computational complexity of these algorithms is such that to the author’s knowledge, they have always been used off-line. Sacrificing strict optimality in order to obtain more efficient algorithms has already been suggested by Wen and Desrochers [196] and Butler and Tomizuka [25]. Other authors have focused on increasing the efficiency of the optimal methods. For instance, Slotine and Yang [176] exploit the geometric properties of the constraint equations to generate more efficient parameterization algorithms. Several of these approaches will be described in detail once the problem is formulated and the notation introduced. Combined Path-Planning and Time Parameterization (Coupled Approach)

The coupled ap-

proach, attempts to obtain a collision-free path that is also optimal with respect to some performance index without going through an intermediate geometric path. That is, it solves both the path-planning and the trajectory-generation problems simultaneously. This is the most general approach. Since it taken into account the shape of the path during the optimization process, the coupled approach will (theoretically4) yield lower values of the performance index than the decoupled approach. However, its computational complexity (recall that the geometric path planning problem is itself PSPACE-hard) is substantially greater than the decoupled approach. Moreover, the planner and 3

The name geometric is used to stress the fact that time is not involved. In practice this will depend on the approximations made. Many implementations of the coupled approach restrict the search space of possible geometric paths. 4


184

time-parameterization subsystems must be developed together which makes the approach less modular and precludes the use of already existing planners. This approach has been proposed by Gilbert and Johnson [52], Bobrow [20], Shiller and Dubowsky [162, 163], etc. Reactive Methods.

A different approach is to never generate a path (or trajectory) explicitly.

Rather, the desired state of the system is computed as a function of the current measured state and some external “virtual potential field”. For instance the potential-function methods presented by Khatib [80] generate reference states by solving the system dynamics, assuming that a virtual potential function (which depends on the system state: position of manipulators, obstacles etc.) is acting on the manipulator. This method can be very reactive to environmental changes. However, it becomes quite difficult to specify the desired (or pre-planned) system behaviour (and hence use goal-oriented planning). Several authors have investigated the construction of potential fields that can be used to guide the robot along a pre-specified path (see Rimon [147] and Arkin [13]). These approaches typically cannot guarantee that actuator limits are not exceeded because the potential function is independent of the current velocity and acceleration of the robot. Hybrid Methods.

Other proposals have been made that combine the decoupled and reactive

methods. For instance Quinlan [138, 137] generates a geometric path using standard planners, and then subjects the path to potential forces representing obstacles in the environment. The resulting geometric path is never time-parameterized. Rather, an instantaneous decision on whether the robot should accelerate, decelerate, or coast is made at each time step based on whether the robot can be safely brought to a stop. This approach is computationally expensive if the robot operates on in a region of large velocity to acceleration ratio (the case of free-flying space robots), because for each time step, the entire remaining trajectory may require re-parameterization. For this research, the decoupled approach was selected for its flexibility and ability to interface to planning subsystems (through the pre-specified system-command interfaces). The remaining sections will describe the decoupled approach in more depth, review several of the proposed solutions in more detail, introduce the efficient proximate-optimal solution proposed in this research, and present experimental results on the performance of the new proposed solution.


185

7.3 Problem Formulation and Known Results The decoupled-optimal-time-parameterization (DOTP) problem has been extensively described in the literature [72, 164, 30]. In order to introduce the notation necessary for the description of the proximate-optimal algorithm, and compare it to the existing approaches, a summary of the problem formulation and some other well-known facts is presented below. The dynamics of a robot, assuming no friction can be written as [38]:

M (q)q + B (q)[q; q] + C (q)[q2] + g(q) =

(7.1)

q is the vector of generalized coordinates, M is the mass matrix, B is the n (n , 1) matrix of coriolis terms, C is the n n matrix of centripetal terms, g(q) is the n 1 matrix of Where

gravity terms, and is the torque vector. A geometric path can be fully described as a function

s 2 [s0; sf ].

q = f (s) where the parameter “s” is

f (s) is required to have continuous second derivatives. It is well known that, given this description, the time evolution of the parameter s = s(t)

in some range:

Typically the function

completely determines the full trajectory of the robot, and therefore, the required torques. In other words, given s

(t).

=

s(t) equations (7.2) below, and (7.1) can be used to obtain q(t); q(t); q(t), and

q(t) = q(s(t)) ) q = fss˙ ) q = fss¨ + fss s˙2

(7.2)

To formulate an instance of the DOTP problem, we also need a performance index and a set of

constraints. The performance index J is a functional of the trajectory history:

=

J [q; q; q; ].

J [s; s;˙ s¨]. For example, the total travel time is a Rf def common performance index that can be written as: J = tf = ss s˙dss . Again using (7.2) and (7.1) we can write

J

J

=

0

( )

Similarly, the velocity, acceleration, and torque constraints are specified as a set of inequalities

(q; q; q; ) 0, which can be written as (s; s;˙ s¨) 0.

The DOTP problem is the search for the time parameterization s = s(t) with s(t) 2 C 2[s0; sf ]5

that minimizes the performance index

J [s; s;˙ s¨] subject to the constraint (s; s;˙ s¨) 0.

Notice

that the a´ priori specification of the geometric path has simplified the problem of minimizing the performance index by replacing the search for vector-valued control history (t) by that of the

scalar valued function s¨(t). 5

The notation

Cn

[a; b]

denotes the set of functions with continuous nth derivative in the interval [a,b].


186

The approaches in the literature differ in their selection of performance index J , the nature of the constraint

0 imposed, and the method used to solve the optimization problem as summarized

in Table 7.1. reference Luh el al. [97] Bobrow el al. [72] Shin el al. [167] Shin el al. [166] Singh el al. [174] Pardo this chapter

perf. index total travel time total travel time total travel time general average of travel time and energy total travel time

constraints constant bounds on q˙ and q¨ [q; q˙] and q˙[q ]

method modified approximate programming direct integration

complexity

[q; q˙] and q˙[q ]

direct integration

O (L 2 )

general

dynamic programming

[q; q˙] and q˙[q ]

dynamic programming

[q; q˙], q˙[q ] and/or q¨[q; q˙]

direct integration

O(L)/iter. O(L2 )

O(L)/iter. O(L)/iter. O(L)

Table 7.1: Related approaches to the Decoupled-Optimal-Time-Parameterization Problem Comparison of approaches to the DOTP problem. The notation [q; q˙ ] indicates limits in the torque that may depend on the state (q; q˙ ), “general” indicates support for a wide variety of possible performance indices or constraints (e.g. minimum energy, jerk etc.). In the complexity column, “L” stands for path length. For iterative algorithms that require convergence, we give the complexity per iteration. See sections 7.5.4 and 7.5.5 for a justification of the complexity values.

7.3.1

Minimum travel time with limits on actuator torque, acceleration and velocity

If the performance index is the total travel time, and the only constraints are on the maximum velocity, acceleration and torques, it has been shown in [72, 164] that the DOTP problem can be transformed into the simpler “Decoupled Minimum-Time Time-Parameterization Problem” (DMTTP) described in this section. Using equations (7.2) and (7.1) we obtain:


(s; s˙) (s; s˙)

m(s) c(s)

= = def

=

def

=

187

M (s)(fss¨ + fsss˙2 ) + (B (s)[fs; fs] + C (s)[fs2])s˙2 + g(s) m(s)s¨ + c(s)s˙2 + g(s) M (s)fs(s) M (s)fss(s) + B (s)[fs; fs] + C (s)[fs2]

(7.3) (7.4) (7.5) (7.6)

The torque constraints can now be written as:

min (q; q) (s; s˙) max (q; q)) Where: min (s; s˙)

max (s; s˙)

def

=

def

=

s¨min (s; s˙)

def

s¨max(s; s˙)

def

= =

() s¨min (s; s˙) s¨ s¨max(s; s˙)

min (q(s); q(s; s˙))

max (q(s); q(s; s˙))

def

=

def

=

max f i (s; s˙)g i=1:::Ndof min min f i (s; s˙)g i=1:::Ndof max

8 1 i 2 i i > i s min (s; s˙) , c (s)s˙ , g (s) > m < 1 i 2 i i i (s; s˙) def i s max(s; s˙) , c (s)s˙ , g (s) min = m > > : ,1 8 1 i i i 2 > i s max (s; s˙) , c (s)s˙ , g (s) > m < 1 i 2 i i i (s; s˙) def i s min (s; s˙) , c (s)s˙ , g (s) max = m > > : 1 ( )

( )

( )

( )

(7.7)

min (f (s); fss˙))

max (f (s); fss˙))

mi(s) > 0 if mi(s) < 0 if mi(s) = 0 if mi (s) > 0 if mi (s) < 0 if mi (s) = 0 if

(7.8)

(7.9)

q

It is easy to see that using (7.2), any constraints in the acceleration , may be expressed in a similar form:

qmin(s; s˙) q(s) qmax(s; s˙) () s¨min (s; s˙) s¨ s¨max(s; s˙)

(7.10)

With the appropriate corresponding definitions for s¨min (s; s˙) and s¨max (s; s˙). Therefore, constraints in the torques and accelerations may be combined into an inequality

constraint of the form s¨min (s; s˙) s¨ s¨max (s; s˙). Or using the fact that ddss˙

= ss¨˙ ,

they can be


188

written as:

ds˙ = s¨ max (s; s˙) min (s; s˙) ds s˙ min (s; s˙) def = s¨min (s; s˙)=s˙ For s˙ 6= 0 max (s; s˙) def = s¨max (s; s˙)=s˙ For s˙ 6= 0

(7.11)

Furthermore, symmetric constraints in the velocity, if present, can be written as:

,qmax(s) q(s) = fs (s)s˙ qmax(s) () s˙ (s) qi i s g

(s) def = mini 1:::Ndof f jmax fs s j

(7.12)

( ) ( )

=

We assume that the torque limits are such that the manipulator can hold its own weight at any point along the trajectory. This condition can be expressed as:

8s : min(s; 0) g(s) max(s; 0) () 8s : min (s; 0) 0 max(s; 0) Borrowing the analogy introduced by [167], the constraints of equations (7.11) can be visualized as a “wedge” associated with each point (s; s˙) in phase space. This “wedge” represents the range

of allowed slopes of any trajectory which traverses that point in phase space (s˙ = s˙(s)), and locally satisfies the constraints. Several authors [72, 167] have noted that for each value of values of s˙ for which the wedge closes (i.e.

s there are

min (s; s˙) > max(s; s˙)) meaning there is no phase

space trajectory that traverses that point (s; s˙) and satisfies the constraints. Figure 7.2 shows these constraints for an example path. Furthermore, in [167] the authors prove that under fairly general

assumptions6, the “allowed” region for s˙ has the form 0 s˙max (s). We can therefore combine this with (7.12) and write the combined torque, acceleration and velocity constraints in the form: 0 s˙ s˙max (s)

ds˙ max (s; s˙)) min (s; s˙) ds

(7.13)

Where s˙max (s) is defined so that: 0 s˙ s˙max (s)

) min (s; s˙) max(s; s˙)

To summarize the previous discussion, for the case in which the performance index is minimum travel time, the original DOTP problem can be recast into the simpler DMTTP problem which takes the form: 6 These results are valid for a manipulator modeled without friction. The only assumption made in the paper is that the torque limits have a dependency on the joint velocities i that is at most quadratic in them.

q


189

Integration constraints in Phase Space 6

5

d s/dt

4

3

2

1

0 0

1

2

3 4 s (path parameter)

5

6

7

Figure 7.2: Optimal time-parameterization constraints in phase-space For a given geometric path and manipulator, the velocity, acceleration, and torque limits map into two types of phase-space constraints: (1) an allowed region that verifies s˙ s˙max (s) and (2) “slope” limits ds˙ max (s; s˙). At each point (s; s˙) in phase-space, there is a “wedge” representing the min(s; s˙) ds range of allowed slopes of legal phase-space trajectories trough that point. The curve s˙max separates the region of the phase-space for which there are no allowed phase-space trajectories because either the velocity limit is exceeded or the “wedge” “closes”.

DMTTP problem:

Given the functions min (s; s˙), max (s; s˙) and s˙max (s) such that the follow-

ing properties hold:

8s 2 [s0 ; sf ] s˙max (s) 0 8s 2 [s0 ; sf ] s˙ s˙max(s) ) min (s; s˙) max(s; s˙) J

Find the time parameterization s˙(s) for s 2 Rs = tf = s0f s˙(1s) ds subject to the constraints:

s ; sf ] that minimizes the performance index

[ 0


190

s˙(s0 ) = s˙0 and s˙(sf ) = s˙f 8s 2 [s0 ; sf ] 0 s˙(s) s˙max (s) 8s 2 [s0 ; sf ] min (s; s˙) dsds˙ max(s; s˙) Among the useful results associated with the DMTTP problem the following lemma–proven in [72]–will be specially useful in the description of the proximate-optimal algorithm.

f[s0; sf ]; s˙0; s˙f ; min(s; s˙); max(s; s˙); s˙max(s)g be an instance on the DMTTP problem and s˙ (s) be its solution. Then for any function s˙(s) that satisfies the constraints of the problem, we have s˙(s) s˙ (s), 8s 2 [s0 ; sf ]. Lemma 1 Let

This lemma and the results that preceded it are used to prove that specific phase-space integration schemes will yield the optimal trajectory. In particular, the algorithms presented in [72, 167, 166] solve the DMTTP problem.

7.3.2 Case of velocity-independent torque limits In the case where the actuator torque limits can be approximated as being velocity-independent (i.e.

imax (s; s˙)

=

imax(s) for each degree-of-freedom “i”), the constraints in the phase-space slope

can be simplified. Replacing imax (s; s˙) = imax (s) and imin (s; s˙) and (7.8), the slope limits of equation (7.11)

=

imin (s) in equations (7.7)

ds˙ = s¨ (s; s˙) min (s; s˙) ds s˙ max Can now be written as:

ds˙ i (s)=s˙ , i (s) s;˙ i1 (s)=s˙ , 2i (s) s˙ ds 1 2

for

i = 1; ::Ndof

(7.14)

The proximate-optimal algorithm uses this approximation to simplify the integration of the trajectory.

7.4 Formulation of the Proximate-Optimal Problem This section presents the main contribution of this chapter: a non-iterative

O(L)7 algorithm to

find a proximate-optimal solution to the DMTTP problem. The algorithm gains its efficiency from 7

L stands for the length of the path.


191

transforming the DMTTP problem into one with stricter constraints. The algorithm then calculates the optimal solution to this modified problem. In many cases these stricter constraints will result in trajectories that are not significantly slower (this has been observed empirically). Further research is needed to precisely characterize the performance loss with respect to the true time-optimal path. The key idea to derive an

O(L) algorithm is to find a criterion which allows the phase-space

integration to be broken into several independent sections. As a side benefit, the resulting algorithm will be highly parallel. Note that an instance of the DMTTP problem (and hence its solution

s˙ (s), is completely determined by the boundary conditions fs˙0 ; s˙f g and the constraint functions fmin (s; s˙); max(s; s˙); s˙max(s)g. Therefore, if we knew in advance any point in the optimal path fsd; s˙(sd )g with s0 < sd < sf , we could break the problem into two independent ones: first solve for s0 s sd and then for sd s sf . In other words, the value s˙ (sd ) provides the boundary condition at s = sd that allows the problem to be divided. Any point along the optimal phase-space trajectory, s˙ = s˙ (s) can be used in this manner. Figure 7.4 shows one such phase-space trajectory.

The algorithm is based in the following characterization of a subset of the points in the optimal trajectory. This characterization allows early identification of these points:

f[s0; sf ]; s˙(s0 ); s˙(sf ); min(s; s˙); max(s; s˙); s˙max(s)g DMTTP problem and s˙ (sd ) its solution.

Theorem 1 Let

be an instance of the

Assume that:

8s 2 [s0 ; sf ] : s˙ s˙max(s) ) min (s; s˙) 0 max(s; s˙) Then the following property (see Figure 7.3) holds:

8s1 ; s2 ; sd 2 [s0; sf ] :

(7

:15)

9

> s1 < sd < s2 > = s˙max (sd) = mins ss fs˙max (s)g > ) s˙(sd ) = s˙max (sd ) > ; s˙(s1 ) = s˙max (sd) = s˙(s2 ) 1

2

Proof. It suffices to show that the phase-space trajectory defined by Equation (7.16) below satisfies all the constraints.

8 < (sd ) s˙(s) = u(s) = : s˙max s˙ (s) def

for s1

< s < s2

(7.16)

otherwise

Once the above statement is proven, applying lemma 1, we see that s˙ (sd ) u(sd ) which combined with the constraint s˙ (sd ) s˙max (sd ) implies s˙ (sd ) = s˙max (sd ).

=

s˙max (sd )


192

Forbidden Region

6 M

. s max ( s )

C . s * (s 1 )

. s

. s * (s 2 )

. s* ( s)

. s* ( s)

s1

sd

s2

s

Figure 7.3: Illustration of conditions of Theorem 1

u(s) satisfies the constraints in the interval ]s1 ; s2 [. Now, in this interval, our hypothesis guarantees s˙(s) = s˙(sd ) s˙max (s), and since s˙(s) is constant ddss˙ = 0, and therefore, min (s; s˙) 0 = ddss˙ max (s; s˙). In view of its definition, we only need to show that

Corollary 1 The theorem holds even if we relax the equality s˙ (s1)

s˙ (s1 ) s˙max(sd) s˙(s2)

=

s˙max (sd )

=

s˙ (s2) to

Proof. Since s˙ (s) is continuous and s˙ (sd ) s˙max (sd ), the intermediate value theorem guarantees that there are values s˜1 ; s˜2 such that s1 s˜1 sd s˜2 s2 with s˙ (s˜1 ) = s˙max (sd ) =

s˙ (s˜2 ). We can now apply the theorem to s˜1 ; s˜2; sd.

The above theorem and its corollary provide a sufficient condition for a phase-space point

fs; s˙max(s)g to belong to the optimal phase-space trajectory s˙(s). This characterization only applies when we can guarantee that the pre-condition fs˙ s˙max (s) ) min (s; s˙) 0 max(s; s˙)g holds. Notice that this condition can always be enforced by redefining s˙max(s) to be : 8 < s˙ (s) s˙max(s) min : max minfs˙ j (min (s; s˙) = 0) or (max (s; s˙) = 0)g This new s˙max is the more strict limit that we were referring to. The more restrictive (proximate-optimal) constraint imposed on the allowable trajectories physically means that we require enough authority left in the actuators at every state in the trajectory


193

Proximate−optimal integration constraints in Phase Space 6 Optimal limits Approximate limits 5

Integration

d s/dt

4

3

2

1

0 0

1

2

3 4 s (path parameter)

5

6

7

Figure 7.4: Integration trajectory in phase space using proximate-optimal constraints This figure shows the proximate-optimal phase-space trajectory. Also shown are the constraints in the phase-space trajectories that correspond to the manipulator dynamic constraints for trajectories along the given geometric path. See Figure 7.2 for the interpretation of these constraints. The strict (optimal) limit occurs when the “wedge” “closes”. The proximate-optimal algorithm imposes a more demanding requirement and disallows phase-space regions where the “wedge” does not contain the zero slope. Therefore the s˙max (s) (approximate) curve above represents the points for which either the maximum or minimum allowed phase-space slope is zero.

that the system is able to change its speed along the trajectory in either direction: s¨ in speed), and s¨ < 0 (decrease in speed)8 .

> 0 (increase

The fact that the optimal trajectory touches the boundary curve s˙ = s˙max (s) at a finite number

of points is also key to the algorithms presented in [72, 167, 176]. Reference [167] shows that 8

This interpretation assumes the parameter “s” is either the path length or related by a strictly monotonic (increasing) function to the path length. This is the usual case.


for the case in which there are no limits in

194

q (and therefore the boundary s˙max (s) is given by

the equation min (s; s˙) = max (s; s˙)) the “switching points” satisfy the necessary condition ds˙max (s) = (s; s˙ (s)). The algorithm presented in [176] exhaustively classifies these points

min

ds

max

and presents an efficient method to calculate them. The proximate-optimal approach differs from the above in that thanks to the introduction of

fs˙ s˙max(s) ) min (s; s˙) 0 max(s; s˙)g we have obtained a This is key to reducing the algorithmic complexity from O(L2) to O(L) as

the extra requirement: sufficient condition.

discussed in sections 7.5.4 and 7.5.5.

7.5 The Proximate-Optimal Time-Parameterization Algorithm This section describes the proximate-optimal algorithm and proves its correctness in the continuous domain. The discrete implementation is discussed in section 7.5.3.

7.5.1

Continuous-Domain version

Assume an instance of the DMTTP problem:

f[s0; sf ]; s˙0; s˙f ; min(s; s˙); max(s; s˙); s˙max(s)g that satisfies the requirement (7.15) in Theorem 1.

The algorithm to integrate s˙ = s˙a (s) consists of three steps illustrated in Figure 7.5:

1. Initialization. Let

H = fs 2 [s0; sf ] j s˙max(s) is a local minimumg And, sl s0 ; s˙l s˙0 ; sr sf ; s˙r s˙f 2. Integration. Let

s1 sl ; s˙a(s1 ) s˙l ; s2 sr ; s˙a (s2 ) s˙r H = H \ ]sl; sr [ sd fsp 2 H j s˙max (sp ) = mins2H fs˙max (s)gg s˙a (sd ) s˙max (sd ) While

s < s2 ] ^ [(s˙a(s1 ) < s˙a (sd)) _ (s˙a(s2 ) < s˙a (sd))])

([ 1

Do :


If s˙a (s1)

s˙a(s2 ) integrate forward (i.e.

195

increasing

s1 ) along the maximum

acceleration curve:

8

ds˙a (s ) = < max (s1 ; s˙a(s1 )) if s˙a(s1 ) < s˙max (s1 ) ds 1 : minf ds˙dsmax (s1); max(s1 ; s˙a(s1 ) g ; otherwise

(7

:17)

Else s˙a (s1 ) > s˙a (s2) and we integrate backward (decreasing s2 ) along the maximum deceleration curve:

8

ds˙a (s ) = < min (s2; s˙a(s2)) if s˙a (s2 ) < s˙max(s2 ) ds 2 : minf ds˙dsmax (s2 ); min(s2; s˙a (s2)) g ; otherwise At any point in the integration, if any element of

H falls outside the (changing) interval

s ; s2[, we eliminate it from H and recompute sd if required. The condition s1 = s2 indicates the integration between sl and sr has completed and we return. The condition [s˙a (s1 ) s˙a (sd )] ^ [s˙a (s2 ) s˙a (sd )] indicates we can break the ] 1

problem into two independent ones and we continue with the next step. 3. Separation. Here we know that [s˙a (s1)

s˙d] ^ [s˙a(s2 ) s˙d]. We divide the problem into

the following two: i) sl ii) sl

s1 ; s˙l s˙a(s1 ); sr sd ; s˙r s˙d = s˙max(sd ) sd ; s˙l s˙d = s˙max (sd); sr s2 ; s˙r s˙a (s2 )

And then invoke (recursively) the integration step on each one of the subproblems. These steps are sketched in Figure 7.5. The actual process for an example path of one of the

arms in the workcell can be seen in Figure 7.2. Each local minimum of the solution s˙a (s) served as a decoupling point at some point during the integration.

7.5.2

Algorithm correctness

This section proves that the algorithm integrates the “optimal9” phase-space trajectory i.e.

s˙a(s) = s˙ (s) 8s 2 [s0; sf ].

It suffices to show that, given boundary conditions (sl ; s˙l ) and (sr ; s˙r ) in the optimal trajectory,

the integration step always generates points in the optimal trajectory. Once we show this, Theorem 1 guarantees that the separation step generates boundary conditions in the optimal trajectory. 9

Optimal with respect to the modified constraints.


196

Forbidden Region

8 6

. s

M C

7

4

3 H

E 2 C B 1 A

D

2

I

J

G 6 F

9

6

L K

5

1

5 M N

s Figure 7.5: Steps of proximate-optimal algorithm Phase-space illustration of the steps taken by the proximate-optimal algorithm to integrate a trajectory. Same numbered pieces are integrated simultaneously. Initially integration proceeds forward from A and backwards from N, keeping both branches balanced (1). As soon as the first local-minima is reached (F), the trajectory is broken into two independent pieces: B–F and F–M. B–F is integrated first (2), until another local minimum is reached at D. From here C–D is integrated (3) finishing that branch and D–E is integrated (4) completing the B–F branch. The right branch is integrated similarly after been separated at points K and I. Notice the possible existence of intervals where the integral follows the boundary of the forbidden region (8).

It is clear by construction that s˙a (s) s˙0 (s) for any function s˙0 (s) that satisfies the constraints

and the boundary conditions. In view of Lemma 1 it is sufficient to prove that s˙a (s) itself satisfies the constraints.

Clearly by construction s˙a (s) s˙max (s)

8s 2 [sl; sr ], so this constraint is always satisfied.

s˙a , to always This is because of the way the integration (see Equation (7.17)) changes the value of dds be smaller than ds˙max (s) whenever s˙a (s) intersects the boundary curve s˙max (s). ds

s˙a does not violate the slope limits. This will proven through So all we need to prove is that dds

contradiction. Let sv 2 [sl ; sr ] be the first value of s for which sa (s) violates the slope constraints. s˙a (s) to be either max (sv ; s˙ a(sv )) or min (sv ; s˙a(sv )) whenever Given the way we choose dds

s˙a(sv ) < s˙max(sv ), the only way the constraint min (sv ; s˙a (sv )) s˙a(sv ) max (sv ; s˙a(sv )) can be violated is at a point where s˙a (sv ) = s˙max (sv ). Given how the integration (Equation (7.17))


197

s˙a (sv ), a slope violation can only occur if either during forward integration we have: chooses dds

s1 = sv

and

ds˙a (s ) = ds˙max (s ) < (s ; s˙ (s )) min v a v ds v ds v

or during backward integration we have:

s2 = sv

and

ds˙a (s ) = ds˙max (s ) > (s ; s˙ (s )) max v a v ds v ds v

This will be shown to be impossible for the forward integration case; the backward integration case being analogous. First we must realize that the terminating conditions of the integration step and our selection of H and sd guarantee that the invariant 8s 2]s1 ; s2[ : s˙a (s1 ) s˙max (s) s˙a (s2) holds at all times during the integration. Then it is obvious that s˙a (sv ) = s˙max (sv ), s1 < s2 and ds˙max (sv ) < min (sv ; s˙ a(sv )) 0 violate this invariant. This contradicts our assumptions, and

ds

therefore there is no point sv where the constraint is violated.

7.5.3

Discrete Implementation

This section presents how the algorithm is adapted to the fact that the input is a discrete sequence of

q[k]gNk 0 (and not a continuous and differentiable path). The “standard” way to handle

via points f

=

this situation would first fit a smooth curve through the via points, obtaining a continuous curve

q = f (s), and then apply the continuous version of the algorithm to obtain s˙(s). Once s˙(s) is R known, we can integrate t(s) = 0s s˙dss and (numerically) invert the function to obtain s(tk ). At the times of interest tk , we can use s(tk ), s˙a (s(tk )) in combination with fs , fss and equations (7.1) and (7.2) to obtain whichever values of q, q, q and/or are required by the controller. 0

( 0)

This procedure has drawbacks in the context of on-line trajectory generation: It is computa-

tionally expensive, requiring s(tk ) to be inverted at every sample. Requires high communication bandwidth between the time-parameterization and controller modules (the desired state must be produced at each sample time). And, it is not suited for the case in which the sample rate is not known to the time-parameterization algorithm. For these reasons an alternative approach has been adopted. The alternative approach is based on the assumption that, in most cases, the via points are fairly sparse compared to the distance moved by the robot between consecutive samples. Via points are often generated in a way that reflects the geometric regularity of the path in the sense that portions of the path that are geometrically complicated (small curvature, rapid changes in the curvature etc.) require for their description, a greater density of via points per unit of path length10. In the proposed 10

These qualitative statements need further research to be formalized


198

method, the first few steps are similar to the prototype “standard” method presented above, but the latter steps are significantly different: 1. Approximate the path length at each via point.

s[0] = 0 ; s[k + 1] = s[k] + kq[k + 1] , q[k]k , for k = 0:::N The norm used is the standard Rn norm: kqk

2

=

Pn q2 . For a given geometric path, path length i=1 i

will depend on the units chosen for each degree of freedom11 . 2. Fit a third order spline interpolation q = f (s) to the sequence of points f(s[k]; q[k])gN k =0 3. Use the spline interpolation to obtain fs[k], fss[k], s˙max [k], i1 [k], 1i [k], and 2i [k] at each via point. 4. During the integration of s˙a (s), use the following interpolating function between via points:

s

s˙a (s) = s˙a [k]2 + s[k s+,1]s,[k]s[k] (s˙a [k + 1]2 , s˙a [k]2)

for

s[k] s s[k + 1]

(7.18)

This interpolating function corresponds to the first-order approximation to s˙ a(s) assuming s¨(s) constant in the s[k] s s[k + 1] interval. In other words, if we assume s(t)

sk

skt

skt

1 = [ ] + ˙[ ] + 2 ¨[ ] 2

s[k] s s[k + 1] interval and we solve for s˙(s) we obtain the interpolation equation (7.18).

in the

5. To ensure all constraints are met in the interpolated region between via points, the constraint equations must be also be interpolated. The proximate-optimal algorithm uses the velocity-independent approximation to the torque limits already described in equation (7.14) (see Section 7.3.2). Therefore, the algorithm only needs to interpolate between successive via points the constraint functions s˙max (s),

i1 (s), 1i (s), and 2i (s) for each degree of freedom “i”. s[k] s s[k + 1], and for i = 1; ::Ndof :

The following interpolations are used for

s

s˙max (s) = s˙max [k]2 + s[k s+,1]s,[k]s[k] (s˙max [k + 1]2 , s˙max [k]2) [k ] i i i1(s) = i1 [k] + s[k s+,1]s, s[k] ( 1 [k + 1] , 1[k]) [k ] i i 1i (s) = i1 [k] + s[k s+,1]s, s[k] ( 1 [k + 1] , 1[k]) [k ] i i 2i (s) = i2 [k] + s[k s+,1]s, s[k] ( 2 [k + 1] , 2[k]) 6. The integration step will find a value of s˙a [k + 1] that approximates the maximum acceleration (deceleration) curve without violating the constraints. 11

The results should be independent of this provided the constraints and equations of motion use these same units.


7. Once the values of s˙a [k]

199

s˙a (s[k]) has been obtained, the via times are derived by simple integration: Z s[k+1] ds Z s[k+1] ds def q = ∆t[k] = s , s [ k ] s ˙ ( s ) a s[k] s[k] s˙a [k]2 + s[k+1],s[k] (s˙a [k + 1]2 , s˙a [k]2) def

=

This integral can be computed analytically.

ft[k]gNk=0 are known, third order splines are fit to the sequence of via-time and via-point pairs f(t[k]; q[k]gN k=0. This involves one spline interpolation for each degree of freedom.

8. Once the via times

This step might introduce small violations of the constraints, but in exchange, provides a trajectory described as a continuous function of time which can now be sampled at any rate.

7.5.4

Complexity of the algorithm

The algorithm presented (and its discrete implementation) have running times that are proportional to the length of the path (i.e it is

O(L)).

Figure 7.6 shows the execution-time dependency

with the number of via points and degrees of freedom for sequences of via points of increasing length. The sequences of via points used to evaluate the computational complexity of the algorithm must truly represent paths of increased length12. In addition for the comparisons to be meaningful, the paths must be ergodic in their geometric properties. The generation of such paths is described in Appendix E.

The time-complexity of the proximate-optimal algorithm is O(Nvia Ndof ),

number of via points and

Ndof

Nvia being the

being the number of degrees of freedom (DOF). To prove this,

we can associate each operation in the algorithm with the via point being integrated at that time. Since each via point is integrated just once, and there is a constant number of such operations per integration step (there is no backtracking, each via point is integrated exactly once), the integration is O(Nvia ). The remaining steps are also O(Nvia Ndof ):

12

The pre-computation stage (steps 1, 2 and 3) of the discrete algorithm are clearly O(Nvia

Ndof ). In particular each spline interpolation for each DOF is an O(Nvia ) operation.

The integration (step 4) is a O(Nvia ) operation as we have justified before.

Computing the via times by integrating 1=s˙ (step 5) is clearly an O(Nvia ) operation.

Fitting the final splines (step 6) is an O(Nvia Ndof ) operation, each spline (to each DOF) being O(Nvia) .

As opposed to representing the same geometric path sampled with greater density of via-points.


x10 -3

200

Running time per via point

5 10 DOF 8 DOF 6 DOF 4 DOF 2 DOF

4.5 4 o

3.5 o

o

o

time ( sec. )

3 o

2.5

o

o

o

o

o

o

o

o o

o

o

o o

2

o

o o o

1.5

o

o

o

o

o

o

o

o

o

o

o o

o

o

o

o

1

o

o

0.5 0 10 1

10 2

10 3

10 4

number of via points

Figure 7.6: Running time versus number of via points for different number of degrees of freedom This figure illustrates that the time-complexity of the algorithm is linear with the number of via points (or path length). We show that the execution time per via point is approximately constant over a 3 order of magnitude change in the number of via points. The timing corresponds to a Sparc station 2.

Figure 7.6 corroborates this analysis.

7.5.5

Complexity of other approaches

In the previous section we have shown the worst-case complexity of the proximate-optimal algorithm to be

O(L Ndof ) where L is the length of the path.

In this section we will discuss the worst-

case complexity of other approaches in the literature. Obviously, worst-case complexity is a very crude measure of the practical performance of an algorithm. Not only does it measure asymptotic behavior which may represent “pathological” scenarios which may never arise in practice, but it also neglects the constant and lower-order coefficients that may be the most relevant in realistic


201

situations. Typical performance of different optimal and proximate-optimal time-parameterization algorithms in practical paths should be determined using statistical methods by comparing the performance of different algorithms in different sets of paths. To this end, the canonical paths introduced in Appendix E may prove useful. This should be the topic of future research. Worst-case complexity is nevertheless a useful metric in that it provides an upper bound that may be very useful for on-line applications. We will not be able to address the worst-case complexity of many approaches that have been proposed in the literature. Rather, a brief justification of Table 7.1 for some of the most characteristic algorithms will be provided. Algorithms using dynamic programming Dynamic programming approaches have been described in [165] and [174]. Shin et. al. [165] show

that this approach has a computational complexity of O(L N2 ) where N is the number of points in

which they discretize the possible values of s˙. Aside from the large space requirements of dynamic

programming algorithms, there is a further complication derived from the need to estimate adequate values

N

which may be problem and path specific. Singh et. al. [174] propose a recursive

refinement of

N to address the problem of its selection.

In any case, dynamic programming

algorithms end-up being iterative, and the compute-time will depend on the specific characteristics of the paths, hence making it very difficult (impossible) to provide accurate estimates of their running time for a specific situation. Classical direct-integration algorithms The algorithms presented by Bobrow, Dubowsky and Gibson [72] and Shin and McKay [164] use a direct integration approach. The aforementioned papers do not address the computational complexity of the algorithms, but it can be shown through example that they have worst-case complexities of

O(L2 ).

The algorithm presented by Shin and McKay in [164] is sketched in

Figure 7.7. The important point to note is that, whenever the accelerating (decelerating) trajectory intersects the boundary region, it is necessary to search for a suitable switching point along the boundary region, and then backtrack until the original integral is met. Backtracking can take us past the region previously integrated, all the way back to the initial trajectory. The number of required backtracks, and the fact that we may end-up integrating the same piece over and over are the reasons

why this algorithm has O(L2 ) worst-case complexity. Figure 7.8 illustrates the general features of


202

Boundary of Forbidden Region

A

C 3

. s

B

4 2

1

s Figure 7.7: Sketch of Shin’s direct-integration algorithm (from [164], Figure 6) The algorithm integrates forward from the start along the maximum acceleration curve (1), and backwards from the end along the maximum deceleration curve (2), until either both branches meet or the boundary region is intersected (points A and C). Then starting from A, the boundary is searched for a point where the slope of the boundary matches the angle at which the “wedge” closes at that boundary point (indicated by arrows in the Figure). Once this point B is reached, integration proceeds backwards (3) until the initial branch is met, and then forward (4), until the either the last branch is met, or the boundary curve is hit again (in which case, the whole process is repeated).

a worst-case scenario with running time O(L2). Appendix F gives a simple analytical path where the scenario described in Figure 7.8 actually occurs. Similar examples can be found for Bobrow’s algorithm. Proposed modification to direct-integration algorithms There is a modification to Shin and McKay’s algorithm that would avoid the worst-case situation sketched in 7.8. To the best of the author’s knowledge, this modification has never been suggested in the literature. The modification changes the order in which the integrations are made and proceeds in three phases: initialization, forward integration, and backward integration. This algorithm is sketched in Figure 7.9 and described below. Initialization The algorithm starts the same way integrating forward from the beginning along the

ds˙ ds

=

max(s; s˙) curve, and backwards from the end along the ddss˙

=

min (s; s˙) until they


203

Figure 7.8: Worst-case scenario for direct-integration algorithms. The algorithm used to obtain the optimal trajectory involves forward integration in phase space along the maximum-acceleration curve. If this curve (c1 above) leaves the admissible region of phase space (point p1 above), the boundary curve of the admissible region (curve s˙ = s˙max (s)) is searched for a max matches the slope of the “closed wedge” described in Figure 7.2 (point p2 point where its slope ds˙ds above). Starting at p2 we integrate backward along the maximum decelerating trajectory until the curve intersects c1 , and forward as before (this generates curve c2 ). This process is repeated to generate curves c3 and c4 . Notice that in this worst-case scenario, each time we have to integrate backwards all the way to the initial curve c1 (that is, c3 does not intersect c2 , c3 does not intersect c2 nor c3 , etc.). Therefore, since the backward integration takes time proportional to the path length L and the number of ci curves is also proportional to L the running time is O(L2 ).

either meet or intersect the boundary region (step (1) in Figure 7.9). Note that one of the basis of Shin-McKay’s algorithm is that the forward branch can only intersect the boundary region at points s1 where:

ds˙max (s ) , (s ; s˙ (s )) < 0 (s1 ) def = max 1 max 1 ds 1

while, the backward branch can only intersect the boundary at a point s2 if

ds˙max (s ) , (s ; s˙ (s )) > 0 (s2 ) def = min 2 max 2 ds 2

and therefore, there must be an intermediate point si

2 [s1; s2] where (si) = 0.

ds˙ = Forward integration The forward integration along the maximum-acceleration curve: ddt m ax(s; s˙) has intersected the boundary region (point B1 in Figure 7.9). Let this point be s1 .

The point s1 must verify (s1 ) < 0. Search the boundary region forward until a point s2 with

(s2) > 0 is found (point C in Figure).

Resume integrating forward from that point (curve


204

(2) in Figure). Each time the boundary region is intersected, the procedure is iterated until the final backward-integration branch (curve A2 – B2 in Figure) is met (point K in Figure). Backward integration Start at the point where the forward-integration branch met the initial backward-integration branch (point K in Figure). Immediately jump to the last point where the forward-integration branch departed from the boundary curve (point J). From this point J integrate backwards until the preceding forward-integration branch is intersected (point L). Every time the backwards integration meets a forward-integration branch other than the very first one, the procedure is iterated jumping back to the point where the corresponding forward integration branch departed from the boundary region (jumps from L!G, M!E, and N!C

in bottom of Figure 7.9). The algorithm completes when the initial forward-integration branch is intersected (point O in Figure 7.9). Notice that the boundary region is never intersected during the backward integration (other than jumping to the beginning of the forward branch) because by construction the only pieces of the boundary region that are “exposed” (can be reached without first crossing the forward integration curve) are those with (s) < 0 and the

backward integration curve can only intersect the boundary region at points where (s) > 0. The preceding algorithm would have O(L) complexity. Following the boundary curve to find the potential switching points can be computationally expensive analytically, although characterizations such as the one presented by Slotine and Yang [176] can be used to speed this process (or it could be done numerically assuming that the different quantities can be linearly interpolated between via points). However, the above approach has several drawbacks compared with the proximate-optimal approach: (1) it potentially integrates every point twice (once forward, the other backward), (2) it is potentially very sensitive to the detailed shape of the path (which may not be well characterized if paths are described as sequences of via points), and (3) its running-time is very dependent on the number of “potential switching points,” and therefore, it will be difficult to predict a´ priori the computational delay: a ‘jagged” boundary region such as the one in appendix F will generate many potential “switching points” and hence be much slower to compute than a smoother version of essentially the same path. Other proximate-optimal algorithms The proximate-time optimal algorithm presented in Butler and Tomizuka [25] also uses a simplified constraint in place of individual joint-torque constraints which speeds up the computations but


205

Forbidden Region

H 6

. s

D

M

4

F

B1 C 2

I

3

G

B2 K

C 1

J

J

E

A1

1

A2

s

Forbidden Region

L 6

. s

M C

M

N

2

O

G C

A1

1 9 K

3 J

E

M

s

Figure 7.9: Modified direct-integration algorithm that avoids worst-case complexity problem Forward-integration stage shown above, backward-integration stage below. For ease of illustration we are assuming that along the boundary, where max(s; s˙) = min (s; s˙), the value of this slope is zero. When the forward-integration branch starting from the beginning (A1) reaches the boundary at B1 it is searched for the first point where (s) > 0 (point C), forward integration proceeds along (2) until the next intersection at D and so on until the last branch is intersected at point K. The backward integration (bottom diagram) starts at J along the min (s; s˙) curve until a forward branch is intersected at L, at this point, we jump to the beginning of that branch (point G) and keep repeating the process until the first branch is intersected at point O.

the unmodified direct integration method is still used to integrate the trajectory, and therefore, the worst-case complexity is still O(L2 ). Iterative algorithms Shin and McKay [166] have also proposed an iterative any-time algorithm combination of a gradient and binary search techniques. Starting from a path that satisfies all constraints, each iteration brings the path closer to the optimal by increasing the intermediate velocities “s˙,” whenever this is compatible with all the constraints. The authors also show that the complexity of their algorithm is O(N2). Where “” is the number of points in which the trajectory parameter “s”is discretized.

This result, however, refers to the complexity with respect to using smaller step sizes in s for the

same path. The complexity with respect to path length (i.e. keeping the step-size in s constant but


206

increasing the length of the path) is clearly O(L Niter ). Where Niter is the number of iterations required by the algorithm to converge.

Niter

should be reasonably independent from the path

length but depends on how close we desire the final approximation to the true optimal path.

Niter ,

however, is likely to be very dependent on the specific geometry of the path, so that accurate a´ priori predictions of the running time for a given path cannot really be made. Shin and McKay, 1985 [Slotine and Yang, 1989]

Bobrow, Dubowsky and Gibson, 1985

4

2

3

4 1

1

2

3

5

5

Shin and McKay: Perturbation Algorithm, 1986

Proximate-optimal algorithm

4’ 3

4

3’ 2’

2 2 1

1

Figure 7.10: Comparison of approaches to time-parameterization This figure compares the proximate-optimal algorithm with several classical optimal algorithms. Each call number indicates a sequential stage in the algorithm. Notice that the proximate-optimal algorithm is the only one that never integrates the same region twice. From this comparison it is clear that it is “minimal” in the number of integration steps.

Algorithm comparison summary Figure 7.10 gives a visual comparison of several approaches. From this it becomes clear that for any scenario (not just the worst case) the proximate-optimal algorithm presented in this chapter is the “baseline” for computational efficiency because each via point is integrated exactly once, and all the other algorithms have to at least do that (and most likely iterate or backtrack). Therefore, the


207

proximate-optimal algorithm could also be used as a first step to provide a seed for other iterative (truly optimal) algorithms, such as the ones described before.

7.5.6

Configuration-independent limits on velocities and accelerations

In this section we discuss the specific case of configuration-independent limits in velocities and accelerations. These constraints may arise naturally in certain scenarios. For example as pointed out by Luh [97], our path may be given in Cartesian space and be limited not by actuator capabilities but by interactions with other objects (they use the example of moving a liquid in an open container as a case in which these constraints would be natural). There are many cases in which our constraints can be approximated in this way without significant performance loss (e.g mobile robots). The proximate-optimal approach is, of course, applicable to this specific case. Nevertheless, by paying special attention to this case, and making some conservative approximations, a computationally more efficient solution can be derived.

, qmax q qmax () qiThese approximations replace the acceleration constraint: P Ndof qi 2 qimax 1 ; i = 1::Ndof by the approximate constraint i 1 ( qimax ) 1. It is useful to write this constraint using a metric tensor Q. =

1 Q def = diag[:: ; i qmax ; ::] T kxkQ def = x Qx The constraint now becomes

kqkQ 1

(7.19)

Obviously, the new constraint (7.19) is more strict than (7.5.6). A simple geometric interpretation follows: constraint (7.5.6) requires

q axes q imax.

q to be within the parallelepiped with sides intersecting the axis

at imax. The new constraint (7.19) replaces this parallelepiped with the ellipsoid with principal It is clear from this interpretation that the sacrifice in performance will not be great.

Presentation of detailed comparisons of the tradeoff between computation time and trajectoryoptimality is beyond the scope of this thesis, but typically we see an order of magnitude reduction in computation time with an travel time that is within 30% of the optimal. The use of constraint (7.19) allows several simplifications: First path-length can be computed using the metric tensor

Q. That is ds2 = dqT Qdq. From this definition it follows that: kfskQ = 1


208

fsT Qfss = 0

kqk2Q =

fs s¨ + fsss˙2

2Q = s¨2 + kfss k2Q s˙4 kqkQ 1 () js¨j s¨max(s; s˙) s q def 2 4 s¨max(s; s˙) = 1 , s¨ kfss (s)kQ = 1 , ( s˙a s¨ (s) )4 max 1 def s˙amax(s) = q kfss (s)kQ We see that the acceleration constraint imposes the limit js˙j s˙amax (s). We can combine this limit by the one imposed by the velocity constraint (7.12) and write the constraints as:

a js˙j s˙max (s) def = minfs˙max (s); (s)g ds˙ js¨j s¨max(s; s˙) () ds max(s; s˙) min (s; s˙) = s¨min (s; s˙)=s˙ For s˙ 6= 0

(7.20) (7.21) (7.22)

Again we have reduced the constraints to the general form of an instance of the DMTTP problem.

The difference is that now s˙max (s) and min (s; s˙) are very simple (and fast) to compute. Also we

s˙max(s) s˙amax(s) ) (s; s˙) 0. limitations on s˙max (s).

note that s˙

So Theorem 1 applies without any further

Section 7.6 shows the performance of this algorithm on several “canonical” paths.

7.5.7

Modification of Ongoing Trajectories

In a dynamic environment it is not advisable to fully commit to a trajectory, even if it is computed on-line. For instance, the trajectory that takes a manipulator above a moving object in order to grasp it, needs to be computed ahead based on estimates on the future motion of the object. These estimates may change as the trajectory proceeds, and therefore, it is important for the trajectory generator to incorporate mechanisms that allow modification (patching) of the ongoing trajectory. Figure 7.11 illustrates the concept of patching an ongoing trajectory: The remaining piece of the geometric path beyond a certain point (called the patch point) is replaced by a new piece (the patch path). As a result, the trajectory which was already started, needs to be modified. The modification starts at the merge point, located between the current and the patch point. The original trajectory remains unchanged until the merge time (time when the trajectory reaches the merge point). From there on, the patch trajectory is followed.


209

y

B

M

y

P

B

P

C E

y

E

y

A

A

XA

XP

XE XB

y

B

M

y

P

B

P’

P

C

E

y

E

y

A

A

tA

tM

t’P t P

t

B

t

E

Figure 7.11: Patching of an ongoing trajectory The top diagram shows the original and patched geometric paths in two dimensions (X and Y). The diagram underneath illustrates the corresponding trajectories for one of the coordinates (there is a corresponding one for every coordinate). The initial geometric path joined points A and B. The trajectory is currently at point C, when a new patch replaces the P-B section by the P-E patch. The resulting trajectory takes the system from C to E. As seen in the bottom diagram, even though the geometric path is the same until point P is reached, the trajectories start to differ at some intermediate point M between C and P. P is called the patch point, M is the merge point, and tM the merge time. Notice how the patch point P is reached at a time “t0p ” different from the original tp .

Clearly, a trajectory that has been initiated cannot be arbitrarily patched. Given the current state of the trajectory, the patch may be impossible to achieve without violating the dynamic constraints on the system (for example, the patch may require the trajectory to stop suddenly, or make a sharp corner, and the system may be moving too fast to do that without exceeding the torque limits on the actuators). Given the initial geometric path and trajectory, the patch to the geometric path, and the current state, the following issues must be addressed by the trajectory-modification algorithm: 1. Is the patch feasible? That is, can the path be patched as required without exceeding the dynamic constraints on the system. 2. For a feasible patch, what is an appropriate (optimal) merge time?


210

3. Given that the geometric path from the merge to the patch point has not changed, can this information be used to recompute the trajectory from the merge point more efficiently? One of the benefits of the proximate-optimal algorithm, is that due to its simplified properties, it provides efficient mechanisms to address all the above issues. The operation of the trajectorymodification algorithm is essentially identical to the regular proximate-optimal algorithm except that integration starts at the end of the trajectory, and whenever a decoupling point is reached, the left (earlier) piece of trajectory is computed first. The process can be seen in the phase-space plots of Figure 7.12. First the phase-space limits are recomputed for the modified region and patched into the original, resulting on new phase-space constraints (middle of Figure 7.12), next since the compute-time of the algorithm is well characterized as a function of path length, the current state of the trajectory can be advanced forward to account for the compute time (point C in Figures 7.11 7.127.13). At this point, integration proceeds backwards from the final point E. The proximate-optimal constraints allow the algorithm to advance backwards (towards the beginning of the path) as soon as the integrated value of s˙ is higher than that of a local minimum (whenever

this happens we know that each piece is independent and we can choose to integrate the earlier one first). This proceeds until the s˙ value of the leftmost integration can be joined with a horizontal line with some point between the current and patch point of original phase-space trajectory (curve from C to P), without intersecting the s˙max (s) boundary (this occurs at point F in Figure 7.13). At this point we know (thanks to Theorem 1) that the patch is feasible. Finally, each one of the independent regions is integrated, including the one from the current point C to F, and this yields the patched trajectory. The particular order in which the integration is performed has been chosen to minimize the time required to determine whether the patch is feasible or not. This gives whoever is specifying the patch (the planner in our case) a chance to try something different. Figure 7.13 shows another example of a trajectory being patched. This figure also illustrates some of the optimizations that have been incorporated to find the merge point that is closest to the patch point. This is important because a common use of patching is to modify the latter stages of the trajectory where the current point is quite far from the patch point. Moreover, these modifications can occur frequently. For instance, the proximate-optimal algorithm has been used to track a seam visually with a robot-mounted camera. As the robot tracks the seam, new points appear in the field of view (the patch is just an addition of a piece at the end). This process occurs continuously as the robot follows the seam so efficiency is critical (see Morse [112] for details on the visual seam-following application).


211

Forbidden Region

. s

C P B

A sA

sP

sB

Forbidden Region 6 M . s

5

C P’ 4 P

F 3 2

A sA

sM

1

E

sE

sP

Forbidden Region M C

P

. s

A sA

sM

sP

E sE

Figure 7.12: Trajectory Modification Algorithm Proximate-optimal algorithm to modify an ongoing trajectory. These plots are the phase-space equivalent to Figure 7.11. The top plot represents the original trajectory which is patched with a new geometric path starting at the patch point P. The middle plot represents the new phase-space constraints (only the s˙max (s) represented for simplicity) that have changed from the patch point onwards. To determine whether the patch is feasible and the merge point M, the integration proceeds backwards from E (stages 1, 2, and 3). Whenever a local minimum of the same height is reached, the integration can jump left to the local minimum (decoupling). As soon as point F is reached, the algorithm determines that the patch is feasible. The remaining backward integrations steps (4, 5, and 6) determine the merge point M. Finally each independent section (horizontal jump) is integrated to obtain the final parameterization (bottom plot).


212

Forbidden Region

6

. s

R

P

M

(M)

C

Q C

L 3

A sA

G

2

F 1 E s PsM

Figure 7.13: Example of Trajectory Modification Algorithm This example illustrates several optimizations that often occur when the patch is added far ahead of the current point. The patch point P and the trajectory up to that point (A to P) are already computed, and the trajectory is being followed. The system is currently at point C (already accounting for the computational delay). The trajectory-modification algorithm starts at E along (1). As soon as the integral curve reaches F, we know that the patch is feasible because there is a horizontal line that reaches the original trajectory between C and P without crossing the boundary region. The integration proceeds along (2) and is immediately separated into two pieces at point G. The left piece is selected first (3). The integration proceeds, until a horizontal line can be drawn to L (the last decoupling point in the original trajectory before the patch point P). The remaining piece is integrated until either we can joint the integral curve with the patch point with a horizontal line (point R), or else the integral curve intersects the original trajectory. The different independent pieces left are integrated as usual. Note that in this case the merge point M is just the patch point P.

Two things are worth noting in the example of Figure 7.13: First, the determination of the feasibility of the path is almost immediate due to the existence of a fairly strict constraint (local minimum K) in the original trajectory between the current and patch points (C-P piece). Second, the original trajectory is never recomputed (i.e. the piece from C to P is used without change or extra effort). In general, only the piece from the last decoupling point before the patch (point L) to the patch may need re-computation.

7.6 Experimental Results: Time Parameterization This section presents results of applying the proximate-optimal time-parameterizing to several sequences of via points. These sequences have been selected to represent geometrically simple


213

paths for illustration purposes. In actual practice, all planner generated-motions of both manipulators and manipulated-objects in the workcell (see Chapter 3) use the proximate-optimal timeparameterization algorithm to interpolate smooth trajectories that traverse the via points generated by the planner.

d (ds/dt)/ds (phase−space slope) [1/sec.]

1. Phase−space integration and speed−limits

15

ds/dt (speed along path) [meters/sec.]

Straight-line trajectories for a point mass

10

5

1.5

2. Slope and slope−limits along phase−space trajectory

25

1

0.5

0

0 0

5

−1.5 0

2

35

4

30 25 20 15 10

10 15 20 25 s (length along path) [ meters ]

30

0 0

3

5

5. Result of time−parameterization: Velocity

3.5 3 2.5 2 1.5

10 15 X position [meters]

20

25

6. Result of time−parameterization: Acceleration

2 X and Y acceleration [m/s^2]

4.5

1.5 1 0.5 0 −0.5 −1

5

1

0

0.5

−5 0

5

5

40 X and Y velocity [m/s]

X and Y position [meters]

30

10

5

4. Result of time−parameterization: Position

45

4

10 15 20 25 s (length along path) [ meters ]

15

−0.5

−1

1

3. Result of time−parameterization: X−Y trajectory

20 Y position [meters]

7.6.1

1

2

3

4 5 6 time [ seconds ]

7

8

9

5

0 0

−1.5

1

2

3


7

8

9

6

−2 0

1

2

3


7

8

9

Figure 7.14: Time-parameterization of straight-line path with acceleration limits The straight-line trajectory is illustrated in the third plot. The acceleration limits are the only ones exercised in this path. The first two plots illustrate the phase-space constraints and the results of the phase-space integration. The last three plots illustrate the integrated trajectory (position, velocity, and acceleration) for of the two degrees-of-freedom (they are on top of each other). As expected, the optimal trajectory is bang-bang, accelerating with the maximum acceleration (0:95m=s2 ) for the first half and then decelerating with the maximum deceleration in the last half.

Figures 7.14 and 7.15 illustrates the result of time-parameterizing a cartesian straight-line path in two dimensions. In these examples, the equations of motion provided to the trajectory-generation algorithm are those of a 1 Kg pure mass. Therefore, torque limits map directly into acceleration limits. These examples have been included because the minimum-time solution can be easily computed analytically and the different constrains are simple to interpret. They will be used to introduce the plots that will later be used to present the results of time-parameterizing real paths of the

workcell manipulators. In these two examples, the geometric path as a function of path length “s” is:


214

p x(s) = y(s) = s= 2, and therefore the velocity, and acceleration limits are: minfx˙ min; y˙ming p p s= ˙ 2 maxfx˙ max; y˙max g and minfx¨ min ; y¨min g s= ¨ 2 maxfx¨ max ; y¨maxg. Figure 7.14 illustrates the case where the trajectory is acceleration-limited (force-limited). The

velocity and acceleration limits are x˙ max = y˙max = ,x˙ min = ,y˙min = vmax = 10m=s, def and x¨ max = y¨max = ,x¨ min = ,y¨min = amax = 0:95 m=s2 . The first plot in Figure 7.14 def

contains the phase-space limits on the speed s˙

s˙ =

= N0, we will now show that starting from s˙ = 0 the maximum acceleration curve s˙ = s˙acc (s), will leave the admissible region of phase space limited by the curve s˙ = s˙max (s)

APPENDIX F. WORST-CASE PATHS FOR TIME-PARAMETERIZATION

255

at a point s0 (N ) LN=2. This can be proven by contradiction, assuming s0 (N ) > LN=2 we would have in the interval 0 s LN=4, 0 k

s˙acc(s) s˙max(k) ds˙acc ds ds˙acc ds

N=4:

q

s˙ a 1 , ss˙˙max ((N= acc max N=4)4 > P (s˙N ) ) s˙acc (s) w(s) dw = P (N )=w where w(s) verifies ds q w(s) = 2P (N )(s , s0 ) + w(s0 )2 s 2 + 2 )2 P (N ) def = a 1 , ((3N= N=4 + 2)2 s s N w(0) = 0 ) w(LN=4) = 2 P (N ) = N2 Therefore, since P (N ) is strictly increasing and for N

p

=

(F.12) (F.13) (F.14) (F.15) (F.16) (F.17) (F.18)

P (N ) , 1) N2

+(

(F.19)

31; (P (N )N=2 , 1) > 2 we see that

> 31, s˙(LN=4) w(LN=4) > N=2 + 2 which contradicts s˙acc(s) s˙max (N=2). Hence, we have proved that for N > 31 ) s0 (N ) LN=2.

for N

We can see from Figure F.2 how the situation sketched in Figure 7.8 occurs in this example for

s s0 (N ) 2 [Lm(N ); Lm(N )+1[. Where we have just proven that m(N ) < N=2. The subsequent forward integration along the maximum acceleration curve will leave the allowed region at each point s

Lk with m(N ) < k < N . Shin’s algorithm [164] then integrates backwards from the point s˙ = s˙max (k + 1) until it intersects the accelerating curve. However, we will show below that for most of these points (specifically k < r(N ) = N , 80), this integration must proceed all the way back to s = Lm(N )+1. The reason is shown in Figure F.2. For k < r(N ) Each backward integration from (s = Lk+1 ; s˙ = s˙max (k + 1)) arrives to s = Lk with a value of s˙ smaller than s˙max(k) being therefore unable to intersect the previously computed s˙(s) curve in the region in which it is decreasing, i.e. all the way back to the segment s 2 [0; s0(N )]. The symmetric situation is replicated in the backward integration branch that starts at the end of the path. Assuming N large enough, the number of backtracks is (N , m(N )) , (N , r(N )) N=2 , 80 i.e. O(N ). =

The computation required for each backtrack is proportional to the length of the path backtracked.


256

8.5 10

allowed region optimal trajectory

8.45

9

8.4

8

8.35

7 d s/dt

d s/dt

8.3 6 5

8.25 8.2

4 8.15

3

8.1

2

forward integration succ. back−integrations

8.05

1 0 0

200

400

600

8 80

s

final integration 90

100

110

s

Figure F.2: Phase space direct integration and detail. Since the length of each segment is constant ( ), this length is O(N ) and therefore the complexity is O(N 2) = O(L2 ).

k < r(N ) = N , 80, each backward integration from (s; s˙) = (Lk+1 ; s˙max(k + 1)) along the maximum deceleration curve s˙ = s˙dec (s), reaches s = Lk with a value s˙ < s˙max(k). For s 2 [Lk ; Lk+1 [ we have s˙dec (s) s˙max (k + 1) and therefore: Finally we prove that (as stated above) for

Q(x)

def

=

s

s˙dec (s)4 ,1 1, s˙max (k)4 s˙dec (s) s + 1)2 a 1 , ((xx + 2)2

ds˙dec (s) ,1 ds s˙dec (s)

s

1,

s˙max (k + 1)4 = ,Q(N , k) s˙max (k)4 s˙dec (s)

Using a bound similar to (F.15) and integrating backwards from Lk+1 , to Lk we can obtain:

q

s˙dec (s) < 2Q(N , k)(Lk+1 , s) + s˙max (k + 1)2 q s˙dec (Lk ) < 2Q(N , k) + s˙max(k + 1)2


=

q

2Q(N

257

, k) + a(N , k + 1)

Q(x) is strictly decreasing and 2Q(80) < a = 0:5, we have that N , k > 18 ) p s˙dec (Lk ) < N , k + 2 = s˙max (k + 1) as stated. Since

Appendix G

Matrix Plotting Utilities This appendix provides a brief description of the matrixPlotServer utility. The matrixPlotServer provides a low-overhead mechanism to visualize ControlShell’s matrices (CSMat’s). The matrixPlotServer isn’t intended to replace sophisticated matrix plotting tools such as Matlab or Xmath, rather its aim is to allow applications that use matrices to display intermediate and final results easily with minimal impact on the applications performance. The matrixPlotServer can save matrices in Matlab format for later post-processing.

G.1 Overview The architecture of the matrixPlotServer consists of a small communications client linked with the application that sends the matrix information over the network to the plot server application. The plot server resides on a Unix workstation, and stores all the matrices sent by the client. Repeated matrices with the same name have a number appended (which represents the order in which they were received), and hence do not replace each other. The plot server allows visualization, zooming and saving operations. Both single and two dimensional matrices can be plotted. Two-dimensional matrices generate one plot per row. A typical session is illustrated in figure G.1. Multiple windows can be open, and any number of matrices can be shown on any window, allowing for easy comparison between matrices. The matrixPlotServer allows visualization of matrices even if the application is running on platforms with no display (or even X-client) capabilities such as VxWorks. The matrix data is serialized using a machine-independent format (XDR) so that it can be displayed in any machine regardless of its architecture. In addition, since a single unacknowledged packet is sent per plot, 258

APPENDIX G. MATRIX PLOTTING UTILITIES

259

it has relatively small impact on the performance of the application code. This architecture allows the client to execute display functions even if no server is available. This has certain advantages if we want the application to be robust to the presence of the server. On the other hand, it is not appropriate for the collection of sequences of critical data or whenever reliable delivery of each matrix is a concern.

Figure G.1: A typical session using the matrixPlotServer The matrixPlotServer allows matrix visualization, zooming and load/store operations on both ControlShell’s and Matlab’s formats. The plot window on the top displays a matrix with two rows (hence the two plots). In this plot the user has both selected an grid and requested each individual matrix element to be displayed. The bottom-right plot window illustrates contains is also displaying the same matrix, but the user has zoomed into the area containing the intersection of both curves.


260

G.2 Application Interface The applications interface to the matrixPlotServer consists of three functions: CSMatPlot, CSMatSetVerbosity, and CSMatPlotSetDisplayHost. CSMatPlot Is the main function that serializes and sends the data to the server. This function is documented in detail in section G.3. CSMatPlotSetDisplayHost This is a convenience function that allows global setting of the name of the (unix) host where the server will execute. CSMatSetVerbosity saging.

This function allows turning on different levels of warning and status mes-


261

G.3 Selected Interface Documentation This section contains the manual page for the main application interface function. NAME CSMatPlot \- Plot matrices SYNOPSIS CSMatPlot(char *displayHost, CSMat m1, CSMat m2) DESCRIPTION Plots m2 versus m1. If m1 and m2 are vectors, CSMatPlot will plot each element of m2 versus each element of m1 i.e. plots the points

{(m1[i],m2[i]),

i=1..n}

If m1 is a row vector and m2 is a matrix, then CSMatPlot will interpret each row of m2 as a vector and plot it versus m1. i.e. for i=1..m plots

{(m1[j],m2[i][j]),

j=1..n}

If m1 is a column vector and m2 is a matrix, then CSMatPlot will interpret each column of m2 as a vector and plot it versus m1. i.e. for j=1..m plots

{(m1[i],m2[i][j]),

i=1..n}

If both m1 and m2 are matrices, then CSMatPlot will interpret each row as a vector and plot corresponding vectors in m1 and m2. i.e. for i=1..m plots

{(m1[i][j],m2[i][j]), j=1..n}

COMPATIBILITY: m1 and m2 must have "compatible" that is: (1) for m1, m2 vectors they have to have the same number of elements. (2) for m1 vector, m2 matrix, m1 must be a row


262

vector with the same number of columns as m2 (3) for m1 matrix, m2 matrix, they must have the same size. (4) m1 matrix, m2 vector isn’t allowed. (5) m1==NULL is compatible with any m2. It will plot m2 versus column number unless m2 is a row vector in which case it DISPLAY: displayHost==NULL has the following meaning: unix: use the same host where the client is running VxWorks: Not allowed! PARAMETERS: char *dysplayHost, # where the display is CSMat m1,

* Matrix/Vector of abscissas

CSMat m2;

* Matrix/Vector of ordinates *

USAGE CSMatPlot(displayHost, m1, m2) or CSMatPlot(displayHost, NULL, m2) or CSMatPlot(NULL, m1, m2) or CSMatPlot(NULL, NULL, m2) RETURNS: m1 if no error occurred, NULL if an error occurred. SEE ALSO CSMat(2) matrixPlotServer(2)

*

Appendix H

Selected Manual Pages This appendix lists the manual pages of several of the software packages developed for this project: The Network Data Delivery Service (NDDS). Included are the main page, some related utilities (nddsgen, nddsStartDaemon, nddsRequest), and the accompanying Hierarchical Hash Table package. NDDS is now a commercially available product [145]. Via-Point Trajectory Generator. (VIP). This is a package that implements the proximate-optimal trajectory parameterization algorithm described in chapter 7. This package is now used by several other projects [112, 104, 195]. The VIP package has also been encapsulated into a ControlShell component and is available with the standard release of ControlShell [144]. The matrixPlotServer. This is a graphical utility that allows visualization (plotting, zooming etc.) of matrices (in ControlShell’s CSMat format). This utility can operate remotely (over the internet) from the generating side, so that clients without displays or X-windows (such as the real-time systems) can use it to display relevant data. It also allows multiple clients to concurrently send the data to the same display server so that their data can be compared. In addition to the manual pages presented here, each individual interface function of the above packages has its own manual page which is omitted for brevity.

263

APPENDIX H. SELECTED MANUAL PAGES

NDDS(2)

NDDS Reference

264

NDDS(2)

NAME NDDS - Network Data Delivery Service. SYNOPSIS NddsInit - Initialize NDDS optionally specify an ndds domain NddsPeerHostsSet - Sets hosts that receive subscription requests NddsTypeRegister - Register a NDDSObject. NddsProducerCreate - Create a new producer. Set its characteristics NddsProducerModify - Modify characteristics of exixting producer NddsProducerAddProduction - Add instance to list of productions NddsProducerSample - Sample all productions of the producer NddsConsumerCreate - Create a new consumer NddsConsumerModify - Modify the characteristics of existing consumer NddsConsumerAddSubscription - Consumer requests subscription. NddsConsumerPoll - Receive the updates of a specific consumer NddsInstanceQuery - Distributed query to all NDDS nodes NddsConsumerGetByName - Get a consumer from its name NddsProducerGetByName - Get a producer from its name NddsUtilityFind - Get an object given its type and name NddsUtilityStore - Enter a record of a given type. NddsDBaseListInstances - prints all NDDSObjectInstances known to NDDS NddsDBaseListConsumers - Print information on matching consumers NddsDBaseListProducers - Print information on matching producers NddsDBaseListProductionsRemote - Print info on matching remote productions NddsDBaseListSubscriptionsRemote - List matching subscription requests NddsDBaseListAll - Hierarchical printing of all NDDS databeses NddsUtilitySleep - Sleep for a specify number of micro-seconds NddsUtilityTimeGet - Get current time in seconds. NddsVerbositySet -

Sets/Gets verbosity level.

NddsProductionsReliableParamSet - Set/Get parameters for reliable updates NddsProductionsAsyncParamSet - Set/Get parameters for async. productions DESCRIPTION The Network Data Delivery Service (NDDS) provides transparent network nectivity

con-

and data ubiquity to a set of processes possibly running on dif-

ferent machines. NDDS allows distributed processes to share data and event information

without concern for the actual physical location and architec-


265

ture of their peers. NDDS allows its "clients" to mode:

share

data

in

a

dual

subscriptions and one-time queries. NDDS especially targets applica-

tions with repetitive data flow, where low latency and high network

robustness

to

changes and failures are required. Distributed control systems are

one common example of such systems. NDDS supports "subscriptions" as the fundamental In

the

means

of

communication.

application context described, subscriptions have some fundamental

advantages over other information sharing models (such as client-server shared-memory).

It

or

cuts in half the data latency over query/response type

models and it allows synchronization on the latest available information as soon as it is produced. NDDS clients register any number of "producers" and

"consumers"

of named

data objects. A single NDDS client may register multiple producers and consumers. Each individual producer and consumer has its

own

(described

a

later)

and

is

a

producer/consumer

objects. A NDDS client registers notify These

to

functions

on

characteristics set of named data

each subscription.

functions are called back whenever a new update arrives. The rate at

which data updates arrive and the notify function is called depends on both the

rate

at

which data is produced and the consumer characteristics. See

the man pages on NddsConsumerCreate and NddsProducerCreate. Consumer notification can be initiated by the NDDS client---this amounts to polling

to see if there are updates for the consumer’s data (in this case

the notify functions are called in the task context of the NDDS client)

or

can be asynchronously triggered by the arrival of the new data itself. Ndds Objects: instance

All data

of an

NDDS

wishes to use by tions

on them.

communicated object.

The

in

NDDS

must

be

declared

the

an

providing the methods required to

perform basic

opera-

NDDS provides a basic library of objects (see the man page

on NDDSobjects). The user can use this objects as models and define that suit

as

user must define any object types it

application.

others

All objects are defined using NddsTypeRegis-

ter(). Data Subscriptions: To subscribe to data you must first as

a

consumer.

This

is

register

achieved with NddsConsumerCreate().

yourself A consumer

specifies some parameters that control things like the maximum data rate and

whether

update

the notify routines are initiated by the user or called

asynchronously on new data updates.

See the man page on NddsConsumerCreate


for further information. objects

266

A registered consumer can subscribe to named data

using NddsConsumerAddSubscription(). See the man pages on NddsCon-

sumerAddSubscription() and NddsConsumerPoll(). Data Production: To produce data you must first declare yourself as a

pro-

ducer. This is achieved using NddsProducerCreate(). A producer must specify some parameters that indicate the type of producer and help conflict resolution

among

multiple

producers. Once registered, a producer can specify

any number of NDDS objects it wishes to

produce.

This

is

achieved

with

NddsProducerAddProduction(). All data produced by the same producer is sampled synchronously when the user calls NddsProducerSample(). This will take snapshots

of

all the data which will be held and later distributed to all

consumers. NDDS supports reliable updates. Any producer can specify any of its updates to

be

delivered reliably. Reliable updates are guaranteed to be delivered

in order. NDDS maintains a database of all the instances of have been

different objects

that

registered. You can access the database using NddsUtilityFind()

and NddsUtilityStore(). BUILDING an APPLICATION: In your code include "NDDS.h". libNDDS.a, NddsXDR.a, libNDDSobjects.a

libHash.a

and netio.a

In

Unix

(in that

link

to

order)

and netio.a (in that order)

In VxWorks link or preload netio.so

libHash.so

NddsXDR.so

libNDDS.so

in

that order.

RUNNING AN APPLICATION Lets assume that you have built 2 applications

"producer"

that communicate between hosts "mars" and "pluto". things that

"consumer"

Here is a check list of

will ensure proper operation:

1.- Make sure the there are ndds daemons is

running on

"pluto"

8000)

on

and

the

same

domain-number

(say

both

that

"mars"

and

used in your

NddsInit() call within

your applications. Typically

either

read from the command line by your program or taken

be

defaulted,

from an environment variable.

this

you

number

will


267

You can verify proper operation of the NDDS daemons by issuing the command: nddsRequest -c PING -n mars -d 8000 If the daemons aren’t running, you can start them with the command: nddsStartDaemon -d 8000 -v 1 both on "mars" and "pluto" 2.- Make sure that your environment variable NDDS_PEER_HOSTS includes

both

mars and pluto (and any other hosts you want) for example NDDS_PEER_HOSTS = mars:pluto:saturn 3.- Start the program on "mars" with the same domain-port number: producer 8000 4.- Start the companion program on "pluto" (same domain-port number): consumer 8000 5.- Depending on the verbosity level selected = 0, 1 or should

2

You

see messages flowing back and forth between the two programs. Refer

to the source in the examples

directory

of

the

NDDS

distribution

for

further details.

EXAMPLES Example source code using NDDS and makefiles to build it can be

found

the examples directory of the NDDS distribution.

SEE ALSO nddsStartDaemon, nddsRequest, NddsXDR, nddsgen, PhoneMessage, and the individual man pages on each function.

in


nddsgen(1)

NDDS

268

nddsgen(1)

NAME nddsgen - Automatically generate code to describe a type to NDDS SYNOPSIS No user-callable interface USAGE nddsgen [-typename ] [-nddstype ] [-example] [-remove] [-architecture ] -- file containing XDR description of data-type

-- name of the type defined in ,

defaults to without any extensions

-- name of the NDDS data type that will be

generated. Defaults to p_.

Cannot be

the same as the XDR type name. -example

-- Generate example programs that use NDDS to

communicate -remove

-- Do not generate ’stub’ files, instead write

the actual files directly. Warning. This will remove any changes you have made to the files.

-- Architecture for the example makefile,

defaults to sun4

DESCRIPTION nddsgen takes a high-level specification of the language

and

generates

data

format

in

the

XDR

code to serialize and deserialize objects of that

type so that they can be distributed using NDDS.


269

nddsgen can also create stub test programs and a makefile so to get a

sim-

ple working NDDS application, the user only needs to fill in some initialization code (i.e., give initial values to the data) and type "make". To use nddsgen you need to write a description language.

XDR

of

the

type

in

the

XDR

stands for eXternal Data Representation and is the industry

standard machine-independent data-representation format

to

exchange

data

across machines. Assuming you have a description for a data of type "myDataType" in the file "myFileName". All you need to do is type: nddsgen

myFileName -t myDataType -example

nddsgen will echo several messages as it progresses and,

in the

end

you

will be left with all the necessary stub files and a makefile! You will need to edit some files (refer to the messages printed by nddsgen) in

order

to

give

your

data initial values etc. after this you can type

"make -f " and you’ll have a working NDDS application!

THE XDR LANGUAGE In the XDR language, data-types are described in a fashion very similar structures

in

"C". Even

to

if you are not familiar with it, looking at the

following examples and more that can be

found

under

the

objectlib

sub-

directory on your NDDS distribution will probably be enough. The complete description of the language can be found in "The External Data Representation

Standard:

Protocol

Specification" chapter in the "Network

Programming Manual", available from most Unix vendors.

EXAMPLES In this example a FloatArray (named variable-sized array of floats) will be constructed. 1.- Create a file called FloatArray.xdr that contains

the

the XDR language. In this case, the complete file contains: struct FloatArrayStruct {

description

in


270

string name; / * a NULL terminated string * / float data;

/ * a variable-size array * /

}; 2.- Next run nddsgen with the command: nddsgen FloatArray.xdr -t FloatArrayStruct -n FloatArray -ex 3.- If this is the first time you tried to create the FloatArray type, files

FloatArray.c,

FloatArray_test.c

FloatArray_producer.c

FloatArray_consumer.c will have been created for you. Otherwise,

the

and you

will

need to incorporate the changes from the corresponding stub files. Edit these files to initialize your data and provide a custom print routine for the data. Failure to initialize the data may cause the example programs to crash. 4.- Now type: make -f makefile_FloatArray_sun4 You

will

end up

with

3

programs: sun4/FloatArray_test,

sun4/FloatArray_producer, and sun4/FloatArray_consumer. 5.- First run the deserialization print

FloatArray_test routines

routine,

the

operate data

to

make sure

properly.

will

be

If

the

serialization

and

you’ve provided a custom

printed before

and after

serialization/deserialization to show that data integrity is maintained. 6.- Start the NDDS daemon on each machine that you want tests

by

to

run

the

NDDS

using nddsStartDaemon (see man nddsStartDaemon for more details)

If two machines are named, say, "mars"

and

"pluto",

type

both

on

both

machines: nddsStartDaemon -v 1 7.- Make sure your NDDS_PEER_HOSTS environment variable is defined and contains

the

colon-separated

list of machines that will be running the NDDS

tests. In this case, on both "mars" and "pluto", type: setenv NDDS_PEER_HOSTS mars:pluto 8.- Run the tests: On "mars" type: sun4/FloatArray_producer


On "pluto" type: sun4/FloatArray_consumer You should see both programs printing information as they communicate.

SEE ALSO NDDS, NddsXDR, PhoneMessage, FloatArrayPtr

271


nddsStartDaemon(1)

NDDS Reference

272

nddsStartDaemon(1)

NAME nddsStartDaemon

- Start the NDDS daemon

SYNOPSIS No user-callable interface DESCRIPTION Use this command to start the NDDS daemon on the local host. For

communi-

cations to take place through NDDS, one NDDS Daemon must exist on each participating host in the ndds-domain used for the communications. This nddsdomain

can be any legal port number in your system greater than 7400. This

must be the *same* numbers specified by the application in

the

NddsInit()

calls. If an illegal domain-number is passed (or it is simply unspecified), a default value will be used. Thus starting the NDDS daemons without specifying

the domain-number and calling NddsInit(0) from the applications will

yield the same domain for the daemon and applications.

USAGE On Unix systems type to the shell: nddsStartDaemon [-d ] [-v ] \ [-c ] [-h] -d domain number of the daemon. See man page on

NddsInit.

The default domain number is used if left unspecified. -v select verbosity level 0 (silent), 1 (status), 2 (debug) -c path to the RTI license file

for

this

application.

omitted, the standard places will be searched (see man page on lmcontrol) -h print brief help message. In VxWorks, type to the VxWorks shell (after loading the NDDS libraries) nddsStartDaemon 8000, 1, \

If


273

"/license.dat"

EXAMPLE UNIX>

nddsStartDaemon -d 8000

-v 1 \

-c $(RTIHOME)/lm/license.dat Will start the NDDS daemon in the local host in the domain-number 8000 with verbosity

1

(the

default)

using

the

key

in

the license

file

verbosity

level),

$(RTIHOME)/lm/license.dat VxWorks> nddsStartDaemon 8000, 1, \ "/local/rti/lm/license.dat" Aside from any message that it prints (depending on the you can check that the daemon is running by typing: UNIX> nddsRequest -c PING -d 8000

SEE ALSO nddsRequest, NddsInit, NDDS.


NddsXDR(2)

NDDS

274

NddsXDR(2)

NAME NddsXDR - Library of XDR utilities for NDDS SYNOPSIS NddsXDRGetSerializingStream - access the raw XDR serializing stream NddsXDRGetDeserializingStream - access the raw XDR deserializing NddsXDRStreamAlloc - Allocate an NDDS data stream NddsXDRStreamFree - Free an NDDS data stream NddsXDRSerializeInteger - Serialize an integer from the NDDSXDRStream NddsXDRDeserializeInteger - Deserialize an integer from the NDDSXDRStream NddsXDRSerializeArrayInt - Serialize array into the NDDSXDRStream NddsXDRDeserializeArrayInt - Deserialize a int array NddsXDRSerializeFloat - Serialize a float into the NDDSXDRStream NddsXDRDeserializeFloat - Deserialize a float from the NDDSXDRStream NddsXDRSerializeArrayFloat - Serialize a float array into the NDDSXDRStream NddsXDRDeserializeArrayFloat - Deserialize a float array NddsXDRSerializeDouble - Serialize an double from the NDDSXDRStream NddsXDRDeserializeDouble - Deserialize an double from the NDDSXDRStream NddsXDRSerializeString - Serialize a string into the NDDSXDRStream NddsXDRDeserializeString - Deserialize a string from the NDDSXDRStream

DESCRIPTION This library provides network utilities to

handle

serialization

and

de-

serialization through XDR. To register a type with NDDS, must be

provided.

These

serialization and

methods

are

used

deserialization

methods

to serialize/deserialize an

NDDSObject into/from an NDDSXDRStream. The serialization/deserialization methods can be *automatically*

generated

from a high-level specification of the data-type using "nddsgen", a utility that comes with your NDDS distribution. With "nddsgen" you can generate the serialization/deserialization methods *without* any programming! To use "nddsgen" you must describe the data types using the XDR language.


275

You don’t need to become an expert on the XDR language, or read mentation

to

any docu-

start using it! Looking at a couple of examples and playing

with "nddsgen" is probably all most people will ever need to do. examples

Complete

on using "nddsgen" to generate the serialization/deserialization

methods are available both in the examples and nddstypes subdirectories your NDDS

distribution.

of

See the man pages on "FloatArray" and "PhoneMes-

sage" for two examples that use "nddsgen". If you want to know more about it, XDR stands for eXternal Data Representation and

is the industry standard machine-independent data-representation

format for data exchange across machines. For further information man

pages

found in: "The External Data Representation Standard: tion"

see

the

on "nddsgen". A complete description of the XDR language can be

chapter

Protocol

Specifica-

in the "Network Programming Manual". This is available from

Sun Microsystems (the original developer of the XDR language) and virtually every UNIX vendor. There are many data types that are so simple that it may be easier to write the

serialization/deserialization

routines by

NddsXDR provides utility functions that can make simple.

An example

can

hand.

For these

writing

these

cases, extremely

be found in the nddstypes directory of the NDDS

distribution. See the man page on "IntArray". Yet in other cases there are data structures whose semantics are very

dif-

ficult to express syntactically in the XDR language (or any other syntactic language for that matter). For example a structure such as: struct StructWithSemanticInfo { int numchar; / * number of characters in "data" * / char *data; / * buffer of size "numchar" * / char *begin; / * begin = &data[0] * / char *end; / * end

= &data[numchar] * /

}; In this cases the best (only) approach is to write the code by

hand using

the utilities provided by NddsXDR. The functions provided by NddsXDR will be sufficient to

handle

most

data

types you may come across. In the few cases in which it may be necessary to


use XDR routines directly, the XDR stream can be accessed directly with the functions: NddsXDRGetSerializingStream() if serialization is intended NddsXDRGetDeserializingStream() if deserialization is intended. See the examples below.

EXAMPLES To serialize the following structure: include "NddsXDR.h" struct PolygonStruct { char *name;

/ * name of the polygon * /

int nVertex;

/ * number of vertices * /

float *x;

/ * x-coordinates of the vertices * /

float *y;

/ * y-coordinates of the vertices * /

} *Polygon; (a) Using NddsXDR functions: ----------------------include NddsXDR.h boolean SerializePolygon(NDDSXDRStream nddsds, Polygon polygon) { int MAX_LEN = 10000; if (

(!NddsXDRSerializeString(nddsds, polygon->name,

MAX_LEN)) || (!NddsXDRSerializeInteger(nddsds, polygon->nVertex)) || (!NddsXDRSerializeArrayFloat(nddsds, polygon->x, polygon->nVertex)) || (!NddsXDRSerializeArrayFloat(nddsds, polygon->y, polygon->nVertex))) { return FALSE;

276


} return TRUE; }

(b) Using XDR functions directly: ---------------------------include include boolean SerializePolygon(NDDSXDRStream nddsds, Polygon polygon) { int i, MAX_LEN = 10000; XDR *xdrs; xdrs = NddsXDRGetSerializingStream(nddsds); if (

(!xdr_string(xdrs, &polygon->name, MAX_LEN))

|| (!xdr_int(nddsds, &polygon->nVertex))) { return FALSE; } for (i=0; inVertex; i++) { if (!xdr_float(nddsds, &polygon->x[i])) { return FALSE; } } for (i=0; inVertex; i++) { if (!xdr_float(nddsds, &polygon->y[i])) { return FALSE; } } return TRUE; }

277


SEE ALSO NddsTypeRegister, nddsgen, NDDS, IntArray, FloatArray, rpc, xdr, and the individual man pages on each function.

278


HT(2)

279

NDDS HT(2)

NAME HT - a hash table library SYNOPSIS HHTSetVerbosity - set verbosity level HHTFindHT - Find a Hash Table Given a list of keys. HHTFindEntry - Find the entry handle for the record HHTFindRecord - Find a record under a list of keys HHTEnterRecord - Enter a record (va_arg version) HHTCreate - Create a hash table HHTForAllMatches - Call a user-specified routines for each match HHTListRecords - List hash tables hierarchically HHTPrune - Recursively remove specific records and hash tables HHTRemoveRecord - Remove a record HHTDestroy - Destroy a hash table and all its descendants. HTSetVerbosity - set verbosity level HTFindEntry - Finds the entry given the key. HTGetRecord - Gets the record given the entry HTGetRecordUserHandle - Gets the user handle given the entry HTSetRecordUserHandle - Sets the user handle given the entry HTGetRecordType - Gets the record type given the entry HTFindRecord - Find the record given the key. HTEnterRecord - Enter a record (removes duplicates) HTRemoveRecord - Remove a record HTCreate - Creates a hash table HTSetAccessType - Sets (concurrent) access type for HashTable HTFree - Free memory associated with a hash table HTGetHashTableUserHandle - Gets the user handle of a HashTable HTForAllMatches - Call a user-specified routine for each match HTForAll - Call a user-specified routine for every record HTPrune - Remove specific records HTDestroy - Destroy a hash table and remove all its records. HTIsEmpty - Return TRUE if the hash table is empty DESCRIPTION This library implements a Hierarchical Hash Table (HHT) Abstract Data Type. A HHT is a hash table whose entries can themselves be hash tables.

Charac-


280

ter strings are used as keys. This package provides an efficient way of organizing data in a It

also

allows

searching

a

hierarchy.

HHT (and all its descendants) and executing

user-provided callback routines for all records that match given templates. Along with each record, a user-provided handle and integer type description are

stored. These

supply the basic functionality to build more sophisti-

cated layers on top of this library. The implementation gives a lot of power to the user. Beware; power its dangers.

SEE ALSO Individual man pages on each function.

carries


vip(2) CS Component

vip(2)

NAME vip.c - Via Point Trajectory Generator SYNOPSIS VIP_SetVerbosity - Set/Get verbosity level: VIP_SetUserParameter - Bind a user parameter to a VIP trajectory VIP_GetUserParameter - Bind a user parameter to a VIP trajectory VIP_GetTrajectoryByName - Get the VipTrajectory handle from the name VIP_SetStrictBoundEnforcement - Enforce Bounds VIP_GetStartAndEndingTime - Get the time the trajectory will end VIP_GetViaPoints - Get the via times of a trajectory VIP_GetViaTimes - Get the via times of a trajectory VIP_SetSmoothingKernel - Provide your own smoothing kernel VIP_GetSmoothingKernel - Get the current Smoothing Kernel VIP_Create - Create a trajectory VIP_SetBeginNotifyFunc - Sets a notify function to call when traj. begins. VIP_SetEndNotifyFunc - Sets a notify function to call when traj. ends VIP_Destroy - Destroy a trejectory (Free memory) VIP_Cancel - Cancel a trajectory VIP_ReTimeParameterize - patch an existing trajectory VIP_TimeParameterize - time parameterize a set of via points VIP_GetTrajectorySample - Get the desired state at any time VIP_SetViaPts - Sets the via points of a trajectory. VIP_SetStartTime - Set the starting time of a trajectory VIP_EvaluateTrajectory - Evaluate trajectory at a given time VIP_ReallocateTrajectoryState - Reallocate space for a traj state. VIP_AllocateTrajectoryStateHistory - Allocate space for a state history VIP_FreeTrajectoryStateHistory - Free memory in state history VIP_GetDesiredStateHistory - Get the trajectory’s desired state history VIP_PrintTrajectoryStateHistory - Print the state history VIP_PlotTrajectoryStateHistory - Plot the state history VIP_AllocateDynamicInfo - Allocate memory to Hold dynamic information VIP_FreeDynamicInfo - Free memory used to hold dynamic information VIP_ApproxAccelLimits - Set/Reset usage of approximate acceleration limits VIP_SetAccelerationLimits - Sets the acceleration limits VIP_SetVelocityLimits - Sets the velocity limits VIP_SetTorqueLimits - Sets the torque limits

281


282

VIP_SetRobotDynamics - Specify robot dynamics

DESCRIPTION Name: VIP

- Via Point Trajectory Generation

Usage: include link to the matrix.a

matrix_plot.a

following

netutils.a

libraries:

netio.a

Unix: vip.a

VxWorks : vip.so matrix.ro

matrix_plot.ro netutils.so netio.so This library implements the trajectory generation ARL

memo:

number

of

ARL-XX-92.

In

degress of

manipulator/robot

a

freedom

capabilities,

nutshell, and

imposed.

dynamic

constraints

on

the

time

subject

to

the

The contraints can by any combination of (configura-

tion independent) velocity, acceleration and torque limits. generates

in

this algorithm will generate a trajectory

that will traverse the set of via points in minimum constraints

algorithm described

given a set of via points in any

a trajectory

The

algorithm

in linear time with respect to both the number of

via points and the number of degreess of freedom. Using this package requires 3 phases: (a) Creating a trajectory ADT and setting up constraints. and

VIP_AllocateDynamicInfo()

See VIP_Create()

VIP_SetVelocityLimits()

VIP_SetAccelerationLimits() VIP_SetTorqueLimits(). (b) Time parameterizing ta set of via points.

See

VIP_TimeParameterize(),

(c) Splining the via points to a set of via times.

See VIP_SetViaPoints().

VIP_ReTimeParameterize()

(c) Getting desireds in the control loop.

See VIP_GetTrajectorySample().

There is also a controlshell component that will do most of that

work

for

you. See man VIP_GenarateState

CAVEATS Generating trajectories in a control loop is risky directions

with

business,

follow

this

care. This comments apply to a specific instance of a VIP

trajectory (As returned by VIP_Create() ).


283

(1) The process running VIP_GetTrajectorySample() must run at higuer priority

than

VIP_SetViaPts()

VIP_SetStartTime().

So that it is never inter-

rupted by those. (2) It must always be

O.K.

VIP_GetTrajectorySample().

to In

zero other

order hold words,

you

the

state,

passed

to

must provide reasonable

values for the parameters. The VIP_GetTrajectorySample() Language: Standard C Written by: Gerardo Pardo-Castellote (gpc), Stanford University, May 1991 Revised: May 1994, gpc

Added support for patching on-going trajectries

SEE ALSO: VIP_GenarateState


VIP_GenerateState(5) CS Component

284

VIP_GenerateState(5)

NAME VIP_GenerateState - Component interface to VIP Trajectory generator SYNOPSIS VIP_GenerateStateClass::ResetTraj - Reset trajectory VIP_GenerateStateClass::vip - Get VIP trajectory object VIP_GenerateStateClass::dInfo - Get VIP trajectory dynamics object DESCRIPTION Use this component to produce trajectories generated by the Via-Point (VIP) trajectory

generator.

You make regular calls to the VIP libraries to con-

vert a set of via points into a smooth, time-parameterized set

of

trajec-

tories. You supply this component with the dynamics of your system and you

would

like

any

limits

to place on the trajectory, and the component will create

the DynamicInformation and VipTrajectory objects for you at instance time. You

can

use

the

vip()

and

dInfo()

public

methods to rertrieve these

pointers at run time in order call the VIP libraries to

calculate

actual

trajectories. This component generates an output called "state" at each

sample

time

of

the position, velocity, and acceleration of each degree of freedom (DOF) in your system. Each row of "state" corresponds to

a

single

DOF,

and

the

columns are: 0 = position, 1 = velocity, 2 = acceleration. The parameter "limitMask" is used to indicate which limits you to

impose on the trajectory generation.

would

like

To impose more than one type, add

the values together. VIP_VEL_LIMIT

= 0x1

VIP_ACC_LIMIT

= 0x2

VIP_TORQUE_LIMIT = 0x4 The parameters "maxVel", "maxAccel", and that contain the limits for each DOF. such thai MIN = -MAX.

"maxTorque"

should

be

matrices

The limits are symmetric about zero,

If the "limitMask"

does

not select a

particular


285

limit, that CSMat may be a dummy matrix. The parameter "initialState" is a matrix that contains a position that trajectory

the

should match when the component is enabled or when you call the

ResetTraj() method. Use this to match desired and actual positions. If you do not impose torque limits, you may ignore the "eom" parameters. The parameter "eomParam" should contain the system.

The

format

for

your

of the data is completely determined by how they are

used in you dynamic equations "eomRtnCreate"

dynamic parameters

of

motion.

"eomParam"

is

passed

to

the

routine to create a data object that can be utilized by the

"eomRtnName_M", "eomRtnName_B",

"eomRtnName_C",

and

"eomRtnName_G"

rou-

tines. These "eomRtnName" funtions combine to form the equatons of motion for your dynamic

system.

They

instance time,

the

parameters

find the

to

are defined

by you in your user source code.

VIP_GenerateState routines

in

component

will

the symbol table.

use these

At

String

If they are not

found, it will generate an error.

IMPORTANT Use extern "C" {} to enclose all the EOM functions to prevent the C++ piler

from

"mangling" the function names.

com-

Otherwise, This component will

not be able to find the functions at instance time.

DFE ENTRY FORMAT VIP_GenerateState: state

limitMask

maxVel

maxAccel

maxTorque

initialState

eomRtnCreate

""

eomParam

eomRtnName_M

""

eomRtnName_B

""

eomRtnName_C

""


eomRtnName_G

SEE ALSO VIP(2)

""

286


matrixPlotServer(2)

287

Reference Manual

matrixPlotServer(2)

NAME matrixPlotServer - CSMat plotting facility. SYNOPSIS CSMatPlotSetVerbosity - Set vervosity level CSMatPlotSetDisplayHost - set default display host CSMatPlot - Plot matrices int CSMatPlotSetVerbosity(int verbosity) void CSMatPlotSetDisplayHost(char *displayHost) CSMat CSMatPlot(char *displayHost, CSMat m1, CSMat m2) DESCRIPTION matrixPlotServer is a CSMat plotting facility based

on

the

client-server paradigm. Any "client" program may request for a

matrix

to

be

plotted

CSMatPlot(,

in

mat1,

a

in

any

host

using

mat2) If a "matrixPlotServer"

is running on host it will receive the plot which can

then be displayed, printed, saved in matlab format etc.

If the plot server isn’t running, there are no

other

side-

In any case the client proceeds without interruption,

there

effects.

is

no waiting for confirmation or checking for success. The

protocol is unreliable and it is possible the plot will

not

be received. Written by: Gerardo Pardo-Castellote (gpc), Stanford University, May 1991 SEE ALSO: CSMat

Bibliography [1] A. Hörmann and U. Rembold. Development of an Advanced Robot for Autonomous Assembly. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 2452–57, Sacramento, CA, May 1991. [2] Bruno Achauer. The DOWL Distributed Object-Oriented Language. Communications of the ACM, 39(9):48–55, September 1993. [3] S. Ahuja, N. Carriero, and D. Gelernter. Linda and friends. IEEE Computer, pages 26–34, August 1986. [4] J. S. Albus, H. G. McCain, and R. Lumia. NASA/NBS Standard Reference Model for Telerobot Control System Architecture (NASREM). Technical Report 1235, NIST, April 1989. [5] James S. Albus. System Description and Design Architecture for Multiple Autonomous Undersea Vehicles. Technical Report 1251, NIST, ARPA, September 1988. [6] James S. Albus. Outline for a Theory of Intelligence. IEEE Transactions on Systems, Man and Cybernetics, 21(3):473–509, May/June 1991. [7] J.S. Albus. RCS: a reference model architecture for intelligent control. IEEE Computer, 25(5):56–9, May 1992. [8] Peter K. Allen, Aleksandar Timcenko, Billibon Yoshimi, and Paul Michelman. A least-commitment approach to intelligent robotic assembly. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 1850–1856, Nice, France, May 1992. [9] G.T. Almen, A.P. Black, E.D. Lazowska, and J.D. Noe. The Eden System: A Technical Review. IEEE Transactions on Software Engineering, 11(1):43–67, 1985. [10] A.L. Ananda and B.H. Tay. A Syrvey of Asynchronous Remote Procedure Calls. ACM Operating Systems Review, 26(2):92–109, April 1992. [11] R. Ananthanarayanan, S. Menon, A. Mohindra, and U. Ramachandran. Experiences in Integrating Distributed Shared Memory with Virtual Memory Management. Operating Systems Review (ACM), 26(3):4–26, July 1992.

288

BIBLIOGRAPHY

289

[12] R. L. Andersson. A Robot Ping-Pong Player: Experiment in Real-time Intelligent Control. MIT Press, Cambridge, Mass., 1988. [13] R. C. Arkin. The Impact of Cybernetics on the Design of a Mobile Robot System: A Case Study. IEEE Transactions on Systems, Man and Cybernetics, 20(6):1245–1257, November 1990. [14] Henri E. Bal, Jenniffer G. Steiner, and Andrew S. Tanenbaum. Programming Languages for Distributed Computer Systems. ACM Computing Surveys, 21(3):261–322, September 1989. [15] J. C. Berets, N. Cherniack, and R.M. Sands. Introduction to Cronus. Technical Report 6986, BBN Systems and Technologies, 10 Moulton Street, Cambridge MA 02138, January 1993. [16] C. Bien. Simulation a necessity in safety engineering. Robotics World, 10(4):22–27, Dec. 1992. [17] A.D. Birrell and B.J. Nelson. Implementing remote procedure calls. ACM Transactions on Computer Systems, 2:39–59, February 1984. [18] John W. Blake. PHIGS and PHIGS PLUS. Academic Press, 1993. [19] Maarten Boasson. Control Systems Software. IEEE Transactions on Automatic Control, AC38(7):1094–1106, July 1993. [20] J.E. Bobrow. Optimal robot path planning using the minimum-time criterion. IEEE Journal of Robotics and Automation, 4(4):443–451, August 1988. [21] R. Peter Bonasso and Marc G. Slack. Ideas on a System Design for End-User Robots. In Cooperative Intelligent Robotics in Space III, volume 1829, pages 352–358. SPIE, 1992. [22] S. Bonner and K.G. Shin. A comparative study of robot languages. IEEE Transactions on Computers, (31):82–96, 1982. [23] Frederick Phillip Brooks. The Mythical man-month. Addison-Wesley Pub. Co, Reading, Mass., 1975. [24] Rodney A. Brooks. A robust layered control system for a mobile robot. IEEE Journal of Robotics and Automation, RA-2(1):14–23, March 1986. [25] J. Butler and M. Tomizuka. A suboptimal reference generation technique for robotic manipulators following specified paths. Transactions of the ASME, Journalof Dynamic Systems, Measurement, and Control, 114:524–527, September 1992. [26] Philip L. Butler. An Integrated Architecture for Modular Control Systems. Robotics and Autonomous Systems, 10(2-3):101–114, 1992. [27] Giorgio C. Buttazzo, Benedetto Allotta, and Felice P. Fanizza. Mousebuster: A Robot System for Catching Fast Moving Objects by Vision. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 932–937, Atlanta, GA, May 1993.

BIBLIOGRAPHY

290

[28] David J. Cannon and Larry J. Leifer. Point-and-direct robotics. In International Conference on Intelligent Teleoperation, Greensboro, NC, November 1991. [29] Vincent W. Chen. Experiments in Adaptive Control of Multiple Cooperating Manipulators on a FreeFlying Space Robot. PhD thesis, Stanford University, Stanford, CA 94305, December 1992. Also published as SUDAAR 631. [30] Y. Chen, S. Y.-P Chien, and A. A. Desrochers. General structure of time-optimal control of robotic manipulators moving along prescribed paths. International Journal of Control, 56(4):767–782, 1992. [31] D.R. Cheriton. The V Kernel: A Software Base for Distributed Systems. IEEE Software, pages 19–43, January 1984. [32] R. S. Chin and S. T. Chanson. Distributed Object-Based Programming Systems. ACM Computing Surveys, 23(1):91–123, March 1991. [33] D. Clark. The structuring of systems using upcalls. In Proceedings of the 10th ACM Symposium on Operating Systems Principles, pages 171–180, Orcas Island, Washington, December 1985. ACM SIGOPS. [34] Douglas E. Comer. Internetworkning with TCP/IP, volume 1-3. Prentice Hall, Englewood Cliffs, N.J., 2 edition, 1991-92. [35] Eve Coste-Maniere, B. Espiau, and E. Rutten. A task-Level Robot Programming Language and its Reactive Execution. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 2751–56, Nice, France, May 1992. [36] Eve Coste-Maniere, B. Espiau, and E. Rutten. A task-Level Robot Programming Language and its Reactive Execution. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 2732–37, Nice, France, May 1992. [37] J.J. Craig. Robot calibration facilitates off-line programmin. Robotics World, 10(1):24–25, March 1992. [38] John J. Craig. Introduction to Robotics Mechanics and Control. Addison-Wesley, Reading, MA, 1986. [39] J. L. Crowley and Y. Deazeau. Principles and techniques for sensor data fusion. Signal Processing, 32(1-2):5–27, May 1993. [40] P. Dasgupta, R. Ananthanarayanan ans S. Menon, A. Mohindra, and R. Chen. Distributed Programming with Objects and Threads in the CLOUDS System. Computing Systems, 4(3):243–275, Summer 1991. [41] J. Denavit and R.S. Hartenberg. A kinematic notation for lower-pair mechanisms based on matrices. Journal of Applied Mechanics, June 1955.

BIBLIOGRAPHY

291

[42] William C. Dickson. Experiments in Cooperative Manipulation of Objects by Free-Flying Robot Teams. PhD thesis, Stanford University, Department of Aeronautics and Astronautics, Stanford, CA 94305, December 1993. Also published as SUDAAR 643. [43] H. F. Durrant-Whyte, B.Y.S. Rao, and H. Hu. Toward a Fully Decentralized Architecture for MultiSensor Data Fusion. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 1331–1336, Cincinnati, Ohio, May 13-18 1990. [44] Tim O’Reilly (editor). X Toolkit Intrinsics Reference Manual, volume 5 of The X window system series. O’Reilly & Associates, 1990. [45] B. Eisenberg, B. Hine, and D. Rasmussen. Telerobotic vehicle control: Nasa preps for mars. AI Expert, 8(8):18–21, Aug. 1993. [46] P. Dasgupta et al. The Design and Implementation of the Clouds Distributed Operating System. Journal of the USENIX Association, 3(1):11–46, Winter 1990. [47] R. Camacho Eugenio Oliveira and C. Ramos. A Multi-agent Environment in Robotics. Robotica, 9(4):431–440, Oct-Dec 1991. [48] Chris Fedor. TCX task communications. School of computer science / robotics institute report, Carnegie Mellon University, 1993. [49] S. Fleury, M. Herrb, and R. Chatila. Design of a Modular Architecture for Autonomous Robot. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 3508–13, San Diego, CA, May 1994. [50] Gene F. Franklin, J. David Powell, and Michael Workman. Digital Control of Dynamic Systems. Series in Electrical and Computer Engineering: Control Engineering. Addison-Wesley, Reading, MA, second edition, 1990. [51] Li-Chen Fu and Yung-Jen Hsu. Fully automated two-robot flexible assembly cell. In IEEE International Conference on Robotics and Automation, pages 332–338, Atlanta, Georgia, USA, May 1993. [52] Elmer G. Gilbert and Daniel W. Johnson. Distance functions and their application to robot path planning in the presence of obstacles. IEEE Transactions on Robotics and Automation, RA-1(1):21– 30, September 1985. [53] A. Goscinski. Distributed Operating Systems The Logical Design. Addison-Wesley, 1st edition, 1991. [54] S. Graves, L. Ciscon, and J. D. Wise. A modular software system for distributed telerobotics. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 2783–85, Nice, France, May 1992. [55] Maki K. Habib, Shooji Suzuki, Shin’ichi Yuta, and Jun’ichi Iijima. New language structure for sensorbased actions to describe the real-time behaviour of autonomous robots. International Journal of Electronics, 70(4):653–670, 1991.

BIBLIOGRAPHY

292

[56] J. K. Hackett and M. Shah. Multi-sensor fusion: A perspective. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 359–66, Cincinnati, OH, May 13-18 1990. [57] G. Hager and M. Mintz. Computational Methods for Task-directed Sensor data Fusion and Sensor Planning. The International Journal of Robotics Research, 10(4):285–313, August 1991. [58] Nicolas Halbwachs. Synchronous Programming of Reactive Systems. Kluwer Academic Publishers, 1993. [59] D. Harel, H. Lachover, A. Naamad, A. Pnueli, M. Politi, R. Sherman, A. Shtull-Trauring, and M. Trakhtenbrot. STATEMATE: a working environment for the development of complex reactive systems. IEEE Transactions on Software Engineering, 16(4):403–14, 1990. [60] S. Hayati. Hybrid position/force control of multi-arm cooperating robots. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 82–89, San Francisco, CA, April 1986. [61] B. Hayes-Roth. A blackboard architecture for control. Artificial Intelligence, 26(3):251–321, 1985. [62] J. Henshall and A. Shaw. OSI Explained. End to End Computer Communications Standards. Ellis Horwood, Chinchester, England, 1988. [63] R. Hinkel, T. Knieriemen, and E.V. Puttkamer. MOBOT-III-an autonomous mobile robot for indoor applications. In R.A. Jarvis, editor, Proceedings of the International Symposium and Exposition on Robots. Designated the 19th ISIR by the International Federation of Robotics, Robots: Coming of Age, pages 489–504, Sydney, NSW, Australia, 6-10 Nov 1988. IFS Publications. [64] G. Hirzinger, B. Brunner, J. Dietrich, and J. Heindl. Sensor-Based Spaced Robotics–ROTEX and its Telerobotic Features. IEEE Transactions on Robotics and Automation, 9(5):649–63, October 1993. [65] Neville Hogan. Impedance control: An approach to manipulation. Transactions of the ASME, Journal of Dynamic Systems, Measurement, and Control, 107:1–24, March 1985. [66] Michael G. Hollars. Experiments in End-Point Control of Manipulators with Elastic Drives. PhD thesis, Stanford University, Department of Aeronautics and Astronautics, Stanford, CA 94305, May 1988. Also published as SUDAAR 568. [67] Andreas Hormann. On-Line Planning of Action Sequences for a Two-Arm Manipulator System. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 1109–1114, Nice, France, May 1992. [68] S. Ims. Inertia measurement system. ARL Memo 75, Stanford University, Department of Aeronautics and Astronautics, Stanford, CA 94305, July 1991. [69] IONA Technologies Ltd., 8-34 Percy Place, Dublin 4, Ireland. Orbix - A Technical Overview, August 1993. [70] IONA Technologies Ltd., 8-34 Percy Place, Dublin 4, Ireland. The Orbix Architecture, August 1993.

BIBLIOGRAPHY

293

[71] C. Jacquemot, P Gautron, H.G. Baumgarten, F. Herrmann, J. Mukerji, H. Hartlage, and P.S. Jensen. COOL: The CHORUS CORBA Complient Framework. Technical report, CHORUS Systemes, 1983. [72] S. Dubowsky J.E. Bobrow and J.S. Gibson. Time-optimal control of robotic manipulators along specified paths. International Journal of Robotics Research, 4(3), Fall 1985. [73] J. P. Jones, P. L. Butler, S. E. Johnston, and T. G. Heywood. Hetero Helix: synchronous and asynchronous control systems in heterogeneous distributed networks. Robotics and Autonomous Systems, 10(2-3):85–99, 1992. [74] Judson P. Jones, Alex L. Bangs, and Philip L. Butler. A system for simulating shared memory in heterogeneous distributed-memory networks with specializations for robotics applications. In IEEE International Conference on Robotics and Automation, pages 2738–2744, Nice, France, May 1992. [75] M. Jourdan and F. Marianinchi. A Modular State/Transition Approach for programming Reactive Systems. [email protected], 1994. [76] E. Jul, H. Levy, N. Hutchingson, and A. Black. Fine-grained mobility in the Emerald system. ACM Transactions on Computer Systems, 6:105–133, February 1988. [77] L. Kavraki, J.C. Latombe, and R.H. Wilson. On the Complexity of Assembly Partitioning. Information Processing Letters, 48:229–235, 1993. [78] A. Kemper and M. Wallrath. An analysis of geometric modeling in database systems. ACM Computing Surveys, 19(1):47–91, March 1987. [79] O. Khatib. Augmented Object and Reduced Effective Inertia in Robot Systems. In American Control Conference, volume 3, pages 2140–7, Atlanta, GA, June 15-17 1988. [80] Oussama Khatib. Real time obstacle avoidance for manipulators and mobile robots. International Journal of Robotics Research, 5(1), 1986. [81] Khashayar Khorasani and M. W. Spong. Invariant manifolds and their application to robot manipulators with elastic joints. In Proceedings of the International Conference on Robotics and Automation, pages 978–983, St. Louis, MO, March 1985. IEEE, IEEE Computer Society. [82] Yoshihito Koga. On Multi-Arm Trajectory Planning. PhD thesis, Stanford University, Department of Mechanical Engineering, Stanford, CA 94305, September 1994. Also published as STAN-CS-TR-941530. [83] Petar V. Kokotovic, R. E. O’Malley, Jr., and P. Sannuti. Singular perturbations and order reduction in control theory—an overview. Automatica, 12:123–132, 1976. [84] John Koza. Genetic Programming : on the Programming of Computers by Means of Natural Selection. MIT Press, 1992.

BIBLIOGRAPHY

294

[85] E. Krotkov and R. Hoffman. Terrain Mapping for a Walking Planetary Rover. IEEE Transactions on Robotics and Automation, 10(6):728–39, December 1994. [86] J. C. Latombe, C. Laugier, J. M. Lefebvre, E. Mazer, and J. F. Miribel. The LM robot programming system. In H. Hanafusa and H. Inoue, editors, Second International Symposium on Robotics Research, chapter 7, pages 377–391. MIT Press, Cambridge, MA, 1985. [87] J.C. Latombe. Robot Motion Planning. Kluger Academic Publishers, Boston, MA, 1991. [88] W.G. Levelt, M.F. Kaashoek, H.E. Bal, and A.S. Tanenbaum. Distributed Programming with Shared Data. Computer Languages, 16(2):129–46, 1991. [89] W.G. Levelt, M.F. Kaashoek, H.E. Bal, and A.S. Tanenbaum. A Comparison of Two Paradigms for Distributed Shared Memory. Software–Practice and Experinece, 22(11):985–1010, Novemeber 1992. [90] H.M. Levy and E. Tempero. Modules, Objects and Distributed Programming: Issues in RPC and Remote Object Invokation. Software–Practice and Experinece, 21(1):77–90, January 1991. [91] T.-Y. Li. On-Line Robot Motion Planning in Dynamic Environments. PhD thesis, Stanford University, Department of Mechanical Engineering, Stanford, CA 94305, June 1995. To be Published. [92] Tsai-Yen Li and Jean-Claude Latombe. On-Line Motion Planning for Two Robot Arms in a Dynamic Environment. In Proceedings of the IEEE International Conference on Robotics and Automation, volume 1, Nagoya, Japan, May 1995. [93] B. Liskov and Shira. Promises: Linguistic Support for Efficirnt Asynchronous Procedure Calls in Distributed Systems. In Proceedings of the SIGPLANN’88 Conference on Programming Language Design and Implementation, pages 260–267, Atlanta, GA, June 22-24 1988. [94] THK Co. LTD. THK Ball Screw-Spline type BNS. Catalog No. 107-3E, 1992. [95] T. C. Lueth and U. Rembold. Extensive Manipulation Capabilities and Reliable Behavior at Autonomous Robot Assembly. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 3495–500, San Diego, CA, May 1994. [96] J. Y. S. Luh, W. B. Fisher, and R.P. Paul. Joint torque control by direct feedback for industrial robots. IEEE Transactions on Automatic Control, AC-28(2), Febuary 1983. [97] J.Y.S. Luh and C.S. Lin. Optimum path planning for mechanical manipulators. Transactions of the ASME, 102:142–151, June 1981. [98] R. Lumia, J. Fiala, and A. Wavering. The NASREM Robot Control System and Testbed. International Journal of Robotics and Automation, 5(1):20–26, 1990. [99] R. Lumia, J. L. Michaloski, R. Russel, T. E. Wheatley, P. G. Backes, S. Lee, and R. D. Steele. Unified Telerobotic Architecture Project (UTAP) Interface Document. Technical report, NIST, Intelligent Systems Division, NIST, Gaithersburg, MD, June 18 1994.

BIBLIOGRAPHY

295

[100] R. C. Luo and M. G. Kay. Multisensor Integration and Fusion in Intelligent Systems. IEEE Transactions on Systems, Man, and Cybernetics, 19(5):901–29, September/October 1989. [101] Martti Mantyla. An Introduction to Solid Modeling. Computer Science Press, Rockville, MD, 1988. [102] Richard L. Marks. Experiments in Visual Sensing for Automatic Control of an Underwater Robot. PhD thesis, Stanford University, Stanford, CA 94305, (August) 1995. To be Published. [103] MBARI (Monterrey Bay Aquarium Research Institute. Data manager user’s guide. Internal Documentation, 1991. [104] T. W. McLain. Experiments in the Coordinated Control of an Underwater Arm/Vehicle System. PhD thesis, Stanford University, Department of Mechanical Engineering, Stanford, CA 94305, (December) 1995. To be Published. [105] David W. Meer. Experiments in Cooperative Manipulation of Flexible Objects. PhD thesis, Stanford University, Department of Mechanical Engineering, Stanford, CA 94305, 1994. [106] David W. Meer and Stephen M. Rock. Experiments in object impedance control for flexible objects. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 1222–1227, San Diego, CA, May 1994. [107] Sun Microsystems. RPC: Remote Procedure Call Protocol Specification Version 2. Internet Network Working Group Requets for Comments RFC 1057, Network Information Center, SRI International, June 1988. [108] Sun Microsystems. XDR: External Data Representation Standard. Internet Network Working Group Requets for Comments RFC 1014, Network Information Center, SRI International, June 198t. [109] D.J. Miller and R.C. Lennox. An Object-Oriented Environment for Robot System Architectures. IEEE Control Systems Magazine, 11(2):14–23, February 1991. [110] D. L. Mills. Internet Time Synchronization: the Network Time Protocol. IEEE Transactions on Communications, 10(39):1482–93, 1991. [111] C. Mirolo and E. Pagello. A Solid Modeling System for Robot Action Planning. IEEE Computer Graphics and Applications, 9(1):55–69, Jan. 1989. [112] D. Morse. Experiments in Multi-Rate, Multi-Bandwidth Control of a Flexible-Drive Manipulator With a Mini-Manipulator. PhD thesis, Stanford University, Department of Aeronautics and Astronautics, Stanford, CA 94305, (August) 1995. To be Published. [113] S. Mullender, editor. Distributed Systems. ACM Press, Frontier. Addison Wesley, 1989. [114] Y. Nakamura, K. Nagai, and T. Yoshikawa. Mechanics of coordinative manipulation by multiple robotic mechanisms. In Proceedings of the IEEE International Conference on Robotics and Automation, volume 2, pages 991–998, Raleigh, NC, April 1987. IEEE, IEEE Computer Society.

BIBLIOGRAPHY

296

[115] Yoshihiko Nakamura and Yingti Xu. An Architecture of Inteligent Controller for Multi-Sensor Robotic Systems. In Proceedings of the International Symposium and Exposition on Robots. Robots: Coming of Age., Robots: Coming of Age, pages 550–92, Sydney, NSW, Australia, 6-10 Nov 1988. IFS Publications. [116] F. Nashashibi and M. Devy. 3-D Incremental Modeling and Robot Localization in a Structured Environment using Laser Range Finder. In Proceeding IEEE International Conference on Robotics and Automation, volume 1, pages 20–7, Atlanta, GA, May 2-6 1993. [117] B. Nitzberg and V. Lo. Distributed shared memory: A survey of uses and algorithms. IEEE Computer, 24(8):52–60, June 1991. [118] F. R. Noreils. Toward a robot architecture integrating cooperation between mobile robots. The International Journal of Robotics Research, 12(1):79–98, February 1993. [119] F. R. Noreils and R. G. Chatila. Plan Execution Monitoring and Control Architecture for Mobile Robots. IEEE Transactions on Robotics and Automation, 11(2):255–266, April 1995. [120] F.R. Noreils and R.G. Chatila. Control of Mobile Robot Actions. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 701–7, Scottsdale, AZ, April 1989. [121] The Object Manager Group, Framingham Corporate Center, 492 Old Connecticut Path, Framingham, MA 01701-4568. The Common Object Request Broker: Architecture and Specification, revision 1.2 edition, December 1993. [122] Michel Occello and Marie-Claude Thomas. A Distributed Blackboards Methodology for Designing Robotic Control Softwares. In IEEE International Conference on Systems Engineering, pages 147–150. IEEE, 1992. [123] G. Pardo-Castellote and S. A. Schneider. The Network Data Delivery Service: A Real-Time Data Connectivity System. In Proceedings of the AIAA/NASA Conference on Intelligent Robots in Field, Factory, Service and Space, volume II, pages 591–7, Houston, TX, March 1994. AIAA, AIAA. [124] G. Pardo-Castellote and S. A. Schneider. The Network Data Delivery Service: Real-Time Data Connectivity for Distributed Control Applications. In Proceedings of the International Conference on Robotics and Automation, San Diego, CA, May 1994. IEEE, IEEE Computer Society. [125] G. Pardo-Castellote, S. A. Schneider, and R. H. Cannon Jr. System Design and Interfaces for Intelligent Manufacturing Workcell. In Proceedings of the International Conference on Robotics and Automation, Nagoya, Japan, May 1995. IEEE, IEEE Computer Society. [126] Gerardo Pardo-Castellote and Henrique A. S. Martins. Real-Time Motion Scheduling for a SMALL Workcell. In Proceedings of the International Conference on Robotics and Automation, volume 1, pages 810–817, Sacramento, California, April 1991.

BIBLIOGRAPHY

297

[127] D. W. Payton, J. K. Rosenblatt, and D. M. Keirsey. Plan Guided Reaction. In IEEE Transactions on Systems, Man and Cybernetics [128], pages 1370–82. [128] D.W. Payton and T.E. Bihari. Intelligent real-time control of robotic vehicles. Communications of the ACM, 34(8):48–63, August 1991. [129] Lawrence Pfeffer, Oussama Khatib, and John Hake. Joint torque sensory feedback in the control of a PUMA manipulator. In Proceedings of the American Control Conference, pages 818–824, Seattle, WA, June 1986. [130] Lawrence E. Pfeffer. The RPM toolbox: A system for fitting linear models to frequency response data. In Proceedings of the 1993 MATLAB User’s Conference, Cambridge, MASS, October 1993. [131] Lawrence E. Pfeffer. The Design and Control of a Two-Armed, Cooperating, Flexible-Drivetrain Robot System. PhD thesis, Stanford University, Stanford, CA 94305, December 1993. Also published as SUDAAR 644. [132] Lawrence E. Pfeffer and Robert H. Cannon Jr. Experiments with a Dual-Armed, Cooperative, FlexibleDrivetrain Robot System. In Proceedings of the International Conference on Robotics and Automation, pages 601–608, Atlanta, GA, May 1993. IEEE, IEEE Computer Society. Flex Primarily in Drive NOT Links, Tight Inner Loop. [133] Lawrence E. Pfeffer, Oussama Khatib, and John Hake. Joint torque sensory feedback in the control of a PUMA manipulator. IEEE Transactions on Robotics and Automation, 5(4):418–425, August 1989. [134] Judson P. Jones Philip L. Butler. A Modular Control Architecture for Real-Time Synchronous and Asynchronous Systems. In Proceedings of the SPIE - Applications of Artificial Intelligence: Machine Vision and Robotics, volume 1964, pages 287–298. SPIE, 1993. [135] J. Postel. User datagram protocol. RFC 768, June 1980. [136] J. M. Purtilo. The POLYLITH Software Bus. ACM Transactions on Programming Languages and Systems, 16(1):151–73, January 1994. [137] S. Quinlan and O. Khatib. Elastic Bands: Connecting Path Planning and Control. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 802–7, San Diego, CA, May 1994. [138] Sean Quinlan. Real-Time Modification of Collision-Free Paths. PhD thesis, Stanford University, Department of Computer Science, Stanford, CA 94305, 1994. Also published as STAN-CS-TR-941537. [139] R. Quintero and A.J. Barbera. A Real-Time Control System Methodology for Developing Intelligent Control Systems. Technical Report NISTIR 4936, NIST, October 1992.

BIBLIOGRAPHY

298

[140] R. K. Raj, E. Tempero, and H.M. Levy. Emerald: A General-Purpose Programming Language. Software–Practice and Experinece, 21(1):91–118, January 1991. [141] S.C.V. Raju and A.C. Shaw. A Prototyping Environment for Specifying, Executing and Checking Communicating Real-Time State Machines. Software–Practice and Experinece, 24(2):175–95, February 1994. [142] A. Ramadorai, T.J. Tarn, A. K. Bejczy, and N. Xi. Task-Driven Control of Multi-Arm Systems. IEEE Transactions on Robotics and Automation, 2(3):198–206, September 1994. [143] Bahram Ravani. World Modeling for CAD-based Robot Programming and Simulation. International Journal of Robotics and Automation, 4(2):96–105, 1989. [144] Real-Time Innovations, Inc., 954 Aster, Sunnyvale, CA 94086. ControlShell: Object-Oriented Framework for Real-Time System Software User’s Manual, 4.0a edition, June 1991. [145] Real-Time Innovations, Inc., 954 Aster, Sunnyvale, CA 94086. NDDS: The Network Data-Delivery Service User’s Manual, 1.7 edition, November 1994. [146] Eberhardt Rechtin. Systems Architecting: Creating and Building Complex Systems. Prentice-Hall, first edition, 1991. [147] E. Rimon and D.E. Koditschek. Exact Robot Navigation Using Artificial Potential Functions. IEEE Transactions on Robotics and Automation, 8(5):501–18, Oct. 1992. [148] Paul S. Rosenbloom, John E Laird, Allen Newell, and Robert McCarl. A Preliminary Analysis of the Soar Architecture as a Basis for General Intelligence. Artificial Intelligence, 47(1-3):289–325, January 1991. [149] M. Rozier, V. Abrossimov, F. Armand, I. Boule, M. Gien, M. Guillemont, F. Hermann, C. Kaiser, S. Langlois, P. Leonard, and W. Neuhauser. Overview of the CHORUS Distributed Operating System. Technical report, CHORUS Systemes, February 1991. [150] J. Russakow, O. Khatib, and S. M. Rock. Extended Operational Space Formulation for Serial-toParallel Chain (Branching) Manipulators. In Proceedings of the IEEE International Conference on Robotics and Automation, Nagoya, Japan, May 1995. IEEE, IEEE Computer Society. [151] J. Russakow and S. M. Rock. Generalized Object Control and Assembly for Space Robots. In Proceedings of the ASCE: Robotics for Challenging Environments, Albuquerque NM, February 28 March 3 1994. [152] Jeff Russakow. Experiments in Cooperative Assembly by Multiple Free-Flying Multi-Arm Robots. PhD thesis, Stanford University, Department of Aeronautics and Astronautics, Stanford, CA 94305, (December) 1995. To be Published.

BIBLIOGRAPHY

299

[153] A. Schill, editor. DCE - The OSF Distributed Computing Environment Client/Server Model and Beyond, International DCE Workshop, Karlsruhe, Germany, October 7-8 1993. Springer-Verlag. [154] S. Schneider. Experiments in the Dynamic and Strategic Control of Cooperating Manipulators. PhD thesis, Stanford University, Stanford, CA 94305, September 1989. Also published as SUDAAR 586. [155] S. Schneider and R. H. Cannon. Object Impedance Control for Cooperative Manipulation: Theory and Experimental Results. IEEE Transactions on Robotics and Automation, 8(3), June 1992. Paper number B90145. [156] S. Schneider and L. Pfeffer. Inertia measurement pendulum documentation. ARL Memo 28, Stanford University, Department of Aeronautics and Astronautics, Stanford, CA 94305, Sep. 1987. [157] S. A. Schneider and R. H. Cannon. Experimental Object-Level Strategic Control With Cooperating Manipulators. The International Journal of Robotics Research, 12(4):338–350, August 1993. [158] S. A. Schneider, V. Chen, and G. Pardo-Castellote. Object-Oriented Framework for Real-Time System Development. In Proceedings of the IEEE International Conference on Robotics and Automation, Nagoya, Japan, May 1995. IEEE, IEEE Computer Society. [159] S. A. Schneider, V. W. Chen, and G. Pardo-Castellote. ControlShell: A Real-Time Software Framework. In Proceedings of the AIAA/NASA Conference on Intelligent Robots in Field, Factory, Service and Space, volume II, pages 870–7, Houston, TX, March 1994. AIAA, AIAA. [160] S. A. Schneider, M. A. Ullman, and V. W. Chen. ControlShell: A Real-Time Software Framework. In Proceedings of the 1991 IEEE International Conference on Systems Engineering, Dayton, OH, August 1991. [161] Alan C. Shaw. Communicating Real-Time State Machines. IEEE Transactions on Software Engineering, 18(9):805–816, September 1992. [162] Z. Shiller and S. Dubowsky. Robot path planning with obstacles, actuator, gripper and payload constraints. International Journal of Robotics Research, 8(6), December 1989. [163] Z. Shiller and S. Dubowsky. On computing the global time-optimal motions of robotic manipulators in the presence of obstacles. IEEE Transactions on Robotics and Automation, 7(6):785–797, December 1991. [164] Kang G. Shin and Neil D. McKay. Minimum-time control of robotic manipulators with geometric path constraints. IEEE Transactions on Automatic Control, AC-30(6):531–541, June 1985. [165] Kang G. Shin and Neil D. McKay. A dynamic programming approach to trajectory planning of robotic manipulators. IEEE Transactions on Automatic Control, AC-31(6):491–500, June 1986. [166] Kang G. Shin and Neil D. McKay. Minimum-time trajectory planning for industrial robots with general torque constraints. In IEEE International Conference on Robotics and Automation, pages 412–415, San Francisco, California, April 6-10 1986.

BIBLIOGRAPHY

300

[167] Kang G. Shin and Neil D. McKay. Selection of near-minimum time geometric paths for robotic manipulators. IEEE Transactions on Automatic Control, AC-31(6):501–511, June 1986. [168] Michael D. Sidman, Franco E. DeAngelis, and George C. Verghese. Parametric system identification on logarithmic frequency response data. IEEE Transactions on Automatic Control, 36(9):1065 – 1070, Sept. 1991. [169] R. Simmons, L.-J. Li, and C. Fedor. Autonomous task control for mobile robots. In Proceedings of the IEEE International Simposium on Intelligent Control, pages 663–8, Philadelphia, PA, Sept 1990. [170] R. G. Simmons. Concurrent planning and execution for autonomous robots. IEEE Control Systems Magazine, 12(1):46–50, Feb. 1992. [171] R. G. Simmons. Structured control for autonomous robots. IEEE Transactions on Robotics and Automation, 10(1):34–43, Feb. 1994. [172] D. Simon, B. Espiau, E. Castillo, and K. Kapellos. Computer-aided design of a generic robot controller handling reactivity and real-time control issues. Research Report 1801, INRIA, Le Chesnay, France, December 1992. [173] B. Simons and A. Spector. Fault-tolerant distributed computing, chapter Argus (Distributed Program Language and System), pages 108–14. Springer-Verlag, 1990. [174] S. Singh and M.C. Leu. Optimal trajectory generation for robotic manipulators using dynamic programming. Transactions of the ASME, 109:88–96, June 1987. [175] Dale Skeen. An Information Bus Architecture for Large-Scale, Decision-Support Environments. In Proceedings of the Winter 1992 USENIX Conference, pages 183–95, San Francisco, CA, January 1992. [176] J. E. Slotine and H. S. Yang. Improving the efficiency of time-optimal path-following algortihms. IEEE Transactions on Robotics and Automation, 5(1):118–124, April 1989. [177] Sparta, Inc., 7926 Jones Branch Drive, McLean, VA 22102. ARTSE product literature. [178] Hal Stern. Managing NFS and NIS. O’Reilly & Associates, Inc., 1991. [179] H.D. Stevens, E.S. Miles, S. M. Rock, and R.H. Cannon. Object-Based Task-Level Control: A Hierarchical Control Architecture for Remote Operation of Space Robots. In Proceedings of the AIAA/NASA Conference on Intelligent Robotics in Field, Factory, Service, and Space, pages 264–73, Houston TX, March 1994. [180] SunSoft Inc., 2550 Garcia Avenue, Mountain View, CA 94043. Introduction to the ToolTalk Service, September 1991. [181] SunSoft Inc., 2550 Garcia Avenue, Mountain View, CA 94043. Migrating to DOE, May 1994.

BIBLIOGRAPHY

301

[182] A. S. Tanenbaum. Computer Networks. Prentica Hall, Englewood Cliffs, New Jersey, 2nd edition, 1989. [183] A.S. Tanenbaum and R. Van Renesse. Distributed Operating Systems. ACM Computing Surveys, 17(4):419–470, December 1985. [184] S. C. A. Thomopoulos. Sensor Integration and Data Fusion. Journal of Robotic Systems, 7(3):337–72, June 1990. [185] Aleksandar Timcenko, Steven Abrams, and Peter K. Allen. APHRODITE, Intelligent Planning, Control and Sensing in a Distributed Robotic System. In IAS-3, International Conference on Intelligent Autonomous Systems, pages 561–71, Amsterdam, Netherlands, 1993. IOS Press. [186] M. M. Trivedi, M. A. Abidi, R. O. Eason, and R. C. Gonzalez. Developing Robotic Systems with Multiple Sensors. IEEE Transactions on Systems, Man, and Cybernetics, 20(6):1285–1300, December 1990. [187] M. Uchiyama and T. Yamashita. Assymettric hybrid control of positions and forces of a dual arm robot to share loads. In Experimental Robotics I: The First International Symposium, pages 100–115, Philadelphia, Canada, June 1989. [188] Christopher R. Uhlik. Experiments in High-Performance Nonlinear and Adaptive Control of a TwoLink, Flexible-Drive-Train Manipulator. PhD thesis, Stanford University, Department of Electrical Engineering, Stanford, CA 94305, May 1990. Also published as SUDAAR 592. [189] M. A. Ullman. Experiments in Autonomous Navigation and Control of Multi-Manipulator Free-Flying Space Robots. PhD thesis, Stanford University, Stanford, CA 94305, March 1993. Also published as SUDAAR 630. [190] R. L. Vasquez. Experiments in Two-Cooperating-Arm Manipulation from a Platform with Unknown Motion. PhD thesis, Stanford University, Department of Aeronautics and Astronautics, Stanford, CA 94305, January 1992. Also published as SUDAAR 617. [191] M.I. Vuskovic, A.L. Riedel, and C. Q. Do. The Robot Shell. International Journal of Robotics and Automation, 3(3):165–76, 1988. [192] E. F. Walker, R. Floyd, and P. Neves. Asynchronous Remote Operation Execution in Distributed Systems. In Proceeding of the 10th International Conference on Distributed Computing Systems, pages 253–259, Paris, France, May 28 - June 1 1990. [193] C. Wang and D. Cannon. A Virtual End-Effector Pointing System in Point-and-Direct Robotics for Inspection of Surface Flaws Using a Neural Network Based Skeleton Transform. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 784–9, Atlanta, GA, May, 2-6 1993.

BIBLIOGRAPHY

302

[194] Fei-Yue Wang and Geroge N. Saridis. Task translation and integration specification in intelligent machines. IEEE Transactions on Robotics and Automation, 9(3):257 – 271, June 1993. [195] H. H. Wang, R. L. Marks, S. M. Rock, and M. J. Lee. Task-Based Control Architecture for an Untethered, Unmanned Submersible. In Proceedings of the 8th Annual Symposium of Unmanned Untethered Submersible Technology, pages 137–147. Marine Systems Engineering Laboratory, Northeastern University, September 1993. [196] J. T. Wen and A. Desrochers. Sub-time-optimal control strategies for robotic manipulators. In IEEE International Conference on Robotics and Automation, San Francisco, California, April 6-10 1986. [197] T. Westbrook. CAD software. IEEE Transactions on Robotics and Automation, 60(19):76–7,80– 1,84,87–8, Aug 1990. [198] T. Williams. Fiber network supports distributed real-time systems. Computer Design, 29(17):60–62, September 1990. dd. [199] R. Wilson and J.C. Latombe. Geometric Reasoning About Mechanical Assembly. accepted for publication in Artificial Intelligence Journal, 1994. [200] Wind River Systems, Inc., 1351 Ocean Ave., Emeryville, CA 94608. VxWorks User’s Manual, 19881993. [201] J. D. Wise and Larry Ciscon. TelRIP Distributed Applications Environment Operating Manual. Rice University, Houston Texas, 1992. Technical Report 9103. [202] J.D. Wise, L.A. Ciscon, and S. Graves. A distributed telerobotics system for space operations. In Proceedings of the SPIE - The International Society for Optical Engineering, volume 1829 of Cooperative Intelligent Robotics in Space III, pages 359–7, Boston, MA, 1992. November 16-18. [203] L. Zahn, T.H. Dineen, P.J. Leach, E.A. Martin, N.W. Mishkin, J.N. Pato, and G.L. Wyant. Network Computing Architecture. Prentice-Hall, 1990. [204] F. Zanichelli, S. Caselli, A. Natali, and A. Omicini. A Multi-Agent Framework and Programming Environment for Autonomous Robotics. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 3501–7, San Diego, CA, May 1994. [205] Y. F. Zheng and J. Y. S. Luh. Optimal load distribution for two robots handling a single object. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 344–349, Philadelphia, PA, April 1988. [206] K. R. Zimmerman and R. H. Cannon Jr. GPS-Based Control for Space Vehicle Rendezvous. In Proceedings of the ASCE: Robotics for Challenging Environments, Albuquerque NM, February 28 March 3 1994. [207] Kurt Zimmermann. Experiments in the use of Differential GPS for Space Vehicle Rendez-Vous. PhD thesis, Stanford University, Stanford, CA 94305, (June) 1996. To be Published.