f - Stanford Artificial Intelligence Laboratory - Stanford University

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

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

By Kyong-Sok Chang March 2000

c Copyright 2000 by Kyong-Sok Chang All Rights Reserved

ii

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

Oussama Khatib Department of Computer Science (Principal Adviser)


Jean-Claude Latombe Department of Computer Science


Bernard Roth Department of Mechanical Engineering

Approved for the University Committee on Graduate Studies:

iii

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

iv

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

v

Contents Abstract

iv

Acknowledgements

v

1 Introduction

1

1.1

Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1

1.2

Joint Space vs. Operational Space . . . . . . . . . . . . . . . . . . . . . .

2

1.3

Branching Robot Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . .

4

1.4

Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

6

1.5

Overview of the Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . . .

8

2 Operational Space Formulation

10

2.1

Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.2

Task Behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.3

Posture Behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

2.4

Dynamic Consistency . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

2.5

Applications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3 Spatial Notation and Quantities

18

3.1

Efficient Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

3.2

Basic Spatial Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

3.3

Spatial Differentiation in Rotating Coordinates . . . . . . . . . . . . . . . 21

3.4

Spatial Velocity and Acceleration . . . . . . . . . . . . . . . . . . . . . . . 22

3.5

Spatial Articulated Quantities . . . . . . . . . . . . . . . . . . . . . . . . . 24 vi

4 Joint Space Dynamics

27

4.1

Control : Recursive Newton-Euler Method . . . . . . . . . . . . . . . . . . 27

4.2

Simulation : Articulated-body Method . . . . . . . . . . . . . . . . . . . . 30

5 Operational Space Inertia Matrix 5.1

5.2

35

Background : Serial-chain Mechanisms . . . . . . . . . . . . . . . . . . . 36 5.1.1

Inertia Propagation Method . . . . . . . . . . . . . . . . . . . . . 36

5.1.2

Force Propagation Method . . . . . . . . . . . . . . . . . . . . . . 38

Recursive Algorithm for Branching Mechanisms . . . . . . . . . . . . . . 40 5.2.1

Analysis of Operational Space Inertia Matrix . . . . . . . . . . . . 41

5.2.2

Derivation of Recursive Algorithm . . . . . . . . . . . . . . . . . . 42

5.2.3

Computational Complexity . . . . . . . . . . . . . . . . . . . . . . 46

6 Operational Space Dynamics

48

6.1

Optimized Dynamic Control Structure . . . . . . . . . . . . . . . . . . . . 48

6.2

Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 6.2.1

Jacobian Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

6.2.2

Bias Acceleration Vector . . . . . . . . . . . . . . . . . . . . . . . 52

6.2.3

Operational Space Inertia Matrix . . . . . . . . . . . . . . . . . . . 53

6.2.4

Null Space Computed Command Vector . . . . . . . . . . . . . . . 54

6.2.5

Computational Complexity . . . . . . . . . . . . . . . . . . . . . . 54

6.3

Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

6.4

Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

7 Closed-Chain Dynamics

63

7.1

Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

7.2

Augmented Object Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

7.3

7.2.1

Single Branching Robot . . . . . . . . . . . . . . . . . . . . . . . 66

7.2.2

Multiple Branching Robots . . . . . . . . . . . . . . . . . . . . . . 69

Algorithm for Dynamic Control . . . . . . . . . . . . . . . . . . . . . . . 70 7.3.1

Operational Space Control Structure . . . . . . . . . . . . . . . . . 70

7.3.2

Optimized Dynamic Control Structure . . . . . . . . . . . . . . . . 71 vii

7.4

Algorithm for Dynamic Simulation . . . . . . . . . . . . . . . . . . . . . . 72

7.5

Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76

8 Contact Dynamics 8.1

77

Basic Concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77 8.1.1

Operational Space Inertia Matrices of Links . . . . . . . . . . . . . 78

8.1.2

Effective Mass . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78

8.1.3

Spatial Impulse . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79

8.2

Efficient Recursive Algorithm . . . . . . . . . . . . . . . . . . . . . . . . 81

8.3

Impulse-based Dynamic Simulation . . . . . . . . . . . . . . . . . . . . . 84

9 System Integration

89

9.1

Hardware/Software Architecture . . . . . . . . . . . . . . . . . . . . . . . 89

9.2

Robot User Interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90

9.3

Real-time Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . 93

9.4

Concluding Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94

10 Future Work and Conclusion

95

10.1 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95 10.1.1 Robot Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . 95 10.1.2 Behavior Control . . . . . . . . . . . . . . . . . . . . . . . . . . . 96 10.2 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98 A Singularity Control

99

A.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99 A.2 Kinematic Singularities . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100 A.3 Control Strategy at Singularities . . . . . . . . . . . . . . . . . . . . . . . 101 A.4 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103 A.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104 Bibliography

105

viii

List of Tables 4.1

Recursive Newton-Euler method . . . . . . . . . . . . . . . . . . . . . . . 30

4.2

Articulated-body method . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

5.1

Inertia Propagation method . . . . . . . . . . . . . . . . . . . . . . . . . . 38

5.2

Force Propagation method . . . . . . . . . . . . . . . . . . . . . . . . . . 40

5.3

Algorithm for operational space inertia matrix . . . . . . . . . . . . . . . . 46

6.1

Algorithm for operational space inverse dynamics . . . . . . . . . . . . . . 55

7.1

Algorithm for dynamic control of simple closed-chain . . . . . . . . . . . . 72

7.2

Algorithm for dynamic simulation of simple closed-chain . . . . . . . . . . 75

8.1

Algorithm for operational space inertia matrices of all links . . . . . . . . . 85

8.2

Impulse Propagation method . . . . . . . . . . . . . . . . . . . . . . . . . 88

ix

List of Figures 1.1

Branching mechanisms . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2

1.2

Joint space vs. operational space . . . . . . . . . . . . . . . . . . . . . . .

3

1.3

A branching robot with a tree-like topology . . . . . . . . . . . . . . . . .

5

1.4

C-3PO . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

5

1.5

A simplified humanoid robot with its tree-like topology . . . . . . . . . . .

9

2.1

Task/posture behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.2

End-effector parameters for a branching mechanism . . . . . . . . . . . . . 12

2.3

Posture behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

2.4

Null space motion with

2.5 Null space motion with

. . . . . . . . . . . . . . . . . . . . . . . . . . 16 . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3.1

Basic notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

3.2

Links, joints, and frames . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.3

Outward recursion: spatial velocity and acceleration . . . . . . . . . . . . . 23

3.4

Force and acceleration propagators . . . . . . . . . . . . . . . . . . . . . . 25

4.1

Forces on link . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

4.2

Recursive Newton-Euler method . . . . . . . . . . . . . . . . . . . . . . . 29

4.3

Articulated-body method: apparent one-joint robot . . . . . . . . . . . . . 31

4.4

An articulated-body with link as its handle . . . . . . . . . . . . . . . . . 31

4.5

Articulated-body inertia: base step . . . . . . . . . . . . . . . . . . . . . . 32

4.6

Articulated-body inertia: recursion step . . . . . . . . . . . . . . . . . . . 32

4.7

Articulated-body method . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

x

5.1

Inertia propagation method . . . . . . . . . . . . . . . . . . . . . . . . . . 37

5.2

Force propagation method . . . . . . . . . . . . . . . . . . . . . . . . . . 39

5.3

Block diagonal matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

5.4

Force/acceleration propagation via nearest common ancestor . . . . . . . . 44

5.5

Block off-diagonal matrices . . . . . . . . . . . . . . . . . . . . . . . . . . 45

5.6

Recursion processes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

6.1

Jacobian matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

6.2

Bias acceleration vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

6.3

Motion sequence – task/posture behavior control . . . . . . . . . . . . . . 56

6.4

Plots – task/posture behavior control . . . . . . . . . . . . . . . . . . . . . 57

6.5

SAMM: Stanford Assistant Mobile Manipulator . . . . . . . . . . . . . . . 58

6.6

Task behavior: moving end-effector only . . . . . . . . . . . . . . . . . . . 58

6.7

End-effector behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

6.8

Task behavior: moving end-effector and base . . . . . . . . . . . . . . . . 60

6.9

Posture behavior: moving end-effector and base . . . . . . . . . . . . . . . 60

6.10 End-effector behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61 6.11 Base behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61 7.1

Cooperation by multiple branching robots . . . . . . . . . . . . . . . . . . 64

7.2

Dynamic coupling effect for fixed-base serial-chain mechanisms . . . . . . 65

7.3

Dynamic coupling effect for a branching mechanism . . . . . . . . . . . . 65

7.4

Augmented object model: concept . . . . . . . . . . . . . . . . . . . . . . 65

7.5

Dynamics of an augmented object . . . . . . . . . . . . . . . . . . . . . . 66

7.6

A branching robot holding an object . . . . . . . . . . . . . . . . . . . . . 66

7.7

Simple closed-chain mechanism . . . . . . . . . . . . . . . . . . . . . . . 67

7.8

Augmented object model: kinetic energy . . . . . . . . . . . . . . . . . . . 68

7.9

Motion sequence – putting a box on the floor . . . . . . . . . . . . . . . . 76

7.10 Plot – putting a box on the floor . . . . . . . . . . . . . . . . . . . . . . . 76 8.1

Operational space inertia matrix of a point . . . . . . . . . . . . . . . . . . 78

8.2

Basic concept . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79

xi

8.3

Base step . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

8.4

Base step: proof . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82

8.5

Recursion step: first part . . . . . . . . . . . . . . . . . . . . . . . . . . . 82

8.6

Recursion step: second part . . . . . . . . . . . . . . . . . . . . . . . . . . 83

8.7

Impulse Propagation method: recursion processes . . . . . . . . . . . . . . 87

9.1

Overall hardware/software architecture . . . . . . . . . . . . . . . . . . . . 90

9.2

Control/simulation servo loop flow chart . . . . . . . . . . . . . . . . . . . 91

9.3

PUMA 560 robotic system and internet-based user interface . . . . . . . . 92

9.4

Graphical user interface tools: disks, planes, and circular belts . . . . . . . 92

A.1 Three basic singular configurations in PUMA 560 . . . . . . . . . . . . . . 102 A.2 Elbow lock and wrist lock: experimental response . . . . . . . . . . . . . . 103 A.3 Wrist lock: experimental response . . . . . . . . . . . . . . . . . . . . . . 104

xii

Chapter 1 Introduction The dynamics of a robotic mechanism play a central role in both its control and simulation. Thus, as the degrees of freedom of a robotic mechanism increase and the tasks performed by it become more complex, the need for a better understanding of its dynamic behavior becomes more significant. The work presented in this thesis is part of an ongoing effort to describe the dynamic behavior of a robotic mechanism in an accurate and simple form while taking into account the computational efficiency.

1.1 Motivation Branching (tree-like) mechanisms represent a large class of practical systems in robotics such as dextrous manipulators with multiple fingers, mobile bases with multiple manipulators, and humanoid robots shown in Figure 1.1. Applications of such systems require fast algorithms to provide intuitive, systematic, and efficient ways to model, control, and simulate the dynamics of these complex systems. In order to efficiently solve complex robot dynamics problems, it is essential to choose a dynamic modeling that naturally renders computationally efficient algorithms for various dynamics problems. The problem to be solved in the control of a robotic mechanism is known as the inverse dynamics problem and the solution to this problem requires the calculation of the correct forces that will produce the desired motion (acceleration) of the robotic mechanism. In simulation, the problem to be solved is called the forward (direct) 1

CHAPTER 1. INTRODUCTION

2

Figure 1.1: Branching mechanisms dynamics problem and the solution to this problem requires determination of the motion (acceleration) of the robotic mechanism in response to the applied forces. Since the equations of motion describe the correct behavior of robotic mechanisms, in principle, solving the branching robot dynamics problems presents no difficulty – a branching robotic mechanism is just a system of rigid bodies with constraints, and the equations of motion of such systems have been known for a long time. The real problem is the practical one of formulating the robot dynamics such that it provides a systematic and uniform way to solve various branching robot dynamics problems such as task/posture behavior, closedchain, and contact dynamics, as well as lead to computationally efficient algorithms. These practical issues provide the main motivation for this thesis.

1.2 Joint Space vs. Operational Space Two main formulations represent the dynamics of robotic mechanisms: the joint space dynamics formulation [67] and the operational space dynamics formulation [30]. Although


3

these two different formulations ultimately describe the dynamics of the same robotic mechanism, each emphasizes different aspects of robot dynamics. The joint space dynamics formulation describes the dynamics of joints and the most common schemes for robot dynamic control and simulation as shown on the left in Figure 1.2. The operational space dynamics formulation is a newer approach describing the dynamics of the tip (endeffector) of a manipulator with its task specification defined directly in its workspace as shown on the right in Figure 1.2.

f τ1

τ2 Figure 1.2: Joint space vs. operational space

Note that dealing with dynamics is essential for achieving higher performance. In free motion, dynamic effects increase with the range of motion, speed, and acceleration at which a robot is operating. In contact operations, they also increase with the rigidity of the colliding objects. Furthermore, when controlling end-effector motion with simultaneous control of contact forces in the orthogonal subspace, inertial coupling can significantly affect performance. These effects must be taken into account to achieve higher performance. The difficulty with joint space control techniques lies in the discrepancy between the space where robot tasks are specified and the space in which the control is taking place. By its very nature, joint space control calls for transformations whereby joint space descriptions are obtained from the robot task specifications. The joint space task transformation problem is exacerbated for mechanisms with redundancy. The typical approach in the case of redundancy involves the use of pseudo or generalized inverses to solve an underconstrained of linear equations, while optimizing some given criterion [17, 20]. The limitations of joint space control techniques, especially in constrained motion tasks, have motivated alternative approaches for dealing with task level dynamics. The operational space formulation, which falls within this line of research, has been driven by the


4

need to develop mathematical models for the description, analysis, and control of robot dynamics with respect to task behavior. In this framework, the control of redundant manipulators relies on two basic models: a task level dynamic model obtained by projecting the robot dynamics onto the operational space [30], and the dynamically consistent force/torque relationship that provides decoupled control of joint motions in the null space associated with the redundant mechanism [32]. This decomposition provides the foundation for the algorithms presented in this thesis.

1.3 Branching Robot Dynamics There have been significant efforts to efficiently compute the operational space inverse

dynamics of a serial-chain robotic mechanism with a single operational point. KreutzDelgado, Jain, and Rodriguez [36] have developed an

recursive method to compute

the operational space inverse dynamics based on the Spatial Operator Algebra [52] using the articulated-body inertias [13]. Lilly and Orin [42] developed an efficient

recursive

method for the computation of the operational space inertia matrix without requiring the extra computation of articulated-body inertias. However, these methods do not provide dynamics in the null space associated with a redundant mechanism. Recently, Russakow and Khatib [59] developed the (extended) operational space formulation that accommodates the operational space dynamics of branching robotic mechanisms with multiple operational points. This formulation is an approach for the dynamic modeling and control of a complex branching (tree-like) redundant mechanism (Figure 1.3) at its task or end-effector level. It is particularly useful for dealing with simultaneous tasks involving multiple end-effectors since its basic structure provides dynamic decoupling among the end-effectors’ tasks and the complex internal dynamics in their associated null space. As an example, a humanoid robot can be controlled by directly specifying tasks involving its feet, hands, and head while using the associated null space to achieve some desired posture behavior such as balancing. In addition, this formulation naturally renders closed-chain dynamics as object level dynamics and contact dynamics as contact-point level dynamics without altering the underlying branching robot kinematics and dynamics. However, in order for this formulation to be usable in interactive or real-time control


5

base

root

h h i

ei

j

ej

i

j

ei

ej

Figure 1.3: A branching robot with a tree-like topology. In its corresponding tree structure, each link becomes a node and each joint becomes an edge of the tree.

of complex -link mechanisms, the complexity

of the exist-

practice. As an example, C-3PO in Figure 1.4 has 20 to 200 links ( ) while less than 5 complexity comes from operational points ( ) will be sufficient for control. This required the explicit inversion operation of the joint space inertia matrix of size ing symbolic method [59] is not acceptable since

is a small constant compared to

in

for the computation of the operational space inertia matrix and the dynamically consistent generalized inverse of the Jacobian matrix [59]. In addition, deriving explicit symbolic equations of motion for each different mechanism requires excessively large set-up costs if it is feasible at all.

Figure 1.4: C-3PO :

= 20–200,

= 1-5

Therefore, in order to achieve interactive or real-time dynamic control/simulation performance, it is necessary to develop more efficient recursive algorithms for branching robot


6

dynamics. This argument similarly applies to closed-chain dynamics and contact dynamics involving branching mechanisms since existing algorithms require altering of the underlying kinematic structure and rely on the explicit operations involving the joint space inertia matrices and its inverse.

1.4 Contributions This thesis proposes efficient and general recursive algorithms, using the operational space

formulation, to model and solve, at the task level, the dynamics problems of highly redundant articulated -link branching mechanisms with

operational points. These al-

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


7

symbolic method with dynamically accounted operational space and null space command vectors.

Operational Space Inertia Matrix – Since the optimized control structure still requires explicit computation of the operational space inertia matrix, an efficient

recursive method is derived to produce the operational space inertia matrix without any explicit computation of the joint space inertia matrix and its inverse. Operational Space Inverse Dynamics – Using the optimized control structure, the efficient algorithm for the computation of the operational space inertia matrix, and the modified spatial notation, an efficient recursive algorithm for computing the operational space inverse dynamics of branching robots is developed. This algorithm

, achieving the overall complexity of

.

completely eliminates any matrix multiplication operations among matrices of size and

Closed-chain Dynamics – The augmented object model for branching robots is developed. This model formulates closed-chain dynamics as an object level dynamics with grasping constraint forces using the operational space formulation without

modifying underlying kinematic and dynamic structures such as the Jacobian matrix. Therefore, this model renders an

algorithm to solve closed-chain

dynamics problems involving branching mechanisms. Contact Dynamics – An efficient

recursive algorithm to compute the opera-

tional space inertia matrix of any point in the branching robot is developed. This

means that the effective mass of any point of a branching robot in any direction can be computed in linear time, achieving an

algorithm to compute the impulse at

the contact point. This is a significant speed-up for real-time impulse-based dynamic simulation. Task/Posture Behavior Control – A basic underlying framework to accommodate high-level behavior control in both task and null space is presented. Applications of this behavior control include balancing, walking, carrying, mobile manipulator coordination, and singularity control (see Appendix A). This framework provides an intuitive and systematic way to model and control dynamically consistent task/posture


8

behavior of complex branching mechanisms.

running time of Note that since

can be considered as a small constant in practice, we obtain the linear

, as the number of links increases.

for these algorithms. Therefore, these algorithms perform signifi-

cantly better than the existing symbolic method,

1.5 Overview of the Thesis The greater part of this thesis deals with the formulation and derivation of efficient algo-

rithms, using the operational space framework, to model and solve the dynamics problems

of highly redundant -link branching (tree-like) robotic mechanisms with multiple ( ) operational points. The desire for real-time performance is emphasized throughout this work. The following is a brief outline of the rest of the chapters in the thesis. Chapter 2 provides background material describing the operational space formulation for branching robotic mechanisms. The importance of the decoupled control structure of this formulation and its applications for branching mechanisms are discussed. In Chapter 3, a modified spatial notation is developed in order to model and analyze complex articulated mechanisms in a more concise and uniform manner.

Chapter 4 provides important results from previous work on joint space inverse and forward dynamics of branching robots in the form of two existing

recursive algorithms

using the modified spatial notation. The concepts and intermediate results from these algo-

rithms are essential for the new algorithms developed in later chapters. Chapter 5 presents an efficient

recursive algorithm for computing the op-

erational space inertia matrix for branching mechanisms without any explicit computation of the joint space inertia matrix and its inverse. In Chapter 6, a computationally optimized operational control structure is developed to eliminate the explicit computation of the joint space inertia matrix or its inverse. Using this optimized control structure, the algorithm for the computation of the operational space inertia matrix, and the modified spatial notation, an efficient and general recursive algo-

rithm for the computation of the operational space inverse dynamics for branching robotic mechanisms is developed and its

complexity is proven.


9

Chapter 7 extends the previous augmented object model for non-branching mechanisms

to accommodate branching mechanisms without altering the kinematic and dynamic structures of the underlying branching mechanisms, resulting in an efficient

algo-

rithm to control and simulate simple closed-chain mechanisms. In Chapter 8, An efficient

recursive algorithm to compute the operational space

inertia matrix of any point in the branching robot is developed. Using this algorithm, the

effective mass of any point of a branching robot in any direction can be found in linear time, achieving an

algorithm to compute the impulse at the contact point.

In Chapter 9, overall hardware/software architecture is presented to illustrate the current configuration of a real-time dynamic control/simulation environment. Issues involving user interface, control/simulation servo loop, and optimization techniques used to implement the proposed algorithms in a real-time environment are further discussed. Chapter 10 concludes this thesis with a discussion of future research and a summary of contributions.

Note that throughout this thesis, real-time simulation results with a basic humanoid redundant robotic mechanism with

and

, as shown in Figure 1.5, are presented

to illustrate the efficiency of the proposed algorithms. root calf thigh hip chest upperArmL

neck

upperArmR

lowerArmL

head

lowerArmR

handL

eyeL

eyeR

handR

Figure 1.5: A simplified humanoid robot with its tree-like topology

Chapter 2 Operational Space Formulation The operational space formulation [30, 59] provides dynamic modeling and control of a complex branching redundant mechanism directly at its task or end-effector level. This formulation is particularly useful for dealing with simultaneous tasks involving multiple end-effectors since its basic structure, the dynamically consistent force/torque decomposition, provides dynamic decoupling among the end-effectors’ tasks (task behavior) and the complex internal dynamics in their associated null space (posture behavior). The following sections introduces the operational space formulation for branching robotic mechanisms in terms of task/posture behavior and explains the dynamically consistent force/torque decomposition in detail. In addition, some of the applications of this formulation are discussed.

2.1 Background The operational space formulation projects the joint space dynamics of mechanisms into

the operational space and its associated null space. The general joint space dynamic equations of motion for an -link

-degree-of-freedom open-chain robotic mechanism with

operational points may be written in the following form [67]:

10

(2.1)

CHAPTER 2. OPERATIONAL SPACE FORMULATION

where

is the

force vector, and is the

Note that

total control torque vector,

joint space inertia matrix, is the

is the

joint acceleration vector,

joint space Coriolis and centrifugal

joint space gravitational force vector.

cannot be greater than

ational point, and that

is the

11

since each link can have maximum of one oper-

is .

since each link has a multiple-degree-of-freedom joint

which can have any number of degrees of freedom up to six. Then, the size of

Also, the relative motion of two free bodies in space can be considered as the action of a six-degree-of-freedom joint between the bodies.

The generalized torque/force relationship [30, 32, 59] provides the decomposition of the total control torque,

in Equation (2.1) into two dynamically decoupled control torque

vectors: the torque corresponding to the task behavior command vector and the torque that only affects posture behavior in the null space:

(2.2)

Notice that this decomposition provides a natural framework for modeling and control of the task/posture behavior of complex branching mechanisms. In Figure 2.1, the task behavior is to keep the position and orientation of both hands constant and the posture behavior is to rock the torso back and forth.

!"#"$&% ' $ (% )*'+,&$ % (-/.0) 1

23% ,&4"'56*!"/#-$ % ' $&(7(-4". .8)*1/

Figure 2.1: Task/posture behavior


12

2.2 Task Behavior

in Equation (2.2) is the torque corresponding to the computed task behavior command

vector, , acting in the operational space:

3 where each of

is a

(2.3)

vertically concatenated vector of the

end-effectors and

Jacobian matrix of each of

is the

spatial force vector of

vertically concatenated matrix of the

end-effectors:

.. .

and

.. .

(2.4)

Figure 2.2 shows these spatial forces and Jacobian matrices associated with end-effectors.

( a e , fe , J e ) 1

1

1

{e1} {ei}

( a e , fe , J e )

qk

i

i

i

( a e , fe , J e )

k

m

m

m

{em} Figure 2.2: End-effector parameters for a branching mechanism Since dynamic control of the task behavior in the operational space is desired,

can

be computed using the operational space (task behavior) command vector for a linearized unit-mass system, , and the dynamics obtained by projecting the joint space dynamics into its operational space:

(2.5)


13

, is an

where the operational space inertia matrix,

-

-

(2.6)

and are the operational space gravitational force and Coriolis and centrifugal

force vectors, respectively and defined as:

The

and

bias acceleration vector,

Jacobian matrix and the

, is the product of the absolute derivative of the

joint velocity vector, :

and

symmetric positive definite

matrix [30, 59]:

and

(2.7)

is the dynamically consistent generalized inverse of the Jacobian matrix (2.4) and

has been proven to be unique [30, 59]:

Note that

is a

each of

end-effectors shown in Figure 2.2:

-

vertically concatenated vector of the spatial acceleration vector of

(2.8)

.. .

(2.9)

using Equation (2.6) is since the two matrix inversion operations of and - require

and . respectively, while the two matrix multiplication operations require

Notice that the complexity of computing


14

2.3 Posture Behavior

:

in Equation (2.2) is the torque that only affects posture behavior in the null space

given any arbitrary null space (posture behavior) command vector,

where

(2.10)

is the dynamically consistent null space projection matrix that maps

to the

appropriate control torque:

where is an

(2.11)

identity matrix.

Dynamic consistency is the essential property for task behavior to maintain its respon-

. In Figure 2.3,

siveness and to be dynamically decoupled from posture behavior since it guarantees not to produce any coupling acceleration in the operational space given any

the robot was commanded to keep the position and orientation of both hands constant (task behavior) while rocking its torso back and forth in the null space (posture behavior). Notice that dynamic consistency enables task behavior and posture behavior to be specified independently of each other, providing an intuitive control of complex systems. Next section explores this important concept in further detail.

Figure 2.3: Posture behavior


15

2.4 Dynamic Consistency In order to achieve dynamically consistent behavior of a redundant mechanism, it is necessary to decouple the motion in the null space from the motion in the operational space. In other words, the selection of null space control torques from the null space should not generate any acceleration at the end-effectors in the operational space. This can be achieved using the dynamically consistent relationship (2.2) between operational space forces and null space joint torques for redundant mechanisms [32]. This relationship provides a decomposition of joint torques into two dynamically de-

coupled control vectors: joint torques corresponding to forces acting at the end-effectors,

&

in Equation (2.3); and joint torques that only affect null space joint motions, in Equation (2.10). is the operational space control forces acting

on the end-effectors;

is the projection onto the null space; and

is the joint control

torques for desired motions in the null space. Using this decomposition, the end-effectors can be controlled by operational forces, while motions in the null space (posture behavior) can be independently controlled by joint torques that are guaranteed not to alter the end-effectors’ dynamic behavior (task behavior). Simulation Example The impact of the dynamically consistent control decomposition is illustrated using the 3Rplanar manipulator shown in Figure 2.4 and Figure 2.5. This manipulator is treated as a redundant mechanism with respect to the task of positioning the end-effector. The goal here is to maintain the end-effector position, while letting the manipulator move in the null space. The end-effector position is controlled by operational forces. An oscillatory motion in the null space is produced by the application, in the null space, of

the negative gradient of an attractive potential, .

, without any dissipative forces so that

Two different generalized inverses are used to construct the projection of onto the - , and the dynamically null space: the Moore-Penrose or pseudo inverse, consistent generalized inverse, Figure 2.4 and Figure 2.5.

from Equation (2.8) The simulation results are shown in


16

Null space with pseudo inverse 1

0.8

Position (m)

0.6 x : solid line 0.4

y : dashed line

0.2

0

-0.2 0

0.2

0.4

0.6

0.8

1 1.2 Time (sec)

Figure 2.4: Null space motion with

1.4

1.6

1.8

2

1.6

1.8

2

Null space with dynamically consistent inverse 1

0.8

Position (m)

0.6 x : solid line 0.4

y : dashed line

0.2

0

-0.2 0

0.2

0.4

0.6

0.8

1 1.2 Time (sec)

Figure 2.5: Null space motion with

1.4

As expected, with the dynamically consistent inverse the motion in the null space does not affect the end-effector position in Figure 2.5, while large coupling forces are produced at the end-effector when the pseudo inverse is used in Figure 2.4. Doty [12] has discussed extensively the difficulty with pseudo inverses and observed the need for dynamically weighted generalized inverses such as

in Equation (2.8).

2.5 Applications The dynamically consistent force/torque decomposition (2.2) provided by the operational space formulation is extremely useful for dealing with simultaneous tasks involving multiple end-effectors of complex branching redundant mechanisms. As an example, a humanoid robot can be controlled by directly specifying tasks evolving its feet, hands, and


17

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

Chapter 3 Spatial Notation and Quantities This chapter presents a modified spatial notation, its derived quantities, and notational conventions used throughout this thesis. Spatial notation has been widely used in the modeling of kinematics and dynamics of complex robotic mechanisms [13, 52, 36, 40, 42, 47]. In spatial notation, each quantity incorporates the appropriate linear and angular components and results in a concise form (

vector or

matrix). Since the size of each quantity

is constant in this notation, the actual implementation can be highly optimized.

3.1 Efficient Modeling The modified spatial notation and quantities developed in this chapter combine various versions of existing spatial notations and conventional vector notations [16, 10, 34] in order to utilize the results from various researchers in a unified way. The major difference between the notation developed in this chapter and the existing ones is that linear components always occupy upper or upper-left corners and angular components always occupy lower or lower-right corners of

vectors or

matrices. Also, note that one of the benefits

of this notation is that all inertia matrices are symmetric. Since any algorithms computing dynamic quantities heavily involve the operations manipulating inertia matrices, this symmetry of inertia matrices enables the actual implementation of the algorithms to be significantly optimized, resulting a higher overall efficiency.

18

CHAPTER 3. SPATIAL NOTATION AND QUANTITIES

19

3.2 Basic Spatial Notation A spatial velocity, , a spatial acceleration, , and a spatial force, , of link are defined as:

where , ,

,

, , , and , are

,

linear velocity, angular velocity, linear acceleration,

angular acceleration, force, and moment vectors expressed in frame , respectively. Also, the spatial inertia matrix of link in frame

, , is a

symmetric positive definite

matrix and defined as:

(3.1)

identity matrix, is the mass of link , and is the inertia tensor matrix of link in frame . The origin of frame is located at the center of mass of link shown in Figure 3.1. where

is a

ai

Si

vi

{i}

fi

% $ (-,

mi {ci}

.&$ (" i

I ci i

Figure 3.1: Basic notation

, is a matrix with full column rank, , when joint has degrees of freedom ( "! ) [13, 40]. Its columns (unit vectors) provides the basis for the motion space of joint . Notice that this matrix is constant since it is expressed in its own frame. For example, if joint is a prismatic joint along # -axis and joint $ is a spherical The general joint model,


20

joint, their corresponding general joint models are:

and

(3.2)

Note that, in Figure 3.1, the origin of each link frame is located at the joint and any variable without the reference frame number (front superscript) is expressed in its own frame. Also, if link is a leaf (outermost) link, end-effector frame

is located at the tip

(operational point) of link shown in Figure 1.3 and Figure 3.2. These conventions will be assumed throughout this thesis.

Si Sh

h

{i}

ri

{h}

% $ (-,

.&$ ("

{ci}

%*$ (",

.&$ ("

{ei}

i

i

h

h

Figure 3.2: Links, joints, and frames The

spatial rotation matrix, , and the spatial transformation matrix, , rotates

and transforms a spatial quantity from frame to frame , respectively:

where is the

and

(3.3)

rotation matrix which transforms a quantity expressed in frame to the same quantity expressed in frame and is the cross-product operator associated with


, the

21

to the origin of frame expressed vector, in frame shown in Figure 3.2. The cross-product operator associated with a

position vector from the origin of frame

, is an skew-symmetric matrix defined as:

(3.4)

For example, the spatial transformations of spatial velocities and forces between frames

and are:

Also, by spatially transforming

and

(3.1) from frame to frame , the

(3.5)

spatial inertia

matrix of link in frame can be computed as:

(3.6)

where is a symmetric positive definite matrix since Equation (3.6) encapsulates the spatial counterpart of the similarity transformation and the parallel axis theorem [16, 10] for

.

Note that while the inverse of the spatial rotation matrix is just its transpose, i.e., -

, the inverse of the spatial transformation matrix is not the same as its transpose:

(3.7)

3.3 Spatial Differentiation in Rotating Coordinates A simple way to obtain the absolute time derivative of a spatial vector in a rotating frame is to rotate the spatial vector to the inertial (absolute) coordinate system (frame ), differenti-

ate it component-wise, and rotate the derivative back to the rotating frame. Then, using and to denote absolute and apparent (component-wise) differentiations respectively, the


is given by:

absolute derivative of a spatial vector

As in the case of the

22

(3.8)

rotation matrix, [16, 10], the apparent derivative of the

spatial rotation matrix (3.3) can be derived in terms of the relative spatial angular velocity between frames and :

where

(3.9)

is the spatial angular velocity of link :

and

is the spatial analogue of the cross-product operator (3.4). The spatial cross-product operator associated with a spatial vector is defined as:

Then, from Equations (3.8) and (3.9), the absolute derivative of a spatial vector be written as:

can

(3.10)

3.4 Spatial Velocity and Acceleration

represents the motion space of joint and is the joint velocity measured from its parent link to link , the relative spatial velocity of link with respect to link is . Then, the absolute spatial velocity of link can be recursively computed in terms of the Since

spatial velocity of its parent link and its joint velocity shown in Figure 3.3. This can be


23

written as:

(3.11)

leaf

v i ai v h a h Si

% $&(",

i

h

% $ (-,

h

root

i

qi qi Figure 3.3: Outward recursion: spatial velocity and acceleration The spatial acceleration is just the absolute derivative of its corresponding spatial velocity. Note that this acceleration is equivalent to the conventional acceleration [16, 10] and differs from the Featherstone’s [13]. Using Equation (3.10), the spatial acceleration is given by:

(3.12)

Now, the apparent derivative of the spatial transformation matrix (3.3) can be derived

and as in the case of the spatial

in terms of the relative spatial velocity between frames rotation matrix (3.9):

(3.13)

Then, from Equations (3.11), (3.12), and (3.13), the spatial acceleration of a chained link can be recursively computed as shown in Figure 3.3, in terms of the spatial acceleration of its parent link, its joint acceleration, and the cross-product of velocity vectors:

(3.14) (3.15)


24

Notice that the spatial velocity (3.11) and acceleration (3.14) can be expressed in the inertial coordinate system (frame ) by just rotating them to frame using the spatial rotation matrix (3.3):

and

3.5 Spatial Articulated Quantities This section presents some of the spatial articulated quantities essential for deriving the algorithms in later chapters. A spatial articulated quantity, a

matrix, transforms

spatial vectors across actuated joint structures in a dynamically consistent manner and is a nonlinear function of the articulated-body inertia matrix [40]. The articulated-body inertia matrix of a link, introduced by Featherstone [13], relates the spatial force and acceleration of the link, taking into account the dynamics of the rest of

the articulated body [13, 52, 40]. This matrix has been used to develop the Articulated-body method, a recursive

algorithm to solve the joint space forward dynamics problems for

branching robots [13, 47]. The Articulated-body method is presented in section 4.2. The force propagator,

, propagates a spatial force from link

across the actuated joint in a dynamically consistent manner [40]:

where

(3.16)

is the resulting propagated spatial force of link

force of link ,

to its parent link

when the propagated spatial at the beginning of

, is propagated across joint . Note that

the recursion. The force propagator is defined as [40]:

where is a

identity matrix.

(3.17)

is the generalized inverse of weighted by the


25

corresponding inertia matrices:

-

(3.18) (3.19)

is the

articulated-body inertia matrix of link (see Equation (4.10) in full rank matrix projecting onto the motion space of section 4.2) and is the joint with degrees of freedom. Notice that is the dynamically consistent generalized inverse of similar to in Equation (2.8). Force propagation is physically valid only if

where

the spatial force is propagated in the inward (tip-to-base) direction shown in Figure 3.4.

f

h i

* h

Si

f i* L

tip

i h

a

base

* h

h i

a*i

LT

Figure 3.4: Force and acceleration propagators Notice that the force propagator (3.17) has the same dynamic property as the dynamically consistent null space projection matrix,

in Equation (2.11), for redundant robotic

systems [30, 34]. Both quantities guarantee that the resulting (propagated or projected) quantity does not produce any dynamic coupling effect in their corresponding motion (operational) space. Similarly, the acceleration propagator, shown to be equivalent to the transpose of the

force propagator (

) [40], propagates a spatial acceleration of link

across the actuated joint in a dynamically consistent manner [13, 40]:

where

is the resulting propagated spatial acceleration of link

to its child link

(3.20)

when the propagated

spatial acceleration of link , , is propagated across joint . Acceleration propagation is


26

physically valid only if the spatial acceleration is propagated in the outward (base-to-tip)

at the end of the recursion.

direction as shown in Figure 3.4. Note that

A

matrix, defines the relationship between the propagated spatial acceleration

of link and the propagated spatial force of the same link at the joint [52, 36, 40]:

(3.21)

operational space inertia matrix, , of a single open-chain mechanism defined as [30]: Finally, the

can be related to

-

(3.22)

using Equations (3.3), (3.5), (3.21), and (3.22): -

where end-effector frame

is at the tip of leaf link .

(3.23)

Chapter 4 Joint Space Dynamics In the joint space formulation, since the joint space inertia matrix is an

.

matrix, the

explicit calculations of the joint space inertia matrix or its inverse will cost a minimum of

Therefore, it is necessary to avoid any explicit calculation of these quantities in

order to achieve

complexity. This is the major motivation for the two

recursive

algorithms described in the following sections. The recursive Newton-Euler method and the Articulated-body method will be presented to solve joint space inverse and forward dynamics problems, respectively.

4.1 Control : Recursive Newton-Euler Method

The recursive Newton-Euler method is the most efficient general method currently known to solve joint space inverse dynamics problems for

. The partially recursive ver-

sion of this algorithm was originally developed for multi-body satellite dynamics [23] and further expanded to the fully recursive one later [44]. Using the force balance approach, the equations of motion (Newton’s and Euler’s equations) are applied to each link and the resulting equations are combined with constraint equations from the joints in such a way

as to give the joint forces in terms of the joint accelerations.

recursive Newton-Euler method [44, 13]. recursive Newton-Euler method using the This section presents the derivation of the Computing

with given

in Equation (2.1) is the classical joint space inverse dynam-

ics problem. This can be solved by the

27

CHAPTER 4. JOINT SPACE DYNAMICS

28

, is given by the

modified spatial notation presented in Chapter 3. Using Equation (3.10), the net force acting on link in frame link’s rate of change of momentum:

(4.1)

(4.2)

where from Equations (3.5) and (3.12),

Note that can be treated as a constant matrix since is constant and the spatial

.

velocity (3.11) already accounts for the changes in

From Equation (3.5), (4.1), and (4.2), the spatial net force in frame can be written as:

where

(4.3)

(4.4)

is the spatial bias force which must applied to produce zero spatial acceleration.

Equation (4.3) combines Newton’s and Euler’s equations for linear and angular motion of a rigid body [13].

joint h

fh f hnet f hext

Σ fi

link h Figure 4.1: Forces on link

Then, the spatial total force on link equation as:

(Figure 4.1) can be arranged to give a recursive

(4.5)


29

represents the index of each child link of link and is the spatial external force acting on link . Note that the gravitational force can be included in this manner: where

where

is the

(4.6)

(4.7)

gravitational acceleration vector in frame and defined as:

(4.8)

Finally, the joint forces are the components of the spatial total force that do work in the motion space of the joint:

(4.9)

(2)

τh f h vh a h fh

%*$ (",

(1)

.&$ (" h

fi

.&$ ("

ah h

% $&(",

i i

Figure 4.2: Recursive Newton-Euler method

Figure 4.2 illustrates the two (one outward and one inward) recursions required and Table 4.1 summarizes the equations for the in this section.

recursive Newton-Euler method developed


30

Table 4.1: Recursive Newton-Euler method

1. Given:

2. Outward Recursion: Compute the spatial net force and gravitational force:

"! # & & ' + -, (*) . / 102

& ,

+ ,

$%

3

3. Inward Recursion: Compute the spatial total force and joint force:

=

! 54 > ?

87 6

9

! ;:

! > > 3

-> * > > ?

02

3. Outward Recursion:

>

-

>

< '

4. Spatial Transformation: Compute the inverse of the operational space inertia matrix at the end-effector:

-

< 102

5. Matrix Inversion: Compute the operational space inertia matrix at the end-effector:

9

-

:-

5.2 Recursive Algorithm for Branching Mechanisms

This section develops an efficient algorithm to recursively compute the operational space inertia matrix, , for branching robotic mechanisms without any explicit computation of - . An efficient recursive algorithm is derived from the basic analysis of the physical

properties of and the relationships among accelerations, forces, and inertia matrices. The proposed algorithm has the complexity of

where

links and operational points in the system, respectively. Since ration only (2.6),

and

are the numbers of

is a function of configu-

can be assumed for the analysis of without loss of generality.

CHAPTER 5. OPERATIONAL SPACE INERTIA MATRIX

41

5.2.1 Analysis of Operational Space Inertia Matrix - relates the forces at the end-effectors

As in the case of a single operational point (3.22),

to the accelerations at the end-effectors:

Also, since

-

(5.8)

- is symmetric, it can be expressed in terms of its

ponents as:

.. . -

-

..

.. . -

.

block matrix com-

(5.9)

From Equations (5.8), (2.9), and (5.9), the additive property of the coupling effect on the end-effector (of leaf link ) can be written as:

where

-

(5.10)

(5.11)

is the coupling acceleration on the end-effector by the force of the $ end-

effector. This additive property of the coupling effect shows that the resulting acceleration of an end-effector is not only dependent on its own force, but also on the forces of all other

end-effectors in the system. Notice that when the $ end-effector produces the only non-zero force in the system,

the coupling effect on the end-effector by the force of the $ end-effector can be isolated. This can be written, from Equations (5.10) and (5.11), as:

for all when and

$ . Then, similar to Equation (3.21), a

-

(5.12)

matrix, defines the relationship between


42

the propagated spatial acceleration of link and the propagated spatial force of the link $ at the corresponding joints. This relationship can be written as:

(5.13)

Also, similar to Equation (3.23), the block inverse operational space inertia matrix, - , can be related to using Equations (3.3), (6.12), (3.5), (5.12), and (5.13):

-

(5.14)

and are at the tips of leaf links and $ . Note that this

where end-effector frames

relationship is necessary since the inertial properties are desired at the tips instead of at the joints.

5.2.2 Derivation of Recursive Algorithm

This subsection will develop a recursive algorithm by separately analyzing the inertial ef fects of the block diagonal matrices, - , and of the block off-diagonal matrices, -

$ , of - in Equation (5.9).

Block Diagonal Matrix

Each block diagonal matrix, - , is the inertial quantity that would occur if link is the only leaf link with an end-effector. With this physical insight, - can be computed using a trivial extension of the Force Propagation Method (5.6), an

recursive algorithm to compute the

inverse oper-

ational space inertia matrix of just a single open-chain mechanism [52, 36, 40]. Using the relationships from (3.21) and (5.13), the Force Propagation Method (5.6) can be extended immediately for a branching robot by replacing

with . This extension

enables the outward recursion to pass through all children instead of a single child:

-

(5.15)


where

43

is the parent link of link . Note that this recursion starts from the root link and

ends at the leaf links with end-effectors shown in Figure 5.3. e1 ,e1

1

{e1}

e2 ,e2

{e2}

2 h

i

h, h

i,i

{e3}

3

e3 ,e3

Figure 5.3: Block diagonal matrices -

Then, the block diagonal matrices, links using Equation (5.14). Block Off-diagonal Matrix

, can be computed by transforming of leaf

-

$ , may be regarded as cross-coupling inertial quantities that are a measure of the inertia coupling to the end-effector from the force

The block off-diagonal matrices,

of the $ end-effector via the nearest common ancestor of leaf links and $ . The nearest

common ancestor of links and $ is the first common link in two paths; one from link to the root link and the other from link $ to the root link. For example, in Figure 1.3 and in

Figure 5.4, link is the nearest common ancestor of leaf links and $ . From this physical property of block off-diagonal matrices, - can be conceptually

viewed as an inertial quantity which propagates the spatial forces from the $ end-effector to link (the nearest common ancestor of leaf links and $ ) and then propagates the result-

ing spatial accelerations of link to the end-effector.

With this conceptual view, a recursive algorithm will be developed to compute

- by

finding the propagation of the spatial force from the $ end-effector to link and the propagation of the resulting spatial acceleration from link to the end-effector. Then, -


44

can be computed by relating the resulting spatial acceleration of link

to the propagated

spatial force from the $ end-effector. Figure 5.4 illustrates this process. T

a*i =hi L* a*h {ei}

i

a*leaf = aleaf * fleaf = fleaf

h

a*h =

* h ,h h

f

j

{ej}

fh* =hj L*f *j Figure 5.4: Force/acceleration propagation via nearest common ancestor

First, using Equations (3.5), (3.16), and (3.17), the force at the $ end-effector can be propagated to any of its ancestors:

(5.16)

(5.17)

is an ancestor link of link $ , link is the descendent link of link in the

results a compound path from link to link $ and link is the parent link of link . propagation of the spatial force from link $ to link [40]. where link

Similarly, using (6.12), (3.20), (3.17), and (5.17), the propagated spatial acceleration of link can be propagated to the end-effector of any of its descendant leaf link :

(5.18)


Now, combining Equations (5.13), (5.16), and (5.18),

and can be related as:

45

Then, from Equations (5.12), (5.13), (5.14), and (5.19),

(5.19)

for the block off-diagonal

matrices can be derived:

where

(5.20)

is the nearest common ancestor of leaf links and $ . A recursive version of

Equation (5.20) is presented in Table 5.3 (step 4). Note that this recursion starts from and ends at the leaf links with end-effectors shown in Figure 5.5. e1 ,e 3

{e1} 1

{e2}

e1 ,e 2

2

h

e 2 ,e 3

k 3

{e3}

Figure 5.5: Block off-diagonal matrices As for the block diagonal matrices, the block off-diagonal matrices, can be computed by transforming

-

$ ,

of leaf links and $ to the corresponding tips using

Equation (5.14). Finally, the operational space inertia matrix,

, can be computed by inverting - in

Equation (5.9). Table 5.3 summarizes the recursive algorithm developed in this section. Note that although most processing occurs along the path from the root link to the leaf links with end-effectors, the effects of the other links enter through the articulated-body inertias (see Equation (4.10) in section 4.2) of the links in the path.


46

Figure 5.6 illustrates the recursion processes of the proposed algorithm for the branching robot shown in Figure 1.3. Arrows indicate the direction of the recursion and there is no computation required among the nodes connected by dotted lines. Table 5.3: Algorithm for operational space inertia matrix 1. Outward Recursion: Compute the spatial transformation matrices:

+ +

+

2. Inward Recursion: Compute the force propagators:

! > >

02

3. Outward Recursion: Compute the block diagonal matrices starting with

>

->

.:

<

6

7

! > > 3

-> > > ?

9

. :

02

3. Outward Recursion: Compute the operational space inertia matrix of each link:

>

! 7 6 - ! > > 7 3 . 6 -> 3 > %> 3 - ! 7 - 3 6

9

:

3 ?

The main concept of the Impulse Propagation method is to propagate the articulatedbody impulse vector of the contact link back to the root link and then, to propagate the instantaneous changes in joint velocities via a single outward recursion. The definition of

, is the integration of the articulated-body bias force of link , in Equation (4.11), over the collision period, : the articulated-body impulse vector of link ,

(8.14)

Note that the collision period is an infinitesimal interval since the rigid-body assumption is used. The effects of the finite magnitude centrifugal, Coriolis, bias and joint forces can be

CHAPTER 8. CONTACT DYNAMICS

ignored since they vanish in the limit,

86

:

(8.15)

Similarly, the effects of the external forces can be ignored in the limit except the impulse

at link

where the collision occurred:

where

(8.16)

is the spatial impulse computed using Equations (8.7) and (8.8) when the collision occurred at contact point . Note that since the configuration of the whole system

does not change during the collision, the terms depending only on configurations such as

, , and stay constant during the collision.

Using Equations (4.11) and (8.14) with the conditions (8.15) and (8.16) at the limit, the articulated-body impulse vector can be propagated from the contact link to the root link via a single inward recursion:

where

is the index of the link where the contact occurred. Note that

(8.17) is constant during

the collision since it is dependent on configuration only. This inward recursion process

is the total change in joint velocity of link during the collision; it is the limit of infinite joint acceleration integrated over an infinitesimal interval:

corresponds (1) shown in Figure 8.7. The quantity

(8.18)

Using Equations (4.12), (8.14), and (8.18) with the conditions (8.15) at the limit, the total change in joint velocity of link can be written as:

-

(8.19)


∆v i ∆qi

87

h

i

y = Ly A h

(2)

h i

k A i

c (1)

y kA =− kc Xy c

Figure 8.7: Impulse Propagation method: recursion processes

is the total change in spatial velocity of link during the collision; it is the limit of infinite acceleration integrated over an infinitesimal interval: Similarly, the quantity

(8.20)

Using Equation (3.14), (8.18), and (8.20) with the conditions (8.15) at the limit, the total change in spatial velocity of link can be written as:

(8.21)

Note that the instantaneous changes in velocities of all joints in an articulated branching system can be computed via a single outward recursion from the root link to the leaf links using Equations (8.19) and (8.21) at each recursion step. This outward recursion pro-

cess corresponds (2) shown in Figure 8.7. Table 8.2 summarizes the Impulse Propagation method. The complexity of this method is

since each step in Table 8.2 takes

operations. Recently, many efficient collision detection algorithms [50, 51] have been developed based on fast distance calculation algorithms [15, 43]. Once correct collision is determined

computed by the

using one of these collision detection algorithms, the correct spatial impulse,

can be

algorithm presented in the previous section. Then, by propagating


88

Table 8.2: Impulse Propagation method 1. Given:

2. Inward Recursion: Compute the articulated-body impulse vector:

!

3 02

3. Outward Recursion: Compute the instantaneous changes in the joint velocity and the spatial velocity:

! - > ! > > 3

3 ?

, in an articulated branching system can be computed using the Impulse Propaga-

this spatial impulse appropriately, correct instantaneous changes in velocities of all joints,

tion method presented in this section. Finally, with appropriate changes in joint velocities,

the configuration after the collision can be computed by integrating forward one time step for dynamic simulation. Therefore, the

algorithms developed in this chapter result in

a significant speed-up for real-time impulse-based dynamic simulation.

Chapter 9 System Integration This chapter describes the implementation and integration of the algorithms presented in this thesis into a complete robotic system. Overall hardware/software architecture is presented to illustrate the current configuration of a real-time dynamic control/simulation environment. Issues involving user interface, control/simulation servo loop, and optimization techniques used to implement the proposed algorithms in a real-time environment are further discussed. Finally, this chapter concludes with a remark about the implication of the capabilities of the current real-time environment that incorporates the ideas presented in this thesis.

9.1 Hardware/Software Architecture A prototype real-time dynamic control/simulation environment [7] has been developed to validate the ideas presented in this thesis. This environment is designed to incorporate and utilize the current technology available both in hardware and software in the industry. The overall hardware/software architecture of the current environment is shown in Figure 9.1. In order to make the user interface platform-independent, a robot user interface has been developed using VRML [18, 2] for graphics modeling/display and JAVA [24, 25] for implementing the front-end software. Through an Internet browser-based front-end user interface, users can communicate with the back-end control/simulation servo loop virtually from any platform. For real-time dynamic control/simulation, the back-end software is 89

CHAPTER 9. SYSTEM INTEGRATION

90

Back-end

Front-end

9 - : 9

User

mputer

Controller

91*7

-

Servo (C++)

4 "78 * 791):* *-6-2"7

91* > 2

&-,./10 2

Drivers (C/C++) 34 # $ %5&%( )) * +

Proxy # $ %'&%( )) * +

Robot

# Sensors ;)@;-. KGLJ 0 /4!;

yes

?4/, DE! FG2!;6H/ BC;0 / ') Control/Simulation

DP @/4= 03 $ @ Q Figure 9.2: Control/simulation servo loop flow chart (Q is a priority queue)


92

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

Figure 9.4: Graphical user interface tools: disks, planes, and circular belts


93

9.3 Real-time Implementation

In order to fully utilize any algorithm, it is necessary to implement it efficiently. For the

recursive algorithms presented in this thesis, this entails reducing the constant co-

efficient of . Some of the major optimization techniques for algorithms dealing with dynamics [13, 41] are modified to make the most common case faster for the algorithms presented in this thesis. In robotics, the most common joints are one-degree-of-freedom revolute or prismatic joints of serial-chain mechanisms. Also, the most common convention to assign frames to

, , ), significant savings can

these one-degree-of-freedom joints is the Denavit-Hartenberg convention [11, 10]. Using the properties of the Denavit-Hartenberg parameters ( ,

be obtained by replacing the spatial transformation matrix (3.3) with two screw coordinate

transformation matrices, the first on the -axis,

-axis,

, followed by a second on the new

:

where the two rotation matrices are defined as:

,

and the two translation vectors are defined as:

Notice that

and

are constant since

,

and are constant parameters. Therefore, the

complete coordinate transformation of a spatial vector or a spatial matrix can be optimized

significantly since the first screw transformation matrix,

, becomes constant and most


94

of the elements in both screw transformation matrices become zero. In addition, in the case of one-degree-of-freedom joints, the general joint model

with the value of one element equal to

becomes a

unit vector

shown in Equation (3.2). Therefore, multiplica-

tion of or is equivalent to selection of a column or a row instead of actual matrix

multiplications. In animation, the most common joints are the spherical joints of branching mechanisms. In this case, the Denavit-Hartenberg convention can not be used. Instead, the quaternion notation [61] should be used for orientation since it gives faster and more robust results

rotation matrices. Notice that in the case of spherical joints, the general joint model becomes a vertically concatenated matrix of a zero matrix and a identity matrix shown in Equation (3.2). Therefore, multiplication of or

than conventional

is equivalent to selection of sub-matrices instead of actual matrix multiplications. Finally, the symmetric positive definite properties [62, 8] of inertia matrices can be used to further decrease the number of additions and multiplications required to manipulate these spatial matrices. Since any algorithm dealing with dynamics heavily involve inertia matrices, optimizing the matrix operations involving inertia matrices such as , ,

should significantly increase the overall performance.

, and

9.4 Concluding Remarks The proposed real-time robotic environment achieves a servo rate of 300–1000 Hz for dynamic control and simulation of complex branching robotic mechanisms such as one shown in Figure 1.5. This branching robot has an operational point at each of its 2 end-effectors and 24 links connected by one-degree-of-freedom joints. This result implies that highly redundant articulated robotic mechanisms such as a humanoid mechanism with multiple operational points can be simulated and controlled directly at the task level with a high servo rate in a low-cost hardware environment. In addition, the combination of the operational space framework with the proposed environment will enable users to intuitively specify task and posture behavior for complex robot commands by providing dynamic decoupling among the end-effectors’ tasks and the complex internal posture dynamics.

Chapter 10 Future Work and Conclusion This chapter concludes this thesis with a discussion of future work and a summary of the ideas which have been presented.

10.1 Future Work With advances in computational and mechanical hardware technology, a growing body of interest has emerged in the area of real-time dynamic control and simulation of branching mechanisms – systems of tree-like topology such as a humanoid robot. Application of such interest requires fast algorithms to provide intuitive, systematic, and efficient ways to model, control, and simulate the dynamics of these complex systems. Future research should take advantage of the exploding technological advances and interests in robot dynamics and behavior control.

10.1.1 Robot Dynamics Efficient recursive algorithms to solve the robot dynamics problem related to control and simulation of robotic mechanisms have been presented in this thesis. Even though each algorithm is efficient, certain combination of these algorithms will achieve the best overall efficiency depending on the domain of the given problem to solve. Further analysis of

95

CHAPTER 10. FUTURE WORK AND CONCLUSION

96

these algorithms has revealed that they produce not only useful final results but also important intermediate results that provide crucial physical insight into the dynamics of robotic mechanisms, the impact of the dynamically consistent generalized inverse of the Jacobian matrix, and its associated null space projection matrix. Future research should be directed towards the further analysis of robot dynamics based on this new insight.

10.1.2 Behavior Control A basic underlying framework is being developed to accommodate high-level behavior control in both the task space and the null space. Applications of this behavior control include balancing, walking, carrying, mobile manipulator coordination, and singularity control. This basic framework provides an intuitive and systematic way to model and control dynamically consistent task/posture behavior of complex branching mechanisms. This section describes a basic framework to support intuitive posture behavior control since, for highly redundant mechanisms, posture behavior requires specifying many more degrees of freedom than task behavior. The main idea is to newly construct parameters

, is a function of

called control variables and incorporate them into the potential field [29]. A control variable,

link frame coordinate spatial vectors, :

(10.1)

and its absolute derivative is a function of the absolute derivatives of :

(10.2)

where the Jacobian matrix associated with the control variable is:

(10.3)

since the absolute derivatives of the link frame coordinate spatial vectors are defined as

(6.4). Then, with Equations (10.1) and (10.2), an additional task to be carried

out using the null space can be realized by constructing a potential function,

, whose


97

minimum corresponds to the desired task:

(10.4)

Finally, using Equations (10.3) and (10.4), the posture behavior control command vector in Equation (6.1) can be computed using the negative gradient of the potential function:

(10.5)

Note that interference from the additional torques on the end-effectors is eliminated by projecting this vector onto the null space using the operational space formulation presented in Chapter 2. This basic framework for behavior control has been implemented and tested for validity using several different robotic systems: PUMA 560 for singularity control presented in Appendix A, ProVAR for assistive rehabilitation [66], and SAMM for real-time motion modification [6] and mobile manipulation [35, 21]. Future work should extend this basic framework in the following areas: Behavior modeling – Since the behavior of all physical systems is governed by dynamics, it is logical to conclude that all behavior should reflect dynamic properties. Therefore, dynamic parameters should be used to parameterize the characteristics of robot behavior. Ultimately, it is desired to model robot behavior by parameterizing the behavior directly from general data such as motion capture data. Motion Planning and Execution – With super-real-time simulation capability, more advanced motion planning and execution techniques [38, 6] should be incorporated into behavior control. This will give robots the capacity to visualize and predict the future more accurately. Robot User Interface – In order for robots to be in a human environment, robot user interface should provide more intuitive methods of communication such as voice, force feedback, and vision. Identification – Identification of dynamic properties is a difficult and tedious process. Even though some algebraic methods exists which use graphics data for simulation


98

purposes [46], accurate identification of a real robot with the complexity of a humanoid is currently near impossible. Therefore, it is critical to develop an easier and more automatic identification process for dynamic behavior control of future robots. Animation – Integration of kinematic and dynamic behavior will be the most critical issue in animation software using dynamics [48] since both realistic and non-realistic behaviors are desired in animation. In addition, using dynamic properties such as inertia and velocity, more realistic sound can be generated dynamically with infinite possibilities. Haptics – Haptic devices can be used both as input and output devices in dynamic simulation/control environments [54, 58, 57, 55] since they provide true 3-dimensional input and force feedback output. These devices should facilitate modeling and control of robot behavior.

10.2 Conclusion This thesis presented efficient recursive algorithms, using the operational space formulation, to model and solve, at the task level, the dynamics problems of highly redundant articulated branching mechanisms with multiple operational points. A modified spatial notation was introduced to model complex robot kinematics and dynamics in an intuitive and systematic way. Using this notation, efficient recursive algorithms were developed for branching mechanisms in order to control and simulate task/posture behavior dynamics, closed-chain dynamics, and contact dynamics. A basic underlying framework was discussed to provide dynamic control of intuitive task-level commands and posture behavior. The application of these algorithms resulted in a significant increase in the interactivity and usability of the dynamic control and simulation of complex branching mechanisms. The computational complexity of these algorithms was analyzed and compared with existing methods. Experimental and real-time dynamic simulation results were presented to demonstrate the effectiveness of the proposed algorithms. Issues in system integration and implementation were further explored. Finally, a discussion of future work concluded this thesis.

Appendix A Singularity Control A.1 Introduction This chapter presents a general strategy for robot control at kinematic singularities. In the operational space framework, the control of redundant robots relies on two basic models: a task-level dynamic model in Equation (2.5) obtained by projecting the robot dynamics into the operational space and the dynamically consistent force/torque relationship in Equation (2.2) that provides decoupled control of joint motions in the null space associated with the redundant mechanism. These two models are the bases for implementing the proposed control strategy for kinematic singularities. When a robot is in the neighborhood of singular configurations, rather than modify the kinematics of the robot using singularity-robust inverses [49, 9], treat it as a redundant mechanism in the subspace orthogonal to the singular directions of the end-effector. Control in this subspace is based on operational forces or task behavior in Equation (2.3), while null space joint torques or posture behavior in Equation (2.10) are used to deal with control in the singular directions. Decoupled behavior is guaranteed by using the dynamically consistent force/torque relationship in Equation (2.5). This chapter proposes a classification of kinematic singularities from a control perspective and presents a general strategy for robot control at kinematic singularities. Experimental results of the implementation of this approach on a PUMA 560 robot are presented.

99

APPENDIX A. SINGULARITY CONTROL

100

A.2 Kinematic Singularities A singular configuration is a configuration

at which the end-effector mobility – defined

as the rank of the Jacobian matrix – locally decreases. At a singular configuration, the endeffector locally loses the ability to move along or rotate about some direction in Cartesian space. Singularities and mobility are characterized by the determinant of the Jacobian matrix

, that vanishes

for non-redundant robots or by the determinant of the matrix product of the Jacobian and its transpose for redundant mechanisms. This determinant is a function,

at each of the robot singularities. This function can be developed into a product of terms,

(A.1)

where each term corresponds to one of the different singularities associated with the mechanism. Here,

is the number of different singularities. A singular configuration always

has a corresponding singular direction. It is in or about this direction that the end-effector presents infinite effective mass or effective inertia. The end-effector movements remain free in the subspace orthogonal to this direction. In reality, the difficulty with singularities

extends to some neighborhood around the singular configuration. The neighborhood of the

singularity,

, can be defined as

!

(A.2)

is a positive value. This value can be determined by manipulability analysis [5]. In the neighborhood of a singular configuration , the robot is treated as a redundant

where

mechanism in the subspace1 orthogonal to the singular direction. End-effector motions in that subspace are controlled using the operational space redundant robot control or task behavior, while null space joint torques or posture behavior are used to deal with control in the singular directions. The use of the dynamically consistent force/torque relationship guarantees decoupled behavior between end-effector control and null space control. 1

a subspace of the end-effector operational space.


101

A.3 Control Strategy at Singularities The dynamically consistent torques/forces decomposition in Equation (2.2) of a non-redundant mechanism is done as follows: First, in the neighborhood of singular configurations, singular directions and the associated singular frames are identified. A singular frame is a frame in which one of the axes is aligned with a singular direction. Next, the Jacobian matrix of the non-redundant mechanism is rotated into the singular frame and the rows corresponding to the singular directions are eliminated. This new Jacobian matrix corresponds to the redundant mechanism with respect to end-effector motion in the subspace orthogonal to the singular directions. The dynamically consistent generalized inverse of this Jacobian matrix in Equation (2.8) is used to construct the projection onto the null space and this projection is used to control motions in the null space or posture behavior in Equation (2.10). Endeffector motions in the subspace orthogonal to the singular directions are controlled using the operational space redundant robot control or task behavior in Equation (2.3). Two Types of Singularities In previous work [37], singularities have been characterized in terms of the internal freedom of motion a robot has at a singular configuration while its end-effector remains fixed. However, for control purposes, singularities are classified into two groups based on the control characteristics of their null spaces. Type 1 singularities are those at which the endeffector can be controlled in the singular directions using motion in the null space. Type 2 singularities are those at which motion in the null space only affects the singular direction. Figure A.1 shows the three basic singularities in a PUMA 560: elbow lock, wrist lock, and overhead lock from left to right. Elbow lock is a Type 1 singularity and the other two are Type 2 singularities. Joint motions in the null space associated with a Type 1 singularity result in motion of the end-effector in the singular direction. However, joint motions in the null space associated with a Type 2 singularity result only in a change of the singular direction through internal joint motions. Two different strategies are developed according to these two types of singularities.


102

Figure A.1: Three basic singular configurations in PUMA 560 Type 1 Singularity

from Equation (A.1) is treated as a new task coordinate. A potential

The end-effector motion in the singular direction is controlled directly through the associated null space.

function of this coordinate is used in the control of end-effector behavior along the singular direction. Using this potential function, the projection of the torques from Equation (10.5) onto the null space affects the end-effector motion only along the singular direction.

Moving the end-effector to a singular configuration is achieved by a control that takes

.

to zero. When moving out of a singularity, the control of the end-effector motion

, it is possible to select the

along the singular direction is achieved by controlling the rate of possible assignments of the sign for the desired rate of

With the two

should be selected according to the desired velocity

posture of the robot among the two configurations that it can generally take when moving

out of a singularity. The rate of at the configuration when

from Equation (A.2), in order to achieve a smooth

transition when crossing the boundary of the neighborhood of singularity. Type 2 Singularity At this type of singularities, the end-effector can only move in or about a direction that is orthogonal to the singular direction. To allow the end-effector to perform a motion in or about a given direction, first, the singular direction must be changed to be orthogonal to this direction. Controlling the direction of singularity can be accomplished by motion in the null space.


Let

103

be the singular direction. The vector

can be controlled by a potential . Control in the

function whose minimum corresponds to the desired singular direction,

null space can be implemented following Equation (10.5).

The control strategy for dealing with this type of singularity is to maintain a constant singular direction during motions in the neighborhood of singularity. This will prevent the large internal joint motions that are generally associated with end-effector motions in the neighborhood of the singularity. For tasks involving the trajectory-following, the desired motion of the end-effector in the neighborhood of singularities should be designed to be in the subspace orthogonal to the singular direction.

A.4 Experimental Results The strategy presented in the previous section has been implemented to control a PUMA 560 for the three different singularities shown in Figure A.1. For some configurations, these singularities can occur simultaneously and the rank of the Jacobian can vary from 3 to 6. The minimum rank of the Jacobian corresponds to the configuration at which the end-effector reaches the highest point directly above the base. Elbow and Wrist lock 1.8 1.6

Position (m) and Orientation (rad)

1.4 1.2 1 0.8 0.6 0.4

angle about y-axis : dotted line x : solid line z : dashed line

0.2 0 -0.2 0

0.5

1

1.5

2 Time (sec)

2.5

3

3.5

4

Figure A.2: Elbow lock and wrist lock: experimental response In Figure A.2, the end-effector is simultaneously moving out of two singularities: the elbow lock (Type 1) and wrist lock (Type 2). The goal is to translate along the singular direction, while maintaining all other positions and orientations. The resulting motion is shown in Figure A.2. In Figure A.3, the end-effector is shown at a singular configuration


104

Wrist Lock 2

1.5 Joint 6 : dashed line

1

Orientation (rad)

Joint 5 : solid line 0.5

Joint 4 : dotted line

0

-0.5

-1

-1.5

-2 0

0.5

1

1.5

2

2.5

Time (sec)

Figure A.3: Wrist lock: experimental response of wrist lock (Type 2). The goal for the end-effector is to perform a rotation of

about

the singular direction. This task is accomplished by first rotating the singular direction to a direction where it is orthogonal to its initial configuration. This is accomplished by the control discussed above, which involves internal joint motions in the null space. The resulting motions of joints 4, 5, and 6 in the wrist are shown in Figure A.3.

A.5 Conclusion This chapter has proposed a classification of singularities into two types following the control characteristics of their null spaces: singularities at which the end-effector can be controlled in the singular directions using motion in the null space (Type 1), and singularities at which motion in the null space only affects the singular direction (Type 2). A general control strategy for these two types of singularities has been developed under the operational space formulation. The implementation of this strategy relies on the dynamically consistent force/torque relationship in Equation (2.5) which guarantees decoupled behavior between end-effector (task behavior) control in Equation (2.3) and null space (posture behavior) control in Equation (2.10) of redundant mechanisms. The experimental results with a PUMA 560 illustrate the effectiveness of this strategy.

Bibliography [1] J. Adams, R. Bajcsy, J. Kosecka, V. Kumar, R. Mandelbaum, M. Mintz, R. Paul, C. Wang, Y. Yamamoto, and X. Yun. Cooperative material handling by human and robotic agents: Module development and system synthesis. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 200– 205, 1995. [2] Andrea L. Ames, David R. Nadeau, and John L. Moreland. VRML 2.0 Sourcebook. John Wiley & Sons, Inc., second edition, 1997. [3] David Baraff. Fast contact force computation for non-penetrating rigid bodies. In SIGGRAPH Conference Proceedings (Computer Graphics), pages 23–34, Orlando, Florida, U.S.A., July 1994. ACM SIGGRAPH. [4] David Baraff. Linear-time dynamics using lagrange multipliers. In SIGGRAPH Conference Proceedings (Computer Graphics), pages 137–146, New Orleans, Louisiana, U.S.A., August 1996. ACM SIGGRAPH. [5] Alan Bowling. Analysis of Robotic Manipulator Dynamic Performance: Acceleration and Force Capabiities. PhD thesis, Stanford University, Stanford, California, U.S.A., June 1998. [6] Oliver Brock. Generation of Robot Motion: Integrating Planning and Excution. PhD thesis, Stanford University, Stanford, California, U.S.A., December 1999. [7] Kyong-Sok Chang. Robotics Library. Computer Science Robotics Laboratory, Stanford University, Stanford, California, U.S.A., 2nd edition, 1998. A dynamic control and simulation library. 105

BIBLIOGRAPHY

[8] Steven C. Chapra and Raymond P. Canale.

106

Numerical Methods for Engineers.

McGraw-Hill Book Company, second edition, 1988. [9] S. Chiaverini, B. Siciliano, and O. Egeland. Experimental results on controlling a 6dof robot manipulator in the neighborhood of kinematic singularities. In Proceedings of International Symposium on Experimental Robotics, Kyoto, Japan, 1993. [10] John J. Craig. Introduction to Robotics. Addison-Wesley Publishing Company, second edition, 1989. [11] J. Denavit and R. S. Hartenberg. A kinematic notation for lower pair mechanisms based on matrices. Transactions of ASME Journal of Applied Mechanics, 22:215– 221, June 1955. [12] K. Doty. A theory of generalized inverses applied to robotics. International Journal of Robotics Research, 12(1), 1993. [13] Roy Featherstone. Robot Dynamics Algorithms. Kluwer Academic Publishers, 1987. [14] Roy Featherstone and Oussama Khatib. Load independence of the dynamically consistent inverse of the jacobian matrix. International Journal of Robotics Research, 16(2):168–170, April 1997. [15] Elmer G. Gilbert, Daniel W. Johnson, and S. Sathiya Keerthi. A fast procedure for computing the distance between complex objects in three-dimensional space. IEEE Journal of Robotics and Automation, 4(2):193–203, April 1988. [16] Donald T. Greenwood. Principles of Dynamics. Prentice-Hall, Inc., second edition, 1988. [17] H. Hanafusa, T. Yoshikawa, and Y. Nakamura. Analysis and control of articulated robot arms with redundancy. In Proceedings of the 8th IFAC, volume XIV, pages 38–83, 1981. [18] Jed Hartman and Josie Wernecke. The VRML 2.0 Handbook. Addison-Wesley Publishing Company, 1996.

BIBLIOGRAPHY

107

[19] Samad Hayati. Hybrid position/force control of multi-arm cooperating robots. In Proceedings of IEEE International Conference on Robotics and Automation, pages 82–89, San Francisco, CA, U.S.A., April 1986. [20] J. M. Hollerbach and K. C. Suh. Redundancy resolution of manipulators through torque optimization. In Proceedings of IEEE International Conference on Robotics and Automation, pages 1016–1021, March 1985. [21] Robert Holmberg. Design and Development of Powered Caster Holonomic Mobile Robots. PhD thesis, Stanford University, Stanford, California, U.S.A., June 2000. To appear. [22] Robert Holmberg and Oussama Khatib. Development of holonomic mobile robot for mobile manipulation tasks. In Proceedings of Field Service Robotics, Pittsburgh, PA, U.S.A., August 1999. [23] W. W. Hooker and G. Margulies. The dynamical attitude equations for an n-body satellite. Journal of the Astronautical Sciences, 12(4):123–128, 1965. [24] Cay S. Horstmann and Gary Cornell. Core JAVA 1.1 – Fundamentals, volume I. Sun Microsystems Press, 1997. [25] Cay S. Horstmann and Gary Cornell. Core JAVA 1.1 – Advanced Features, volume II. Sun Microsystems Press, 1998. [26] D. Jung, G. Cheng, and A. Zelinsky. Experiments in realizing cooperation between autonomous mobile robots. In Proceedings of International Symposium on Experimental Robotics, pages 513–524, 1997. [27] Brian W. Kernighan and Dennis M. Ritchie. The C Programming Language. Prentice Hall P T R, second edition, 1988. [28] Oussama Khatib. Commande Dynamique dans l’Espace Opérationnel de Robots ´ Manipulateurs en Présence d’Obstacles. PhD thesis, Ecole National Supérieure de l’Aéronautique et de l’Espace, Toulouse, France, December 1980.

BIBLIOGRAPHY

108

[29] Oussama Khatib. Real-time obstacle avoidance for manipulatros and mobile robots. International Journal of Robotics Research, 5(1):90–98, Spring 1986. [30] Oussama Khatib. A unified approach to motion and force control of robot manipulators: The operational space formulation. IEEE Journal of Robotics and Automation, RA–3(1):43–53, February 1987. [31] Oussama Khatib. Object manipulatin in a muti-effector robot system. In R. Bolles and B. Roth, editors, Robotics Research, volume 4, pages 137–144. MIT Press, 1988. [32] Oussama Khatib. Motion/force redundancy of manipulators. In Proceedings of JapanU.S.A. Symposium on Flexible Automation, 1990. [33] Oussama Khatib. Inertial properties in robotic manipulation: An object-level framework. International Journal of Robotics Research, 14(1):19–36, 1995. Reduced effective inertial property. [34] Oussama Khatib. The impact of redundancy on the dynamic performance of robots. Laboratory Robotics and Automation, 8:37–48, 1996. [35] Oussama Khatib, Kazuhito Yokoi, Oliver Brock, Kyong-Sok Chang, and Arancha Casal. Robots in human environments: Basic autonomous capabilities. International Journal of Robotics Research, 18(7):684–696, 1999. [36] K. Kreutz-Delgado, A. Jain, and G. Rodriguez. Recursive formulation of operational space control. In Proceedings of IEEE International Conference on Robotics and Automation, pages 1750–1753, April 1991. [37] Z. C. Lai and D. C. H. Yang. A new method for the singularity analysis of simple six-link manipulators. International Journal of Robotics Research, 1986. [38] Jean-Claude Latombe. Robot Motion Planning. Kluwer Academic Publishers, 1991. [39] Paul U. Lee, Diego C. Ruspini, and Oussama Khatib. Dynamic simulation of interactive robotic environment. In Proceedings of IEEE International Conference on Robotics and Automation, pages 1147–1152, San Diego, CA, U.S.A., May 1994.

BIBLIOGRAPHY

109

[40] Kathryn W. Lilly. Efficient Dynamic Simulation of Robotic Mechanisms. Kluwer Academic Publishers, 1992. [41] Kathryn W. Lilly and D. E. Orin. Alternate formulations for the manipulator inertia matix. International Journal of Robotics Research, 10(1):64–74, February 1991. [42] Kathryn W. Lilly and D. E. Orin. Efficient O(n) recursive computation of the operational space inertia matrix. IEEE Transactions on Systems, Man, and Cybernetics, 23(5):1384–1391, September/October 1993. [43] Ming C. Lin and John F. Canny. A fast algorithm for incremental distance calculation. In Proceedings of IEEE International Conference on Robotics and Automation, pages 1008–1014, Sacramento, CA, U.S.A., April 1991. [44] J. Y. S. Luh, M. W. Walker, and R. P. C. Paul. On-line computational scheme for mechanical manipulators. Transactions of ASME Journal of Dynamic Systems, Measurement, and Control, 102(2):69–76, June 1980. [45] Brian Mirtich. Impulse-based simulation of rigid bodies. In Proceedings of Symposium on Interactive 3D Graphics, pages 181–188, Monterey, CA, U.S.A., April 1995. [46] Brian Mirtich. Fast and accurate computation of polyhedral mass properties. Journal of Graphics Tools, 1(2), 1996. [47] Brian Vincent Mirtich. Impulse-based Dynamic Simulation of Rigid Body Systems. PhD thesis, University of California at Berkeley, Berkeley, California, U.S.A., December 1996. [48] Motion Factory, Inc. Motivate v2.0. Motion Factory, Inc., Fremont, California, U.S.A., 1999. An interactive animation software. [49] Yoshihiko Nakamura. Advanced Robotics Redundancy and Optimization. AddisonWesley Publishing Company, 1991. [50] Sean Quinlan. Efficient distance computation between non-convex objects. In Proceedings of IEEE International Conference on Robotics and Automation, volume 4, pages 3324–3329, 1994.

BIBLIOGRAPHY

110

[51] Sean Quinlan. Real-Time Modification of Collision-Free Paths. PhD thesis, Stanford University, Stanford, California, U.S.A., December 1994. [52] G. Rodriguez, K. Kreutz, and A. Jain. A spatial operator algebra for manipulator modeling and control. In Proceedings of IEEE International Conference on Robotics and Automation, pages 1374–1379, May 1989. [53] B. Roth, M. Raghavan, O. Khatib, and K. Waldron. Kinematic structure for a force controlled redundant manipulator. In Proceedings of International Meeting on Advances in Robot Kinematics, pages 176–180, Ljubljana, Yugoslavia, September 1988. [54] Diego C. Ruspini. Haptics Library. Computer Science Robotics Laboratory, Stanford University, Stanford, California, U.S.A., 2nd edition, 1997. A haptics/simulation library. [55] Diego C. Ruspini. Beyond the Looking Glass: The Haptic Exploration of Virtual Environments. PhD thesis, Stanford University, Stanford, California, U.S.A., June 2000. To appear. [56] Diego C. Ruspini and Oussama Khatib. Collision/contact models for the dynamic simulation of complex environments. In Workshop on Dynamic Simulation, IEEE/RSJ International Conference on Intelligent Robots and Systems, Grenoble, France, September 1997. [57] Diego C. Ruspini and Oussama Khatib. Collision/contact models for dynamic simulation and haptic interaction. In 9th International Symposium of Robotics Research (ISRR’99), pages 185–195, Snowbird, Utah, U.S.A., October 1999. [58] Diego C. Ruspini, Krasimir Kolarov, and Oussama Khatib. The haptic display of complex graphical environments. In SIGGRAPH Conference Proceedings (Computer Graphics), pages 345–352, Los Angeles, CA, U.S.A., August 1997. ACM SIGGRAPH.

BIBLIOGRAPHY

111

[59] Jeffrey Russakow, Oussama Khatib, and Stephen M. Rock. Extended operational space formulation for serial-to-parallel chain (branching) manipulators. In Proceedings of IEEE International Conference on Robotics and Automation, pages 1056– 1061, Nagoya, Japan, May 1995. [60] Jeffrey S. Russakow. Experiments in Manipulation and Assembly by Two-arm, Freeflying Space Robots. PhD thesis, Stanford University, Stanford, California, U.S.A., December 1995. [61] Ken Shoemake. Animating rotation with quaternion curves. In SIGGRAPH Conference Proceedings (Computer Graphics), volume 19, pages 245–254, San Francisco, CA, U.S.A., July 1985. ACM SIGGRAPH. [62] Gilbert Strang. Introduction to Applied Mathematics. Wellesley-Cambridge Press, 1986. [63] Bjarne Stroustrup. The C++ Programming Language. Addison-Wesley Publishing Company, second edition, 1991. [64] T-J Tarn, A. K. Bejczy, and X. Yun. Design of dynamic control of two cooperating robot arms: Closed chain formulation. In Proceedings of IEEE International Conference on Robotics and Automation, pages 7–13, April 1987. [65] Masaru Uchiyama and Pierre Dauchez. A symmetric hybrid position/force control scheme for the coordination of two robots. In Proceedings of IEEE International Conference on Robotics and Automation, pages 350–356, April 1988. [66] H.F. Machiel Van der Loos, Joe Wagner, Neils Smaby, Kyong-Sok Chang, Oscar Madrigal, Larry Leifer, and Oussama Khatib. Provar assistive robot system architecture. In Proceedings of IEEE International Conference on Robotics and Automation, volume 1, pages 741–746, Detroit, MI, U.S.A., May 1999. [67] M. W. Walker and D. E. Orin. Efficient dynamic computer simulation of robotic mechanisms. Transactions of ASME Journal of Dynamic Systems, Measurement, and Control, 104(3):205–211, September 1982.

BIBLIOGRAPHY

112

[68] David Williams and Oussama Khatib. The virtual linkage: A model for internal forces in multi-grasp manipulation. In Proceedings of IEEE International Conference on Robotics and Automation, volume 1, pages 1025–1030, Atlanta, GA, U.S.A., May 1993. [69] J. David Williams. Characterization and Control of Multiple-grasp Robotic Systems. PhD thesis, Stanford University, Stanford, California, U.S.A., June 1995. [70] Y. F. Zheng and J. Y. S. Luh. Joint torques for control of two coordinated moving robots. In Proceedings of IEEE International Conference on Robotics and Automation, pages 1375–1380, April 1986.