CONSTRAINED MANIPULATORS AND CONTACT FORCE CONTROL OF CONTOUR FOLLOWING PROBLEMS by Han-Pang Huang A dissertation submitted in partial fulfillment of the requirements for the degree of Doctor of Philosophy (Electrical Engineering) in The University of Michigan 1986 Doctoral Committee: Professor N. Harris McClamroch Chairman Assistant Professor Pierre T. Kabamba Associate Professor Kang G. Shin Associate Professor Michael W. Walker

ABSTRACT Constrained Manipulators and Contact Force Control of Contour Following Problems by Han-Pang Huang Chairman: N. Harris McClamroch A contour following problem involves motion of the end effector of a manipulator from an initial location, which is assumed not to be in contact with the constraint surface S, to a point in S; then the end effector is moved along a specified curve in S with a specified contact force; finally, the end effector is moved back to the starting point. A contour following problem consists of both constrained and unconstrained motions. In order to implement the contour following task, the dynamics of a constrained manipulator are considered as a basis for the study of path and motion planning of a constrained manipulator, and performance simulation. The first part of this thesis focuses on the development of a new manipulator model. Contact forces, which are not taken into account in present manipulator models, are reflected in the manipulator dynamics in new models. We first develop a model for a manipulator system with a bilateral constraint; then we extend the model to the case of a unilateral constraint. In both cases, the equations of motion are derived, and the well

posedness of the new models are justified. In addition, the inverse dynamics problems and direct dynamics problems are solved. The bilaterally constrained manipulator system is related to a "singular" model defined in terms of differential equations and algebraic equations; the unilaterally constrained manipulator system is related to a "nonlinear complementarity" model defined in terms of differential equations and algebraic inequalities. In the second part, planning issues for a constrained manipulator system are discussed. A parameterization approach is used so that the planning problem can be divided into path planning and motion planning. The path planning problem is to select a suitable path function so that the constraint is satisfied. The motion planning problem is to generate the motion sequence and to compute the joint forces/torques. Both a kinematic approach and a minimum-time approach to motion planning are discussed. The third part considers simulation methodology for constrained manipulator systems. Simulation algorithms for both bilaterally constrained and unilaterally constrained manipulators are addressed. Impact and transition handling are included. Simulation difficulties are also discussed. Finally, an example contour following task is implemented. The results of previous chapters are applied and computer simulation is carried out to validate the responses.

~ Han-Pang Huang 1986 All Rights Reserved

To my Father and Mother Te-Chin and Ching-Hsing Huang -ii -11

ACKNOWLEDGEMENTS I would like to express my sincere gratitude to my advisor, Professor N. Harris McClamroch for his kind support, persistent patience, and thoughtful guidance during the research and development of the thesis. Sincere thanks are also extended to the members of doctoral committee, Professor Pierre T. Kabamba, Professor Kang G. Shin, and Professor Michael W. Walker for their generous suggestions and constructive criticism. I am also indebted to Dr. Neil David Mckay who gave generously of his time in conversations on the implementation of phase plane technique. In addition, I wish to thank my government, Taiwan, the Republic of China, and CRIM (Center for Research and Integrated Manufacturing) in The University of Michigan for their financial support. Special thanks go to my friends Kuo-Fu Lin, Su-Yueh Pan, Ruey-Fu Ho, and Shwu-Yun Tsay who lifted my spirits when they are low, especially during the inevitable moments of frustration. Finally, I thank my parents to whom this thesis is dedicated for their loving support and encouragement; I shall always be grateful.

TABLE OF CONTENTS DEDICATION....................................................................................................... ii ACKNOWLEDGEMENTS................................................................................ iii LIST OF FIGURES................................................................... vii CHAPTER 1. INTRODUCTION.................................................................................. 1 1.1. Motivating Task —Contour Following Problems............................... 2 1.2. Concept and Significance of Constrained Manipulators................... 5 1.3. Related work........................................................ 9 1.4. Objective and Outline of Dissertation.............................................. 11 2. MODELS OF BILATERALLY CONSTRAINED MANIPULATORS............................................................................... 13 2.1. Assum ptions...................................................................................... 13 2.2. Lagrangian Formulation for Manipulator Dynamics........................ 14 2.3. Well-Posedness of the Model............................................................. 18 2.3.1. Solution Concept.................................................................... 18 2.3.2. Inverse Dynamics Problem............................................... 19 2.3.3. Direct Dynamics Problem...................................................... 22 2.3.4. Remarks................................................................................ 23 2.4. Constraint Elimination and Reduction Transformation.................. 23 3. MODELS OF UNILATERALLY CONSTRAINED MANIPULATORS................................................................................. 27 3.1. Assum ptions...................................................................................... 27 3.2. Lagrangian Formulation for Manipulator Dynamics........................ 29 3.3. Transition and Impact Issues.................................................... 35 3.3.1. Entry Time and Impact........................................................ 36 3.3.2. Exit Time............................................... 42 -iv

3.4. Well-Posedness of the Model........................................................... 43 3.4.1. Solution Concept.................................................................... 43 3.4.2. Inverse Dynamics Problem................................. 44 3.4.3. Direct Dynamics Problem.................................................. 47 4. PLANNING FOR CONSTRAINED MANIPULATORS.............. 50 4.1. Definition of a Planning Problem.................................................... 4.2. Path Planning........................................................ 54 4.2.1. Properties of Parameterization Functions............................. 54 4.2.2. Selection of Path Function................................................... 56 4.3. Motion Planning............................................................ 57 4.3.1. Conditions for Avoidance of Impact...................................... 60 4.3.2. Kinematic Approach to Motion Planning............................. 61 4.3.3. Minimum-Time Approach to Motion Planning..................... 64 4.3.3.1. Limits on Path Acceleration.................................... 65 4.3.3.2. Minimum Time Problem Formulation..................... 67 4.3.3.3. Phase Plane Technique for Minimum Time problem........................................................ 72 5. SIMULATION FOR CONSTRAINED MANIPULATORS.......... 76 5.i. Review of Software Packages for Bilaterally Constrained Systems....................................................................... 77 5.2. Simulation for Bilaterally Constrained Manipulators....................... 78 5.2.1. Concept of Index........................................................ 79 5.2.2. Simulation Models........................................................ 80 5.2.3. Simulation Methods........................................................ 82 5.2.4. Difficulties in Simulation and Proposed Solution.................. 87 5.2.5. Other Simulation Methods.................................................... 91 5.2.6. Remarks............................................................................... 92 5.3. Simulation for Unilaterally Constrained Manipulators.................... 93 5.3.1. Simulation for Unconstrained Motion.................................. 93 5.3.2. Simulation for Constrained Motion....................................... 94 5.3.3. Transition Procedures........................................................ 95 5.3.4. Complete Program Arrangement........................................... 100 6. A CONTOUR FOLLOWING EXAMPLE........................................ 103 6.1. Description of a Contour Following Task........................................ 103 6.2. Planning Issues for Contour Following Problems............................. 106 6.2.1. Path Planning........................................................ 106 -V"

6.2.2. Motion Planning......................................................108 6.2.3. Remarks................................................................................. 114 6.3. Simulation............................................................... 115 6.3.1. Simulation Examples........................................................ 116 6.3.2. Remarks...................................................... 118 7. CONCLUSIONS................................................................................... 50 BIBLIOGRAPHY................................................... 155 -vi

LIST OF FIGURES Figure 1.1. A contour following problem...................................................... 3 1.2. A schematic diagram of contour following problem.................................. 5 3.1. Three-segment manipulator motion, t -= entry time, t2 = exit time. (a) Impact occurs at t1. (b) Impact does not occur at t 1........................................ 36 4.1. A minimum-time trajectory with three switching points.......................... 75 5.3.1. Compute an entry time te. (a) Interpolation. (b) Extrapolation..................................................................................... 9 5.3.2. Compute an exit time tL....................................................................... 98 5.3.3. M ain Program............................................................................................ 100 5.3.4. Procedure MSU........................................................................................... 101 5.3.5. Procedure MSC................................................................. 102 8.1.1. Two-degree-of-freedom Cartesian robot................................................... 104 6.2.1. Path with discontinuous slopes at the entry and exit times..................... 119 6.2.2. Path with continuous slopes at the entry and exit times.......................... 120 6.2.3. Phase plane plot of kinematic approach with Fig.6.2.1 path.................... 121 6.2.4. Displacement plot of kinematic approach with Fig.8.2.1 path.................. 122 6.2.5. Joint forces plot of kinematic approach with Fig.6.2.1 path..................... 123 8.2.8. Phase plane plot of minimum-time approach with Fig.6.2.1 path............ 124 6.2.7. Displacement plot of minimum-time approach with Fig.8.2.1 path.......... 125 *Vi-I

6.2.8. Joint forces plot of minimum-time approach with Fig.6.2.1 path............. 126 6.2.9. Phase plane plot of kinematic approach with Fig.6.2.2 path.................... 127 8.2.10. Displacement plot of kinematic approach with Fig.6.2.2 path.............. 128 6.2.11. Joint forces plot of kinematic approach with Fig.8.2.2 path..................... 129 8.2.12. Phase plane plot of minimum-time approach with Fig.6.2.2 path............ 130 8.2.13. Displacement plot of minimum-time approach with Fig.6.2.2 path.......... 131 8.2.14. Joint forces plot of minimum-time approach with Fig.6.2.2 path.............. 132 8.3.1. Displacement x without disturbance................................................. 133 6.3.2. Displacement y without disturbance........................................ 134 6.3.3. Velocity i without disturbance..................................... 135 8.3.4. Velocity j without disturbance......................................................... 136 6.3.5. Contact force X without disturbance................................................... 137 6.3.6. Displacement x with disturbance due to inexact radius............................. 138 8.3.7. Displacement y with disturbance due to inexact radius........................... 139 6.3.8. Velocity i with disturbance due to inexact radius................................. 140 6.3.9. Velocity y with disturbance due to inexact radius................................ 141 6.3.10. Contact force X with disturbance due to inexact radius............................. 142 6.3.11. Path and constraint for the inexact radius case..................................... 143 6.3.12. Displacement x with disturbance due to Coulomb friction....................... 144 6.3.13. Displacement y with disturbance due to Coulomb friction...................... 145 *- rll1 or ~~~~~~~~~144

6.3.14. Velocity i with disturbance due to Coulomb friction.............................. 148 6.3.15. Velocity j with disturbance due to Coulomb friction.............................. 147 86.3.18. Contact force X with disturbance due to Coulomb friction..................... 148 6.3.17. Path and constraint for the Coulomb friction case................................... 149 mIX ~ ~ ~ ~ ~ ~ ~~~~4

CHAPTER 1 INTRODUCTION Industrial robotic manipulators have recently been given increased attention. In particular, manipulators are indispensable parts of assembly automation plants. There is no universal definition of a robotic manipulator. In general, a robotic manipulator is a mechanism, usually consisting of a series of links, joined or sliding relative to one another, for the purpose of grasping and moving objects in several degrees of freedom. It can be programmed and controlled to perform some manipulation tasks. The tasks performed by a manipulator are accomplished by the manipulator "end effector." The end effector is a gripper, tool, actuator, or other mechanical device attached to the end of the manipulator by which objects can be grasped or acted upon. Manipulators have been widely used for pick-and-place operations in industry. But, they are seldom applied to perform deburring, seam grinding, and seam tracing tasks due to a lack of development of the manipulator technology in these areas. These applications of manipulators are examples of "contour following problems." This thesis is motivated by these types of contour following problems. The motivating task is discussed in detail as follows. -1

-21.1. Motivating Task-Contour Following Problems A contour following problem usually involves a path constraint, which describes a path or contour to be followed by the end effector of the manipulator, and a desired contact force, which is generated from the interaction between the end effector of the manipulator and the constraint surface. The path and manipulator dynamics of a contour following problem are modeled as stochastic processes by Whitney and Edsall [86]. An edge-following problem is discussed by Starr [75]. However, they do not explicitly model the contact force in their approaches; instead, they use force sensors to measure the contact force, and then use the measured force to control the manipulator motion. In addition, they assume the end effector of the manipulator is initially on the constrained path; but that is not the usual case. In this sense, they have not treated contour following problems in a fundamental way. More accurately, a specific contour following problem can be stated as follows: Assume the end effector of the manipulator is not initially in contact with a constraint surface S (see Fig.l.1), then (1) move the end effector from its initial position p to point a in surface S; (2) move the end effector along a curve C in S from point a to point b so that it has a given contact force with S; (3) move the end effector from point b back to its initial position p. For a contour following problem, the motion of the manipulator consists of two parts: an unconstrained motion, where the end effector is not in contact with the constraint surface S (i.e., from point p to point a and from point b to point p in Fig.1.1), and a constrained motion, where the end effector is in contact with the constraint surface S (i.e., from point a to point b in Fig.1.l). When the manipulator performs an

..3-.. N, Fig. 1.1 A contour following problem unconstrained motion, it is referred to as an open kinematic chain manipulator. On the other hand, when the manipulator performs a constrained motion, where the contact force must be considered, it is referred to as a closed kinematic chain manipulator. In typical contour following problems, the open chain manipulator dynamics, the constrained path and contact force, and the initial and final positions of the manipulator are given. However, a question arises: how can the manipulator perform the contour following problem efficiently? In order to answer this question, several issues should be considered: (1) Modeling: A suitable model to describe the contour following problem is required. When the end effector enters the constraint surface, an impulsive impact force may occur at that instant. It is desirable to characterize this impulsive impact force as a part of the manipulator dynamics. Moreover, when the manipulator moves on the

-4constraint surface, its end effector interacts with the constraint surface. The interaction effect and the contact force due to this interaction should be reflected in the manipulator model. (2) Planning: A motion planner should generate the manipulator input to achieve the desired manipulator motion and the desired contact force, based on the manipulator model developed in (1). Thus, the manipulator follows the prespecified path and exerts a specified contact force on the constraint surface. It may be desirable to perform the contour following task according to some optimality criteria, such as in minimum time. (3) Feedback control: A feedback controller [40,41,62,77,79,851 should correct for deviations of the manipulator motion due to outside disturbances and uncertainties of the manipulator dynamics. An appropriate controller should be able to control the manipulator motion and the contact force simultaneously. The existence of the contact force presents a challenge to feedback controller design. (4) Simulation: When each part of the contour following problem has been specified, simulation may be required to verify the performance of the task. It will be seen later that the manipulator model of the contour following problem consists of a set of differential equations and algebraic equations (or algebraic inequalities). This characteristic presents many difficulties in simulation. The above four ingredients of a contour following problem are shown in Fig. 1.2. The contour following problem described above is dealt with in this thesis. The main focus is on modeling, planning and simulation; feedback controller design is not studied in this thesis.

MSmanipulat dired tul manipultor manipulator manipulator dynamics Motion motion + Constrained motion -— m-p lannert _ Manipulator tual assumptions contact n on costrained force path and contact force Controller Fig. 1.2 A schematic diagram of contour following problem 1.2. Concept and Significance of Constrained Manipulators The contour following problem poses an interesting point: the manipulator end effector can not move arbitrarily; instead, the manipulator motion must satisfy certain path constraints. In other words, the manipulator is constrained. Suppose the constraint surface S is defined by an equality (p) 0, (1.2.1) where O:R -R 1, and pER" denotes the position vector of the end effector. The equality Eq.(1.2.1) defines a bilateral constraint. A manipulator constrained by the bilateral constraint, Eq.(1.2.1), is referred to as a bilaterally constrained manipulator. On the other hand, if the path constraint is given by an inequality

-84(p ) >0, (1.2.2) this path constraint is called a unilateral constraint. It means that when the end effector is not in contact with the constraint surface, the constraint can be ignored; however, when the end effector is in contact with the constraint surface, there exists a contact force and the constraint must be regarded as an intrinsic part of the manipulator dynamics. A manipulator constrained by the unilateral constraint, Eq.(1.2.2), is called a unilaterally constrained manipulator. We use constrained manipulators to represent both bilaterally constrained manipulators and unilaterally constrained manipulators. In contrast, an open kinematic chain manipulator is regarded as an unconstrained manipulator. In this sense, a manipulator which performs a contour following task can be characterized by a unilaterally constrained manipulator. Throughout this thesis bilaterally constrained manipulators and closed chained manipulators share the same meaning. It is clear that the concept of constrained manipulators and contact forces are major issues for contour following problems. We further discuss why previous approaches to constrained manipulators are inadequate and discuss the significance of constrained manipulators and contour following problems. Modeling, planning and simulation are further elaborated. There has been considerable research on the dynamics of unconstrained manipulators. However, much of that research is based on the assumption that the manipulator does not interact with its environment in any significant way. When there exists contact between the end effector and the constraint surface, the contact force may be measured by a force sensor and used to control the manipulator motion [47,56,58,62,77]. This approach is inadequate for several reasons:

-7* the contact force is not explicitly expressed as part of the manipulator dynamics; hence, simultaneous control of motion and force is neither easy nor accurate; * the force sensor is usually modeled as a spring, with the sensed force proportional to the difference between desired manipulator displacement and the actual manipulator displacement, i.e., the measured force depends only on the manipulator displacement; such a view is inappropriate for hard contact between the end effector and a constraint surface which is not compliant; * it is not always practical to install a sensor at the exact tip of the manipulator to measure the contact force; most force sensors are located at the wrist of the manipulator [4,77,85] rather than at the very end of the end effector; the wrist is a noisy place to make measurement; thus, the measured force does not represent the true contact force. We argue that the contact force must be modeled as a part of the manipulator dynamics. Since the analysis and control of the contact force are critical, a suitable model for a constrained manipulator should meet the following requirements: - explicit consideration of contact constraints, * explicit consideration of contact forces, * useful for motion/force planning, * useful for sensor-based feedback control, * useful for understanding interaction between the manipulator and task. Planning for constrained manipulators involves not only specification of desired manipulator motion (i.e., displacement, velocity, and acceleration time history of the end effector) but also specification of desired contact force time history. Given such specifica

-8tions which satisfy the imposed constraint, the joint torques can be computed so that the manipulator satisfies these desired specifications. The joint torque computations require solution of the inverse dynamics problem as well as solution of the direct dynamics problem [45]. If a cost criterion is used to evaluate the planning performance for a constrained manipulator, an optimal planning problem results. In this case, the manipulator motion is to be determined to satisfy an optimality criterion, and the contact force may be pre-specified. Computer simulation can be used to evaluate the dynamic performance of a constrained manipulator. However, a bilaterally constrained manipulator consists of a set of differential equations and an algebraic equation; numerical simulation for this type of differential algebraic equations (DAE) system is quite difficult. Since a DAE system is inherently stiff, most ordinary differential equation solvers can not be used. On the other hand, a unilaterally constrained manipulator consists of a set of differential equations and an algebraic inequality; numerical simulation is even more difficult than a bilateral constraint case. Difficulties come from several factors: (i) two types of equations of motion of the manipulator system- when the manipulator end effector is not in contact with the constraint surface, the equations of motion of the manipulator are described by a set of differential equations; when the manipulator is in contact with the constraint surface, the equations of motion of the manipulator are governed by a DAE system; (ii) transition time- it is difficult to determine when the manipulator transits from an unconstrained motion to a constrained motion, and vice versa; (iii) impact issues- when the manipulator transits from an unconstrained motion to a constrained motion, impact is possible. Many other applications of manipulators, such as, cutting, drilling, grinding, scribing, drawing, and insertion have the same features as the contour following problem.

However, at present, manipulators are seldom apl are that we know very little about constrained m control the contact force. Thus, solution to the coi solution approach to many other similar operattol ing problem may also prove useful for study of coi izes the ability of a manipulator to react to interac types of compliance- manipulator compliance ame Manipulator compliance arises from the manipu] tor compliance. Whitney's RCC (Remote Cente]

-10a formal model is not given. Hemani and Wyman [25], Hemani and Weimer [26] give a model for a biped locomotion system, which takes into account the contact forces. Again they do not explore the underlying idea for a closed chain manipulator. McClamroch and Huang [45] derive a model for a closed chain manipulator. There, the well-posedness of the model was proved and the closed chain manipulator system was related to a socalled singular system [11]. McClamroch [46] further elaborates this approach and extends it to a multi-manipulator case. Zheng and Luh [89] explore a seven joint manipulator, where a closed chain is formed through the link designs of the manipulator (rather than formed through contact between manipulator end effector and the constraint surface). West and Asada [831 also consider a contact problem, where the contact occurs between a manipulator link and the constrained environment rather than the end effector and the constraint environment. Recently, Kankaanaanta and Koivo [31] have derived a closed chain manipulator model with consideration of friction. But, they do not show the well-posedness of their models. Although modeling a bilaterally constrained manipulator has only recently attracted attention in the robotics area, it has been welldiscussed in mechanics area [22,24,87]. Two well-known closed chain mechanical examples are: a four bar linkage and a slider crank mechanism. In contrast to a bilaterally constrained manipulator, the dynamics of a unilaterally constrained manipulator are even less well developed. In the robotics area, Zheng and Hemani [90] describes impulsive impact effects when the foot of a biped system hits the ground. In the mechanics area, several Russian papers [76,91] discuss mechanical systems with unilateral constraints. They treat unilateral constraints as additional generalized coordinates; then they investigate the properties of the unilaterally constrained system, based on the augmented generalized coordinates. A good discussion of a mechanical system with unilateral constraints is given by Lotstedt [38]. He relates a unilaterally

-11constrained system to complementarity problems [14,32]. But he makes some unusual assumptions about the system when he proves the well-posedness of the system model. Nevertheless, to our knowledge, a formal system model for a unilaterally constrained system has not been derived, nor have such problem been examined from a control point of view. Since the dynamics of constrained manipulators are not well understood, the motion planning issue for constrained manipulators has not been yet explored. As to simulation issues for constrained manipulators, most work has focused on numerical procedures for a bilaterally constrained manipulators [2,6,7,50,80], except [63]. There are still many difficulties in simulation due to the existence of algebraic equations in the system. Specially, a bilaterally constrained manipulator system is an index three singular system (this is shown in Chapter 5), which is a high index system; high index characteristics present a lot of difficulties in simulation. Simulation for a unilaterally constrained manipulator is even more difficult than a bilaterally constrained manipulator due to possible impulsive impact effects. Lotstedt [39] formulates unilaterally constrained dynamics as a quadratic programming problem. But impulsive impact is not handled there; it is not clear if his approach is suitable for a unilaterally constrained manipulator. 1.4. Objective and Outline of Dissertation The objective of this thesis is to discuss a typical contour following problem and associated constrained manipulator problems. Some known results in several fields are incorporated and related to constrained manipulators. Analysis of contact force and interaction between the manipulator end effector and the constraint surface are discussed in detail. Also, an integrated consideration of a complete constrained manipulator

-12system- modeling, planning, and simulation- are presented in this thesis. The remainder of this thesis is organized as follows: Chapter 2 derives a model for bilaterally constrained manipulators, and discusses the well-posedness of the model. Chapter 3 derives a model for unilaterally constrained manipulators, and discusses impact issues and the well-posedness of the model. Chapter 4 presents an approach to planning issues for constrained manipulators, and describes associated difficulties. Chapter 5 gives a detailed discussion of simulation issues for constrained manipulators, and describes associated difficulties. Chapter 6 applies the results developed in Chapter 2 through Chapter 5 to a contour following example. Chapter 7 concludes this thesis, and points out some possible extensions of this work.

CHAPTER 2 MODELS OF BILATERALLY CONSTRAINED MANIPULATORS In this chapter, we derive a model for a bilaterally constrained manipulator. The model is complete in the sense that it characterizes not only the manipulator dynamics but also the contact dynamics. In fact, contact between the end effector and the constraint surface is maintained by a contact force. We also show that the model is wellposed by considering the inverse dynamics problem and the direct dynamics problem. The development of this chapter is primarily based on our previous paper [45], where multiple bilateral constraints have been considered. 2.1. Assumptions In order to derive a mathematical model for a bilaterally constrained manipulator, several assumptions about the manipulator and the constraints are made. Manipulator Assumptions: (Al) The Lagrangian function L: R" XR" — R is continuously differentiable with the form L(q,q)=K(q,q)- V(q ), (2.1.1) where -13

-14K(q,') =. TM(q)4 (2.1.2) is the manipulator kinetic energy with the inertial matrix function M(q) which is continuously differentiable with a continuously differentiable inverse; and V(q) is the manipulator potential energy. (A2) The kinematic transformation for joint coordinates to Cartesian coordinates H: R m -RR " has three continuous derivatives and is invertible. (A3) The Jacobian matrix J(q)- aH (q) is nonsingular. (A4) The scalar defined by A (q)-D(H(q))J(q)M-l(q)JT(q)D T(H(q)) (2.1.3) is nonzero, with a continuously differentiable inverse. Constraint Assumptions: (A5) The end effector satisfies b(p )=0, and the constraint function b:R" -+R1 is twice continuously differentiable with non-zero gradient, i.e., the constraint surface is a smooth surface. (A6) Contact between the end effector and the constraint surface occurs at a point; it is assumed that the constraint surface is frictionless. With these assumptions, we use Hamilton's principle [22,24,87] to derive a model for the manipulator with a bilateral constraint. 2.2. Lagrangian Formulation for Manipulator Dynamics The Lagrangian formulation for the bilaterally constrained manipulator model is based on the Lagrangian function L (q,). Since we tacitly assume that the manipulator

-15system is holonomic and conservative, Hamilton's principle can be directly applied to the system to derive the equations of motion. Let pER" denote the position vector of the end effector of the manipulator, in terms of a fixed workspace coordinate system. Suppose that a constraint on the end effector is given as (p ) = O. (2.2.1) A closed kinematic chain is formed through continuous contact of the end effector of the manipulator with the the constraint surface. Then, the frictionless constraint surface S is given by S= pERn: (p)=o}. (2.2.2) If Po is a point on 5, then we can define the normal space of S at po as N(po) = { P:P = aVB(po), aER' I (2.2.3) and the tangent space of S at po as T(po) = {:p y = 0, EN(.o) (2.2.4) N (p ) and T (p ) are orthogonal subspaces of R n so that R "= T(po) D N(po). (2.2.5) We use qER" to denote the position vector of robot joint coordinates; thus, the relation between robot coordinates and workspace coordinates can be expressed as p = H(q). (2.2.6)

-15In general, it is convenient to define the manipulator equations of motion in terms of the joint coordinates [34,52,58,78]. We adopt this convention in our derivation. In order to apply Hamilton's principle to derive the manipulator model, we form the action integral I of the manipulator system. The action integral is I = L( q,)dt. (2.2.7) to Since the manipulator is constrained by the holonomic constraint b(p )=O, we adjoin this constraint to the action integral by a Lagrange multiplier X. The augmented action integral I becomes I- f L(q,) + X(p)]. dt(2.2.8) to Taking the variation of I yields QI = i q [ Sa + a — 6 + \ d p[ dt i 9q Qq 9p \ (2.2.9) (p) a( ) (2.2.10) f' _L d Lt + JT( )D (p)X=\, where J(a) and D ns daa iam s adeined choose X so that aqn dt <fq

-17where Jn(q) is the n-th column of matrix J(q). The remaining 6q%, i=l,...,(n-l) are free variations such that the constraint is satisfied, and hence the coefficient of each 6q, must vanish separately. Consequently, we have the equations of motion as d dL aL T+JT(q)D (p)X, (2.2.11) dt Da d q and the constraint b(p) = 0, (2.2.12) where T is the input joint torque. Using (2.1.1) and (2.1.2) in (2.2.11), we obtain M(q)q + F(q,q) = T + JT(q)D T(p), (2.2.13) where F(qq)= [d M(q)] - aL(q ) consists of the Coriolis term, the centrifugal term, and the gravitational term; the second term of the right hand side represents the torque required to maintain satisfaction of the constraint q(p )=0. Define the contact force due to the constraint as f = D (p)\. (2.2.14) Then, the complete set of equations of motion is obtained as M(q)q4 + F(q,q) = T + JT(q)f, (2.2.15) f =D T(p), (2.2.16) p =H(q), (2.2.17) q(p) =0. (2.2.18) Since the equations of motion of a constrained manipulator consist of a set of differential equations and an algebraic equation, they can also be written in the form

-180 I 0 d- = M-l(q)(T+JT()D T (H(q))X.-F(q,q)) (2.2.19) 0 001 Xt O(H(q)) Since the coefficient matrix multiplying the derivatives is singular, these equations have been referred to as nonlinear singular equations or nonlinear equations in descriptor form [11,71]. Note that the contact force vector f is implicitly defined through the algebraic equation. The contact between the end effector and the constraint surface is maintained by the contact force. This requires that the velocity vector p lie in the tangent space of S and the contact force vector f lie in the normal space of S. In other words, the degree of freedom of position (velocity) is defined in the tangent space, and the degree of freedom of force is defined in the normal space. Since these requirements are due to contact, they are referred to as "natural constraints" on the velocity of the end effector and on the contact force. 2.3. Well-Posedness of the Model In order to justify the well-posedness of the manipulator model developed in the previous section, the solution concept of the manipulator system is first discussed. Then, the inverse dynamics problem and the direct dynamics problem of the constrained manipulator are investigated. 2.3.1. Solution Concept Since the bilaterally constrained manipulator system is modeled as a nonlinear singular equation, the solution concept is defined in terms of Eq.(2.2.19). We define the solution concept of the bilaterally constrained manipulator system in terms of consistent

-19initial values and solvability of the system. Definition 2.3.1: A set of initial values (qo,?o) is said to be consistent for the bilaterally constrained manipulator system, Eq. (2.2.19), if q(H(qo))=O and D (H(qo))J(qo)q0=O; namely, qoES and J(qo)qE T(H(q )). Definition 2.3.2: The bilaterally constrained system, Eq.(2.2.19), is solvable if for each set of consistent initial values (go, go) the system (2.2.19) has a unique solution q(t) for to<t <t1, satisfying q(to)=qo and 4(to)=qo. With this solution concept, we discuss the well-posedness of the bilaterally constrained manipulator system in the next two sub-sections. 2.3.2. Inverse Dynamics Problem As far as the well-posedness of the manipulator model described by Eqns. (2.2.15) —(2.2.18) is concerned, the inverse dynamics problem should be considered first. The inverse dynamics problem can be stated as follows: Given the input joint torque vector and consistent initial values, what is the manipulator motion and the corresponding contact force, which satisfy Eqns. (2.2.15)~(2.2.18) or (2.2.19)? Calculation of the contact force plays an important role in this problem. We state an existence and uniqueness theorem for the inverse dynamics problem as follows.

-20Theorem 2.3.1: Given the torque vector T(t )ER piecewise continuous on to<t<tf and consistent initial values (qo, o), then with assumptions (A1)-(A5) there ezists a unique motion q(t) (and p (t)) and a unique contact force vector X(t) satisfying (2.2.15) —(2.2.18) for to<t <tf (assuming there is no finite escape time). Proof: Our objective is to obtain an expression for the contact force which guarantees satisfaction of the path constraint. To this end, suppose q(p (t ))=O so that d(p(t)) _D ( ) d ((t )) D (p) +D(p,)p =, d~ddt2 where D(p,P) dt D (p). Since M(q) is nonsingular, q is obtained as q = M- (q)[ T - F(qg,) 1 + M-l(q)JT(q)f The velocity and acceleration of the end effector in workspace coordinates are - = J(q)), = J(q,) + J(q)}, where J(q,q) d J(q). These equations, together with (2.2.16), lead to A(q)X = D (H(q))J(q)M-l(q)[ F(q,q)- T - [D(H(q))J(q,q) + D(H(q),J(q )q)J(q) ] q. This is a linear equation for the scalar multiplier X; since A(q) is non-zero, X can be uniquely determined as a linear function of q,q,T X = g(q,q, T), (2.3.1)

-21where g (q,,T) =A -'(q )D (H(q))J(q )M-'(q )[ F(q,q) - T] (2.3.2) - A -'(q)[ D (H(q))J(q,q) + D(H(q ),J(q )q)J(q) ]q. Hence, the resulting contact force is f =D T(H(q))g(q,,T). With the explicit expression of the contact force, Eqns. (2.2.15), (2.2.16) reduce to the following initial value problem: M(q)q + F(q,q) = T + JT(q)D T(H(q))g(q,,T),(2.3.3) (2.3.3) q(to) = qo0, (to) = 0. From the stated assumptions, this initial value problem has a unique solution q(t) defined for to<t <to + 6 for some 6>0 [13]. Assuming there is no finite escape time, the solution is defined for t < t <t. Next, we show that the constraints are satisnlea turougnout tue motion. 6ince the contact force X = g(q,,T), t<t tf, it can be verified that p (t )=H(q(t)) satisfies d 2(p (t)) o, to<t <t dt - But, since H(qo)ES, J(qo)qoE T(H(qo)) it follows that dk(p(to)) O(P (to)) 0=, dt ---. Thus, we have (p(t))-O, tot<t tf. The basic idea of this proof is to develop a nonsingular differential equation, namely, equation (2.3.3), which characterizes the motion within the constraint surface.

-22An important part of this development is that the contact force is explicitly given by Eqns. (2.3.1) and (2.3.2). In the above, we have shown that under the stated assumptions the inverse dynamics problem is well-posed. The assumptions (A1)-(A5) have been stated in their most obvious form, but those assumptions can be relaxed in several ways. 2.3.3. Direct Dynamics Problem In order to complete the justification of the well-posedness of the manipulator model, the direct dynamics problem is addressed. The direct dynamics problem is stated as follows: Given a specified motion of the manipulator and a specified contact force satisfying the imposed constraint, what is the input joint torque vector which would generate this specified motion? It is a problem of motion synthesis. The following theorem gives the relationship between the input joint torque and the contact force. Theorem 2.3.2: Given the contact force X(t) (f (t)) for t ot <tf and the motion q(t) which is twice continuously differentiable and satisfies e(H(q(t))) = 0 for to<t <tf, then under assumptions (A 1).(A 3) and (A 5) there exists a unique input joint torque vector T(t) satisfying (2.2.15) for to0t <tf t Proof: From (2.2.15), (2.2.16), we have M(q)q + F(q,) =- T + JT(q) D T(H(q)). Clearly, given q(t) and X(t), to0t < t, there is a unique torque vector T(t) for toQt <t satisfying this equation. *

-23. As indicated in the proof, for a specified contact force function, the torque T(t) is uniquely defined. If the contact force X(t) is not specified, there are many input joint torque vector functions and contact force functions which generate the same given motion. The case of an open kinematic chain (unconstrained) manipulator corresponds to the assumption that the contact force X(t )=O for to0 t < tf so that the torque T(t) is uniquely determined. 2.3.4. Remarks Although contact between the end effector and the constraint surface is maintained by the contact force, the contact force can be zero. If the input joint torque vector T satisfies D (H(q ))J(q )M(q )T=D (H(q ))J(q )M-'(q )F(q,) (2.3.4) -[ D(H(q))J(q,4)+D(H(q),J(q)q)J(q) ], then the contact force is equal to zero. This special case is referred to as the degenerate case of the contact force. In the above, we have carefully developed a general model for a bilaterally constrained manipulator, and we have shown that the direct and inverse dynamics problems are well-posed. We believe that the constrained manipulator model is the appropriate model to use in control design and analysis, where the manipulation task involves contact between the manipulator end effector and a fixed constraint. 2.4. Constraint Elimination and Reduction Transformation It is possible to eliminate the path constraint and the contact force from the dynamic model of the bilaterally constrained manipulator by a suitable development.

-24However, the resulting equations are extremely complicated; moreover, elimination of the contact force from the model may not be desirable if the contact force represents a variable to be controlled. Although the elimination of the constraint may not always be desired in control analysis, it does provide certain theoretical and computational advantages in cases where the contact force need not be computed. Two approaches to eliminate the constraint are discussed. The first approach is based on expressing the contact force X in terms of the position q, the velocity g, and the input joint torque T; then this expression is substituted in Eq.(2.2.15) to eliminate the constraint. This approach was used in the proof of the inverse dynamics problem. As in that development, the nonsingular manipulator dynamics become M(q )q + F(q,q) T T + J (q)D T(p )g(q,,T), (2.4.1) where g(q,q,T), defined in Eq.(2.3.2), is the contact force required to maintain satisfaction of the constraint. The second approach is based on use of the implicit function theorem [15] to partition the joint space coordinates into a set of independent and dependent coordinates; then the constraint equation can be explicitly solved. Suppose that there exists a function & such that q,= to(q) and (H qIIl))) = - 0 for all qIIER-1 Differentiating the constraint, we have D(p)J(q) = 0, or

-25Bi(q )ql + Bl(q 1)ql 0, (2.4.2) where [B (q )] (p)J(q ). Assume that B (q) is nonzero, then qI can be expressed in terms of II/ as q = -B1 l( q )B1I (q )qlI. (2.4.3) Similarly, qI can be obtained by differentiating Eq.(2.4.3) as = -Q(q,,q)q - Q(q )'i', (2.4.4) where Q(qq) = d [-B 1( )B(q)], Q(q) = -B -1(q)B (q)] Now we decompose Eq.(2.2.15) in the same way as Mll(q)ql + M12(q)ql' + F/(q,-) = Tl + BI T(q)X, (2.4.5) M21(q )ql + M22(q ))q' + FI(q,q) = T1! + B11 T(q)X. (2.4.6) Since B1(q) is non-zero, we can solve for X from Eq. (2.4.5) as X = B- (gq)[Mll(qf) +Ml2(q)u, + F(q,q) - T,]. (2.4.7) Using Eqns.(2.4.4), (2.4.7) in Eq.(2.4.6) and re-arranging, we obtain {M= q -M2(q)Q(q)-Bt T(q)B-(q) [M2-M(q)Q()] q +Fl (qq) -BII r(q)B-rT(q)[FI(q,q-Mll(q)Q(q,q)qll-M21(q)Q(qq)q (2.4.8) -= T - Bn T( )BI-r(Q )TI,

-26where q ( — = qL', 1. This equation is a second order differential equation in the n-1 vector q1 only. In general, it is difficult to find a global function $(q) satisfying b4H4j(q q ))l = 0 for all qjj R E -. In summary, each of the above methods can be used to transform the original singular equation into a nonsingular differential equation. The first approach was used primarily in the proof of theorem 2.3.1, but the explicit expression for the contact force is complicated and the order of the system is not reduced. The second approach is particularly useful in that the order of the system is reduced. But, a global expression for the independent coordinates in the this approach may be difficult to obtain. These constraint elimination methods will be further discussed in Chapter 5.

CHAPTER 3 MODELS OF UNILATERALLY CONSTRAINED MANIPULATORS In the previous chapter, we developed a model for a manipulator with a bilateral constraint. The model can be extended to the case of a manipulator with a unilateral constraint. A unilateral constraint can be used to characterize the manipulator motion where the end effector may or may not be in contact with a constraint surface. In other words, the constraint condition, defined as an inequality, can be active or inactive. Since many manipulation problems in practical application, such as contour following problems are of this type, a unilaterally constrained manipulator model is appropriate to describe these types of manipulations. In this chapter, we derive a model for a unilaterally constrained manipulator, based on Hamilton's principle. Assumptions about the manipulator and the constraint, impact issues, and the well-posedness of the model are discussed in the subsequent sections. 3.1. Assumptions In order to derive a mathematical model for a unilaterally constrained manipulator, several assumptions about the manipulator and the constraints are made. -27

-28Manipulator Assumptions: (Al) The Lagrangian function L: R" XR" -*R 1 is continuously differentiable with the form L (q,q) -K(q,q) - V(q), (3.1.1) where K(q,i) = I TM(q)q (3.1.2) is the manipulator kinetic energy with the inertial matrix function M(q) which is continuously differentiable with a continuously differentiable inverse; and V(q) is the manipulator potential energy. (A2) The kinematic transformation for joint coordinates to Cartesian coordinates H: R" -R " has three continuous derivatives and is invertible. (A3) The Jacobian matrix J(q) 8 —A(q) is nonsingular. 9q (A4) The scalar defined by A (q) = D(H(q))J(q)M-(q)JT(q) D T(H(q)) (3.1.3) is nonzero, with a continuously differentiable inverse. Constraint Assumptions: (A5) The end effector satisfies 0>(p )>0, and the constraint function q:Rn "-.R1 is twice continuously differentiable with non-zero gradient, i.e., the constraint surface is a smooth surface. (A6) Any contact between the end effector and the constraint surface occurs at a point; it is assumed that the constraint surface is frictionless.

-29. (A7) Any impact of the manipulator colliding with the constraint surface is assumed to be inelastic. With these assumptions, we use the variational approach to derive a model for the manipulator with a unilateral constraint. 3.2. Lagrangian Formulation for Manipulator Dynamics The Lagrangian formulation for the model of a unilaterally constrained manipulator is based on the Lagrangian function L(q,q). The key point in deriving the manipulator model is to convert a unilateral constraint into a bilateral constraint by using a slack variable so that Hamilton's principle can be directly applied. We give special attention to those time instants at which the constraint changes from inactive to active or from active to inactive. The unilaterally constrained manipulator model is derived as follows. Let p ER denote the position vector of the end effector of the manipulator, in terms of a fixed workspace coordinate system. Suppose that the motion of the end effector of the manipulator is constrained by q(p)> 0. (3.2.1) A strict inequality holds when the end effector is off the constraint surface, and an equality holds when the end effector is in contact with the constraint surface. Hence, the feasible region of the end effector, f, can be defined as n= p: { (p) >o, pER". (3.2.2) Every point in n is called a feasible point. We assume that the end effector can not violate the constraint, i.e., k(p)<0 is not allowed. We use Hamilton's principle for a holonomic system to derive the equations of motion of a unilaterally constrained

-30manipulator system. Notations defined in Chapter 2 are also used here. We assume that there are (n+1) motion segments such that q(p(t)) > 0, t2+i<t <t2i+2l, O(P (t)) = t2i+1:t<t2i+2 t (3.203) where to<t1<t2<...<t, <to I+=tI. In short, the manipulator motion is assumed to consist of two types: unconstrained motion where b(p(t))>0, and constrained motion where +(p(t))=O. We assume that k(p(to))>O and q(p (tf ))>0. Consider the Hamiltonian action integral I 3 ti, +1 I f L (q,q)dt f L (q,q)dt, (3.2.4) to i= —Oti+ where ti+ and ti- denote right hand and left hand limits, respectively; to+=to, and tn +1 — tn +1' The inequality constraint (3.2.1) can be converted into an equality constraint by using a slack variable zER 1 as O(p) z2 = 0. (3.2.5) With this transformation of the constraint, we can directly apply Hamilton's principle to the action integral I. Since b(p )-z20 must be satisfied always and the condition q(p(t, ))=0 must be satisfied at each t,, they can be adjoined to I. The augmented action integral I becomes p(t))+ [Ldt, (3. Ire E Ea(p(ti)) + Ie |[L(q, )+ \X(p)- XZ2]dt, (3.2.6) i=O i =0 t.+ where e and X are scalar multipliers. Taking the variation of I yields

-3161= oD(P(ti ))^pi + f f aq+- L +XPD (p)6p-2zz dt. (3.2.7) i=0 - =0ti+ Using the following relations 8L. \dt — i1 f d 9L }q+t,is 9q =[aq J ( dt -aq q ti+ t fi +. q(ti~ ) =qi -(ti~)bti in (3.2.7), we obtain sI = D (P (ti ))^pi+J(qD p + + DT(p)X q-2 Xzz dt +, L(q ) + Xp)- Xz2} )t, - L(q,q) + where J(q) and D (p) are Jacobian matrices defined by J 5q) 1(q D ()6~0 D(p)= 1 and ( )T denotes the transpose. After re-arrangement of terms, we obtain'i;1 c- r eL Idq 6a= S pf)I9L d - L + JT(q)DT(p )\ q-2z6z dt i=Oti+ -q dt ad (3.2.8) + EO[lL I+(ti)-i) (ti- i)]+ [L (q,(4 L(q +- ao''

-32+ [x(P X) — x(pi t,] - [x1 t, — Xz ti, + {-. T,,++ -- + J (q (t,))DT ( (t)) }6qj The stationarity condition of Hamilton's principle gives 6I=0. Consider the term with 6q. Suppose ad(P ) 0; choose X so that aPn L d 8 L + JT(q)D (pX= -O 0 Oqn dt 9qn where J,(q) is the n-th column of matrix J(q). The remaining (n-1) components of 6q are free variations such that the constraint is satisfied; hence, the coefficient of each components of 6q must vanish separately. The same argument can be applied to the last braced term with 6qj. In addition, each 6ti is free variation. Consequently, we have the following conditions: L (q,) d L (q,1 ) + JT(q)D r(p) =-T, (3.2.9) aq- it a4 z - 0, (3.2.10) L (,) +(,) - L (, |,-) + L (q - L (q + X\(_p It - X(p- \(pj+ - Xzt - z t,+ =, _aL(,) I + + L (q,) + JT(q(t,))D T(p(ti)) = O, (3.2.12) and the constraint (p )- 2 = 0; (3.2.13) here T is the input joint torque. Using Eqns. (3.1.1) and (3.1.2) in (3.2.9), it becomes

-33M(q)q + F(q,q)= T + JT(q)D T(q)X, (3.2.14) where F(q,) M(q - is composed of the Coriolis term, the centrifudt J dq gal term, and the gravitational term; T is the input joint torque; and X is the contact force multiplier. Since the second term of the right hand side of (3.2.14) represents the force required to satisfy the constraint qb(p )>0, we define the contact force as f = D (p)X. (3.2.15) Note that the contact force is always directed toward the feasible region 2, this implies that X>0 (3.2.16) always. Multiply Eq.(3.2.13) by X to obtain X^(p)- x2 -. Using (3.2.10) in this equation yields Xq(p) = 0. (3.2.17) This relation states: when the constraint is inactive, i.e., q(p)>0 so that contact between the end effector and the constraint surface is not maintained, then X=0O and thus f =-0. In view of Eq.(3.2.11), notice that if 0(p (t ))>0 for t < ti and q(p (t ))=O for t > t5, then X(ti-)=0, z (ti+)=O, and q(p (ti+))=0. On the other hand, if q(p (t))=O for t < t and (p (t ))>0 for t >ti, then X(t,+)=O, z (t-)=O, and q(p(tj-))=O. Using these relations in (3.2.11), we obtain

-34[L (q (q- )j(,+, aL q, L(q)-. (3.2.18) t,+ atRecall Eqns. (3.1.1) and (3.1.2), Eq.(3.2.18) reduces to V(q(ti-)) = V(q(ti+)) so that the manipulator potential energy is continuous at each ti. Eq.(3.2.12) describes the velocity relations before and after impact. Using Eq.(3.1.2) in (3.2.12) yields M(q(ti))(ti+)- M(q(tj))(tij-) JT(q(ti))D T(p(ti)), (3.2.19) where q is assumed to be continuous at 4t. Since M(q)q has a momentum interpretation, Eq.(3.2.19) can be interpreted as an impulse momentum relation. The change in momentum is given by T (q (t,))D T(p(t,))C. Notice that the multiplier C is associated with the discontinuity of the velocity of the end effector at ti. It will be shown later that S is exactly the magnitude of the impact force at ti, modeled as a "delta function." Therefore, from Eqns. (3.2.14), (3.2.15), (3.2.1), (3.2.16), (3.2.17), and (3.2.19), we can write down a complete set of equations of motion for a unilaterally constrained manipulator as M(q)q + F(q,q) = T + JT(q)f, (3.2.20) j D T(p)X, (3.2.21) p = H(q), (3.2.22) ((p ) >0 (3.2.23) X >0, (3.2.24) X\(p) = 0, (3.2.25) M(q(ti ))[ i(ti+ ) - (tij-)] - JT(q(ti))D T(p(ti )), (3.2.26)

-35where ti is a transition time at which (p (t))> 0, til<t< t; p(p(t))=O, ti <t t ti+l. Eq.(3.2.20) is the dynamic equation of the manipulator. Eq.(3.2.21) gives the contact force. Eq.(3.2.22) denotes the kinematic relation between the end effector position and the robot joint vector. Eq.(3.2.23) together with (3.2.24) and (3.2.25) are referred to as complementarity conditions. Eq.(3.2.26) gives the impact relation. The impact relations not only indicate whether there exists an impulsive impact force at ti or not but also relate to the well-posedness of the manipulator model. In the remainder of this chapter, impact issues are discussed in detail in section 3.3, and the well-posedness of the unilaterally constrained problem is discussed in section 3.4. 1.3. Transition and Impact Issues The feasible region n consists of two parts: (1) an open set where q(p )>0 so that Phe constraint is inactive, and (2) a boundary set where b(p )=0 so that the constraint is tctive. A transition of the manipulator motion occurs when the end effector moves from,he open set to the boundary set or from the boundary set to the open set. The time nstants ti at which the transitions of the manipulator motion occur are called transition imes. Transition times can be characterized by either (1) q(p(t))>0 for ti-e<t <ti md #(p(t ))=0, or (2) q(p(t))>0 for t,<t<ti,+ and b(p(ti))=O, or (3) O(p(t))>0 or ti-e< t < t +e and (p (ti ))=0. These transition times are termed: entry time, exit ime, and transient time, respectively. Since a transient time represents a degenerate ase and is unimportant to contour following problems, it will not be further discussed. or a three-segment manipulator motion example, transition times are indicated in ^ig.3.1. At an entry time, an impulsive impact force may occur; but no impulsive impact

-38force occurs at an exit time. to J OP)>O t2 O(P)>O It o t~t t //// / / AP )<O.P ()<O (.) > Fig. 3.1. Three-segment manipulator motion, t =entry time, t2= exit time. (a) Impact occurs at t 1. (b) Impact does not occur at t. 3.3.1. Entry Time and Impact The entry time is a time at which the inactive constraint on the end effector becomes active, e.g., tj in Fig. 3.1 denotes an entry time. When the manipulator transits from an unconstrained motion segment to a constrained motion segment at an entry time, there may or may not exist an impulsive impact force. In this sub-section, we show that the multiplier ( in Eq.(3.2.26) can be interpreted as the magnitude of the impulsive impact force at the entry time. Then, we derive the velocity of the end effector after impact, an impact occurrence condition, and an impact avoidance condition.

-37First, assume there exists an impulsive impact at the entry time tl. The relations between the magnitude of the impulsive impact force and the change in velocity of the end effector are derived as follows. Let the contact force be x(t) - x(t ) + 6(t-tl), (3.3.1) where X(t )ER1 is continuous, and ER 1 is the magnitude of the impulsive impact force; and 6(t) is the "delta function." Assume that 4(t) is right continuous. If the impact is assumed to occur in the interval [t l-e,tl, where e>0 is sufficiently small, then Eq.(3.2.20) can be re-written as Mii(q ) j + F (q,) =T, + S (X + (t -ti ))Di (p)J; (q) i=i i=l (3.3.2) (i =-l,..,n ), where Mi (q), Di (p), and Ji, (q) are components of matrices M(q), D (p), and J(q), respectively. Integrating the above equation over [t -e,tl and letting e-+0 yields lim I Mii (q)q i dt = lim f Ti - Fi (q,q)+ X(t )Di (p)Jii (q dt j1 + lim f i iDo(p)Jii(q)6(t)dt, (i=l,...n). C Otl/j=l Since the position and the velocity of the end effector, and the input joint torque of the manipulator remain finite during the interval [t 1-,t 1, the first term of the right hand side is equal to zero. Thus, n n _ Mii (q (t 1))[qi (t)- i (t)] = Di (p (t 1))Jii(q (tl)), (3.3.3) j=i j1 i or equivalently in matrix form

-38M(q(t ))[q(t +) - q(t)] = J T(q(t ))D T (p(t )), (3.3.4) lim wnere ql )- (t1 t), {t) = e —+0(tl-e). Note that this equation is exactly the same >0 as Eq. (3.2.26); thus e in Eq.(3.2.26) can be interpreted physically as the magnitude of the impulsive impact force. Next, we derive an expression for the the velocity of the end effector after impact. Since M(q) is invertible, we can solve for the velocity after impact, q(t ), in terms of 4(t) and (. We have (t+) = 4(t,) + M-l(q(tl))JT(q(tl))D T(p(tl)). (3.3.5) From the constraint satisfaction, we have the following relations: Theorem 3.3.1 p(tl+) lies in the tangent apace of the constraint surface at the entry time tl, i.e., D(p(t,))fj(t ) O. (3.3.6) or equivalently (p (t 1))J(q (t ))(t 1) = 0. (s.3.7) Using (3.3.5), (3.3.7), and assumption (A4), we obtain the magnitude of the impulsive impact force = -A -l(q(t ))D (p(t ))J(q(t ))(t ) (3.3.8) = -A -'(q(t ))D (p(t ))(t j). Substitution of (3.3.8) in (3.3.5) yields q(t+)= [I-M-l(q)JT(q)D T(p)A-l(q)D(p)J(q)],=Q(t;), (3.3.9)

-39or equivalently as At) = [I- Q(q)D T(p)A-l(q)D(p)] lP(t ), (3.3.10) where Q (q) = J(q)M-l(q )JT(q). The velocity p(t ) can also be expressed in terms of (as it 1) i= ( t) + Q (q(t ))D T(p(t )). (3.3.11) Hence, the contact force X(t) can be written as \(t ) = t ) - A -l(q(t 1))D (p(t ))J(q (t ))t )6(t -t1) (3.3.12) (3.3.12) to<t<tf, where X(t) is continuous, except possibly at instants where T(t) is discontinuous. Eqns.(3.3.8) and (3.3.9) characterize the impulse momentum relation at the entry time An impulsive impact force is often undesirable in practical applications. Thus, it is important to know conditions for impact avoidance. Several conditions are given in the following theorems. Lemma 3.3.2 The end effector velocity is continuous at an entry time tl, i.e., p(t )=p(ti+), if and only if p(t 1)E T(p ), where p — p(t1). Proof: Sufficiency: Suppose p(t')E T(p 1) holds. This implies D (p (t I))p(t )=0. Using this relation in Eq.(3.3.10), we have I(t -)=p(tl). Necessity: Suppose pi(t-)=p(t +) holds. We multiply both sides of this equation by D(p(t1)) to obtain

-40D (p(t t))Dt l)-D (p (t 1))t ), but D (p(t 1))i(t +)=0 due to the constraint satisfaction. Thus, D (p(t,))i(t )=-o and hence p(t )E T (p ). 5 Theorem 3.3.3 There exists no impulsive impact force, i.e., =0O, at the entry time tl if and only if p (t ) lies in the tangent space of S at p (t1), i.e., p (t )ET(p1). Proof: Sufficiency: If p(t )E T(p ), then from Eq.(3.3.8), we have C=O. Necessity: If (=O, then from Eq.(3.3.11), we have p(t )=p(t ). Thus, p(t )E T (p ) by Lemma 3.3.2. | Corollary 3.3.4 If the velocity of the end effector is continuous at an entry time t1, i.e., p(t l)=3(t ), then there exists no impulsive impact force, i.e., -=O. Corollary 3.3.5 If the end effector velocity at an entry time t1 is zero, i.e., p(t-)=O, then there is no impulsive impact force, i.e., -O0. Physically, the above theorem and corollaries suggest that an impulsive impact can be avoided if the velocity vector of the end effector is tangent to the constraint surface at an entry time. In particular, this is true if the velocity is zero. This can be seen in Fig. 3.1.(b).

-41The following theorem describes the relation between the change in velocity and the contact force at an entry time. Theorem 3.3.6 Assume that the joint torque T is continuous at an entry time. The velocity of the end effector is continuous at an entry time if and only if the contact force is continuous at the entry time; namely, the contact force is zero. Proof: Sufficiency: The continuity of the contact force at tl, i.e., X(t -)=X(t+ )=O, implies =O. From the necessity of Theorem 3.3.3 and Lemma 3.3.2, this also implies the velocity is continuous at the entry time t 1 Necessity: Assume 1p(t 1)=-(t ). From the sufficiency of Theorem 3.3.3 and Lemma 3.3.2, this implies (=O. Since X(t) is continuous, X(t' )=X(t-)=O. Thus, x(t +)=x(t )==. O One question arises: under what condition does an impulsive impact force occur? The impact occurrence condition is given by the following theorem. Theorem 3.3.7 An impulsive impact occurs at the entry time tl, i.e., (>0, if and only if lim d e- - d (p(t l-))<O. e>o dt Proof: Sufficiency: The assumption implies D (p (t))k(t -)<0, or D(p(t,))p(t-) + z2 _ O, 2zER' But, D(p(t ))p{(t )=O due to the constraint satisfaction after the entry time t1.

-42Clearly, p(t f)Ap(t+); i.e., the velocity of the end effector is discontinuous at t. Then, from Theorem 3.3.3 and Lemma 3.3.2, we know that there exists an impulsive impact at t1 with ~>0. Neceasity: If there exists an impulse impact at t1, recall Eq.(3.3.11) (t +) -- (f) + Q(q(tl))D T(p(t )), with (>0. Multiply both sides by D (p(t )), we have D (p (t 1))it +) -D (p (t 1)) t;) +D (p (t)) Q (q (t ))D T (p (t )) But, D (p (t ))( t 1+) =0, we obtain D (p(tl)) p(t ) -D (p(tl)) Q (q(tl))D T(p(tl)). Since +(p) has a non-zero gradient and Q(q) is a positive definite matrix, D (p(t )) Q (q(t1))D T(p(t )) is a positive scalar. With e>0 we obtain D (p (t l)) p(t ) <O, or equivalently the stated result. * 3.3.2. Exit Time The ezit time is a time at which the active constraint on the end effector becomes inactive, e.g., t2 in Fig. 3.1 denotes an exit time. During a constrained motion segment, q(p(t))=0O and - t i(P (t ))=O must be satisfied. An impulsive contact force can not occur throughout the constrained motion so that at the exit time, the velocity of the end effector must lie in the tangent space. We have the following theorem.

-43Theorem 3.3.8 The velocity of the end effector is continuous at the exit time t2. The condition D (p (t 2))(t2) 0 (3.3.13) is called the "tangency condition." The following theorem gives the relation between the contact force and the input joint torque at the exit time: Theorem 3.3.9 If the joint torque is continuous at the exit time, then the contact force is continuous at the exit time; i.e., X(t2)=0. 3.4. Well-Posedness of the Model In order to justify the well-posedness of the manipulator model developed in the previous section, the solution concept of the manipulator system will first be discussed. Then, the inverse dynamics problem and the direct dynamics problem of the constrained manipulator are investigated. 3.4.1. Solution Concept The solution concept of a unilaterally constrained manipulator is explained through the definitions of consistent initial values and solvability of the manipulator system. They are given as follows: Definition 3.4.1: A set of initial values (go, go) is said to be consistent for the unilaterally constrained

-44manipulator system Eqns. (3.2.20) —(3.2.28) if q(H(qo))=0O and D (H(qo))J(qO)qO>0; or if q(H(qo))>O. Definition 3.4.2: The unilaterally constrained system Eqns.(3.2.20)-(3.2.26) is solvable if for each set of consistent initial values (qo, qo) the system (3.2.20)~(3.2.26) has a unique solution q (t) for to<t <tf, satisfying q(to) qo and (to)o=4. With this concept in mind, we are able to discuss the well-posedness of the manipulator model. The inverse dynamics problem are discussed in the next sub-section. 3.4.2. Inverse Dynamics Problem The inverse dynamics problem can be stated as follows: Given the input joint torque vector T(t) and consistent initial values (qo,qo) finds the motion q(t) and i(t), and the contact force X(t ), satisfying Eqns. (3.2.20)~(3.2.26). Calculation of the contact force, including possible impact, plays an important role in this problem. The existence and uniqueness of the manipulator motion and joint torques are given in the following proposition: Proposition 3.4.1 Suppose the joint torque vector T(t)ER " is piecewise continuous on to< t <t. Given T(t) and consistent initial values q(to), to), then with assumptions (A1)k(A5) there exists a unique solution q(t) for Eqns.(3.2.2)0-(3.2.26), for all t0<t <tf, with q(t) piecewise continuously differentiable, and X(t) piecewise continuous with finite number of impulses for almost all t < t < t1.

-45We state the above as a proposition since a formal mathematical proof is not available. But we can give a heuristic justification that the model is well-posed by suggesting an algorithmic procedure for construction of the solution. To justify the above proposition, we show that the motion of the manipulator exists and is uniquely defined on each unconstrained motion segment, on each constrained motion segment, and at each transition time. In section 3.3, we have shown that the manipulator motion at each entry time and at each exit time is well defined in the sense that the magnitude of the impulsive impact force ( and the velocity after impact p(ti') are uniquely determined. Furthermore, if the transition times can be determined, then each motion segment is uniquely defined. We provide two procedures to justify the proposition so that the determination of the transition times is part of these procedures. The procedure MSU deals with an unconstrained motion segment, and the procedure MSC deals with a constrained motion segment. It will be seen that these two procedures are recursive in nature. Procedure MSU The motion of the end effector during an unconstrained motion segment is defined by M(q )q+F(q,q)= T, for til<t ti. (3.4.1) Given q(ti-1) and i(ti1) satisfying O(H(q(ti_,))>O and D (H(q(t;-1))))J(q(t.1))4(t>-)>O, there exists a solution to the initial value problem Eq.(3.4.1), denoted by q(t) and i(t), satisfying q(H(q(t)))>O for t;,l<t <t; where t, >tl- is the first time (the entry time) at which one of the following conditions is satisfied:

-46(1) q(H(q(t,)))=0 and D(H(q(ti)))J(q(ti))q(ti)<0 so that the constraint k(p) becomes active at time ti. a) If D (H(q(ti )))J(q (ti ))q(ti)=O, then there is no imDact. Thus, i(t) is continuous at ti, ti+) ti) and the impact contact force is f(t)-=O. b) If D (H(q(ti )))J(q(ti ))(ti )<0, then there is an impact. Thus, i(t) is discontinuous at ti and the velocity after impact is t+) I- M-(q)J(q)D T(H(q))A-l(q)D(H(q))J(q)],=ti-', and the impact contact force is f (t) = D T(H(q(t; )))6(t -ti), where =(-A -(q (ti ))D (H(q (t )))J(q(ti ))(ti-). c) Set i-1-i, go to procedure,MSC. (2) ti =t, stop. Procedure MSC The motion of the end effector during a constrained motion segmel M(q)q + F(q,) = T + JT(q)D T(H(q))g(q,,T), (34.2) (3.4.2) for ti.l<t ti, where

-47g(q,q,T ) A-l (q )D (H(q ))J(q )M-l(q )[F(q,q) - T - A -(q )[D (H(q))J(q,4)+ D(H(q ),J(q )q)J(q )]. Given q(ti-1) and (ti l) satisfying 4(H(q (ti,_)))=0 and D (H(q(ti 1)))J (q(t, 1))q(ti_J=O, there exists a solution to the initial value problem Eq.(3.4.2), denoted by q(t) and i(t), satisfying O(H(q(t )))=O for t,.i1t <t,; where ti > t,_ is the first time (the exit time) at which one of the following conditions is satisfied: (1) lim (H(q(ti,,)))>O and D(H(q(ti)))J(q(ti))(ti)>O, where e>0, and q(ti,)-=q(ti )+e$ti )+2 9( ). a) The velocity 4(t) is continuous at ti, and the contact force is f(t) -D T(H(q))g(q,j,T), til<t <t b) Set i-l —i, go to procedure MSU. (2) t =tf, stop. 3.4.3. Direct Dynamics Problem In order to complete the justification of the well-posedness of the manipulator model, the direct dynamics problem is addressed. The direct dynamics problem is stated as follows: Given a specified motion of the manipulator q(t) and a specified contact force X(t) satisfying the imposed constraint, compute the input joint torque vector T (t) which would generate the specified motion. For a unilaterally constrained manipulator, it is difficult to construct a meaningful manipulator specification because we have to supply information about numbers of motion segments, transition times (the entry time and the exit time), and the magnitude of the impulsive impact in a consistent way.

-48The following proposition characterizes the direct dynamics problem: Proposition 3.4.2: Given the contact force X(t) for to0t <t1 and the motion q(t) which is twice continuously differentiablc, ezcept possibly at times ti at which q (H (q (t ))) >0 for ti1 <t < ti and q<(H(q(t))) — for t <t <tj+, and satisfies {(H(q(t)))>0 for to<t <t, then under assumptions (Al)$(A5) there exists a unique input joint torque vector T(t) satisfying Eq.(S.2.20) for to<t <tf. In order to justify this proposition, consider the construction of a manipulator motion with three motion segments: an unconstrained motion segment for t < t < t 1 a constrained motion segment for t 1 t <t2, and an unconstrained motion segment for t2 t <tf; where the entry time t1 and the exit time t2 are given. Note that an unconstrained motion segment is governed by M(q)q + F(q,)- = T, for to<t<t, and t2<t <t, (3.4.3) and a constrained motion segment is governed by M(q)q + F(q,q) = T+ JT(q)D T(H(q))X, for t<t <t2. (3.4.4) Suppose the contact force X is specified as X(t) A(t)u(t), for to<t<tf, (3.4.5) where A(t ) is a given function, and l t1<t <t2, u(t) = o, otherwise, (3.4. then the joint torque T(t ) can be uniquely determined for to< t < tf

.49If i(t) is discontinuous at the entry time t1, we must use Eqns. (3.3.8) and (3.3.9) to compute the impact relations e and 4(t +), then these values are used as values for initiating the constrained motion segment, i.e., Eq.(3.4.4). Again, the input joint torque can be uniquely determined.

CHAPTER 4 PLANNING FOR CONSTRAINED MANIPULATORS So far we have discussed the modeling of a constrained manipulator. However, in order to perform a useful task, a manipulator motion has to be planned and executed. Although there may exist many possible motions between the two given end-points, some of them may be inadmissible due to constraints on the manipulator. Even for admissible motions, it is difficult to select a specific, efficient, and meaningful motion, especially if constraints on the manipulator model are considered. Planning for constrained manipulators involves not only specification of desired manipulator motion, i.e., displacement, velocity, and acceleration time history of the end effector, but also specification of desired contact force time history. Given such specifications which satisfy the imposed constraints, we have shown in the previous chapter that the joint torques can be computed so that the manipulator satisfies these desired specifications. For planning of an unconstrained manipulator, the joint-interpolation approach is the most commonly used approach [4,36,57,58]. The parameterization approach [3,48,681 is another approach. However, these approaches are not directly applicable to planning of a constrained manipulator in that they do not take into account the contact force. In general, the contact force and an associated algebraic equality (or inequality) constraint -50

-51present difficulties in the planning problem. In section 4.1, we define a planning problem for a constrained manipulator and discuss associated difficulties. The general idea of the Darameterized planning problem is also discussed. In sections 4.2 and 4.3, we extend the parameterization approach for unconstrained manipulators to the case of constrained manipulators. 4.1. Definition of a Planning Problem A planning problem for a constrained manipulator is one that given the desired contact force, determines the joint input torques of the manipulator such that the manipulator is driven from a given initial configuration to a given final configuration, subject to: (i) constraints on the end effector motion, (ii) impact avoidance, and (iii) input torque constraints. If a cost criterion is imposed, then this problem becomes a minimum-cost planning problem or a so-called optimal planning problem. In general, an optimal planning problem can be formulated as an optimal control problem with constraints on state and control variables. This can be seen in the following. For a bilaterally constrained manipulator, the optimal planning problem can be formulated as: t/ minimize I = f ( T,X,p,p)dt to subject to M(q)q + F(q,q) = T + JT(q)D T(p)\, p = H(q), A(P)=O, to<t <t, (4.1.1) TEl, p(to) po, Pto) O, p(t ) = p, p(t ) =, where n is the control set. Clearly, there are constraints on state and control variables.

-52Notice that the equality constraint q(p )=O is an intrinsic part of the constrained manipulator dynamics; additional constraints on state variables could also be imposed. For a unilaterally constrained manipulator, the optimal planning problem can be formulated as: t! minimize I -= L(T,X,p,p)dt to subject to M(q)q + F(q,q) = T + JT(q)D T(p), p H(q), X >0 +(p) L O, to<t _<tl, O(P~) ~0,.to.~t tt (4.1.2) X::(p ) = M(q(ti ))[4(ti+) -(ti-)] = JT(q(ti))D T(p (ti ))f, i —l,..,n, P(to) Po, (to) -0, O P(t ), (t/ ) 0, where ti, i l,...,n are entry times at which q(p(t))>0 for ti < t < ti and q(p(t))=0 for ti t ti+l. In this case, there are equality and inequality constraints on state and control variables. Note that these constraints, except for control constraints T E, are intrinsic parts of the manipulator model. Notice that the final time tf in (4.1.1) —(4.1.2) can be either fixed or free. If tf is free and L(T,X,p,p)=l, then problems (4.1.1) —(4.1.2) become minimum time problems. Optimization problems with equality and/or inequality state variable constraints, such as problems (4.1.1) and (4.1.2), have been extensively studied by Makowski and Neustadt [44], Warga [81], Bryson et al. [8], Bryson and Ho [9], Dreyfus [17], Sethi et al. [66], and Jacobson et al. [291]. In those approaches, necessary conditions for optimality are applied to the problems to obtain a set of nonlinear two-point-boundary-value problems. Usually, these types of optimization problems are quite difficult to solve because

-53the state variable constraints are infinite dimensional; it is difficult to determine transition times at which constraints change between active and inactive. Bobrow et al. [3] and Shin and McKay [68] used a scalar parameterization method to solve a minimum time planning problem for an unconstrained manipulator. The scalar parameterization, has a physical interpretation as the selection of an a priori path which. guarantees satisfication of the state constraints. The parameterization method also reduces the dimensionality of the optimization problem from 2n to two. Since the scalar parameterization method has advantages of reducing dimensionality and "eliminating" the state constraints, we extend Bobrow and McKay's approach to the problems described by Eqns. (4.1.1) and (4.1.2). In using the parameterization approach, the planning problem can be stated as: Given configuration constraints and initial and final manipulator velocities, select a specific path, and then determine the joint torques so that the manipulator is driven along the specified path. Thus, a parameterized planning problem consists of three parts: (1) path planning —select a parameterization function P(8 ) so that p =P(8 ) satisfies the state constraints for 0<8 1; (2) motion planning-find a function 8a =(t) so that p (t )=P(a (t)) for t < t <tf; (3) joint torque computation-compute the required joint torques from the manipulator dynamic equations. The path planning problem is discussed in section 4.2, and the motion planning and the joint torque computation are discussed in section 4.3. Since the planning problem of a bilaterally constrained manipulator is similar to the planning problem of a unilaterally constrained manipulator, only the planning problem of a unilaterally constrained manipulator is discussed. Furthermore, we assume that the manipulator only has three sequential path segments; that is, an unconstrained path segment, a constrained path

-54segment, and an unconstrained path segment. 4.2. Path Planning The first level of parameterized planning problem is called the path. planning problem. This is a problem of selecting a suitable geometric path in the workspace of the manipulator to satisfy the constraints and other imposed requirements. We may regard path planning as one kind of spatial planning in the sense that the manipulator path is planned in the configuration space and the manipulator dynamics are not involved. In section 4.2.1, we discuss the properties of parameterization functions. In section 4.2.2, we discuss the selection of a parameterization function for unconstrained path segments. A heuristic kinematic approach is proposed to solve path planning problem. The avoidance of impact may be crucial to the selection of unconstrained paths. Since the constrained path is specified a priori in contour following problems, the parameterization function must be appropriately chosen during a constrained path segment. 4.2.1. Properties of Parameterization Functions Since a unilaterally constrained manipulator involves an inequality constraint, 4(p)>O, a parameterization function should be chosen in the following way for the specific case where the manipulator has only three sequential path segments: an unconstrained path segment, a constrained path segment, and an unconstrained path segment. Let P: [0,1] —R n be a parameterization function satisfying ((P )>O, O<j <1, (4.2.1) and P(O)=-po, P(1)=p, with the following properties:

455(i) For 0<8a<a2<1 (P(8 ))>, _ 0<8<<1 and 82< <1, O(P(a ))=0, 1:< <o 2, (ii) P () is continuous on [0,1]; (iii) P (a) is twice continuously differentiable except possibly at l and a 2. Let a mapping Q: [0,1]-+R " satisfy P()= H(Q(a)), 0< <1. (4.2.2) If the transformation H is three times continuously differentiable, Q (a) possesses the same smoothness properties as P(s ). Thus, we have the parametric representations p-P(,), ap =P(a ),. (4.2.3) q= Q9(). The variable a defined above is referred to as a path variable. If the manipulator path is parameterized by the arc length, then the path speed and path acceleration reflect the true speed and acceleration of the end effector. Consider the definition of arc length: Definition 4.2.1 (Sokolnikoff [74]) Consider a parametric representation of vector function P: [0,1] —R P =P(a), 0< <1. Suppose W(P, P' )>0, unless P' =0, and for every positive number k W(P, kP' )-kW(P, P' ). The integral fW(P(a),P' (r))d&, (4.2.4) o

-565 is called the arc length of P(a ); and the space R" is said to be metrized by formula (4.2.4). Different choices of function W(P, P ) lead to different metric geometries. In Riemannian space, the arc length is given by [p1 T M(o (P P. (a) lda, (4.2.5) 0 where M(P) is a symmetric and positive definite matrix function. Note that in Euclidean space M(P)=I. From Eq.(4.2.5), if the normality condition of a parametric representation, P' T( )M(P( ))P' (a) = 1, for all a, (4.2.6) where P' (s )= aP (), is satisfied, then p =P (a) is parametrized by the arc length a. as 4.2.2. Selection of Path Function A kinematic approach is proposed to choose parameterization functions for the path. The kinematic approach is based on the satisfaction of the constraint, and/or an impact avoidance condition, and the boundary conditions. Recall that there are two unconstrained path segments: one is defined on 0<s <s 1, the other on 82<8 <1. For the first unconstrained path segment, the end effector of the manipulator makes contact with the constraint surface when a =s 1. In order to avoid an impact at the entry time, it is required that OP () I lie in the tangent space of the constraint 9a *I surface at 8 1. It will be shown that this impact avoidance condition can be written as D(P(a1))P' (a) = 0, (4.2.7)

-57where P' (s )= aP() a9 During the constraint motion, the constraint q(P ( ))=O must be satisfied for 81<a8<a 2. Thus, a kinematic approach to selection of a parameterization function P(a ), O<a<1 should satisfy the following conditions: P()- o, - P(J )-=Pi, O(P ( ))>0, 0< <a 8, (4.2.8) D (P ( 1))P' (a) 0, P( P ))=- O,,1 —<' 82, (4.2.9) P(82)-P2, P(1)= pl, O(P ( ))>0,' 2<8 <1 (4.2.10) D (P(a2))P' ( 2) 0. A kinematic approach for path planning is always possible and is simple. Other approaches, such as minimization of the arc length, may be used for path planning [69]. 4.3. Motion Planning The second level of a parameterized manipulator planning problem is the motion planning problem. It can be stated as: Given a path and the manipulator dynamics, determine the motion sequence of the manipulator so that the manipulator moves along the prescribed path from the initial position to the final position. The main purpose of the motion planning problem is to determine a function =8 (t) so that the required motion sequence of the manipulator is generated. If a function a: [tot1 ]-[0,1] is twice continuously differentiable and satisfies a (to)=0, a (t1 )=1, then s (t) is referred to as a path history.

-58In this section, we first derive the equations of motion for a constrained manipulator along a parameterized curve. In section 4.3.1, conditions for the avoidance of impact are discussed in detail. In section 4.3.2, a kinematic approach to motion planning is proposed. The development is based on curve fitting using cubic polynomials. In section 4.3.3, a minimum-time planning problem is discussed. We formulate the motion planning problem for a constrained manipulator in two different ways according to the impact avoidance condition; in each case a phase plane technique can be applied. Suppose that the path of a manipulator is given by p =P( ), O0< <1. We would like to derive the equations of motion of the manipulator along the prescribed path. For a given path history 8(t), the position vectors in work space and joint space are defined by p(t)=P(.(t)), t t<tf (4.3.1) q(t ) Q ( (t)), to<t <tf where P(a) and Q(s) satisfy the conditions developed in the previous section. The velocity and acceleration in joint space can be written as t ) = B(s (t ))J(t ), (4.3.2) q(t) B(s (t))1'(t) + B( (t))j2(t ) where B(a(t))= Q(s(t)) and B(A(t))=-aB( (t)) are n-dimensional vectors. We 0. Os assume B( (t ))yO for to<t <tf. Assume the contact force X(t) is given so that X(t) =A( (t))u(a(t)), to<t<tf, (4.3.3) where A: [s 1,s 2]-R1 is a known scalar function, and

-59. J1, l<- 2 -~ $2, it (j8 2 - { (4.3.4) u(s) 0, otherwise, ( is an indicator function. Substitution of Eqns.(4.3.1)-(4.3.3) in Eq.(3.2.20) yields the equations of motion along the specified path as M( Q ( ))B( )' + M( Q ( ))B( ) 2 + F(Q (),B(a )) (4.3.5) =T + JT(Q(a ))D (H(Q( )))A(a )u( )). In view of Eq.(4.3.5), the joint torques T can be expressed as function of s, i, and'i, i.e., T =M(Q(a ))B(a )' + M(Q ( ))B(a )2 +F(Q ( ),B(a )i)+JT(Q (a ))D T(H(Q (a )))A( )u ( ), or equivalently as T(t)-= T(s(t ),(t ),((t )). (4.3.7) As long as a(t), i(t), and *((t) are known, the input joint torque T(t) can be uniquely determined from Eq.(4.3.7). Thus, in order to compute the joint input torques we must first find the path history a (t). Two methodologies can be used: an approach based on curve fitting to obtain maximization of the amoothneaa of the path apeed and an approach based on the avoidance of impact. The former is a kinematic approach; it does not depend on the manipulator dynamics. The latter approach results in an optimization problem taking into account the manipulator dynamics. Before we get to this issue, we first discuss conditions for avoidance of impact, which are used in both the path planning and the motion planning problems.

60-O 4.3.1. Conditions for Avoidance of Impact The impact avoidance condition was discussed in section 3.3.1. It is possible to choose a path such that no impulse occurs when the end effector makes contact with the constraint surface. Based on Theorem 3.3.3, and Corollaries 3.3.4 and 3.3.5, the impact avoidance condition can be expressed in parametric form as follows: Theorem 4.3.1 Suppose t1 is an entry time with <f(P(s(t))))>0, t<t, (P ( ((t )))=O, t>tl,. There is no impulse at t if and only if D(P(s ))J(Q (s ))B(s,)(t ) =, (4.3.8) where s (t )=s1. Corollary 4.3.2 If s(t)=0O, then there is no impulse at the entry time t1. Corollary 4.3.3 If the path satisfies D (P(s 1))J(Q ( 1))B(s 1) ==, then there is no impulse at the entry time t1. In the case described in Corollary 4.3.2, the manipulator comes to rest as it contacts the constraint surface and no additional assumptions about the path are required. In the case described in Corollary 4.3.3, the path speed s(tl) can be arbitrary, but the

-61path must be tangent to the constraint surface at s 1; i.e., the parameterization function P( ) must be differentiable at a1. The above two corollaries also imply that the avoidance of impact can be achieved either in path planning level, i.e., Corollary 4.3.3, or in motion planning level, i.e., Corollary 4.3.2. The end effector must exit the constraint surface with a velocity in the tangent plane. Thus, at the exit time t2, we require D (P( 2))J(Q ( 2))B( 2)i(t2) = 0. (4.3.9) Note that condition (4.3.9) is a tangency condition; no impact can occur at such an exit time. 4.3.2. Kinematic Approach to Motion Planning We now consider a curve fitting approach for which the path history a(t) is generated without consideration of the manipulator dynamics; hence it is referred to as a kinematic approach. Although this approach to motion planning is simple, it may not necessarily be efficient since manipulator dynamics are ignored. In a contour following problem, it is desirable that the end effector moves as smoothly as possible on the constraint surface in order to avoid bouncing and jerking. Consequently, two kinematic approaches are considered below. Kinematic approaches to unconstrained motion planning are well known [4,36,58,78]. CASE 1 One way to achieve smooth motion on a constrained motion segment is to move the end effector with zero acceleration along the constrained path. Note that the path acceleration bi(t) reflects the true acceleration of the end effector if the path variable a

-82has been chosen as the arc length of the parameterization function. Assume the path has been selected to satisfy the required constraints and D (P ( 1))J(Q (1))B( 1) - 0, (4.3.10) and D (P(a2))J( Q(2))B(e2). (4.3.11) Suppose that a (t), tot < t, is chosen so that (a) i'(t) is a linear function of t, for to<t <tl, with (t O) = O, (t l) 1, 2-`l (4.3.12) i(to) = O, i(t) -; t2-1t (b) y(t)=O, for t1_t <t2; (c) i(t) is a linear function of t, for t2<t <tf, with I;(t2)= 2, 2 (t ) 1=, = 2- 1 ( ) (4.3.13) *(t2) f —, ~(I ) - 0o t* 2-t where to<t <t2<tf and 0<8 1<8 2<1 are given. The path history (t) is given by el(t-t2)3+e4(t-t2)2+v(t-t2)+2, t t <t~, a (t) ((t -t )+J 1, t <t < t2, (4.3.14) e 3(t -t2)3+ 4(t -t 2)2+v (t -t 2)+8 2 t 2< t < tl where S2-8 1 v t —- - (4.3.15) t2-tl and

-632e1 v -3l 2v (t -t 1)3 (t -t)2; 2 (o-t )2 (t - ); (43.1 2(82-1) v -3(s 2-1) 2v (tf -t2)3 (tf -t2)2 (t+ -t 2)2 (t. -t2) In this choice, the path speed i(t) is not zero at either the entry time or the exit time. The end effector moves on the constraint surface with constant speed, this feature of constant speed may be useful for certain applications. CASE 2 In this case, the path acceleration on the constrained path segment is chosen as a linear function of time, assuming that the manipulator stops at the entry time and exit time. This assumption prevents an impact at the entry time. Again, we assume the path has been selected to satisfy the required constraints. Suppose that a(t), to<t <tf, is chosen so that (a) a8(t) is a linear function of t, for to<t < t, with _ (to) 0, (tl) - l, *(t0o)~=~O,~ ~e(t1()=e1~ (4.3.17) (to)=, i(tl) =0; (b) 1J(t) is a linear function of t, for t 1t <t2, with,(t,)=-'1,'(t2) ='2, (tl= 8I X *(t 2) ~= 82, X(4.3.18) i(t) = 0, i(t2) 0; (c)'(t ) is a linear function of t, for t2<t tf, with (t2) = 2 (t) 1 (4.3.19) i(t2) 0, i(t/ ) O, where to<t1<t2<t- and 0<81<s2<1 are given. Note that a(t1) and e(t2) need not satisfy conditions (4.3.10) and (4.3.11), respectively. The path history a (t) is given by

-04C,(t -t)3+C2(t-t 1)2++81 to<t < t 1 8(t) c3(t-t1)3+c4(t-t1)2+,l, t1<t <t2, (4.3.20) c s(t-t2)+C6(t-t2)2+'2, t2<t <t, where 2i 1 -3s 1 2(8 1-8 2) c1~ -- (totoLt$; C(2 (); (t Ott 1).3 t3 —-)2 (t2-t 1)3 -3( 1-' 2) 2(8 2-1) -3(-1) ( ) (c t 2-t 1)2 (tO-t 2) (tf -t2)2 In this approach, the path history 8(t) can be shown to have the property that it minimizes the integral f'2dt; thus (t) is a smooth function in this optimal sense. 4.3.3. Minimum-Time Approach to Motion Planning In the previous section, two kinematic approaches to motion planning have been discussed. They are conceptually simple; however, they may not be efficient since the manipulator dynamics are not taken into account. In practice, the planning goal may be to reduce the cost of performing a certain task. This can be accomplished if the manipulator is moved as fast as possible. Kahn and Roth [30] have studied a minimum-time motion planning and tracking problem. Bobrow et al. [3] and Shin and McKay [68] have studied a minimum time motion planning problem for an unconstrained manipulator. We also use the minimum time criterion for the optimal motion planning of a constrained manipulator. In the remainder of this section, we formulate a minimum time motion planning problem; then we extend Bobrow and McKay's phase plane method to the case of a constrained manipulator problem.

-854.3.3.1. Limits on Path Acceleration In a time optimal problem, minimizing traversal time is equivalent to maximizing traversal speed. In fact, the minimum time solution consists of an accelerating part and a decelerating part; hence, limits on the path acceleration must be considered. These limits are assumed to be imposed by the input joint torques T. For simplicity, the argument t is omitted from expressions throughout this sub-section. Let us consider constraints on the input joint torques of the form Tij (q,q) T Ti m q,q),,...,n (4.3.22) Since q is function of a and q is function of a and i, the torque constraints along the specified curve can be expressed in terms of a and i as Tim'(a,s)< Ti < Timsa,i), il,...,n. (4.3.23) To evaluate the limits on the path acceleration', Eq.(4.3.8) can be substituted into Eq.(4.3.23) so that Tm(. )< Mij (Q ( Q())Bj (Q)) ( + E Mj(Q ( ))B)j ()2 + F,(Q({ )B( )); ~~j1 t;;~~~~~ j1~~ ~(4.3.24) - Jii (Q ))Di (H(Q (s )))A( )U ( )< Tmax(s) i,..,n. ji=1 -— 1 Assume that B(s)4O0 and M(Q(a)) is nonsingular for all 0<. <1. Suppose Mii(Q (a ))Bi (s )50, for i =l,...,n, then manipulation of inequalities (4.3.24) gives -=! < g n( MSj((Q (s))Bi (s ))'8< h Mii (Q (8 ))Bi (a ) Mii= (Q (zQ ))Bi ( 4..25) jiw ij= where i=l,...,n, and

E1 (o,J)T (,i) - Mji (Q (J))Bj (J )2-F (Q (J ),B(o )i) (4.3.26) + Jii(Q ())Di(H(Q (a)))A(J )u (a), ij1 E2 (,8)- Tm,j) - s M,(Q ( )) (( )i2-F (Q ( ),B ( )J).~j-1~~~ -l~ ~. (4.3.27) + E Jj(Q (a))Dj(H(Q(s )))A(a )u(8). ji1 Eq.(4.3.25) can be re-written as Ah (a,J)<J_<-yr (a,.J), (4.3.28) where E15(a,i) I Es l,i ), if Mj(Q(s ))Bi(8)>O, I Mij (Q(J ))Bj (J )I 1 Ah (8,AE') = (4.3.29) -- --, if Mj (Q (( ))Bj ()<0, *- - M-j(-(1))Bi(6)1 }=1 E2(..., if S Mij(Q( ))Bj()>0, I E Mi(Q( ))Bj(a)l 1 i(8s,j) =1 (4.3.30) - -----,( if Mij(Q( ))Bj( )<0. I E M,(Q9())Bj (s)I =1 j =1 Eq.(4.3.28) gives limits on s'. Since all joints have to satisfy these limits, we have h (a,i)<J' <^(,i), (4.3.31) where h (a,) = max hi (,),''(a'A) = mm ~~(a'A) i=1,...,n. (4.3.32) tJ,J) — min ", (J,8), ~'=l,...,n. i

-67Eq.(4.3.31) defines the lower limit and the upper limit of the path acceleration. In other words, the admissible acceleration is defined by (4.3.31). Using information about the limits on the path acceleration, we are able to formulate a minimum time motion planning problem in terms of a and i. This is discussed in the next section. 4.3.3.2. Minimum Time Problem Formulation In this section, we formulate a minimum-time problem for the constrained manipulator incorporating the impact avoidance condition in two different ways. A phase plane technique, developed for an unconstrained manipulator problem in [3,48], is applied to our problems. We now obtain a single differential equation expressing - in terms of a, i and T. From Eq.(4.3.5) we obtain B T( )B(a)b + BT( )B()i2 + BT()M-1(Q ( ))F(Q ( ),B( )i) Tr j1 ~ T (Q (a T ~~~(4.3.33) = B T()M-(Q ( T)) T+J( ())D (H(Q ()))A( )u (s)]. Since the scalar BT (a )B(a )O0, dividing both sides of the above equation by B T (1 )B ( ) and re-arranging it, we obtain a single differential equation for'i as'-vj(a, )B T(a )B( )i2+,(a, )B T( )M-(Q ( ))[T-F(Q (a ),B(a )i)] (4.3.34) +r,( )B T (a )M-l(Q (a ))JT(Q (8 ))D T(H(Q (a )))A(8 )u ( ), where rt( )=[B T(a )B(a )-1. Now Eq.(4.3.34) gives the desired equation of the manipulator along the specified path. Clearly, the dimensionality of the optimization problem has been reduced from 2n to 2. With this information, the traversal time of the path, t1, can be written in terms of a and i as

-68t - f ldt- - f do (4.3.35) 0 0 Thus, the cost function for the three-motion-segment problem becomes ex ~2 1 t, =If +I df +;. (4.3.36) 0 i 81 8 B2 In order to guarantee the continuity of the velocity of the end effector at both the entry time and the exit time, additional conditions must be satisfied. Recall the impact avoidance condition and the tangency condition as D(P(a1))J(Q (s))B(a 1)(t 1) =0, (4.3.37) D (P(s 2))J(Q (s 2))B(a 2)( t2) = 0, (4.3.38) Given the state equation Eq.(4.3.34), the cost function Eq.(4.3.36), and constraints Eqns. (4.3.37) and (4.3.38), we can formulate the minimum time motion planning problem as follows. Problem 1 Given the path p =P(s) and q Q (a ) satisfying Eqns. (4.2.1) —(4.2.3), find a (t), to t <tf, the transition times t and t2, the final time tf, and the joint torque Ti(t ), i=l,...,n which minimize tf given by Eq. (4.3.36), subject to the equation Eq. (4.3.34), the joint torque constraints Eq.(4.3.23), the impact avoidance condition (4.3.37), the tangency condition (4.3.38), and the boundary conditions a(0)=0, i(O)=0, 8a(tf )=1, Ii(tf )=0, (t1)=ai, and a(t2)= 2. This formulation guarantees that no impact occurs at the entry time, and that the end effector leaves the constraint surface tangentially at the exit time. As mentioned earlier,

.89. there are two ways to satisfy the impact avoidance condition and the tangency condition; they are, i(t,-)=, (4.3.39) i(t 2)=, (4.3.40) or D (P(a 1))J(Q(a 1))B(a 1) = 0, (4.3.41) D(P ( 2))J(Q (a 2))B ( 2) = 0. (4.3.42) where a =a (t 1) and a 2 (t 2). Therefore, the minimum time motion planning problem can be re-formulated in two different ways in terms of these two cases. CASE 1: Suppose it is required that the end effector stop at the entry time and the exit time so that i(tl)=0, i(t2)=O. The impact avoidance condition and the tangency condition are automatically satisfied. The minimum time planning problem becomes Problem 2 Given the path p =P ( ) and q Q (s ) satisfying Eqns. (4.2.1) —(4.2.3), find a(t), to<t <tf, the transition times tI and t2, the final time tf, and the joint torque Ti(t), i=l,...,n which minimize tf given by Eq. (4.3.38), subject to the equation Eq. (4.3.34), the joint torque constraints Eq.(4.3.23), and the boundary conditions a(0)=0, i(0)-=0, (tf )=1, j(tf )=0, a (t1)=, i(t 1)=0, (t 2)= 2, and (t 2)=0. Since the time optimal trajectory must satisfy i(t)=0 and 8(t2)=0 at the entry time and the exit time, respectively, we have the following theorem:

-70Theorem 4.3.4 The time optimal trajectory of problem 2 can be accompliahed by minimizing each motion segment independently. The minimum-time motion planning problem of each motion segment in problem 2 are defined as follows: O<a <al, min rT = 0 a subject to Eqns. (4.3.34), (4.3.23), (0) = 0, J(O) = 0, (4.3.43) s(t1) =i, (t1) = 0. 2 mmin r2= f d 5 subject to Eqn. (4.3.34), (4.3.23), i(tl) = s, (t ) = 0, (4.3.44) (t 2) - 2:, i(t2) -- o. s2<8~1, mmin r3 = subject to Eqns.(4.3.34), (4.3.23), s (t2) = 2, i(t2) 0, (4.3.45) (tf )=1, (tj)= 0. Theorem 4.3.4 says that the total minimum traversal time of problem 2 is equal to tf = i + T2 + 3,

-71where rl, r2, and r3, are the minimum times of corresponding motion segment, respectively. CASE 2: Suppose that the path function is differentiable and tangent to the constraint surface at the entry time and the exit time; then the impact avoidance condition and the tangency condition are automatically satisfied. The minimum time motion planning problem in this case becomes Problem 3 Suppose that 1 and 2 satisfy D(P(s 1)J( Q ( 1))B (1)=O and D(P(e2))J(Q(F2))B(a2)=O. Given the path p=P(a) and q=Q(a) satisfying Eqns. (4.2.1)~(4.2.3), find a(t), to<t <tf, the transition times tl and t2, the final time tf, and the joint torque Ti(t), i=1,...,n which minimize tI given by Eq. (4.3.36), subject to the equation Eq. (4.3.34), the joint torque constraints Eq.(4.3.23), and the boundary conditions (O0)=0, i(0)=0, a(tf )=1, (tf )=0, a (t)=i — 1, and' (t2)=a 2 In this case, the end effector need not stop at the entry time and the exit time since the path speed is not constrained at these two instants. However, the path must be chosen so that s1 and 82 meet the requirements of Eqns.(4.3.41) and (4.3.42). The resulting minimum time planning problem for a constrained manipulator is a tractable minimum time planning problem without explicit state constraints.

-724.3.3.3. Phase plane Technique for Minimum Time Problem In the previous sub-section, we have shown that the minimum time motion planning problem for a constrained manipulator can be re-formulated as an equivalent minimum time motion planning problem without state variable constraints by properly choosing the parameterization function and including the desired contact force. The phase plane technique proposed by Bobrow [3] and McKay [48] can be directly applied to these cases. We summarize the phase plane technique in this sub-section. The phase plane technique is based on the observation that to achieve minimum time control, the path acceleration i* always takes its largest or its smallest possible values subject to satisfaction of Eq.(4.3.31). That is, finding the optimal time amounts to finding the times, or locations, at which *9 switches between maximum acceleration and maximum deceleration. From Eq. (4.3.31), the maximum permissible acceleration is defined by.' = --,i), (4.3.46) and the maximum permissible deceleration is defined by' = h(s,i). (4.3.47) di di Since -i d — d i, Eqns.(4.3.46) and (4.3.47) can be used to obtain the (a,i) phase dt ds plane relations as di = (ai) (4.3.48) da J if the acceleration is at its maximum value, or if = acclertio iatm mu le(4.3.49) if the acceleration is at its minimum value.

-73Define the admissible region in (a,i) phase plane as p =__ {(s,s)'(a,a)>A_ (a,J)}, (4.3.50) we now summarize the details of the phase plane technique as follows: (1) Construct an initial accelerating trajectory by integrating Eq.(4.3.48) from (a,^)=(O,0) until this accelerating curve either leaves the admissible region of the phase plane or goes past s =1. Call this curve C1. (2) Construct a final decelerating trajectory by integrating Eq.(4.3.49) from (a,i)=(l,0) backward until this decelerating curve either leaves the admissible region of the phase plane or goes past a =0. Call this curve Cf. (3) If C1 and Cl intersect, then stop. The point at which the trajectories intersect is the (single) switching point; and the optimal trajectory consists of the accelerating curve from a =0 to the switching point and the decelerating curve from the switching point to s =1. (4) If C1 and Cf do not intersect, then they must both leave the admissible region. Call the point where the accelerating curve leaves the admissible region?1 (see Fig. 4.1). Suppose that the boundary curve h(s,)}-(a,)=0 can be expressed as A di da di =a( ). Define ( )= - d where - is the acceleration defined in de da da Eq.(4.3.48). Then starting from 81 we search along the boundary curve until a point at which f(a ) changes sign. Call this point Ti. It is the next switching point da (see point B in Fig. 4.1). Note that if a(s) has a discontinuity, then a- must be careully treated in terms of is right hand limit at the discontinuity. carefully treated in terms of its right hand limit at the discontinuity.

-74(5) Construct a decelerating curve, using Eq.(4.3.49), backward from ad until it intersects an accelerating curve. This gives another switching point (see point A in Fig. 4.1). (6) Construct an accelerating curve, using Eq. (4.3.48), forward from a until it either intersects the final decelerating trajectory C! or leaves the admissible. If it intersects Cf, then the intersection gives another switching point (see point C in Fig. 4.1), and the procedure terminates. Otherwise, go back step 4. The above procedure is only applicable to an admissible region without islands; that is, the admissible region is simply connected. For a case where the admissible region contains islands, the above procedure must be modified and a grid traversal algorithm must be included. The details have been given in McKay's thesis [48]. Now, we indicate how this phase plane technique can be applied to solve the optimal problems that have been developed. In case 1, the minimum-time motion planning problem is divided into three uncoupled sub-problems, i.e., Eqns. (4.3.43), (4.3.44), and (4.3.45). Each sub-problem is solved by phase plane technique independently; then the results of the complete problem are obtained by piecing the results of the three subproblems together. In case 2, the motion planning problem can not be divided into several sub-problems; it must be solved as a whole. The unconstrained motion segment and the constrained motion segment are governed by two different sets of equations of motion. When the phase plane technique is applied to this case, i.e., problem 3, there likely occur discontinuities in the boundary of the admissible region. Also the admissible region may become very complicated. These two features, different sets of equations of motion in different intervals, and complicated admissible region (possible discontinuities and cusps in the boundary curve) present a challenge to solve the minimum-time motion

-76planning problem. Boundary Curve tat C I I I.'1 /I I I C IA I I I I I 0 A J (B) S'4d e2 C 1 acceleration deceleration celeration deceleration phase plane trajectory C1C2C3Cf Fig. 4.1 A minimum-time trajectory with three switching points

CHAPTER 5 SIMULATION FOR CONSTRAINED MANIPULATORS We have developed models for both bilaterally and unilaterally constrained manipulators. In this chapter, these models are applied to the contour following problem, which is described in Chapter 1. Computer simulation can be used to evaluate the dynamic performance of the implementation. Since a constrained manipulator contains an algebraic equation or an algebraic inequality as an intrinsic part of its model, most existing ordinary differential equation solvers [18] can not be directly applied. Although some packages [2,12,35,59] have been developed for bilaterally constrained systems, most of them either ignore the contact force or deal with a low index system (the index concept is discussed later). None of them is designed from a control point of view. Research on simulation for unilaterally constrained systems are seldom addressed. Lotstedt [39] converts a unilaterally constrained system into a quadratic programming problem; then a quadratic programming procedure is applied to it. However, the impulsive impact effect, which is an important issue for unilaterally constrained systems, is not handled there. Thus, it is desirable to investigate the simulation issues for constrained systems and to develop our own simulation approach. In section 5.1, we briefly review some software packages for bilaterally constrained systems. In section 5.2, we discuss the simulation issues for a bilaterally constrained -78

~77manipulator. Finally, the simulation issues for a unilaterally constrained manipulator are discussed in section 5.3. 5.1. Review of Software Packages for Bilaterally Constrained Systems The currently available software packages for a bilaterally constrained system have been developed and applied in simulation of mechanical systems. Recently, there has been a growing interest among people working in numerical methods with the simulation of bilaterally constrained systems. Software packages and research in these two areas are reviewed in this section. Bilaterally constrained systems have been studied for mechanical systems. At the University of Michigan, Chace has developed a series software packages for mechanical systems, from DAMN (Dynamic Analysis of Mechanical Networks) [72,73], DRAM (Dynamic Response of Articulated Machinery) [2] to ADAMS (Automated Dynamic Analysis of Mechanical System) [12,53]. At the University of Wisconsin, IMP (Integrated Mechanisms Program) [67] has been developed by Sheth and Uicker. At the University of Pennsylvania, Paul developed a package called DYMAC (DYnamics of MAChinery) [54,55]. At the University of Iowa, DADS (Dynamic Analysis and Design System) [35,49,82] was developed under the supervision of Haug. The main purpose of all these packages is to automatically formulate and to generate the equations of motion of a mechanical system. When there are kinematic constraints, most of these packages use the constraints to eliminate redundant coordinates; that is, the contact forces are not explicitly integrated in simulation. The elimination of the contact forces may not be desired since the contact forces may be of direct interest. All these packages do not provide a procedure for controller design; in addition, they are not capable of handling unilateral constraints and impact issues.

-78In the past couple years, people working in numerical methods have also put much effort in study of differential algebraic systems (DAE). Brenan [6,7] extensively studied numerical methods for DAE systems. Petzold developed a software package called DASSL (Differential Algebraic System SoLver) [59] for DAEs. Several others have also made research contributions to DAE systems [5,19,20,21,37,60,61,63,71]. Their approaches differ from the approaches used in the mechanics area. Rather than eliminate the contact forces, the backward difference formulas (BDF) method are used to compute the contact forces (algebraic variables) and non-algebraic variables simultaneously. However, there are many difficulties; especially when the index (discussed later) of the system is high. Again unilateral constraints and impact issues have not been addressed there. In the next section, simulation concepts and methods for bilaterally constrained manipulators are discussed. 5.2. Simulation for Bilaterally Constrained Manipulators Since a bilaterally constrained manipulator is a singular system, as seen in Chapter 2, the algebraic equation in the system presents a challenge to computer simulation. In order to understand how to approach the simulation, several issues should be discussed: the index concept, simulation models, simulation methods, and simulation difficulties. Section 5.2.1 discusses the index concept of a singular system. Section 5.2.2 presents three different simulation models. Section 5.2.3 discusses the BDF (Backward Difference Formulas) method and uses it as a basic simulation method. Section 5.2.4 describes simulation difficulties and ways to overcome them. Section 5.2.5 discusses two other simulation methods. Some remarks are made in section 5.2.6.

-795.2.1. Concept of Index Since the index of a singular system characterizes the complexity of the system, it is important to understand how the index affects the system. We illustrate the index concept of a nonlinear singular equation system by considering the reduction algorithm proposed by Gear and Petzold [20,60]. Consider a nonlinear singular equation system E(y)yj+ G(y) r. (5.2.1) The reduction algorithm is as follows: (i) If matrix E(y) in (5.2.1) is nonsingular, the reduction process is terminated. Otherwise, go to next step. (ii) Premultiply (5.2.1) by a nonsingular matrix P(y) to zero out a maximal number of rows of E (y) and permute the zero rows to the bottom to obtain E11(y) [+ G11(y) A [L j Oj Gljy )] (iii) Differentiate the bottom half of the system to obtain the new system, [E11(y) * + [l,(y)] [G1(y)J+ 0 -r, d where we assume dt GC,(Y)=G12(Y)Y (iv) Now apply the reduction algorithm to this new system; namely, E( ll(y ] G to# G(y) r + r (v) Go to step (i).

-80Now we define the index for a nonlinear singular system in terms of the reduction algorithm. Definition 5.2.1 The index of the nonlinear system (5.2.1) is the number of iterations before the reduction algorithm terminates. The idea behind the reduction algorithm is that by continuously differentiating the algebraic equation, eventually the singular equation will become a set of nonsingular differential equation (after properly re-arrangement). This implies that if the reduction algorithm terminates, then the singular equation has a solution. The bilaterally constrained manipulator system described by Eq.(2.2.19) includes an explicit algebraic equation. If we differentiate the algebraic equation with respect to time twice, then the algebraic variable X explicitly appears. Therefore, we say the bilaterally constrained manipulator system has an index of three. In general, the higher is the index of a system, the greater is the complexity of the system. A singular system with an index greater than two is often referred to as a high index system. Unfortunately, the bilaterally constrained manipulator system falls into this group. However, the concept of index reduction provides a way to reformulate the bilaterally constrained manipulator system. This reformulation can provide different models for simulation purpose. It is discussed in the next section. 5.2.2. Simulation Models Frequently, different choices of simulation models affect the complexity of simulation. We have seen that the index of the bilaterally constrained system can be reduced

-81by differentiating the algebraic equation at the expense of increasing the complexity of the algebraic equation. In terms of the index reduction, we can formulate the manipulator equations in three different ways: index three model, index two model, and index one model. In the index three model, the original equations of motion of the manipulator are used as a simulation model. Namely, M(q )q + F(q,)-= T + JT(q)D r(H(q))X, (5.2.2) q(H(q))= 0. (5.2.3) If the constraint is differentiated, we obtain At Oa(H(qg)) = 0. (5.2.4) Consequently, the indez two model consists of M(q)q + F(q,q) = T + JT(q)D T(H())X, (5.2.5) OJ(H(q))= 0. (5.2.6) If the constraint is differentiated again, we obtain Oa (H(q )) = 0. (5.2.7) dt2 Thus, the indez one model is M(q)q + F(q,)= T + JT(q)D T(H())X, (5.2.8) d q(H(q)) =. (5.2.9) Theoretically, the above three simulation models are equivalent to one another as long as the initial values are consistent. In practice, the constraint {(H(q))=0 is exactly satisfied in the index three model; but it is not for the index two and index one models.

-82In general, the lower is the index of the manipulator model, the fewer are the simulation difficulties. From the computational point of view, we prefer to using a model with lower index. An index one model is the most desired since it is the easiest one and its properties has been well studied [5,7,20,60,61,71]. Unfortunately, the complexity of the algebraic equation increases dramatically as the index is reduced from three to two to one. In this sense, an index reduction may not be always useful. Therefore, it is important to choose a suitable simulation method and to understand how the system's index affects the accuracy of the numerical solutions. 5.2.3. Simulation Methods After the selection of a simulation model for the bilaterally constrained manipulator, i is desirable to choose a suitable numerical method to simulate the system. Gear's method [18] is a well-known method to solve stiff systems of differential equations. It can be modified to solve certain types of DAEs (Differential Algebraic Equations) [21]. Brayton et al. [5] has used the BDF (Backward Difference Formulas) method with variable step size and variable order to solve a DAE with low index. Sincovec et al. [71], Petzold [59,61], Liniger [37], Gear and Petzold [19,20], and Brenan [7] have also used the BDF method to solve DAE systems with low index. There are some successes; but there are no algorithms that can handle general DAEs, especially for a nonlinear DAE system with index greater than two. We adopt the BDF method as a basis for our method for two reasons: First, the contact force X (i.e., the algebraic variable in the DAE system) can be directly computed; this is important since the contact force is of interest in contour following problems. Secondly, the BDF method has been successfully used for some DAE systems with index less than three. The detail of the BDF method is discussed below.

-83In order to apply the BDF method, let us re-formulate the manipulator equations, Eqns. (5.2.2)(-5.2.3), as h= y, = R (z,y,X,T,t), to<t tf, (5.2.10) 0 = (), where z =q, y=4, and q(z )=z(H(q )). The time interval [to,tf ] is assumed to be partitioned by time step h, where h — At" =t.+l-t, with to<t <...<tn <tn+l<...tN =t/ Suppose n,, +, yn+l, and X +1 approximates the true solution z(t,,l), y(t,,,), and X(t,,+) to Eq. (5.2.10). Then, to apply the k-step BDF method to Eq. (5.2.10) at t,,+=tn +h, the derivative z and y are replaced by the difference approximation =i + -s+1 - i z +2-i -1 k+1 n+l - S ai Y.n+2-i, for 1<k <6 h = —1 where al,.*, at+l are numerical constants. Using this approximation, Eq.(5.2.10) is converted into a set of nonlinear algebraic equations as R(zn,+lyn+ln+1^ T,+l,t, +) =. (5.2.12) A Secant method is used for solving for z,,l, y,,+, and X,+,. According to the implicit 0R function theorem [15], Eq.(5.2.12) has a unique solution if the Jacobian, where z T=[ T,yT,X, is nonsingular. At each iteration of Secant method, initial guesses of these variables are given by

-84~A k-+1,s +1 - i xn +l4-i, j=1 +1 Y"+1 =,i Yn+l-i, (5.2.13) 1=l +1 Xn+1 - Ai Xn+l-i, i=1 where * *, f', +l are numerical constants. The step size h used in the above can be fixed or varied. Sincovec et al. [71] have shown that when a k-step, constant stepsize BDF method is applied to a linear timeinvariant system of index v, the solution is O(hk) accurate globally after (-1)k +1 steps. However, if the stepsize is varied, but the ratio of adjacent steps is kept bounded, Gear and Petzold [19] show that the global error in the numerical solution is O(h ma), where r =min (k,k -v+2) and i ma is the maximum step size. This implies if we solve an index three system with a variable stepsize, one-step BDF (i.e., the backward Euler method), the numerical solution contains 0(1) error. Petzold [61] further indicates two disasters for a variable stepsize BDF method: (i) the iteration matrix, i.e., the Jacobian 9R R-, used in the BDF becomes more and more poorly conditioned as the current step9Z size tends to zero; (ii) the difference between the predictor and the corrector does not tend to zero as the current stepsize tends to zero; namely, the initial guess for next iteration may not improve with. decreasing stepsize. For these reasons, we will use a constant stepsize BDF. A constant stepsize, twostep BDF method is used to solve the bilaterally constrained manipulator problem. It is defined by Eqns.(5.2.11) with

-85k=2, -3 -1 al1=, a2-2=, =2 (5.2.14) f1=3, 2=-3, B3=1 Since the BDF method is a multistep implicit method, it is not a self-starting algorithm. For k >2 the k-step BDF method requires not only initial values z0, yo, and X0 but also function values at past k points. Namely, the starting values required by the kstep BDF method are s, Y Z^n-,.., n-k +1 Ys t Yn-ly — et Yn-k+1 f ^n^n-lfy ^"-k+l * In order to compute the starting values for the k-step BDF, one-step implicit algorithms are often used as start-up procedures. The software package DASSL [59], implemented with the BDF method, uses the one-step backward Euler method to provide the starting values. However, as mentioned earlier, the backward Euler method gives a 0(1) error in the first step for an index three problem. To alleviate this difficulty, we may use the backward Euler method to the index one model defined in Eqns. (5.2.8)-(5.2.9), and then compute the starting values. The backward Euler method is given by Eqns. (5.2.11) with k=1, a= —1, a2=1, (5.2.15) j=,2, 2=-l1 One alternative method to compute the starting value is to use a one-step, implicit Runge-Kutta method. A fixed stepsize, two-stage, third order, singly-implicit RungeKutta method (10] can be summarized as follows:

-8z2+1= zn + h(clk l + C2k2s) Yn.+ = y, + h(c k,y + c2k2y), (5.2.16) X+1 = Xn + h(c lk X + c2k2X), where ks = z, + hbilkl, + hbi2k2s, Pijy = y, + hbi k ly + hbi2k2y, P, = Xn + hbijklx + hbi2k2, (5.2.17) kiy R(zn +hbi k,, +hbi2k 2, Pi, pix, Tn+Tih, t, +ai h) 0 = (, +hbi lk 1, +hbi2k2s ), i=1,2, and the coefficients a,, c;, bij for i=1,2 are given by.a = (3+v)/6, a X = p(2-2)' a 2 == (2+ V), CI = [4.p(1+v'_)-v]/(8p) (5.2.18) c = [4p(1-vf{)+v2']/(8p), b 1 - (1l-v2/4), b 12 p (1-3v'/4), 21 -= p(1+3/2/4), b22 = p(1+V/24) The differences of computing the starting values with the above two methods are: (i) Ideally, the backward Euler method has 0 (h ) accuracy; but it is 0(1) accurate to an index three system at the first step. The two-stage, third order, singly-implicit RungeKutta method has at least 0 (h2) accuracy for non-algebraic variables z and y, and O (h) accuracy for algebraic variable X [7]. (ii) For the index three system Eqns. (5.2.2)-(5.2.3), (2n+1) equations must be solved at each corrector iteration by the backward Euler method, compared to 2(2n+1) equations by the singly-implicit Runge-Kutta method.

-87In summary, the algorithm for the constant stepsize, two-step BDF method is stated as follows: (i) Given to, tf, zo, yo, Xo, h, ai, and i, for i=1,2,3. (ii) Set flag=-l, t,=+-t+h. (iii) If tn +1<t/, then go to next step, otherwise stop. (iv) If flag=1 then compute the starting values by using the backward Euler method Eqns. (5.2.11) and (5.2.15), or the singly-implicit Runge-Kutta method Eqns.(5.2.16)-~(5.2.18), and set flag=O; otherwise go to next step. (v) Use Eq.(5.2.13) to obtain z,1l, y,+l and X,,+. (vi) Solve Eq.(5.2.12) for z,,+l, y,+ and X,n+ by Secant method with z,,l, 3y,n+ and X,+I as the initial guess. (vii) tn+14-tnl+h; go to step (iii). 5.2.4. Difficulties in Simulation and Proposed Solution The bilaterally constrained manipulator is an index three DAE system. It can be thought as a limiting case of a stiff system as the stiffness becomes infinite. That means most difficulties encountered in stiff systems are also found in the bilaterally constrained manipulator system. Since the manipulator system belongs to the class of high index DAE, errors in simulation are propagated in an unfortunate way. These errors primarily arise from starting values, initial values, round-off (precision of computer), illconditioned iteration matrix, and constraint violation. These issues and possible solutions to them are discussed below. Errors due to Inexact Starting Values

-88As mentioned earlier, additional (k-1) starting values must be computed before a k-step BDF method can be applied to a DAE. Due to the algorithm for computing the starting values, the starting values may contain errors. The BDF method is sensitive to the errors in the starting values; one digit difference in the starting values may result in different solutions [61]. In order to alleviate this difficulty, we may choose an appropriate- initial stepsize for the startup procedure and may use index one model to generate the starting values for high index systems. Errors due to Inconsistent Initial Values Inconsistent initial values (in the sense of Chapter 2) are major factors to cause errors in simulation of a DAE. For the index three system Eqns. (5.2.2)-(5.2.3), there are two sources to inconsistent initial values: (i) go and go are not consistent; (ii) q0=q(to) and q0=q(to) are consistent, but Xo=X(to) is not exact. For the first case, our approach is to treat the inconsistent initial values as consistent values plus disturbances; then a feedback controller is used to reduce the disturbance. For the second case, since no information about the contact force X is given, if we start the BDF method with arbitrary estimate on X, the numerical divergence may occur. The remedy is to compute Xo by Eqns.(2.3.1) and (2.3.2); namely, Xo=A -(qo)D (H(qo))J(qo)M-(q o)[ F(q o,o) - T(to) ] (5.2.19) -A -(qo)[D (H(qo))J(qojgo)+D(H(qo),J(qo)o0)J(qo)]q, where A (q) = D(H(q))J(q)M-(q)J T (q) D T(H(q)), D(p,j) -J D (p), and (q,q) A d- J(q). Note that given go and qo, X0 depends on the input joint torque at to, T(to).

-89Errors due to Round-off Round-off errors are caused by the precision of computing machine. It is usually required that the BDF algorithm be implemented on a double precision machine. In addition, when stepsize becomes too small, the round-off errors dominate the integration errors. Hence, a too small stepsize should be avoided. Errors due to Ill-Conditioned Iteration Matrix an In numerical simulation, the iteration matrix, R-, of a DAE system is usually ill9z conditioned. This results from the stiff nature of a DAE system. The stiff nature of a DAE causes superficial high frequency components in the numerical solution. The high frequency components are introduced to the solution numerically; they do not represent the physical system. Since there exist high frequency components in the solution, the stepsize should be chosen relatively small. This increases computation time as well as results in the ill-conditioned iteration matrix. In order to avoid ill-conditioned iteration matrix, we may use a fixed stepsize algorithm, or we may scale the iteration matrix, especially scaling the row and column corresponding to the algebraic variable X. For the bilaterally constrained manipulator system, a constant stepsize is suggested. Errors due to Constraint Violation The constraint violation primarily is caused by the integration error, where an index reduction procedure is used. In the index reduction procedure, the index of the manipulator is reduced by one whenever the constraint is differentiated. Theoretically, * *q(p )=O, (p )=0O and 0(p )=O are equivalent for consistent initial conditions. However, they are not the same in real implementation due to the propagation of integration errors.

00-s Consider the index one model, Eqns. (5.2.8)-(5.2.9). Let us assume that, after n steps, the numerical solution gives -=6 and e=c which deviate from the exact values q==0 and 0=0. Then, from =-0, the numerical solution will produce 0=e+t+6 at the (n+l)th step. Thus, the violation in the constraint grows as time increases. In order to correct the propagation of the integration error on the constraint, a method for stabilization of constraint violation is proposed by Baumgarte [1]. It is based on the observation that -=0 describes an unstable system, and feedback terms can be used to stabilize the constraint violation, e.g., 0 + 2p, 2 + p222 = 0 (5.2.20) is constructed, where pi and P2 are constants. In short, the integration error is reduced by use of the constraint Eq.(5.2.20). The criteria for choosing pi and P2 depend on the required performance of Eq.(5.2.20), such as damping ratio, overshoot, settling time, etc. In general, we want the integration error to be damped out at each integration step, if it is possible. This implies that a quick response is desirable. Thus, a critically damped system or an underdamped system are qualified. For a critically damped system, pi=wn =P2h where w,, is the natural undamped frequency of the system. For an underdamped system, if we choose the damping ratio 5=0.707, which has the most desirable response, then Pi and P2 can be chosen from the following relations, according to the natural undamped frequency w, and the settling time t,, 4.24328 p=.84083. wn = P2=Wn P=0.84083. te' Nikravesh [50] indicates that, according to his experience, it is adequate to choose the values of pi and P2 between 5 to 50 for most practical problems.

-915.2.5. Other Simulation Methods In addition to the BDF method, a method using independent coordinates and a method using direct integration with constraint stabilization are two potential candidates for the simulation of a bilaterally constrained manipulator. They are discussed below. The independent coordinates method is a constraint elimination method per se. The constraint equation is used to eliminate the redundant coordinates so that a set of ordinary differential equations is used in the simulation. The generalized coordinates partition method [49,82], and the differentiable null space method [35] fall into this group. Their differences are mainly in the ways a set of independent coordinates are chosen. The independent coordinates method may save certain amount of computational time, compared to the BDF method; but the contact force is not computed and it is not always useful to eliminate the constraint. Since the constraint elimination has been discussed in Chapter 2, it will not be further addressed here. The direct integration method with constraint stabilization [35,50] uses the index one model plus constraint stabilization. It directly performs on the second order differential equation system to solve a set of linear equations with unknowns q and X. Recall the index one system with constraint stabilization as M(q )q+F(q, )=T+J T(q)D T(H(q))X, (5.2.21) {(H(q)) + 2p, 2(H(q)) + p22q(H(q)) = 0. (5.2.22) After some manipulation of Eq.(5.2.22), it is written as D (H(q))J(q )q+[D (H(q ))J(q,)+D(H(q ),J(q )})J(q ) +2p2D (H(q))J(q)lq+p22+(H(q ))= 0, or equivalently as

-02D(H(q))J(q )q+h (q,) = 0, (5.2.24) where h (q,i)=[D (H(q))J(q,4)+D(H(q ),J(q )q)J(q) (5~2.25) +2pl2D (H (q ))J(q )]+ 0p22(H(f )). Re-arrange Eqns. (5.2.21) and (5.2.24) into a matrix form, we have [(M( ) JT()DO (H(q))] [ ]=[7T-^ ) (q (5.2.26) D (H(q )J(q) 0 J'-X(,) J' Note that Eq.(5.2.26) is a linear system with unknowns qi and X. If q, q and T are given, then Eq.(5.2.26) has a unique solution for q- and X [50,82]. Since q, q and T are known at t =t, we can use Eq.(5.2.26) to obtain q` and X at t ==to+At; then we integrate q to obtain q and q at t=to+At. If we continue this process, all q and q for t<t <t/ can be found. Thus, the direct integration method is possible. But, the integration errors may be large. 5.2.8. Remarks So far, we have discussed simulation models, the k-step BDF method, simulation difficulties, and other possible simulation methods. Based on the discussion above, we choose the index one model with constraint stabilization as the simulation model, and use the constant stepsize, two-step BDF method as the simulation method for the bilaterally constrained manipulator system. Note that the Newton-Euler formulation [28,42,51,52,70] of the manipulator dynamics may be useful for simulation of the bilaterally constrained manipulator.

-935.3. Simulation for Unilaterally Constrained Manipulators Simulation of the motion of a unilaterally constrained manipulator, as described in the contour following problem of Chapter 1, contains three generic parts-unconstrained motion, constrained motion, and transition. They are discussed in subsequent subsections. A complete program arrangement is given in the last sub-section. 5.3.1. Simulation for Unconstrained Motion The unconstrained motion segment of the unilaterally constrained manipulator is defined by M(q)q-+F(q,q4)=T. (5.3.1) Since the inertia matrix M(q) is nonsingular, Eq.(5.3.1) is a set of ordinary differential equations. Any ordinary differential equation solvers can be used to solve Eq.(5.3.1). Here we choose the variable stepsize, fourth order Runge-Kutta-Gill method [64]. This method is chosen because it not only controls the growth of round-off errors at each step but also provides a way to compute the truncation errors; whereas, the classical RungeKutta method [18] neither computes nor estimates the truncation errors in the calculation procedure. The fourth order Runge-Kutta-Gill method is stated as follows. Given the differential equation Y= (yt) (5.3.2) with the initial conditions y(to)=yo, then the iteration formula at the (n+l)-th step is defined by Y, +1 = 3+1 (k4-273), (5.3.3) where

-94kI = h(yn t ) I = y, + (k1-27-o), k- A n +(z, t,, + ), k3 = ht(z2, t+ + ) 3 = Z2 + (1+1/,)(k3-72) k4 = hA(z3, t,+Ah), 1 -'To + 3[- (k1-2-yo)l- kl, "72 -=' + 3((i-1/vf2)(k2-'1)l - (1-1/vX)k2, s3 -72 + 3[(I+1/V')(k3-y2)J - (1+1/V2)k3,'4 73 + 3[1 (k -273)1- k 4. q 2 Note that 74 represents approximately three times the round-off error in y,,lj accumulated during one step. In order to compensate for this accumulated round-off error, initially, let 0o=O at t =to; thereafter, in advancing the solution, let 70(to )=-'4(tn-), n=-1,2,... 5.3.2. Simulation for Constrained Motion The constrained motion segment of a unilaterally constrained manipulator is defined by M(q)q + F(q,q)= T + JT(q)D T(H(q))X, (5.3.4) f(H(q))= 0. (5.3.5) Eqns. (5.3.4) and (5.3.5) exactly define a bilaterally constrained manipulator. Therefore, the simulation algorithm described in section 5.2 can be used for a constrained motion segment. Namely, an index one system with constraint stabilization is used as the simulation model, and the constant stepsize, two-step BDF method is used as the numerical

-95method. When a unilaterally constrained manipulator changes from an unconstrained motion segment to a constrained motion segment, an impulsive impact may occur. This impulsive impact results in inconsistent values for the constrained motion segment. Another factor that causes inconsistent values is due to the accumulated integration errors from the previous unconstrained motion segment. Thus, it is required that Eqns. (3.3.8) and (3.3.9) be used to obtain the consistent values for initiating the constrained motion. Furthermore, the two-stage, third order, singly-implicit Runge-Kutta method, Eq.(5.2.16), must be used for the first one or two steps after an entry time in order to compute the starting values for the two-step BDF method. 5.3.3. Transition Procedures Transition procedures include a procedure for computing an entry time, a procedure for computing an exit time, and procedure for computing the impact relations at an entry time. Each of these procedures is presented below. Procedure for Computing an Entry Time Recall that an entry time is the time instant where the manipulator moves from an unconstrained motion segment to a constrained motion segment. If the "exact" entry time can be obtained, then we can accurately compute the impact relations and provide more accurate values for initiating the constrained motion segment. Unfortunately, the constraint I(p )=O is never exactly fulfilled at transition. Instead, the condition q(p)<e, where e is a small positive number, is used as the testing condition for transition in real implementation. Consequently, it is necessary to "estimate" the entry time. Consider the two cases shown in Fig. 5.3.1.

*~6t..(P )>0' "p>o. - i(p)<O p)< t6 (a) (b) Fig. 5.3.1 Compute an entry time tE. (a) Interpolation. (b) Extrapolation. We assume t, <t4, (p(t, ))=-, >f, qP(p(tb ))=b <, and tE is an entry time. In Fig. 5.3.1a, O <0, the entry time tE occurs between t, and t4; thus, an interpolation technique can be used to find tE. In Fig. 5.3.1b, 0<kb <f and tE >t; thus, an extrapolation technique must be used to find tE. Let us first discuss case one. Assume t, < tE < tS; O(p ) is expanded about p (t, ) and p (tb ) in a Taylor series to obtain b, ==O +D (P(as ))(fs Xtb -ts ), (5.3.6) o, =O +D (p (b ))(b7 X t, -tb ), (5.3.7) where the higher order terms are ignored, and q, =(p(t, )), b- ='(p (tb )). Solve Eqns. (5.3.6) and (5.3.7) for a, and a, then solve k(p (tE ))=0 to obtain tE as tE, - (p (,,)),(o ) for t < t <tb. (5.3.8)

-97Now we summarize the interpolation algorithm for computing an entry time: (i) Solve nonlinear equations Eqns. (5.3.6) and (5.3.7) for A, and a. (ii) Use o, in Eq.(5.3.8) to obtain the entry time tE. (iii) If (p (tE ))='E >, then t,- tE, -a -OE, go to step (i). (iv) If'bP <'E <0, then t6'-tE, ob -O"E, go to step (i). (v) If OQ<E <e, then stop. Next, let us consider case two. When 0<qb <E, as shown in Fig. 5.3.1b, the entry time can not be obtained by interpolation. Instead, an extrapolation technique must be used. Again, O(p(t)) is expanded about p(tb) in a Taylor series and the higher order terms are ignored, we obtain p (tE ))=-b +D (P (tb ))(tb )(tE-tb ) * (5.3.9) Since (p(tE ))=O, then tE is tE ~ - D( (t))(t) for t> tb. (5.3.10) Note that we do not solve any nonlinear equations in this extrapolation. Now we summarize the extrapolation algorithm for computing an exit time: (i) Use Eq.(5.3.10) to obtain the entry time tE. (ii) If'(p (tE ))='E <0, then tE = t, stop. (iii) If O<_E _b6, then stop. Procedure for Computing an Exit Time Recall that an exit time is the time instant where the manipulator moves from a constrained motion segment to an unconstrained motion segment. It is important to

98&. compute the exit time so as to provide accurate values for initiating the next unconstrained motion. Numerically, the constraint 0(p )0= is not perfectly satisfied during a constrained motion segment. The same interpolation technique used for computing an entry time can also be used to compute the exit time tL (see Fig.5.3.2). From Eqns. (5.3.8)-(5.3.8), the exit time tL is given by tL " D (p,))^ )' <tL <.11) t tL - H )>0'.^r\vJ —-'''''''' -..Kp )<o Fig. 5.3.2 Compute an exit time tL Given t,, t,, q(p (t, ))=s,, O(p (t( ))= with I, I <c and hb >c, the algorithm for computing an exit time is summarized as follows: (i) Solve nonlinear equations Eqns. (5.3.6) and (5.3.7) for a, and b.

(ii) Use ab in Eq.(5.3.11) to obtain the exit time tL. (iii) If q{(p(tL ))-=L >e then tb -tL, ob "OL, go to step (i); otherwise stop. Finally, let us discuss procedure for computing the impact relations. Procedure for Computing the Impact Relations To compute the impact relations is equivalent to compute the impulsive impact force e and the velocity (t,+) of the end effector after impact, where t, is an entry time. This 4(ti+) is used as the initiating values for the next constrained motion segment. Recall Eqns. (3.3.8) and (3.3.11), the impact relations are described by f-= -A -(q(ti ))D (p(ti ))J(q(ti ))(ti-), (5.3.12) = ti+) =- (ti -)+M-(q (ti ))J (q (t ))D(p (t)), (5.3.13) where tj- and ti denote left and right hand limits, and A (q) D(p )J(q)M-4(q)JT(q)D T(p). (5.3.14) Now, the algorithm for computing the impact relations is stated as follows: (i) Solve the set of linear equations M(q(ti ))C=-J(q(tj ))D T(p(ti)) to obtain the n-dimensional vector C. (ii) Compute A (q(ti)) by A (q(ti))=D (p (ti ))J(q(t ))C. (iii) Solve the linear equation A (q (ti )) -D (p (ti ))J(q(t )) ti-) for ~.

-100(iv) Compute t(ti+) by t,+) = -(ti-) + C~. 6.3.4. Complete Program Arrangement This section presents an overview of the complete program arrangement for a unilaterally constrained manipulator. Two procedures- MSU (Unconstrained Motion Segment) and MSC (Constrained Motion Segment)- are employed. Procedure MSU deals with an unconstrained motion segment, and procedure MSC deals with a constrained motion segment. The implementation of these two procedures are based on propositions 3.4.1 and 3.4.2. Note that these two procedures are recursive in nature. The main program is illustrated in Fig.5.3.3. The program flow of procedure MSU is given in Fig. 5.3.4, and the program flow of procedure MSC is given in Fig. 5.3.5. Note that we use start Initialization a E, IB m,,,^ hmi to, t/ tn+ to+h; tout-tn + Specify the initial motion segment. Call procedure MSU Fig. 5.3.3 Main Program

.101Use 4-th order Runge-Kutta-Gill method to solve Eq.(5.3.1) Transition tfst(p} )_i_ +i condition D (p )pO 0l+l IY Compute the entry time tE Zn +1-2 (tE) tout- tE impact D(p)p<O condition Y Compute the impact relations e and q(tE+) in+,-i(tE) Change to procedure MSC Fig. 5.3.4 Procedure MSU

-102SdYS:- ----- s —y Initialization compute the starting values Use constant step-size, 2nd order BDF method to solve Eqns. (5.3.4), (5.3.5) Transit'ion p)s>.c N+ to ti +a 1Y condition (p)P>Ot tl Compute the exit time tL z +2,.+1-z (tL) tout —tL Change to procedure MSU Fig. 5.3.5 Procedure MSC

CHAPTER 6 A CONTOUR FOLLOWING EXAMPLE To this point, all developments in this thesis have been aimed at solving the contour following problem, which was described in Chapter 1. In this chapter, we study a simple example contour following task and show how the previous developments can be applied to this task. The description of the contour following task is discussed in section 6.1. The generation of the desired path and motion planning issues are discussed in section 6.2. Finally, a simulation of this example task is presented in section 6.3. All progams used in this chapter are written in PASCAL and have been executed on a VAX 11/780. 6,1. Description of a Contour Following Task The objective of the contour following problem is to move the end effector of the manipulator from an initial location, which is assumed not to be in contact with the constraint surface S, to a point in S; then the end effector is moved along a curve in S from that point to another point in S with a specified contact force; finally, the end effector is moved back to the initial location. In this contour following task, the initial and final locations, the manipulator dynamics, and the constraint are given. The main purpose of this section is to derive the constrained manipulator dynamics and to carefully define the -103

-104task objective. A simple two degree-of-freedom Cartesian robot, such as a SIGMA robot manufactured by the Italian company, Olivetti, is used to illustrate the contour following task. A Cartesian robot consists of orthogonal prismatic joints and a nonrotary-base axis, as shown in Fig. 6.1.1. Fig. 8.1.1 Two degree-of-freedom Cartesian robot The position vector of the end effector in planar workspace coordinates is denoted by p =z,y)T. For simplicity, we assume that each link of the robot has a unit mass, and that gravitational, centrifugal and coriolis effects can be neglected. Thus, the open chain manipulator dynamics are given by 2 - F.,.. i;, (6.1.1)

-105where F, and Fg are joint forces. Assume the manipulator performs a contour following task on a constraint circle defined by'(p) = z2 + (y-1.5)2 - 0.25>0. (6.1.2) Consequently, from Chapter 3, the complete set of equations of motion used to describe the contour following task is obtained as 2 = Fs + 2zX, y Fy + 2(y-1.5)X, \>,:2 + (y-1.5)2 - 0.25>0, (6.1.3) X[z2 + (y-1.5)2 - 0.251=0, ( t,) = t-)+2 (t, ) It;+) =- (t-)+2[y(t, )-1.51, where X is the contact force, t, is an entry time, and f is the magnitude of the possible impulsive impact force. Now a contour following task can be stated as follows: The end effector of the manipulator is initially at rest at position (,y )=(0.4,0.8). With the manipulator dynamics defined in Eq.(6.1.3), the end effector is to move from the initial position (z,y )=(0.4,0.8) and contact the constraint at position (z,y)=(0.1303,1.0173). Then, the end effector follows the constraint in the counter clockwise direction with a specified constant contact force, say X=l1 nt, to another point on the constraint, (z,y )=(0.3716,1.1654). Finally, the end effector leaves the constraint and moves back to the starting point. Given this example contour following problem, path planning and motion planning issues are discussed in the next section.

-1058.2. Planning Issues for Contour Following Problems The planning issues consist of two parts: path planing and motion planning. These two parts are discussed in the following two sub-sections, based on the results developed in Chapter 4. 6.2.1. Path Planning The objective of path planning is to select a path parameterization function. Let 0<. <1 denote the path parameter, P T( )=(fz(), (s )] denote the parameterization function, and pjT=[zo,yol, p lT —[Zl,y, p2r=[z2,2J, P2 =-[z,yvf denote the initial point, the entry point, the exit point, and the final point, respectively. According to the problem description in section 6.1, we have [x0 o_[ __ 0.4] [Zi] _O.1303] [z2J [0.3716 YI LY[I L *8JY' y -l.0173' [y 1.1654j (6 ) where these values are measured in meters. As mentioned in Chapter 4, the path can be chosen to satisfy the impact avoidance condition. Two different choices of the path function are considered. Path with discontinuous slopes at the entry time and the exit time In this case the path is chosen to satisfy the boundary conditions (z0,yo), (zI,y1), (22,y2), (z2,yf ); the normality condition, [z' (a )2+[yg (a )12_L-, 0<s <1; and the constraint, z2(a )+[((e ) -1.512-0.25>0, O< <1; where z' ( )= aZ( ). We can adjust a1 and 2 so that the normality condition is satisfied. After some computation, the path is obtained as follows: 0<s <sl

-107z(a)- a la + a2, (a) = al + a2 ~, (6.2.2) y(s) = a3s + a, 81<8 <82 z( ) 0.scoo (2 -2), (6.2.3) y(a ) 1.5+0.5 in(28-2), 82<8 <1 z(8) — CIS + C2, X1(J~)=-Cl'+~c~~2, (6.2.4) y()-= c38 + C4, where z (0)=z (1)=0, z(81)=z1, z(S2)=z2, and 1 -0.3464, 2 - 0.6335, a s -0.7788, a 2 = 0.4, a - 0.6273, a 4 -0.8, (6.2.5) c = 0.0776, C 2 0.3224 c - -0.997, c 4 1.797. The desired path and constraint are shown in Fig. 6.2.1. Points (zo,yo), (z1,yj) and (Z2,Y2) denote the initial point, the entry point, and the exit point, respectively. The slope of the path is discontinuous at the entry point and the exit point. Path with continuous slope In this case the path is chosen to satisfy the boundary conditions (zo,yo), (z 1,V1), (z2,Y2), (Zl,l ); the impact avoidance condition at s, z(as1)z ( J)+[y( 1) —1.5]y' (a )=O; the tangency condition at s2, Z(x2)z' (a2)+[Y(2)-l15]Y' (82)=~; and the constraint, z2( )+[y(s )-1.5]2-0.25>0, 0<a <1; where z' (a )= aZ(a ). In order that the path is tangent to the constraint at the entry time and the exit time, two more conditions must be satisfied:

-108a9Y( 9) a9y(J) 9Y(s8 ) ag(J) - = and -. After some computation, the path is oz ax ax dx obtained as follows: 0<s <51 z(s) — dls2 + d2 + d3, (8.2.6) y(.) d4a2 + d5a + d, 81:58 <82 z () = 0.5co (2 -2), (6.2.7) (6.2.7) y(s ) - 1.5+0.5ein(2 -2), s2<8 <1 Y(a) = eC12 + e2C + C3, y(8) = e4s2 + e5s + es, where z (O)-2 (1)=0 2 ( 1)=-z, z (x 2)=22, and = -- 0.3464, 2 - 0.6335, d -= 5.036, d2 = -2.5231, d3 = 0.4, d4 = -1.0589, d5 = 0.994, d6 = 0.8, (6.2.9) e - =-1.614, e2 2.7139, e3 -0.7, e =- -4.7475, e 5 6.758, e - -1.2104. The desired path and constraint circle are shown in Fig. 6.2.2. Points (z0,yo), (xl,yl) and (z 2,Y2) denote the initial point, the entry point, and the exit point, respectively. The slope of the path is continuous at the entry point and the exit point. 6.2.2. Motion Planning Motion planning is to generate the desired motion sequence; then the desired joint forces are computed. Motion planning can be formulated in two ways in terms of

-109satisfaction of the impact avoidance conditions: namely, (1) the end effector comes to rest at both the entry time and the exit time, (2) the path is tangent to the constraint at both the entry time and the exit time. These two cases are considered separately. In each case, a kinematic approach to motion planning and a minimum time approach to motion planning are discussed. CASE1 I (t 1)O and )(t-= In this case, the end effector comes to rest at both the entry time and the exit time. Hence, no impact occurs at the entry time t l. Eqns. (6.2.2) —(6.2.5) are used to generate the desired path for motion planning. The desired contact force is specified by 1 1other<is < (6.2.10) With the above information, the joint forces F, and Fy can be obtained by the following equations: 0< <<1 Fs -al, F_3~ =.... a1~~~~(6.2.11) Fy= - a3', 81i8 <.2 F, -a in (2. -2)'S-2 i2co (2s -2)-coa (2. -2), (6.2.12) Fy = Co (2. -2)1-2i28in (2a -2)-sin (2s -2), a2<a <1 F, -'- d',.Fs.= c11,8 Y(6.2.13) Fy - C3a, where a,, c;, and al,.2 are given in Eq.(6.2.5). In order to find the path history a(t),

-110two approaches, a kinematic approach and a minimum-time approach, are used. (a) Kinematic approach In this approach, the entry time t1, the exit time t2, and the final time tf are specified. They are tl=1.56 sec, t2=5.21 sec, and t1 =7.0 sec. From Eq.(4.3.20), the path history a (t) is given by b l(t-t l)3+b 2(t-t 1)2+, O<t <t, a(t) bS(t-t1)3+b (t-tl)2+l, tl<t <t2 (6.2.14) b5(t-t2)3+b6(t-t2)2+2, t2<t <7.0 where -= 0.1825, b2 -0.427, b3 =-0.0118, 4 -= 0.0647, b6s -0.1278, b6 = 0.3432, (6.2.15) i =0.3464, 82 -0.6335. The joint forces F, and Fy are obtained from Eqns. (6.2.11)~(6.2.13). The plot of i versus a is given in Fig. 6.2.3. Fig. 6.2.4 shows z(t) and y(t) versus time. Fig. 6.2.5 shows the desired joint forces. The phase plane plot, Fig. 6.2.3, shows that the end effector comes to rest at the entry time where a =0.3464, and at the exit time where a =0.6335. The joint forces plot shows that the joint forces F, and Fy change in a linear way when the end effector performs an unconstrained motion and decrease from the entry time to the exit time when the end effector performs the constrained motion. The joint forces are discontinuous at both the entry time and the exit time. (b) Minimum-time approach In this approach, the entry time t1, the exit time t2, the final time tf, the path history s (t), and the joint forces F,, Fy are to be determined so that the boundary con

-111ditions, (0)=O, i(0)=O, J(tl)=ai, J(t1)=O, a (t2)=-2^ J(t2)=O, J (tl )=1, i(tf )=O, and the following joint forces constraints -1<F, Fy <1 (6.2.16) are satisfied and tf is a minimum. The single equation for i' is obtained as a 1F+a 3Fy 0, O< <,' -sin (2s -2)F, +cos (2a -2)Fy, s 1 <s2, (6.2.17) lF +c3Fy, 2<8 <1, where relations, a +a2=1 and c 1 +c2=l, are used; a1, a3, C, C3, i, J2 are given in Eq.(6.2.5). From discussion in Chapter 4, the minimum-time motion planning problem for this case can be divided into three sub-problems. The phase plane technique can be used to solve these three sub-problems; we obtain r1 - 1.0383 sec, r2 = 2.4333 sec, r3 = 1.2014 sec, tf rl+r2+r3 = 4.673 sec, where r1 denotes the cost function of the minimum-time sub-problem for 0<s <al, r2 for < I < < 2, and T3 for 02<5 <1. The complete phase plane trajectory and inadmissible region are shown in Fig. 6.2.6. The trajectory passes through points (0.3464,0) and (0.6335,0). This shows that the end effector comes to rest at the entry time and the exit time. Note that inadmissible region exists only for the constrained motion; there are no inadmissible regions for unconstrained motion segments. In addition, the phase plane trajectory never touches the inadmissible region. There is only one switching point for each motion segment. The displacement z (t) and y(t) are given in Fig. 6.2.7. The joint forces Fs and Fy are shown in Fig. 6.2.8. Clearly, the joint forces for minimum time motion planning are larger than those for kinematic motion planning. Moreover, the joint forces are discontinuous not only at the entry time and the exit time but also at

-112each switching point. CASE 2 i(t ).0 and s(t2)i0 In this case, the path is chosen to satisfy the impact avoidance condition so that no impact occurs at the entry time t. Eqns. (6.2.6)-(6.2.9) are used to generate the desired path for motion planning. The desired contact force is specified by Eq.(6.2.10). With the above information, the joint forces F, and Fy can be computed by the following equations: 0<O <lI F, - (2des +d2)8+2dl 2, (6.2.24) Fy =(2do +d5s)+2d42, 81_8 <82 F = -sin (2s -2)'f-2i2co (2. -2)-cos (2s -2), Fy = cos (2s -2)'-2i2ain (2s -2)-sin (2a -2), o2<s <1 = -=(2*la +e2)~ +2el2, BFi5 =~ (2e~ 4 +eC s2)+2c~ (6.2.26) F = (2Ce4+e 6)+2e42, where di, e,, and sI, 82 are given in Eq.(6.2.9). In order to find the path history a(t), two approaches, a kinematic approach and a minimum-time approach, are used. (a) Kinematic approach In this approach, the entry time tl, the exit time t2, and the final time tf are prespecified. They are tl=1.42 sec, t2=4.25 sec, and tf =5.67 sec. From Eq.(4.3.14), the path history a (t) is given by

-113l(t —t1)3+/2(t -t 1)2+V(t-t)+l. 0<t <tl, (t )= v(t -tl)+8, t<t <t2, (6.2.27) 3(t -t2)3+14(t -t)2+ (t -t2)+8 2 t2<t <5.67, where v = 0.1014, 1 = 0.3464, 8 2 0.6335, -= -0.1916, 12 -0.3724, 13 = -0.2057, (6.2.28) 14 = 0.4024. The joint forces F, and Fy are obtained from Eqns. (6.2.24)-(6.2.26). The plot of i versus a is given in Fig. 6.2.9. The path speed i of the end effector is maintained constant, i.e., 0.1014m/ lec, throughout the constrained motion segment. That is, the end effector does not come to rest at the entry time and the exit time. Fig. 6.2.10 shows z(t) and y(t) versus time. Fig. 6.2.11 shows the desired joint forces. Note that the joint forces almost change in a linear way during the constrained motion. In addition, the joint forces are discontinuous at both the entry time and the exit time. (b) Minimum-time approach In this approach, the entry time t1, the exit time t2, the final time tf, the path history 9 (t ), and the joint forces F5, Fy are to be determined so that the boundary conditions, (0)=0,'(O)=O, (t )=, a (t2)=a2, (tf )=1, i(tf )=0, and the joint forces constraints Eq.(6.2.16) are satisfied and tf is a minimum. The single equation for'1 is obtained as

-114(2dla +d2XFs -2dli2)+(2d4a +dsXFy-2d4i2) (2d8 +d2)2+(2d48 +ds)2 0<_ <8,' = -.in(2s-2)F, +co (2a -2)Fy sl a <z2, (6.2.29) (2e a+eC2)(F2-2epi2)+(2e4 +e 5XFy -2e4i2) 82<8 <1 (2e 1a +e 2)2+(2e 4 +e 5)2 where di, e, a1, and s2 are given in Eq.(6.2.9). From discussion in Chapter 4, the minimum-time motion planning problem for this case must be treated as a whole. The phase plane technique can be applied to solve this problem, resulting in t! = 3.992 sec, t = 1.4285 sec, t2 = 2.492 sec, where a(t )==a 1=0.3484, and a (t2)=as2=0.335. The complete phase plane trajectory and inadmissible region are shown in Fig. 6.2.12. The boundary curve is denoted in dashed line. There are two cusps and two jumps in the boundary curve. Due to this complicated boundary curve, it is difficult to determine the phase plane trajectory, especially at those points with discontinuity. The resulting trajectory has six switching points. The displacement z(t) and y(t) are given in Fig. 6.2.13. The joint forces F, and F. are shown in Fig. 6.2.14. The joint forces always try to exert at their maximum or minimum values. They are discontinuous at switching points. 6.2.3. Remarks If the end effector comes to rest at the entry time and the exit time, the total minimum traversal time (CASE l(b)) is 4.673 seconds. On the other hand, if the end effector does not stop at the entry time and the exit time, the minimum traversal time (CASE 2(b)) is reduced to 3.992 seconds. However, the minimum-time problem in CASE 2(b) is a more difficult computational problem than the problem in CASE l(b).

-1158.3. Simulation Simulation is used to validate the response of the motion planner. As mentioned in Chapter 5, an index one system is used as a simulation model. Rather than using the original constraint equation 2 + (y-1.5)2-.25 -, (.3.1) the following modified constraint equation 2zF, +2(y-1.5)Fy +4[z 2+(y -1.5)1X+2(i2+y2) (6.3.2) +4p2[z+(y -1.5)l]+p2[ 2+(y-1.5)2-0.25l =, is used for 81<a< 2, where pl-5, p2=25. In addition, a PD (proportional and derivative) controller is used for closed loop simulation. Let Fd, Fd, Zd, yd, id, and iy denote the desired joint forces, displacements, and velocities, respectively, which are generated from the motion planner. Then, the PD controller is governed by the following two equations: F, Fd _ -1.5(i-id )-(.- d), ((.3.3) F, Fd- 1.5(-d)-y) (y-yd) ) The feedback control gains have been selected so that the undamped natural frequency is 1.0 and the damping ratio is 0.75 for each closed loop, if the constraint is inactive. A Runge-Kutta-Gill method is used to simulate the unconstrained motion; a constant stepsize, second order BDF method is used to simulate the constrained motion. The motion changes from an unconstrained segment to a constrained segment or vice versa are handled by transition procedures.

-11a6.3.1. Simulation Examples Since the main purpose of the simulation is to justify the performance of the motion planner, we do not simulate all four cases discussed in section 6.2.2; only CASE 1(a) is simulated. In addition, in order to determine the effect of model errors on the manipulator system, three cases are considered: (I) No model errors This is the ideal case. The results are shown in Fig. 6.3.1-Fig. 6.3.5. As expected, the computed responses z (t ) d (t), y(t )=yd(t) exactly. The computed contact force closely follows the desired contact force. (II) Inexact radius of the constraint circle The radius of the original constraint circle, used in the planning phase, is 0.5 m. Suppose the actual radius is increased by 10%, i.e., to 0.55 m. With the same motion planner, this effect should cause early contact between the end effector and the constraint. Simulation results are shown in Fig. 6.3.6~Fig. 6.3.10. Note that an impulsive impact force exists in this case. By examining Fig. 6.3.10, we find that there is end effector chatter near the ideal entry time t =1.56 sec. In order to understand the end effector motion behavior around the ideal entry time, the original constraint circle (dashed line), the new constraint circle (dashed line), the desired path (dashed line), and the computed path (solid line) are plotted in Fig. 6.3.11. Point A now is the new entry point. It seems that the end effector moves clockwise after point A, then it moves back counterclockwise after a short time period. The portion Q is enlarged and attached to Fig. 6.3.11 so that we can closely examine the end effector motion. The velocity components of the end effector just after impact at point A are negative, i.e., z<0 and y<0, so that

-117the end effector moves along the constraint circle in the clockwise direction. The reason is that the end effector motion is planned so that the end effector should come to rest at the planned entry time. In other words, the planned joint force component Fy at point A is negative, i.e., F. <0. This results in negative values of the velocity of the end effector just after impact at point A. Therefore, the end effector moves clockwise and leaves the constraint. There is an end effector chatter as motion proceeds to point B. This chatter is thought to arise as a consequence of the numerical methods used. We suspect that the chatter effect may be caused by the numerical difficulties in the DAE routine, as mentioned in Chapter 5. After point B, the end effector completely moves off the constraint. At point C, the values of velocity components of the end effector become positive. The end effector moves onto the constraint at point D; an impulsive impact also occurs at point D. At point E, the end effector leaves the constraint. From the above discussion, we conclude that feedback control of the contact force is very important. If we can control the contact force effectively, the chatter effect may not occur and improved tracking accuracy may be achieved. (III) Coulomb friction on the constraint circle Suppose there is Coulomb friction on the constraint surface; let p denote the friction coefficient. The friction forces reflected in z and y coordinates are given by f _t -,X 4 /= -A (6.3.4) when the constraint is active. Thus the simulation includes the effects of these two terms; A is chosen as 0.05. Simulation results are shown in Fig. 6.3.12~Fig. 6.3.16. Note that in Fig. 6.3.14 and Fig. 6.3.15, the slopes of i and y are discontinuous at t=5.21 sec,

-118due to the discontinuities in friction forces when the end effector leaves the constraint surface. The constraint (dashed line), the desired path (dashed line), and the computed path (solid line) are plotted in Fig. 6.3.17. The end effector exactly follows the desired path up to point F. Since the end effector motion is planned so that the end effector should come to rest at the exit point (z2,Y2), the joint forces component F, is negative so that the end effector can be slowed down before the exit time. At point F, the joint forces can no longer overcome the friction forces. Thus, the end effector reverses direction to move to point G; then it leaves the constraint and returns to point H. The errors due to Coulomb friction are quite large; the main reason is that the selected PD controller does not directly regulate the contact force, hence resulting in excessively large tracking errors. To remedy this large error, we may incorporate the friction forces in planning stage, or we may design a feedback controller so that the contact force is directly regulated. 8.3.2. Remarks Only a few simple example simulations have been presented. But the difficulties in the computer simulation methodology are indicated. As indicated previously, development of effective simulation procedures for this class of robotic problems is a challenging task. These simulation results also indicate the importance of selecting a suitable feedback controller. In the examples, the feedback controller was selected based on the assumption that the constraint is inactive. But, as seen from the resulting responses in Fig. 6.3.11 and Fig. 6.3.17 the tracking errors can be quite large. This suggests that a careful investigation of feedback control of constrained manipulators is required.

-119. 2.500 - 2.000 - Constraint Circle. 500 - 1.000 -.5000 - 0.000 - -—.u I I -.5000.0000.5000 1.000 1.500 2. 00 x Fig. 6.2.1 Path with discontinuous slopes at the entry and exit times

o1202.500 - 2. 00 - Constraint Circle X. 50 U m i\ {(12,2)} 1. 000 - (zo0, y0).5000 - 0.000 -, I I I -.5000.OUOO.5000 1.000 1.500 2.000 Fig. 8.2.2 Path with continuous slopes at the entry and exit times

*121-.3330 -.2664 -.1998 -.1332 -.0666 0.000 -I,.. - 0.00200.00.600 0.8000 1. 000 Fig. 6.2.3 Phase plane plot of kinematic approach with Fig.B.2.1 path

-122X - 1. 165 ^Q.. ~ ~ ~ o \.9584 -' i-n.7514 -I l.5443 CZ.3373 \.130.L302 1 --..... *''', —', 0.000 o1.o.2.800 4.200 5.600 7.000 T ime Fig. 6.2.4 Displacement plot of kinematic approach with Fig.8.2.1 path

-123Fx F 4.9988 -.. F'air.6332- ta~~ a U.2676' 0 0 -.0979, I -.82921,, 0.00o 1.400 2.800 4.200 5.600 7?. 0 ri.me Fig. 8.2.5 Joint force plot of kinematic approach with Fig.6.2.1 path

-124trajectory - boundary.5600 Inadissible Region a,2800 -.L100 - 0.000 - 0.000.2000.4000.6000 8000 1.000 Fig. 8.2.8 Phase plane plot of minimum-time approach with Fig.8.2.1 path

-125X ---.?, —-514 - 1.165' - a.-.... —-— "'.33517 - rime Fig..2. Displacement plot or minimum-time a ah with Fig..2.1 path, al

-126F _ - Fx 1. 000.................,-rr ~,. e I 1C,000 ---- ------ ---------- -. —. —- -.6000 - a*.e 0~080 flue c i _.. on oc lto iiu-ieapoc ihFg^21pt -.2000 T a -.6000 - -.2000 - - 0.000 Time Fig. 6.2.8 Joint force plot of minimum-time approach with Fig.6.2.1 path

-127-.3639 -.291 t - 32183.456-.0727 0.oo00., -- I. 000.2000.4 000.8000.0 1.000 Fig. 6.2.9 Phase plane plot of kinematic approach with Fig.8.2.2 path

*128, 1. 194,"';0 t'*- ^ no tA ~ e CU tlj ai.0839 0.000 1. l34 2.26a 3.402 4.536 5.6?0 Time Fig. 6.2.10 Displacement plot of kinematic approach with Fig~..2.2 path Fig. 6.2.10 Displacement plot of kinematic approach with Fig.6.2.2 path

-12 Fx F. 2.594 1 1.627 u.6605 2 4 0 0.000 1. l34 2.268 3.402 4.536 5.670 Time I. C: -' I 0.000 1.1t3 2.269 3.402 4.526 s.6?0

.130traje t or - boundary.9067''(< // / // %'/ / 1' -,I / / / / / / / / r''///, ^/// // ///////A^ ^.3627 *';^\f.181 3 -. / \~~~/.7253 a a 0^. 000.20 00.40 00.60 00 S8000 1.00 0.5uu0~~~~~~~ Fig. 9 h..2Pasadn po fmissibu-tm epro eit Fion. pt V /r 9I ~cn ~ ~~~ ~ ~~~~~~ I c -.i813 0.000.60.80 0.000.2000.4000 1.000800i~o S Fig. 8.2.12 Phase plane plot of minimum-time approach with Fig.8.2.2 path

0131x 1.195 - C,,= 2974 - 24 i' ", %.750 3 - 0U:a.5282.3061l.0839 0.000. 984 1.59? 2.395 3.194 3.992 Time Fig. 8.2.13 Displacement plot of minimum-time approach with Fig.6.2.2 path

-132F, F 1.000. —. —--—. —--—....... I I SI 0 o -.20 00 -- - * —---- -.000 I I I Time I Fg 821 J fc lomI ap c wt Fi 4, I 0.000.-984 1.597 2.395 3.194 3.992 Tome Fig. 0.2.14 Joint force plot of minimum-time approach wRith Fig.~.2.2 path

-133- computed value -- desired value.4000.3460 292 t -.a38t -.1302 0. 000 1. 400 2.800. 200 5.600 7.000 Time Fig. 6.3.1 Displacement x without disturbance

-134-- computed value -- desired value 1.165 - 1.092 /!, 01^9 0 9/.9462 - /.8731 -.8000', 0.000 1.400 2.800 4.200 5.600 7.000 Time Fig. 6.3.2 Displacement y without disturbance

-13S- computed value -- desired value.1021.0298 - -.0424 - OX - -41 148 i. -.1871 - -.2594, I I i 0. 000 1.40 2.800 4.200 5.600 7.000 Time Fig. 8.3.3 Velocity x without disturbance

-13S- computed value -- desired value.2089 -. 1059 -.0028 - - [100 l - -.203L - -.3061, I I I I 0.000.400 2.800 4.200 5.600?.000 T lnme Fig. 6.3.4 Velocity y without disturbance

-137- computed value -- desired value 1.002.8015 -. 012 _1 _.4008 -1.2004 - 0.000 I,, 0. 000 1. 40 2.80 4 20 0 5.600 7. 0 0 Time Fig. 6.3.5 Contact force X without disturbance

computed value - esired value.4054 - o~O u.3504.2954.240 3/.L853 - \ /__ /.1302 - I 0.000 1.400 2.800 4.a20 5.600 7. 000 Time Fig. 8.3.8 Displacement x with disturbance due to inexact radius

-139. - computed value - desired value 1.~,16, i ime 1.088- / \ \di *7769 lV - ---------------—.\,.""1 0. 000 1.OO 2.800,'20O 5.600 7.OOO Time Fig. 6.3.7 Displacement y with disturbance due to inexact radius

-140- omputed value - desired value.LOuL 1.0313 -.0413 - I I' i i.i867? -.2594 -. 0.000 1.400 2.800 4. 200 5.600 7.000 Time Fig. 8.3.8 Velocity i with disturbance due to inexact radius

*141- computed value -- desired value.2089 -.1059 -.0029 -.1001 0.000 1.400 2.800 4.200 5.600?.000 Time Fig. 8.3.9 Velocity g with disturbance due to inexact radius -;203t -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~.30~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~3~~~~~~~~~~ 0.000 l~~~~~~~~~~r00 2.900 u.200 5.600 7,000~~~~~~~~~~~~~~~~~~~~~~~~~~~ Time ~ ~ ~~t Fig. 8,3.9 Velocity ~ ~ith disturbance dne to inexact ra%,

-142- computed value - desired value 2.657 2.125 - 1.594 - 1.063 -.5314 - 0.000 - - 1XI —- -- 0.000 1.400 2.800 4.200 5.600?.000 Time Fig. 6.3.10 Contact force X with disturbance due to inexact radius

.143.96935 2.550., -. D *" igil 10% increue. constraint i rdi 0 9 crc l e t....1_. 1.530 *,.,I, t< path (, Oto).5100 0,000 - - -. --. —--. 0. 000 -.5500 -.0399.4700.9800 L.+90 0.00o Fig. 8.3.11 Path and constraint for the inexact radius case

-144- computed value - desired value.4000.3460,.2921.2381 \ \ /.1842 - \:'a Time Fig. 6.3.12 Displacement x with disturbance due to Coulomb friction

-14computed value desired value 1.165 -,. — ~ \ 1.082-.9976 - 9137 \.8298 -.7159,, 0.000 1.400 2.800.200 5.600 7.000 Time Fig. 6.3.13 Displacement y with disturbance due to Coulomb friction

*14S- computed value desired value m, "/ -.04 24oX -.1871 - \ -.2594 0. 000.400 2.800 4. 200 5.600?.000 Ti me Fig. 8.3.14 Velocity 2 with disturbance due to Coulomb friction

-147-- computed value -- desired value.2089 -.1059.0028 -.1001 - I \<. i \ i \ o -.306L1 \ - 0.000 1.400 2.800 4.200 5.600?.000 Time Fig. 6.3.15 Velocity j with disturbance due to Coulomb friction

-14- computed value -- desired value 1.036.8288 -.6216 - 1<.4144M -.2072 - 0.000, -, 1 0.000 1.400 2.800 4.200 5.600 7.000 Time Fig. 6.3.16 Contact force X with disturbance due to Coulomb friction

-1492.500 constraint circle * 000 I, 1?500 -~ * mj>~ ~ ~ \^is~ (:2,2)/,'._,, computed path path H (2oto).500 0.000 -,.. -.5000.0000.5000 1.000 1.500 2.000 X Fig. 6.3.17 Path and constraint for the Coulomb friction case

CHAPTER 7 CONCLUSIONS A new class of manipulator problems, defined by constrained manipulators, has been presented. The constrained manipulator problems differ from previous manipulator problems, i.e., open chain manipulator problems, in that the new problems explicitly take into account constraints defined by algebraic equalities or inequalities and resulting contact forces as part of the system dynamics. We feel that constrained manipulator models are appropriate to describe many manipulator operations, such as contour following, scribing, cutting, and deburring. One of the main contributions of this thesis is the development of models for constrained manipulators. These models have been shown to be well-posed. Impact issues have also been considered. These models should provide a basis for further developments for constrained manipulators. In the analysis of constrained manipulators, the contact force has been shown to play an important role: (i) It should be incorporated into any optimal planning scheme. We have developed several approaches to achieve this, as shown in Chapter 4. (ii) It should be incorporated as part of any basic simulation procedure for performance evaluation. This requires procedures for solution of singular problems and/or complementarity problems. Difficulties and possible solutions have been discussed in -150

-151Chapter 5. (iii) It should be incorporated into any feedback control scheme. Namely, the contact force should be treated as a variable that can be sensed and used for feedback control. Although feedback controllers have not been studied in this thesis, the results of Chapter 8 indicate the importance of this issue. A parameterization approach has been applied to planning for a constrained manipulator. Path planning and motion planning have been systematically discussed. In motion planning, a kinematic approach and a minimum-time approach have been presented. In the minimum-time approach, we have reformulated the problems so that a phase plane technique can be applied. This also demonstrates that some developments for open chain manipulator problems can be applied to constrained manipulator problems with suitable modifications. A computer code has been developed to simulate constrained manipulators. Three procedures have been designed to handle constrained motion, unconstrained motion, and transitions. We have found that it is very difficult to develop a completely general computer simulation code for a general constrained manipulator problem. Research in this area is still required. Finally, an example contour following task has been used to illustrate the ideas developed in this thesis. The results are quite satisfactory. Although the manipulator used in the example is a simple Cartesian robot, the developed theory is, in principle, applicable to other robot configurations. This thesis provides an integrated approach to constrained manipulator problems, including modeling, planning and simulation. But there is much work to be done for constrained manipulator problems. For instance, the following extensions of the work in this thesis are of substantial importance:

-152Modeling of Constrained Manipulators (a) Flexible manipulator structure: We have only considered robots with rigid links; extension to flexible links is possible. (b) Elastic impact: We have assumed any impact is inelastic; it is possible to extend our models to the elastic case, including coefficient of restitution. (c) Time varying constraint function: If the constraint function is extended from time invariant case to time varying case, a slight modification can be made to the previous developments. (d) Nonholonomic constraints: It is non-trivial to extend holonomic constraints to nonholonomic constraints due to the non-integrability of nonholonomic constraints. (e) Multiple inequality constraints: We may generalize our results for scalar constraint (in Chapter 3) to multiple inequality constraints. However, subtle complications arise and the extension is not straightforward. (f) Frictional constraints: If the constraint surface is not frictionless, extra frictional terms should be added to the system dynamic equations. (g) Discontinuous constraints and/or constraints with discontinuous derivatives: We have assumed in the thesis that the constraint is smooth. If the constraint is discontinuous or has discontinuous derivatives, impacts may occur at those discontinuities, resulting in substantial complications of the model. (h) Uncertain knowledge of constraints: In this thesis a priori knowledge of the constraint is assumed; in case the constraint is not known a priori, constraint estimation/identification scheme may be required.

-153Optimal Planning of Constrained Manipulators (a) No a priori specification of contact force in optimal planning formulation: Rather than specify the contact force in advance, the contact force may be determined by the optimal planning procedure. (b) Performance criteria other than minimum time: Other performance measures, such as minimum energy, may be used. (c) Multiple constrained/unconstrained segments: In our development, only three motion segments have been considered; our results can be extended to multiple motion segments. (d) Including impact effects in performance measure. Feedback Control of Constrained Manipulators A feedback controller should incorporate feedback of the contact force. The major difficulty is to choose a control structure and gains for closed loop stabilization. To our knowledge, there are no known results. Simulation of Constrained Manipulators (a) New numerical algorithms: Both BDF methods and constraint elimination methods have certain drawbacks. New numerical algorithms may be more effective. (b) Development of algorithms for extended constrained manipulator models: We may extend the simulation algorithms to a more general class of constrained manipulator models, such as models including friction and multiple inequality constraints. Applications to Other Tasks In this thesis, the emphasis has been on constrained manipulators applied to a contour following problem. The development can also be applied to other tasks, such

-154as deburring, cutting, seam tracing, scribing, insertion, and other compliant motion tasks. In summary, the main purpose of this thesis has been to introduce a new class of manipulator problems and to provide some fundamental insight into those problems. The development in this thesis should shed light on future research in related problems. We hope this thesis will initiate more research on constrained manipulator problems.

BIBLIOGRAPHY -155

-158BIBUOGRAPHY [1] Baumgarte, J. "Stabilization of Constraints and Integrals of Motion in Dynamical Systems," Computer Methods in Applied Mechanics and Engineering, 1, 1972, 1-16. [2] Bayazitoglu, Y.O., "Methods for Automated Analysis of Three-Dimensional Dynamic Mechanical Systems with Application to Nonlinear Vehicle Dynamics." Ph.D. dissertation, Dept. of Mech. Eng., Univ. of Michigan, Ann Arbor, 1972. [3] Bobrow, J.E., et al., "Time Optimal Control of Robotic Manipulators along Specified Paths," The Int. J. of Robotics Research, Vol. 4, No. 3, Fall 1985, 3-17. [4] Brady, M., et al., Robot Motion. Cambridge: MIT Press, 1982. [5] Brayton, et al., "A New Efficient Algorithm for Solving Differential-Algebraic Systems Using Implicit Backward Differentiation Formulas", Proc. of the IEEE, Vol. 60, No. 1, January 1972, 98-108. [6] Brenan, K.E., "Difference Approximations for Higher Index Differential-Algebraic Systems with Applications in Trajectory Control," 198 Conf. on Decision and Control, 291-292. [7] Brenan, K.E., "Stability and Convergence of Difference Approximations for Higher Index Differential-Algebraic Systems with Applications in Trajectory Control," Ph.D. dissertation in Dept. of Mathematics, UCLA, 1983. [8] Bryson, A.E., et al., "Optimal Programming Problems with Inequality Constraints I: Necessary Conditions for Extremal Solutions", AIAA Journal, Nov. 1963, 25442550. [9] Bryson, A.E., Ho, Y.C., Applied Optimal Control New York: Hemisphere Publishing Co., 1975. [10] Burrage, K., "A Special Family of Runge-Kutta Methods for Solving Stiff Differential Equations," BIT 18, 1978, 22-41. [11] Campbell, S.L., Singular Systems of Differential Equations II. San Francisco: Pitman Advanced Publishing Program, 1982. [12] Chace, M.A., "Computer-Aided Engineering of Large-Displacement Dynamic Mechanical Systems", National Conf. on University Programs in Computer-Aided Engineering, Design and Manufacturing, April 26-29, 1983, Brigham Young Univ., Salt Lake City, Utah.

-157[13] Coddington, E.A., and Levinson, N., Theory of Ordinary Differential Equations. New York: McGraw-Hill, 1955. [14] Cottle, R.W., "On a Problem in Linear Inequalities", J. London Math. Soc., 43, 1968, 378-384. [15] Dieudonne, J., Foundations of Modern Analysis. New York: Academic Press, 1969. [16] Draganoiu, G.,et al., "Computer Method for Setting Dynamical Model of an Industrial Robots with Closed Kinematic Chains." The 12 th Int. Symp. on Industrial Robots, June 1982, Paris, France. [17] Dreyfus, S., "Variational Problems with Inequality Constraints", J. of Mathematical Analysis and Applications, 4, 1962, 297-308. [18] Gear, C.W., Numerical Initial Value Problems in Ordinary Differential Equations. Englewood Cliffs, N.J.: Prentice-Hall, 1971. [19] Gear, C.W., Petzold, L.R., "ODE Methods for the Solution of Differential/Algebraic Systems," SIAM J. Numer. Analy., Vol. 21, No. 4, August 1984, 716-728. [20] Gear, C.W., Petzold, L.R., "Differential/Algebraic Systems and Matrix Pencils," Report No. UIUCDCS-R-82-1086, Dept. Computer Science, Univ. of Illinois at Urbana Champaign, 1982. [21] Gear, C.W., "Simultaneous Numerical Solution of Differential-Algebraic Equations," IEEE Trans. on Circuit Theory, Vol. CT-18, January 1971, 89-95. [22] Goldstein, H., Classical Mechanics. Cambridge, Massachusetts: Addison-Wesley Press, 1950. [23] Good, M.C., et al., "Dynamic Models for Control System Design of Integrated Robot and Drive Systems." ASME J. of Dynamic Systems, Measurement, and Control, March 1985, Vol. 107, 53-59. [24] Greenwood, D.T., Classical Dynamics. Englewood Cliffs, N.J.: Prentice-HAll, 1977. [25] Hemami, H., and Wyman, B.F., "Modeling and Control of Constrained Dynamic Systems with Appl. to Biped Locomotion in the Frontal Plane." IEEE Trans. on Automatic Control. Vol. AC-24, No. 4, August 1979, 526-535. [26] Hemami, H., and Weimer, F.C., "Modeling of Nonholonomic Dynamic Systems with Applications", J. of Applied Mechanics, March 1981, Vol. 48, 177-182.

-158[27] Hogan, N., "Impedance Control: An Approach to Manipulation: Part I Theory, Part II Implementation, Part III Applications," ASME J. of Dynamic Systems, Measurement, and Control, March 1985, Vol. 107, 1-24. [28] Hollerbach, J.M., "A Recursive Lagrangian Formulation of Manipulator Dynamics and a Comparative Study of Dynamic Formulation Complexity." IEEE Trans. on Systems, Man, and Cybernetics. Vol. SMC-10 No. 11, Nov. 1980, 730-736. [29] Jacobson, D.H., et al., "New Necessary Conditions of Optimality for Control Problems with State-Variable Inequality Constraints", J. of Mathematical Analysis and Applications, 35, 1971, 255-284. [30] Kahn, M.E., Roth, B., "The Near-Minimum-Time Control of Open-Loop Articulated Kinematic Chains", ASME J. of Dynamic Systems, Measurement, and Control, Sept. 1971, 164-172. (31] Kankaanranta, R., Koivo, H.N., "A Model for Constrained Motion of A Serial Link Manipulator." 1986 IEEE Robotics and Automation Conf., 1186-1191. [32] Karamardian, S., "The complementarity Problem", Mathematical Programming, 2, 1972, 107-129. [33] Kazerooni, H., et al., bust Compliant Motion for Manipulators, Part I: The Fundamental Concepts of Compliant Motion; Part II: Design Method." IEEE J. of Robotics and Automation, Vol. RA-2, No. 2, June 1986. [34] Lee, C.S.G., "Robot Arm Kinematics, Dynamics, and Control." Computer. 62-80, Dec. 1982. [35] Liang, C.G., "Dynamic Analysis and Control Synthesis of Integrated Mechanical Systems", Ph.D. dissertation of Dept. of Mechanical Eng., the University of Iowa, August 1985. [36] Lin, C.S., et al., "Formulation and Optimization of Cubic Polynomial Joint Trajectories for Industrial Robots." IEEE Trans. on Automatic Control. Vol. AC-28, No. 12, Dec. 1983, 1066-1073. [37] Liniger, W., "Multistep and One-Leg Methods for Implicit Mixed Differential Algebraic Systems," IEEE Trans. on Circuits and Systems, Vol. CAS-26, No. 9, Sept. 1979, 755-762. [38] Lostedt, P., "Mechanical Systems of Rigid Bodies Subject to Unilateral Constraints", SIAM J. Appl. Math., Vol. 42, No. 2, April 1982, 282-296.

-159[39] Lostedt, P., "Numerical Solution of Time-Dependent Contact and Friction Problems in Rigid Body Mechanics", SIAM J. Sci. Stat. Comput., Vol. 5, No. 2, June 1984, 370-393. [40] Luh, J.Y.S., "Conventional Controller Design for Industrial Robots-a Tutorial." IEEE Trans. on Systems, Man, and Cybernetics, Vol. SMC-13, No. 3,1983, 298-316. [41] Luh, J.Y.S., "An Anatomy of Industrial Robots and their Controls." IEEE Trans. on Automatic Control, Vol. AC-28, No. 2, 1983, 133-153. [42] Luh, J.Y.S., et al., "On-Line Computational Scheme for Mechanical Manipulators." ASME J. of Dynamic Systems, Measurement, and Control. Vol. 102, June 1980, 6978. [43] Mason, M.T., "Compliance and Force Control for Computer Controlled Manipulators." IEEE Trans. on Systems, Man, and Cybernetics. Vol. SMC-11, No. 6, June 1981, 418-432. [44] Makowski, K., Neustadt, L.W., "Optimal Control Problems with Mixed ControlPhase Variable Equality and Inequality Constraints", SIAM J. Control, Vol. 12, No. 2, May 1974, 184-228. [45] McClamroch, N.H., Huang, H.P., "Dynamics of a Closed Chain Manipulator", American Control Conf., Boston, June 1985. [46] McClamroch, N.H., "Singular Systems of Differential Equations as Dynamical Models for Constrained Robot Systems," 1986 IEEE Robotics and Automation Conf., April, 1986, San Francisco, California. [47] McClamroch, N.H., "Compliance at the End Effector of an Electro-Hydraulically Controlled Robot", CRIM report No. RSD-TR-14-84, The University of Michigan, October 1984; Proc. of 1985 Conf. on Decision and Control, Dec. 1985. [48] McKay, N.D., "Minimum-Cost Control of Robotic Manipulators with Geometric Path Constraints," Ph.D. dissertation, Dept. of Electrical Eng. and Computer Science, The University of Michigan, October 1985. [49] Nikravesh, P.E., Haug, E.J., "Generalized Coordinate Partitioning for Analysis of Mechanical Systems with Nonholonomic Constraints", J. of Mechanisms, Transmissions, and Automation in Design, Sept. 1983, Vol. 105, 379-384. [50] Nikravesh, P.E., "Some Methods for Dynamic Analysis of Constrained Mechanical Systems: A Survey," in Computer Aided Analysis and Optimization of Mechanical System Dynamics, edited by E.J. Haug, Berlin: Springer Verlag, 1984, 351-367.

-160[51] Orin, D.E., and Oh, S.Y., "Control of Force Distribution in Robotic Mechanisms Containing Closed Kinematic Chains." ASME J. of Dynamic Systems, Measurement, and Control. Vol. 102, June 1981, 134-141. [52] Orin, D.E., et al., "Kinematic and Kinetic Analysis of Open-Chain Linkages Utilizing Newton-Euler Method." Mathematical Biosciences. Vol. 43, 1979, 107-130. [53] Orlandea, N., et al., "A Sparsity-Oriented Approach to the Dynamic Analysis and Design of Mechanical Systems-Part I and II," J. of Engineering for industry, August 1977, 773-784. [54] Paul, B., Kinematics and Dynamics of Plannar Machinery. Englewood Cliffs, New Jersey: Prentice-Hall Inc., 1979. [55] Paul, B., "Dynamic Analysis of Machinery via Program DYMAC," SAE paper No. 770049, 1977. [56] Paul, R.P., and Shimano, B., "Compliance and Control." 1976 Joint Automatic Control Conf., 694-699. [57] Paul, R.P., "Manipulator Cartesian Path Control." IEEE Trans. on Systems, Man, and Cybernetics. Vol. SMC-9, No. 11, Nov. 1979, 702-711. [58] Paul, R.P., Robot Manipulators: Mathematics. Programming, and Control. Cambridge: MIT Press, 1981. [59] Petzold, L.R., "A Description of DASSL: A Differential/Algebraic System Solver," in IMACS Trans. on Scientific Computation. New York: North Holland, 1983. [60] Petzold, L.R., Gear, C.W., "ODE methods for the Solution of Differential/Algebraic Systems", SANDIA Report No. SAND82-8051, October, 1982. [61] Petzold, L.R., "Differential/Algebraic Equations are not ODES's", SIAM J. Sci. Stat. Comput., Vol. 3, No. 3, Sept. 1982, 367-384. [62] Raibert, M.H., and Craig, J.J., "Hybrid Position/Force Control of Manipulators." ASME J. of Dynamic Systems, Measurement, and Control. Vol. 102, June 1981, 126-133. [63] Rheinboldt, W.C., "Differential-Algebraic Systems as Differential Equations on Manifolds", Mathematics of Computation, Vol. 43, No. 168, October 1984, 473-482. [64] Romanelli, M.J., "Runge-Kutta Methods for the Solution of Ordinary Differential Equations," in Mathematical Method for Digital Computers, edited by A. Ralston and H.S. Wilf, New York: John Wiley & Sons, 1960.

-161[65] Salisbury, J.K., "Active Stiffness Control of a Manipulator in Cartesian Coordinates." IEEE 1980 Decision and Control Conf., 95-100. [66] Sethi, S.P., et al., "A Unified Framework for Linear Control Problems with StateVariable Inequality Constraints", J. of Optimization Theory and Applications, Vol. 36, No. 1, January 1982. [67] Sheth, P.N., Uicker, J.J., "IMP (Integrated Mechanisms Program), A ComputerAided Design Analysis System for Mechanisms and Linkage", J. of Engineering for Industry, May 1972, 454-464. [68] Shin, K.G., McKay, N.D., "Minimum-Time Control of Robotic Manipulators with Geometric Path Constraints", IEEE Trans. on Automatic Control, Vol. AC-30, No. 6, June 1985, 531-541. [69] Shin, K.G., McKay, N.D., "Selection of Near-Minimum Time Geometric Paths for Robotic Manipulators", IEEE Trans. on Automatic Control, Vol. AC-31, No. 6, June 1986, 501-511. [70] Silver, W.M., "On the Equivalence of Lagrangian and Newton-Euler Dynamics for Manipulators." The Int. J. of Robotics Research. Vol. 1, No. 2, Summer 1982, 6070. [71] Sincovec, R.R., et al., "Analysis of Descriptor Systems using Numerical Algorithms", IEEE Trans. on Automatic Control, Vol. AC-26, No. 1, February 1981, 139-147. [72] Smith, D.A., "Reaction Force Analysis in Generalized Machine Systems." J. of Eng. for Industry, 617-623, May 1973. [73] Smith, D.A., et al., "The Automatic Generation of a Mathematical Model for Machinery Systems," J. of Engineering for Industry, May 1973, 629-635. [74] Sokolnikoff, I.S., Tensor Analysis, New York: John Wiley & Sons, 1964. [75] Starr, G.P., "Edge-Following with a PUMA 560 Manipulator Using VAL-II", 1986 IEEE Robotics and Automation Conf., 379-383. [76] Stavrakova, N.E., "The Principle of Hamilton-Ostrogradskii for Systems with OneSided Constraints", PMM, Vol. 29, No. 4, 1965, 874-878. [77] Stepien, T.M., et al., "Control of Tool/Workpiece Contact Force with Application to Robotic Deburring", 1985 IEEE Conf. on Robotics and Automation, March 1985, St. Louis, 670-679.

-1~02[78] Vukobratovic, M., and Potkonjak, V., Dynamics of Manipulation Robots-Theory and AppD. New York: Springer-Verlag, 1982. [79] Vukobratovic, M., and Stokic, D., Control of Manipulation Robots-Theoryv and AppD., New York: Springer-Verlag, 1982. [80] Vukobratovic, M., and Potkonjak, V., Applied Dynamics and CAD of Manipulation Robots, New York: Springer-Verlag, 1985. [81] Warga, J., "Unilateral Variational Problems with Several Inequalities", Michigan Mathematical Journal, Vol. 12, 1965, 449-480. [82] Wehage, R.A., Haug, E.J., "Generalized Coordinate Partitioning for Dimension Reduction in Analysis of Constrained Dynamic Systems", J. of Mechanical Design, January 1982, Vol. 104, 247-255. [83] West, H., Asada, H., "A Method for the Design of Hybrid Position/Force Controllers fro Manipulators Constrained by Contact with the Environment", IEEE Int. Conf. on Robotics and Automation, March 1985, St. Louis, 251-259. [84] Whitney, D.E., "Quasi-Static Assembly of Compliantly Supported Rigid Parts," ASME J. of Dynamic Systems, Measurement, and Control, 104, March 1982, 65-77. [85] Whitney, D.E., "Force Feedback Control of Manipulator Fine Motions," Proc. of the 1976 Joint Automatic Control Conf., 687-693. [86] Whitney, D.E., Edsall, A.C., "Modeling Robot Contour Processes," American Control Conf. June, 1984. [87] Whittaker, E.T., A Treatise on the Analytical Dynamics of Particles and Rigid Bodies. London, Cambridge: University Press, 1944. [88] Wu, C.H., Paul, R.P., "Manipulator Compliance based on Joint Torque Control." Proc. of the 1980 Conf. on Decision and Control, 88-94. [89] Zheng, Y.F., Luh, J.Y.S., "Kinematic and Dynamic Behavior of Industrial Robots with Spatial Closed-Chain Linkage Structure", IEEE Conf. on Decision and Control, Dec. 1984. [90] Zheng, Y.F., Hemami, H., "Impact Effects of Biped Contact with the Environment", IEEE Trans. on System, Man, and Cybernetics, Vol. SMC-14, No. 3, May/June 1984, 437-443. [91] Zhuravlev, V.F., "Equations of Motion of Mechanical Systems with Ideal One-Sided Links", PMM, Vol. 42, No. 5, 1978, 839-847.