NEAR-MINIMUM-TIME CONTROL OF A ROBOT MANIPULATOR By Tao Fan B. Eng., Xian Jiaotong University, 1991 A THESIS SUBMITTED IN PARTIAL F U L F I L L M E N T OF T H E REQUIREMENTS FOR T H E D E G R E E OF M A S T E R OF A P P L I E D SCIENCE in T H E FACULTY OF G R A D U A T E STUDIES D E P A R T M E N T OF MECHANICAL ENGINEERING We accept this thesis as conforming to the required standard T H E UNIVERSITY OF BRITISH COLUMBIA June 2003 © Tao Fan, 2003 In presenting this thesis in partial fulfilment of the requirements for an advanced degree at the University of British Columbia, I agree that the Library shall make it freely available for refer-ence and study. I further .agree that permission for extensive copying of this thesis for scholarly purposes may be granted by the head of my department or by his or her representatives. It is understood that copying or publication of this thesis for financial gain shall not be allowed without my written permission. Department of Mechanical Engineering The University of British Columbia 2075 Wesbrook Place Vancouver, Canada V6T 1Z1 Date: Abstract This thesis deals with the problem of minimum time control of a rigid robot manipulator with point-to-point motion subject to constraints on the control inputs. Due to the nonlinear and coupled dynamics of the robot manipulator, finding minimum time strategies is algorithmically difficult and computationally very intensive, even when the dynamic equations and parameters of the manipulator are precisely known. As a result, the practical applicability of the available methods currently is very limited. In this research, we assume the control inputs are always bang-bang and switch once. Using the Principle of Work and Energy, a simple and practical "zero-net-work" searching approach is proposed. The proposed method focuses on changes in the manipulator's kinetic energy during the time optimal motion, instead of concentrating on the system's state variables, as is usually done in conventional approaches. The "zero-net-work" method is used to develop the controllers for one-link manipulators, a 3-degree of freedom cylindrical manipulator and a two-degree of freedom revolute manipulator. The results show that if the structure of the exact minimum time control is bang-bang with a single switch, using the "zero-net-work" method we will get the exact minimum time solution. If the exact minimum time control has more than one switch, using the "zero-net-work" method we will get a near-minimum-time solution. The major advantages of the proposed method are that it does not require initial boundary value guesses and is computationally efficient. n Table of Contents Abstract ii List of Tables vii List of Figures viii Acknowledgement xii Nomenclature xiii 1 Introduction 1 1.1 Review of Literature 1 1.2 Purpose and Scope of This Work 6 2 Problem Formulation and Solution Guidelines 7 2.1 Review of Conventional Variational Calculus Method 7 2.1.1 Optimal Control Problem for General Dynamic Systems 7 2.1.2 Exact Minimum Time Control Problem for Robot Manipulators 9 2.2 Guidelines for Developing a Near-Minimum-Time Algorithm 13 3 Development of the Proposed Zero-Net-Work Method 14 3.1 Motivation 14 3.1.1 The Structure of the Exact Minimum Time Control Law 14 3.1.2 The Double Integrator Problem 16 3.1.3 Dynamically Decoupled Two Link Robot Manipulator 22 3.1.4 Work Energy Behavior 23 iii 3.2 Zero-Net-Work Method 24 4 Development of the Zero-Net-Work Controller for One-Link Manipulators 34 4.1 One-Link Horizontal Manipulator 34 4.1.1 Dynamic Model . 34 4.1.2 Exact Minimum Time Solution 34 4.1.3 Zero-Net-Work Solution 34 4.1.4 Computer Simulation 37 4.1.5 Summary 39 4.2 One-Link Vertical Manipulator 41 4.2.1 Dynamic Model 41 4.2.2 The Exact Minimum Time Solution 42 4.2.3 Zero-Net-Work Solution 46 4.2.4 Computer Simulation 47 4.2.5 Summary 52 5 Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 56 5.1 Dynamic Model 56 5.1.1 Forward Recursion 60 5.1.2 Backward Recursion 60 5.2 The Exact Minimum Time Solution 62 5.3 Zero-Net-Work Solution 66 5.4 Computer Simulation 68 5.4.1 Simulation Case 1: 69 5.4.2 Simulation Case 2: 73 5.4.3 Simulation Case 3: 80 5.4.4 Simulation Case 4: 80 6 Development of the Zero-Net-Work Controller for a Revolute Manipulator 85 iv 6.1 Dynamic Model 85 6.1.1 Forward Recursion 87 6.1.2 Backward Recursion 88 6.2 The Exact Minimum Time Solution 92 6.3 Zero-Net-Work Solution 95 6.3.1 Offline Calculation 95 6.3.2 Online Application 98 6.4 Computer Simulation 101 6.4.1 Simulation Case 1: 102 6.4.2 Simulation Case 2: 103 6.4.3 Simulation Case 3: 107 6.4.4 Simulation Case 4: I l l 7 Conclusions and Future Work 118 7.1 Conclusions 118 7.2 Suggestion for Further Research 119 Bibliography 120 Appendices 124 A Simulation Program Listings 124 A . l S-function of One-link Horizontal Manipulator 124 A.2 S-function of One-link Vertical Manipulator 124 A.3 Exact Minimum Time Solution of One-link Vertical Manipulator 125 A.4 Zero-net-work Solution of Two-link Revolute and Prismatic Manipulator 128 A.5 Exact Minimum Time Solution of Two-link Revolute and Prismatic Manipulator 135 A.6 S-function of Two-link Revolute and Prismatic Manipulator 140 A.7 Zero-net-work Solution of Two-link Revolute Manipulator 141 v A.8 Exact Minimum Time Solution of Two-link Revolute Manipulator 148 A.9 S-function of Two-link Revolute Manipulator 155 A. 10 S-function of Zero-net-work Controller for Two-link Revolute Manipulator . . . .156 A. 11 S-function of Proportional and Derivative (PD) Controller for Two-link Revolute Manipulator 159 Subroutines Used in This Thesis 161 B. l Subroutine MUSN for Solving Nonlinear TPBVP 161 B.2 Subroutine DDASS for Solving Nonlinear IVP 167 vi List of Tables 5.1 D-H parameters of two-link revolute and prismatic manipulator 58 5.2 Physical parameters of two-link revolute and prismatic manipulator 68 5.3 Case 1: Simulation results of two-link revolute and prismatic manipulator . . . . 69 5.4 Case 2: Simulation results of two-link revolute and prismatic manipulator . . . . 73 5.5 Case 3: Simulation results of two-link revolute and prismatic manipulator . . . . 80 5.6 Case 4: Simulation results of two-link revolute and prismatic manipulator . . . . 81 6.1 D-H parameters of two-link revolute manipulator 86 6.2 Case 1: Simulation results of two-link revolute manipulator 102 6.3 Case 2: Simulation results of two-link revolute manipulator 107 6.4 Case 3: Simulation results of two-link revolute manipulator 107 6.5 Case 4: Simulation results of two-link revolute manipulator I l l vii List of Figures 3;1 One-link-torque system 16 3.2 Joint position and velocity of the one-link planar manipulator 20 3.3 Applied joint torque of the one-link planar manipulator 21 3.4 Costate trajectories of the one-link planar manipulator 21 3.5 Dynamically decoupled two-link robot manipulator 22 3.6 Free body diagram of one-link robot for case 1 26 3.7 Position and velocity trajectories of case 1 27 3.8 Free body diagram of case 2 27 3.9 Position and velocity trajectories of case 2 . 28 3.10 Free body diagram of case 3 28 3.11 Position and velocity trajectories of case 3 29 3.12 Forces and moments on link i 31 3.13 Overall scheme of the zero-net-work method 33 4.1 Block diagram of the zero-net-work control system 36 4.2 Simulation block diagram of the horizontal robot 36 4.3 State trajectories of zero-net-work control simulation 1 38 4.4 Applied joint torque of zero-net-work control simulation 1 38 4.5 State trajectories of zero-net-work control simulation 2 39 4.6 Applied joint torque of zero-net-work control simulation 2 40 4.7 One-link vertical manipulator 40 4.8 Case 1: Simulation block diagram of the one-link vertical manipulator 49 viii 4.9 Case 1: State trajectories of the one-link vertical manipulator (zero-net-work solution) 50 4.10 Case 1: Control trajectory of the one-link vertical manipulator (zero-net-work solution) 50 4.11 Case 1: State trajectories of the one-link vertical manipulator (exact minimum time solution) 51 4.12 Case 1: Control trajectory of one-link vertical manipulator (exact minimum time solution) 51 4.13 Case 1: Costate trajectories of the one-link vertical manipulator (exact minimum time solution) 52 4.14 Case 2: Simulation block diagram of the one-link vertical manipulator 53 4.15 Case 2: State trajectories of the one-link vertical manipulator (zero-net-work solution) 53 4.16 Case 2: Control trajectory of the one-link vertical manipulator (zero-net-work solution) 54 4.17 Case 2: State trajectories of the one-link vertical manipulator (exact minimum time solution) 54 4.18 Case 2: Control trajectory of one-link vertical manipulator (exact minimum time solution) 55 4.19 Case 2: Costate trajectories of the one-link vertical manipulator (exact minimum time solution) 55 5.1 Sketch of the cylindrical robot 57 5.2 Sketch of two-link revolute and prismatic manipulator 58 5.3 Forces and torques act on link 1 and link 2 61 5.4 Case 1: Simulation block diagram 69 5.5 Case 1: State trajectories of link 1 70 5.6 Case 1: State trajectories of link 2 70 ix 5.7 Case 1: Control trajectories of link 1 71 5.8 Case 1: Control trajectories of link 2 71 5.9 Case 1: Work done by forces/torques for each link (zero-net-work solution) . . . 72 5.10 Case 2: Simulation block diagram 74 5.11 Case 2: State trajectories of link 1 74 5.12 Case 2: State trajectories of link 2 75 5.13 Case 2: Control trajectories of link 1 75 5.14 Case 2: Control trajectories of link 2 76 5.15 Case 2: Work done by forces/torques for each link (zero-net-work solution) . . . 76 5.16 Case 3: Simulation block diagram 77 5.17 Case 3: State trajectories of link 1 77 5.18 Case 3: State trajectories of link 2 78 5.19 Case 3: Control trajectories of link 1 78 5.20 Case 3: Control trajectories of link 2 79 5.21 Case 3: Work done by forces/torques for each link (zero-net-work solution) . . . 79 5.22 Case 4: Simulation block diagram 82 5.23 Case 4: State trajectories of link 1 82 5.24 Case 4: State trajectories of link 2 83 5.25 Case 4: Control trajectories of link 1 83 5.26 Case 4: Control trajectories of link 2 84 5.27 Case 4: Work done by forces/torques for each link (zero-net-work solution) . . . 84 6.1 Sketch of two-link revolute manipulator 85 6.2 Forces and torques act on each link 89 6.3 Block diagram of overall zero-net-work control scheme 96 6.4 Flowchart of offline calculation 99 6.5 Simulation block diagram of two-link revolute manipulator 100 6.6 Case 1: State trajectories of link 1 104 x 6.7 Case 1: State trajectories of link 2 104 6.8 Case 1: Control trajectories of link 1 105 6.9 Case 1: Control trajectories of link 2 105 6.10 Case 1: Work done by forces/torques for link 1 106 6.11 Case 1: Work done by forces/torques for link 2 106 6.12 Case 2: State trajectories of link 1 108 6.13 Case 2: State trajectories of link 2 108 6.14 Case 2: Control trajectories of link 1 109 6.15 Case 2: Control trajectories of link 2 109 6.16 Case 2: Work done by forces/torques for each link (zero-net-work solution) . . . 110 6.17 Case 3: State trajectories of link 1 I l l 6.18 Case 3: State trajectories of link 2 112 6.19 Case 3: Control trajectories of link 1 112 6.20 Case 3: Control trajectories of link 2 113 6.21 Case 3: Work done by forces/torques for each link (zero-net-work solution) . . . 113 6.22 Case 4: State trajectories of link 1 115 6.23 Case 4: State trajectories of link 2 115 6.24 Case 4: Control trajectories of link 1 116 6.25 Case 4: Control trajectories of link 2 116 6.26 Case 4: Work done by forces/torques for each link (zero-net-work solution) . . . 117 xi Acknowledgement I wish to thank Dr. D. B. Cherchas for his advice and help throughout my work on this thesis. Partial financial support for the work was obtained from NSERC Research Grant A4682. Nomenclat ur e X State vector. * X Optimal state vector. XiO Initial state of link i . Final state of link i . e Coordinate for rotational link. Oi Position of rotational link i . q Joint position vector. q Joint velocity vector. d Coordinate for prismatic link. a c , i Acceleration of the center of mass of link i . Acceleration of the end of link i . Wi Angular velocity of frame i w.r.t. frame i . O-j Angular acceleration of frame i w.r.t. frame i . g i Acceleration due to gravity (expressed in frame i). fi Force exerted by link i-1 on link i . Torque exerted by link i-1 on link i. Rotation matrix from frame i + 1 to frame i . Homogeneous transformation from frame i to frame ,j. rrti Mass of link i . k Length of link i . h Inertia matrix of link i about the center of mass of link i. Vector from joint i to the center of mass of link i . I"i+l,ci Vector from joint i + 1 to the center of mass of link i. xiii < ri,i+l Vector from joint i to joint i+1. U Control input vector. * U Optimal control input vector. Maximum control input vector. T Joint torque vector. T~max Maximum joint torque vector. Ft Control force of link i . J Cost functional. X Adjoint variable vector. H Hamiltonian function. L Lagrangian. KE Kinetic energy of the system. PE Potential energy of the system. wAl Work done by the actuator of link i . wRi Work done by reaction forces of link i wGl Work done by gravity of link i . M Manipulator inertia matrix. C Coriolis-centrifugal matrix. G Gravity matrix. S Switching function vector. to Initial time. tf Final time. tsw Switching time. xiv Chapter 1 Introduction 1.1 Review of Literature The problem of minimum time control of a robotic manipulator concerns the determination of control efforts and corresponding trajectories that will drive the end-effector of a manipulator from a given initial position to a specified desired position in minimum time while satisfying constraints e.g. the limits on the actuator efforts. Kahn and Roth (1971) first studied this problem. Since then, the interest in this problem has increased tremendously. The problem can be divided into two categories in terms of different constraints on the manipulator motion: a) There are constraints on the intermediate configurations of the robot arm so that the manip-ulator either follows a specified path or performs the movement avoiding collisions with obstacles in the work space. b) The motion path space is obstacle-free and unconstrained between the two end points. For the first category, some algorithms have been proposed for minimum time control of robot manipulators along a specified geometric path (Shin and Mckay 1985; Bobrow et al. 1985; Chen and Desrochers 1989; Shiller and Dubowsky 1991). Improvements in computational efficiency of the algorithm were studied by Slotine and Yang (1989). With given paths, 2n dimensional (n is the number of the links) trajectory planning problems reduce to simple 2 dimensional searching problems. Solving the minimum time control problem for the second category represents a difficult and elusive point-to-point minimum time control problem. The approaches to this problem can 1 Chapter 1. Introduction 2 be categorized into two groups: a) Dynamic programming or space search methods. b) Calculus of variation based methods. The first group includes some approximation methods, such as artificial intelligence (Al) ap-proaches (Sahar and Hollerbach 1985; Rajan 1985; Shiller and Dubowsky 1988) and nonlinear parameter optimization methods (Bobrow 1988). The basic idea is the use of an approximation of an initial feasible trajectory in conjunction with an exhaustive search to determine the uncon-strained motions between two end points. In general, the algorithm for this class of approaches consists of three steps: a) Characterize the path in some manner (e.g. spline function); b) Given a path determine the time optimal trajectory by using existing algorithms (e.g. Bo-brow's algorithm 1988; or Shin's algorithm 1985); c ) Vary the path until the minimum time path and trajectory are obtained. Shiller and Dubowsky (1988) proposed a practical method to obtain the global time optimal motions of robotic manipulators. They extend the predefined path methods (e.g. Bobrow et al. 1985; Shin and Mckay 1985) to point-to-point problems. In their work, a set of best paths are obtained first in a global search over manipulator workspace, using graph search and hierarchical pruning techniques; and then vary the path under certain criteria to search for the shortest travel time between two given configurations. Following the same idea, a technique was developed by Bobrow (1988). Similar to (Rajan 1985), in his work the Cartesian path along cubic B-splines and the shape of this path is varied in a manner that minimizes the travel time using a nonlinear parameter optimization algorithm. For any set of B-spline vertices (parameters to be determined) the algorithm in (Bobrow et al. 1985) was used to evaluate the minimum final time. In order to speed up convergence and Chapter 1. Introduction 3 improve computational efficiency for those existing optimization methods, a close initial guess for the path is necessary. The calculus of variation based methods use the techniques of variational calculus and apply Pontryagin's maximum principle to reduce the time optimization problem to a Two Point Boundary Value Problem (TPBVP). The difficulty in finding a solution for the T P B V P problem is that state and costate variables at the initial and final times are only partially known, hence initial values must be guessed to match the correct set of final values. Since most of the robot manipulators are nonlinear systems, the numerical methods used to solve the nonlinear TPBVP were found to be highly sensitive to the initial guess of the costate values. However, it is extremely difficult to have a good guess of the initial costate values intuitively, because the costate variables do not have any simple interpretation or physical meaning. In general, time optimal control solutions are very difficult to obtained by direct solving the TPBVP. Kahn and Roth (1971) first studied the problem. A near minimum time solution for a three-degree of freedom articulated arm was obtained by using Pontryagin's maximum principle. The major portion of this work consisted of finding a closed-loop regulation around the time optimal nominal trajectory using linearization, gravity compensation, and averaged compensation of the effects of angular speeds. The possibility of singular time optimal control was not investigated. Weinreb and Bryson (1985) were the first to obtain substantive information about the minimum time behavior of the two-link manipulator with open initial conditions. They did not assume that the optimal controls were bang-bang, but results from Weireb's adjusted control weight (ACW) algorithm, which was based on Bryson's steepest descent algorithm (Bryson 1975), indicated that most cases tended toward bang-bang. Because the ACW program was a continuous function optimization code, it was unable to achieve sharp discontinuities in the controls, and it was computationally intensive. Wen and Desrochers (1986) examined a sub-optimal time control algorithm which utilizes the concept of "Averaged Dynamics" (Kin and Shin 1985). It uses all available dynamics information of the current and final states to update the dynamics continually at each sampling Chapter 1. Introduction 4 interval. Unfortunately there is no guarantee that the system will achieve the desired final state with this method. In Geering's paper (1986), time optimal trajectories for a cylindrical and a spherical robot, and a robot with a horizontal articulated arm with two links were obtained by using the shooting algorithm and a parameter optimization method. It was pointed out that the most important physical effects responsible for the nature of the time optimal solution are: (a) Minimization of the mass moment of inertia with respect to revolute joints; (b) Enhancement of accelerations and decelerations by exploiting reaction torques and forces. Chen and Desrochers (1988) proved that for an n degree of freedom robot system with no path constraints on the motion, the structure of the optimal control requires that at least one of the control torques be always in saturation on every finite time interval. In other words, not all control torques are simultaneously singular (where the control variables are not bang-bang) on the time interval. Meier and Bryson (1990) suggest a fast algorithm for a planar two-link manipulator to travel a specified distance in minimum time with initial and final position unspecified. The method reduces the computation time by forcing the control inputs to be 'bang-bang' on both regular and singular intervals, and thus makes it feasible to find minimum time solutions for various initial and final positions and various dynamic properties of a robot. Lin (1992) proposed an efficient two-level (master and slave) parallel algorithm for solving the general time optimal problem in discrete form. However, the convergence of the algorithm was shown under the following two conditions: a) The step size in the solution to the master problem is small enough. b) The initial guess of final time is sufficiently close to the minimum final time. The second condition is in general not easily satisfied since the knowledge of the final time is not known a priori. Chen et al. (1993) develop a general procedure for computing the time optimal trajectory Chapter 1. Introduction 5 for robotic point-to-point motion based on perturbation and continuation methods. Both non-singular and singular cases can be handled by converting the original time optimal problem into a perturbed time optimal problem. The algorithm is composed of two phases: initialization and refinement. In the initialization phase, a Two Point Boundary Value Problem (TPBVP) resulting from the perturbed time optimal problem is solved for an appropriately chosen per-turbation parameter. Then, the solution obtained from the initialization phase is refined by solving a set of Initial Value Problems (IVP) sequentially and/or in parallel until the desired solution is achieved. The method is computationally efficient since the resulting TPBVP is solved once for a large perturbation parameter and the remaining problem becomes solutions to a set of IVP sub-problems. In Yang (1994) , a "two-step" Lyapunov approach generating near-minimum-time point-to-point motion of a robot manipulator is proposed, motivated by the physics of the minimum time motion. The method is based on the observation of the kinetic energy mechanisms in minimum time operations of robot manipulators. In summary, it is extremely difficult, if not impossible, to obtain an exact closed-form solution to the problem of point-to-point minimum time control of robotic manipulators, due to the nonlinearity and the coupled nature of the robotic manipulator dynamics. The available numerical solutions are computationally very intensive, require good initial boundary values guesses, or are sensitive to the time-step or some other variables in the numerical procedures. Also the numerical solution is essentially an open-loop control and does not accommodate any system disturbance or parameter uncertain. The complete structure of the solution of exact minimum time motion of robotic manipulators is currently still unclear. In particular, the question of existence of time optimal controls containing singular arcs is open. As a result, the practical applicability of available methods is very limited. Chapter 1. Introduction 6 1.2 Purpose and Scope of This Work For most manufacturing tasks, it is desirable to move a manipulator at its highest speed to minimize the task cycle time. This project is significant in the sense that the execution time for robot tasks and industrial productivity are directly related, productivity may be increased significantly by fully exploiting the potential of existing robots. Furthermore, this is an im-portant problem in the implementation of automatic programming for intelligent robots. The short term objective of this work is the determination of a minimum time control algorithm for robot manipulator motion. The long term objective is the reduction in execution time in the performance of industrial robot tasks and subsequent improvement in industrial productivity. This thesis deals with time optimal point-to-point motion (obstacle-free and unconstrained motion paths between two endpoints) control problem for robot manipulators with actuator bounds. We assume the friction forces can be neglected compare with the control inputs. For simplicity, we assume the initial and final velocity of each link equal to zero. In this thesis, an approximation to the optimal control which results in a near-minimum-time control will be developed. While not producing exactly minimum time solutions, the algorithm can be easily used in practical applications to generate the near-minimum-time control inputs of robot manipulators. Chapter 2 Problem Formulation and Solution Guidelines 2.1 Review of Conventional Variational Calculus Method 2.1.1 Optimal Control Problem for General Dynamic Systems Suppose the plant is described by the nonlinear time-varying dynamic equation x(i) = f(x,u,i) (2.1) with state vector x(i) E R n and control input vector u(t) E R m. The performance index is J(x,u,t) =(x(i/),i/) + [ tf L(x(t),u(t),t)dt (2.2) Jto where [to,*/] is the time interval of interest. The final time weighting function 0(x(t/),£/) is a function of the final state. The weighting function L(x(i), u(t), t) depends on the state and input at intermediate times in [in,*/]. The performance index is selected to make the plant exhibit a desired type of performance. The optimal control problem is to find the input u*(i) on the time interval [to, tf] that drives the plant given in Equation (2.1) along a trajectory x*(£) such that the cost function in Equation (2.2) is minimized. To solve this problem, we shall use Lagrange multipliers to adjoin the constraints given in Equation (2.1) to the performance index in Equation (2.2). Since Equation (2.1) holds at each t E [to,tf], we require an associated multiplier X(t) E R n, which is a function of time. If we define the Hamiltonian function as: H(x, u, A, t) = L(x, u, t) + A T f (x, u, t) (2.3) 7 Chapter 2. Problem Formulation and Solution Guidelines 8 Using the variational calculus analysis as in Lewis (1986) to minimize J , the necessary conditions for the optimal controller are : State equation: Costate equation: Ft J-f x = | r = f(x,u,t) (2.4) A = - f (2.5) ax Boundary conditions: x(t 0) = x 0 (2.6) (x - Xf \tf 6x(tf) + (0 (2.25) undetermined if Si = 0 where Si(t) is the ith component of the m, x 1 switching functions S(t) defined by S(t) - B T A (2.26) When Si = 0 for a period of time, the Pontryagin maximum principle is not able to define any optimal value of Ui. It is called a singular condition, and the control is referred to as singular control. Thus, for singular control, the necessary condition that u must minimize H provides no information about the value of the controls. If the switch function Si is zero at individual point only, the control is nonsingular. This type of control is referred to as bang-bang control. The controls take their extremal values throughout the whole motion to minimize the maneuver time. If H is not an explicit function of time, the additional necessary condition is: : i7(X*,A*,u*) = 0 te[t0,tf] (2.27) This means if H is not explicit function of time, for the exact minimum time trajectory, the Hamiltonian is always equal to zero. The boundary conditions are: x(*0) = x 0 (2.28) x(t/) = x / (2.29) The set of differential equations (Equation (2.22) and (2.23)), the requirement equations (Equation (2.25) and (2.27)), and the boundary conditions (Equation (2.28) and (2.29)) com-pletely define the EMTC problem for robot manipulator. This leads to a bang-bang exact Chapter 2. Problem Formulation and Solution Guidelines 13 minimum time control solution, except when a singularity occurs, i.e. Si(t) is zero for some finite time interval. As can be seen from the above optimality conditions, no boundary condi-tions for the costate state variables Aj are defined. This fact leads to the problem of solving a Two Point Boundary Value Problem (TPBVP) with boundary conditions on the state x(t) at the initial and final times but unspecified boundary values for A , which is a major cause of the difficulties in finding exact minimum time solution for nonlinear systems such as the robot manipulators. 2.2 Guidelines for Developing a Near-Minimum-Time Algorithm From Section 2.1, we can see that using the calculus-of-variations approach to the Exact Mini-mum Time Control (EMTC) problem for robot manipulators will lead to a Two Point Boundary Value Problem (TPBVP). Due to the nonlinearity of the equations of motion, a numerical so-lution is usually the only approach to the problem. Since the boundary conditions for the canonical adjoint system of differential equations are not known, solving the T P B V P for robot manipulators is very difficult and computationally intensive. It requires good initial guesses for the unknown parameters, and is very sensitive to the unknown initial conditions. The solution is optimal for the unique initial and final conditions, Hence, the computations for the optimal control have to be performed for each manipulator motion. In this thesis, we will develop a new method for generating not an exact minimum time point-to-point manipulator motions, but rather a near-minimum-time solution. The near-minimum-time solution has a similar structure of the exact minimum time solution and satisfies all the boundary conditions and work-energy constraints. The major advantage of the new method is that it is computationally efficient and easy to use and does not require good initial boundary value guesses. So the method is practical for improving the efficiency of robot manipulators. Chapter 3 Development of the Proposed Zero-Net-Work Method 3.1 Motivation 3.1.1 The Structure of the Exact Minimum Time Control Law From Chapter 2, we have found that Pontryagin's maximum principle yields the optimal control law: IHmax if S{ < 0 <(*) = I -Uimax if Si>0 undetermined if Si = 0 where S(t) — B r A is the switching function vector. Definition 3.1 Bang-bang Control: A control u is bang-bang if Ui(t) — Uimax or —Uimax with finite switchings for i G {1,2, ...,n}. For the bang-bang control, the switching functions Si can only have finite zeros, and Ui(t) switches among Uimax and —Uimax at the zero of Si. Definition 3.2 Singular Control: A control u is singular if the canonical equations (Equation (2.22 - 2.23)) are satisfied and Si(t) = 0 for a finite time interval t e [*i,*2], where to where u*(0) is the optimal control at the initial point. At the switching point, Htsw) = -htsw - = 0 (3.38) Substituting Equation (3.35) into Equation (3.38) we obtain U*(0)tsw 7**(0) V 10o So 7 _ 1 _ 1 /Umax / „ O Q \ ™-*®m (3- 4o) For the numerical calculations the following values are used for the one link robot manipulator: Tmax = 10JV.M, I = 10m2kg, 0O = 1. Substituting above values into Equation (3.35) and Equation (3.34), the switching time tsw = Is, and the final time tf = 2s. The state equations before switching (0 < t < 1) are: 9\f{t) = -t (3.42) 9bf(t) = -0.5£ 2 + l (3.43) Chapter 3. Development of the Proposed Zero-Net-Work Method 20 The state equations after switching (1 < t < 2) are: eaf(t) = t-2 (3.44) 9{t)af = 0 .5 t 2 -2 i + 2 (3.45) The costate variables are: Ai(t) = 1 (3.46) A2(t) = 1 - t (3.47) Figure 3.2 shows the position and velocity trajectories. Figure 3.3 shows the applied joint torque, we can see the control is bang-bang and switches once at t = tsw = Is. The costate trajectories are shown in Figure 3.4. The costate variable A 2 determines when to switch. If A 2 changes the sign once, the optimal control is bang-bang and switches once. If the sign of A 2 changes more than once, and is not equal to zero at finite time interval, the optimal control is still bang-bang, but will have more than one switch. Slate trajectories of one-link manipulator 0.5 -1 — Position — Velocity i g l I I I 1 1 1 1 1 1 1 ' 0 0.2 0.4 0.6 0.6 1 1.2 1.4 1.6 1.8 2 Time (second) Figure 3.2: Joint position and velocity of the one-link planar manipulator Chapter 3. Development of the Proposed Zero-Net-Work Method 21 Control trajectory of one-link manipulator 1 1 £ 0 o c 5 0.2 0.4 0.6 0.8 1 1.2 Time (second) 1.4 1.6 1.8 2 Figure 3.3: Applied joint torque of the one-link planar manipulator 1.5 lambda lambd 0.5 -0.5 -1.5 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 time(sec) Figure 3.4: Costate trajectories of the one-link planar manipulator Chapter 3. Development of the Proposed Zero-Net-Work Method 3.1.3 Dynamically Decoupled Two Link Robot Manipulator 22 L i n k l Link 2 Figure 3.5: Dynamically decoupled two-link robot manipulator Next, consider a dynamically decoupled two-link robot manipulator as shown in Figure 3.5. The dynamic equations of motion can be written as 777,1 + 777,2 0 <7i T\ 0 r77,2 fa T2 where m,\, m 6 ™ w In this case the switching point is greater than the zero-net-work switching point. This means the robot arm switches earlier than it should be. The free body diagram of this case is shown in Figure 3.8 . When the velocity equals to zero, the robot hasn't reached the final position yet as shown in Figure (3.8). In this case Work ^ 0 at the final position. The position and velocity trajectories are shown in Figure 3.9. Case 3. 8SW < 9 ™ w In this case the switching point is smaller than the zero-net-work switching point. This means the robot arm switches too late than it should be. The free body diagram of this case is shown in Figure 3.10. When the robot reach the final position, the velocity is not equal to Chapter 3. Development of the Proposed Zero-Net-Work Method 26 zero, so it can't stop at the final position as shown in Figure (3.11). In this case Work ^ 0 at the final position. The position and velocity trajectories are shown in Figure 3.11. Initial position For the zero-net-work method, we set the initial value of the switching point smaller than the zero-net-work switching point. If the searching step size is small enough, case 2 will not happen. We can always control the robot reach the final position. If the switching position is not equal to the zero-net-work switching position, at the final position the work energy constraint can't be satisfied and Worki > 0. If we assume the initial switching position for each link equals to zero, and then increase the values of the switching position step by step until we get for each link Worki = 0, then we find the zero-net-work near-minimum-time solution for the robot. From the work energy behavior of the robot we obtain for link i , the total work done by all the forces: Switching position Final Position Figure 3.6: Free body diagram of one-link robot for case 1 Worki = WAl + Wm + WGi (3.52) Because we assume the control inputs switch once so the work done by the actuator, WM = WbAl + WaAi (3.53) 0o) + Tmax sw } (3.54) Chapter 3. Development of the Proposed Zero-Net-Work Method 27 switching point 51 1 1 1 1 1 1 1 1 1 1 ' 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (second) Figure 3.7: Position and velocity trajectories of case 1 Initial position Figure 3.8: Free body diagram of case 2 Chapter 3. Development of the Proposed Zero-Net-Work Method 28 ^Positior / / / / Stop position % * N Vjelocity ^/ ] Switching point 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (second) Figure 3.9: Position and velocity trajectories of case 2 Initial position Stop position Figure 3.10: Free body diagram of case 3 Chapter 3. Development of the Proposed Zero-Net-Work Method 29 0.5 -0.5 — i 1 \ |~~~~~--~~Jpositiorji | ] ] final point ^ - 7 1 ! I~ I 1 n i ] " - ^velocity) ! ! ! ! ] ! ! switching po in t . - ' i ! ! ! ! ! ! ! 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (second) Figure 3.11: Position and velocity trajectories of case 3 where WAi is the work done by the actuator before the switching point, W^i is the work done by the actuator after the switching point, 6o,9f are the initial and final position of link i, respectively, 8SW is the switching position. From above equations we get, Worki = -2Tmax6sw + Tmax60 + WRi + WGi (3.55) The work done by the reaction and gravity forces can be found by using Newton-Euler formulation. Because we know the switching positions, so we know the control inputs, we can then integrate the dynamic equations of the system. We can get the vectors q , q , q , which are the position, velocity and acceleration of the robot. By using Newton-Euler formulation we can calculate the work done by all the forces for each link. Next we will develop the general Newton-Euler formulation of n-link revolute manipulator. Newton's Second Law: The rate of change of the linear momentum equals the total force applied to the body. f = dP d(mv) dt dt (3.56) where P is the linear momentum. Chapter 3. Development of the Proposed Zero-Net-Work Method 30 Euler's Angular Momentum Law: The rate of change of the angular momentum equals the total torque applied to the body. r = = dJM (3.57) dt dt v ; where H is the angular momentum. In the Newton-Euler formulation we treat each link of the manipulator separately. Newton-Euler approach is a recursive method for computing joint forces and torques. We first step forward through the chain of links to compute the kinematic parameters of the links (velocity, angular velocity, linear & angular acceleration), then step backwards through the links and using Newton's second law and Euler's angular momentum law to computer the joint forces and torques. Each link of the manipulator is coupled to other links, by using forward-backward recursion we can determine all the coupling forces and torques. Thus through Newton-Euler approach, we can calculate all the forces and torques acting on each link. Also the closed-form dynamic equations of the system can also be developed. We first choose frame Fo,... ,Fn using Denavit-Hartenberg (D-H) convention. Frame FQ is an inertial frame, and frame F{ is rigidly attached to the end point of link i . The following vectors are all defined in frame Ff. aC)j = the acceleration of the center of mass of link i . a e )j = the acceleration of the end of link i (i.e. joint i+1). u>j = the angular velocity of frame Fi w.r.t. frame FQ. at = the angular acceleration of frame Fi w.r.t. frame Fo. gi = the acceleration due to gravity (expressed in frame Fi). fi = the force exerted by link i-1 on link i . T j = the torque exerted by link i-1 on link i. R* + 1 = the rotation matrix from frame Fi+\ to frame Fi. m,i = the mass of link i . Ii = the inertia matrix of link i about a frame parallel to frame Fi whose origin is at the center of mass of link i . Chapter 3. Development of the Proposed Zero-Net-Work Method 31 r;,ci = the vector from joint i to the center of mass of link i . ri+hci = the vector from joint i+1 to the center of mass of link i . r i t i + i = the vector from joint i to joint i+1. - R fi+i Figure 3.12: Forces and moments on link i Now consider the free body diagram shown in Figure (3.12), this shows link i together with all forces and torques acting on it. Applying Newton's second law, we have the force balance equation for link i: fi = R - + 1 f , + i + mjaCji - migi (3.58) Applying Euler's law about center of mass, the moment equation of link i is: Ti = R - + 1 r i + 1 - U x r i ) C i + (R- + 1f i +i) x r i + i ) C i + /.a; I W J X ( 2 ^ ) (3.59) We can now state the Newton-Euler formulation as follows. Forward recursion: Start with the initial conditions u>0 = 0, a o = 0, aC io = 0, a 6 j o = 0 and solve following equations to compute W j , a,, and aC;, for i increasing from i to n. OLi = &r..i — (Rt i fwi-i + b ^ (Ri_i) Tai-i + biiji + Ui x bitji (Ri_i) T a e jj_i +d>iX r M + 1 + Ui x (ut x r i j i + 1 ) (Ri_ x)Ta e ii_i +w;x ritd + WiX (o>j x r i ) C i) (3.60) (3.61) (3.62) (3.63) (3.64) Chapter 3. Development of the Proposed Zero-Net-Work Method 32 where b* = (R | , ) r Zi_ i is the axis of rotation of joint i expressed if frame F,. Backward recursion: Start with the terminal conditions f n+i, Tn+\ and use Equation (3.58) Equation (3.59) to compute fj and T j for i decrease from n to 1. By using above forward and backward procedure we can find all the forces and torques acting on link i . The work done by all the forces and torques acting on link i can then be calculated. The overall scheme of the proposed zero-net-work method is shown in Figure 3.13. We first set the initial switching position for each link to zero, that is the link torque does not switch until the link reaches the final position. By doing this, case 2 will never happen for each link, that means we can always control the robot to reach the final position. But case 3 may happen, that is the robot can reach the final position but the work done by all the forces for each link is not equal to zero at the final position. The control input can be found based on the switching point Qsw, q > CLsw, T — -Tmox (3.65) q < 0, this means the initial value of switching position qsw is too small, we then increase c[sw by one step c\sui — Qsw + Iswstep, recalculate the work, and continue to iterate on qsw until Work = 0. After we find the zero-net-work switching positions for each link. They can then be used online to control the robot manipulator. This will result a near-minimum-time control law. Chapter 3. Development of the Proposed Zero-Net-Work Method 33 Q S t a r t ^ 1 . S t a r t f r o m i n i t i a l c o n d i t i o n s 2 . I n p u t i n i t i a l v a l u e s a n d p a r a m e t e r s 3 . S e t s w i t c h i n g p o i n t q s w = 0 I n i t i a l i z e d q , q p r i m e , T , T o u t , W o r k D e c i d e d c o n t r o l T o r q u e T a u = f - T a u m a x q > q s w J + T a u m a x q < q s w I 0 q = 0 T o u t = T o u t + T s t e p I n t e g r a t e t h e d y n a m i c e q u a t i o n o n e t i m e s t e p I ^ D o n e ^ q s w t o o s m a l l q s w = q s w + q s w s t e p N o Figure 3.13: Overall scheme of the zero-net-work method Chapter 4 Development of the Zero-Net-Work Controller for One-Link Manipulators 4.1 One-Link Horizontal Manipulator Next we will investigate the control of a one-link horizontal manipulator. First we will use the traditional method to obtain the exact minimum time solution of the manipulator, and then use the proposed zero-net-work method to solve the problem. We will compare the zero-net-work solution with the true minimum time solution using a computer simulation. 4.1.1 Dynamic Model The one-link horizontal manipulator in Chapter 3 which is shown in Figure 3.1 will be used. 4.1.2 Exact Minimum Time Solution The Exact Minimum Time Control (EMTC) of the one-link horizontal robot is a double inte-grator problem. In Chapter 3, we integrate the dynamic equation of the system and find tf for the exact minimum time solution as, Where tf is the final time and tsw is the switching time, and 9Q is the initial joint position. When t < tsw the control input is —Tmax, when t > tsw the control input is +Tmax. 4.1.3 Zero-Net-Work Solution Next we will use the zero-net-work method to control the one-link horizontal manipulator. First we assume the control input is bang-bang and has one switch. The torque before the switching (4.1) 34 Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 35 point 6SW is Tfc = — | r m a i | , the torque after the switching point is r a = +|r m a x | . The work before the switching point is: f&sw wb = / ndB = n(9sw - 0O) = -\Tmax\(0sw - Oo) (4.2) The work after the switching point is: [Of Wa = radd @ SW = ra(0f - 8sw) = I ' T m a x l ^ s u i (4-3) The work energy equation of the system is: Work = Wb + Wa = 0 (4.4) Substituting Equation (4.2) and Equation (4.3) into Equation (4.4) we obtain Work = -2\Tmax\0Sw + \Tmax\Oo = 0 (4.5) So the switching position: 0Sw = y (4.6) From above we can see , for one link robot manipulator, using zero-net-work method the switching point can be easily found. Base on this we can design the zero-net-work controller for the one-link horizontal manipulator as follow: We use the position sensor to get the position of the robot arm at every moment. When 0(t) < 6SW the control input is +Tmax; when 6(t) > 6SW the control input is — Tmax- Figure 4.1 is the block diagram of the zero-net-work control system. Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 36 Zero Net Work Controller T(t) = + h™,| if e(t) < e T(t) = - | t m „ | if e(t) > e Position Sensor Figure 4.1: Block diagram of the zero-net-work control system Input T (t) Output 9(t) The Robot System Zero Net Work Controller K=2 0.5 Sum1 Constant Relational Operator Switching Point Output Position e(t) onel OneLink Horizontal Manipulaotr Output Control Trajectories State Trajectories Figure 4.2: Simulation block diagram of the horizontal robot Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 37 4.1.4 Computer Simulation Next we will show the simulation results of the zero-net-work control. We use the same param-eters for the manipulator as in Chapter 3 , and use the Simulink for the simulation. Figure 4.2 is the block diagram of the simulation. The system's s-function "onel.m" can be found in the Appendix A . l . The simulation results for two different motion configurations are presented in the follow: Simulation 1: Move the arm from initial state (1, 0) to final state (0,0). Using the zero-net-work method, we obtain the switching position for this case is: 0SW = j = 0.5 (4.7) Figure 4.3 shows the position and velocity trajectories. Figure 4.4 shows the applied joint torque. The simulation results show that the final time of the zero-net-work solution is tf = Isecond. The exact minimum time solution for this case is: tf = 2tsw = 2i / = 2second (4.8) V T M A X So for this case using the zero-net-work method we obtain the exact minimum time solution. Simulation 2: Move the arm from initial state (7r/2,0) to final state (0,0). The switching position for this case is: 0SW = y = TT / 4 (4.9) Figure 4.5 shows the position and velocity trajectories. Figure 4.6 shows the applied joint torque. The simulation results show that the final time of the zero-net-work solution is tf = 2.51 second. The exact minimum time solution for this case is: Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 38 State trajectories of one-link manipulator 0.5 -1 • I — _ Position Velocity ' 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (second) Figure 4.3: State trajectories of zero-net-work control simulation 1 Control trajectory of one-link manipulator 1 ! ! ! r j I I , , i , I " 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (second) Figure 4.4: Applied joint torque of zero-net-work control simulation 1 Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 39 State trajectories of one-link manipulator 0.5 2 -0.5 1 — H 1 1 I — Position j Velocity '• i 0.5 1 1.5 Time (second) 2.5 Figure 4.5: State trajectories of zero-net-work control simulation 2 tf = 2tsw = 2JL^- = 2.51 second (4.10) So for this case using the zero-net-work method we can also obtain the exact minimum time solution. 4.1.5 Summary From above we can see, for one-link horizontal manipulator, the zero-net-work method can obtain the exact minimum time solution. The zero-net-work method is very simple for this one link manipulator and the switching position can be easily found based on the initial position. Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 40 Control trajectory of one-link manipulator 151 ! 1 1 10h 5h 1 o -o c o O -5 --10 0 0.5 1 1.5 2 2.5 Time (second) Figure 4.6: Applied joint torque of zero-net-work control simulation 2 Figure 4.7: One-link vertical manipulator Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 41 4.2 One-Link Vertical Manipulator 4.2.1 Dynamic Model Next we will consider a one-link vertical manipulator as shown in Figure 4.7. Assume gravity acts vertically downward. The equation of motion of the robot can be obtained using the Euler-Lagrange equation. The kinetic energy of the system is: KE = ^I62 (4.11) where I is the moment of inertial about the joint O. The potential energy of the system: PE = ^mglsinO (4.12) The Lagrangian L is given by: L = KE-PE = )-I62 -)-m,glsin6 (4.13) Substituting above expression into Euler-Lagrange equation yields the equation of motion: The absolute value of the control torque r is bounded: \T\ < T M A X (4.15) The state space equations are: xi{t) = x2(t) (4.16) . . . r(t) mqlcosixxit)) X2(t) = - y - y 2 y y > ) (4.17) where Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 42 xi = 9(t) (4.18) x2 = 9(t) (4.19) \T\ < W (4.20) 4.2.2 The Exact Minimum Time Solution The initial conditions of the robot are: *i(0) = *io (4.21) .T2(0) = 0 (4.22) The final conditions are: Xl(tf) = 0 (4.23) x2(tf) = 0 (4.24) The cost functional is: J= ldt (4.25) Jto The Exact Minimum Time Control (EMTC) problem of the one-link vertical robot can be stated as follows: For the robot governed by the differential Equations (4.16 - 4.17) find an admissible control r satisfying the constraints (|r| < T M A X ) which transfer the robot from initial position the fixed final position in minimum time, i.e., such that the cost functional J denned in Equation(4.25) is minimized. Define the Hamiltonian as, 77(x,r,A) = l + A 1 , 2 + A4-^^i] (4.26) Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators where A x , A 2 are the costate variables. Using the techniques of variational calculus and ap-plying Pontryagin's maximum principle, the necessary conditions for the EMTC can be written as: State equation: Costate equation: x = H = / ( x , T , t ) (4.27) Stationary Condition: H(x*, A*, r*) < H(x*, A*, r) (4.29) Where r* is the optimal control, x* and A* are the corresponding optimal state and costate trajectories. Substitute Equation (4.26) into Equation (4.29), we obtain: A 2 r* < A 2 r (4.30) From above equation, we can see the optimal control r* is depend on costate variable A 2 . T*(t) = Tmax if A 2 < 0 - T M A X if A 2 > 0 Substitute Equation (4.26) into Equation (4.27) and Equation (4.28), the necessary condi-tions for EMTC of the one-link vertical robot are: Xl - X2 T m,glcosx\ X2 21 Ai = ——X2smxi A 2 = - A i (4.31) Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 44 where the control input r is depend on A 2 . For the exact minimum time control problem the initial and final position are fixed so the boundary conditions for the above differential equation are: x1(0) = x10 .T 2 (0)=0 x1(tf)=0 x2(tf)=0 (4.32) Since the Hamiltonian function does not explicity depend on time: H(X*,X*,T*) = 0, Vt €[(), 9SW the control input is — Tmax. The block diagram of the zero-net-work control system of the one-link vertical robot is the same as for the one-link horizontal manipulator as shown in Figure 4.1. 4.2.4 Computer Simulation The proposed zero-net-work method will now be simulated for the one-link vertical manipula-tor, and its performance will be compared with exact minimum time solution. The physical Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 48 parameters of the manipulator are : T~max = 8N.m,, m = 5kg, I = 0.3m, / = Q.\bm?kg. We use Simulink for the simulation. Figure 4.8 is the block diagram of the simulation. The system's S-function "one2.m" can be found in Appendix A.2 . The control input is bang-bang and has one switch and at every moment we compare the position output 9(t) with the switching position 9SW. The control input switches from the maximum value to minimum value when 9{t) = 9SW. If 9(t) < 9SW, the control input r(t) = -rmax\ if 9(t) > 6SW, control input r(t) = + T m a x . The simulation results for two different motion configurations are presented in the following: Simulation Case 1: In this case we will move the manipulator from initial state (1,0) to final state (0,0). Using zero-net-work method from Equation (4.48), we can find the switching position as: 9SW = 0.8866 (4.49) Once we find the switching point, we can use this value in simulation block diagram shown in Figure 4.8 and use Simulink runs the simulation. Figure 4.9 and Figure 4.10 show the state and control trajectories of the robot. The simulation results show that the control input switches once at tsw = 0.0531s, and the final time of the zero-net-work solution is tf = 0.6268s. The exact solution for this case can be obtained by using MUSN to solve the equivalent T P B V P (Equation 4.43). The FORTRAN program for the one-link vertical exact minimum time solution can be found in Appendix A.3. Figure 4.11 is the state trajectories and Figure 4.12 is the control trajectory, Figure 4.13 is the costate trajectories of the exact minimum time solution. For the exact minimum time solution the control input switches once at tsw = 0.0531s, the exact minimum time is ttf = 0.6268s. The initial values for the costate variable are Ai = 0.2446, A 2 = 0.0125. From Figure 4.12 and Figure 4.13 we can see the control input depends on A 2 . When A 2 > 0 the control input is —Tmaxi £Lncl when A2 ^> 0 the control input is Trr\,ax' From above we can see for the one-link vertical manipulator in this case, using zero-net-work method Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 49 8 Tmax K=2 0.5 Sum1 Constant Relational Operator input.mat One-Link Vertical Manipulaotr To File 1 Control Trajectories 0.8866 Switching Point thetasw output, mat To File2 Demux Output • State Trajectories Figure 4.8: Case 1: Simulation block diagram of the one-link vertical manipulator we obtain the exact minimum time solution. Simulation Case 2: In this case we will move the manipulator from initial state (1.57,0) to final state (0, 0). Using zero-net-work method from Equation (4.48), we can find the switching position as: esw = 1.2444 (4.50) Once we find the switching point, we can use this value in simulation block diagram shown in Figure 4.14 and use Simulink runs the simulation. Figure 4.15 and Figure 4.16 show the state and control trajectories of the manipulator. The simulation results show that the control input switches once at tsw = 0.1079s, and the final time of the zero-net-work solution is tf = 0.7525s. The exact solution for this case can be obtained by using MUSN to solve the equivalent TPBVP (Equation 4.43). Figure 4.17 is the state trajectories and Figure 4.18 is the control trajectory, Figure 4.19 is the costate trajectories of the exact minimum time solution. For the exact minimum time solution the control input switches once at tsw = 0.1079s, the exact minimum time is ttf = 0.7525s. The initial values for the costate variable are Ai = 0.2051, A 2 = 0.0187. From above we can see for the one-link vertical manipulator in case 2, using Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 50 State trajectories of one-link vertical manipulator (zero-work solution) 0 -31 - 5 ' I 1 ! ! ! i J r u s m — Velox; ty 1 \ I \ \ \ \ \ \ \ \ \ / / / / \ \ • / / / \ ••/••• • \/ 0 0.1 0.2 0.3 0.4 0.5 0.6 Time (second) Figure 4.9: Case 1: State trajectories of the one-link vertical manipulator (zero-net-work solu-tion) Control trajectory of one-link vertical manipulator (zero-work solution) 0.2 0.3 0.4 Time (second) Figure 4.10: Case 1: Control trajectory of the one-link vertical manipulator (zero-net-work solution) Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 51 State trajectories ot one-link vertical manipulator (exact minimum time solution) s 1 1 1 1 1 Position -Velocity 1 I \ : - * * " * i : - "* T 1 I " \ ^ ' : t • i I : • : \ / \ \ / \ •/ * A i / : • i i i 0.3 0.4 Time (second) Figure 4.11: Case 1: State trajectories of the one-link vertical manipulator (exact minimum time solution) Control trajectory ot one-link vertical manipulator (exact minimum time solution) 0.2 0.3 0.4 Time (second) Figure 4.12: Case 1: Control trajectory of one-link vertical manipulator (exact minimum time solution) Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 52 Costate trajectories of one-iink vertical manipulator (exact minimum time solution) — Lambdal Lambda2 : : : : : "~ ~ ~ ~ s _ ~" " t - -i i i i i ' 0 0.1 0.2 0.3 0.4 0.5 0.6 Time (second) Figure 4.13: Case 1: Costate trajectories of the one-link vertical manipulator (exact minimum time solution) zero-net-work method we can also obtain the exact minimum time solution. 4.2.5 Summary From above simulations we can see, for a one-link robot manipulator, the zero-net-work method yields the exact minimum time solution. The zero-net-work method is much easier to imple-ment and much simpler compared to the traditional method which requires the solution of the TPBVP. The traditional method requires the solution of the T P B V P for different initial positions, and also requires a good initial guess for the missing boundary values. For different initial conditions, the zero-net-work method can use the Equation (4.48) to find the switching position very easily, and the control input is based on the switching position. Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 53 To File, Control Trajectories To File2 State Trajectories Figure 4.14: Case 2: Simulation block diagram of the one-link vertical manipulator State trajectories of one-link vertical manipulator (zero-work solution) -41 1 1 — Position Velocity \ \ ^ - -< \ \ \ \ \ \ / \ \ \ . / / / / \ \ \ / / / / \ \ 1 1 1 v / 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 Time (second) Figure 4.15: Case 2: State trajectories of the one-link vertical manipulator (zero-net-work solution) Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 54 Control trajectory of one-link vertical manipulator (zero-work solution) 1 1 1 ! 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 Time (second) Figure 4.16: Case 2: Control trajectory of the one-link vertical manipulator (zero-net-work solution) State trajectories of one-link vertical manipulator (exact minimum time solution) 2 1 — Position — Velocity \ \ \ \ v \ \ \ / \ t \ / / / \ \ \. / / / / \ / 1 : V 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 Time (second) 6 4 — 2 Figure 4.17: Case 2: State trajectories of the one-link vertical manipulator (exact minimum time solution) Chapter 4. Development of the Zero-Net-Work Controller for One-Link Manipulators 55 Control trajectory of one-link vertical manipulator (exact minimum time solution) 1 ! 1 1 1 1 0 _ 2 0.2 0.3 0.4 0.5 Time (second) 0.6 0.7 Figure 4.18: Case 2: Control trajectory of one-link vertical manipulator (exact minimum time solution) Costate trajectories of one-link vertical manipulator (exact minimum time solution) 1 — Lambda 1 — Lambda2 ~ — — — . 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 Time (second) Figure 4.19: Case 2: Costate trajectories of the one-link vertical manipulator (exact minimum time solution) Chapter 5 Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 5.1 Dynamic Model Next we will investigate the control of a cylindrical manipulator as shown in Figure 5.1. The first joint is revolute and produces a rotation about the base, while the second and third joints are prismatic. The joints of the robot's hand are neglected here. The first degree of freedom is the rotation 9\. This coordinate is driven by a limited torque T\ . The second degree of freedom is the radial translation r. It is driven by a limited force F2. The third degree of freedom is the vertical translation. This coordinate is decoupled both from the first and second coordinate, so we can treat it separately as a one-link problem. We can use the zero-net-work method shown in Chapter 4 to design the zero-net-work controller for this link. The detail is neglected here, because it is same as the one-link manipulator in Chapter 4. The control of the cylindrical robot can be treated as two parts: the first part is the one-link robot control problem of the third link; the second part is the planar Revolute and Prismatic (RP) two-link robot control problem. The influence of the third link on the first and second link is neglected and the second and the third link are lumped into a point mass m,^. The configuration for the second part of the cylindrical robot can be simplified as a two-link planar Revolute and Prismatic (RP) robot as shown in Figure 5.2, where I\ is the moment of the inertial about the center of mass of link 1, mi is mass of link 1 and m,2 is mass of link 2. The absolute value of the control torque for link 1 is bounded: I T i l < Timax (5.1) 56 Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator Sketch of the cylindrical robot Figure 5.1: Sketch of the cylindrical robot Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 58 Figure 5.2: Sketch of two-link revolute and prismatic manipulator Link cn Cti ch 6i 1 0 TT / 2 0 TT / 2 + 6{ 2 0 0 d*2 0 Table 5.1: D-H parameters of two-link revolute and prismatic manipulator The control force F 2 for link 2 is also bounded: 1^21 < F2max (5.2) We establish the base frame Fo as shown in Figure 5.2. Once the base frame is established, the F\ frame and F 2 frame is fixed as shown by the Denavit-Hartenberg (D-H) convention. The D-H parameters are shown in Table 5.1,where * means variable. The homogeneous transformation from frame F Q to frame F\ is: Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 59 Hoi = —sin9\ 0 cos9\ 0 cos9\ 0 sin8\ 0 O l O O 0 0 0 1 The homogeneous transformation from F\ to F2 is: (5.3) H 12 1 0 0 0 0 1 0 0 1 0 1 d2 0 0 0 1 So the homogeneous transformation from FQ to F2 is: (5.4) H-02 = H01H12 = (5.5) —sinOi 0 cos9\ d2cos9\ cos9\ 0 sin9\ d2sin9\ 0 1 0 0 0 0 0 1 Next we will use the Newton-Euler method shown in Chapter 3 to obtain the dynamic equations of the robot. We begin with the forward recursion to express the various velocities and accelerations in term of 9\, d2 and their derivatives. The angular velocity and acceleration of link 1 are: ui = [0 e\ Of aj = [0 9\ 0] T The angular velocity and acceleration of link 2 are: u, 2 = [0 6\ 0] T <*2 = [0 9\ 0 ] r (5.6) (5.7) (5.8) (5.9) Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 60 5.1.1 Forward Recursion The acceleration of the end of link 1 is: ae,i = w i x r ^ + wi x(wi x r y ) = [d26\ 0 -d,29\]T (5.10) The acceleration of the center of mass of link 1 is: ac,i = wixri ) C i + wix(wiXri, c i) = [ | * o - ljelf ( 5 . i i ) The acceleration of the center of mass of link 2 is: aC ) 2 = (R?)Tae>i + u2 x r 2 , c 2 + u>2 x (u>2 x r2, c 2) + 2u>2 x z i d 2 + z i d 2 = [{d29\ + 20\d2) 0 {-d29\ + d2)]T (5.12) 5.1.2 Backward Recursion Now we carry out the backward recursion to compute the forces and joint torques. The force exerted by link 1 on link 2 is: f2 = m 2a C i2 d29\ + 2B\d2 = rn,2 | 0 -d2e\ + d2 The torque exerted by link 1 on link 2 is : r 2 = [0 0 0] T (5.13) (5.14) So there are two forces act on link 2, f2x and f2z, Figure 5.3 shows all the forces and torques acting on link 1 and link 2. hx = (d291 + 29ld2)m,2 hz = {~d29l + d2)m2 (5.15) (5.16) Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 61 Initial Position l i n k l Final Position Figure 5.3: Forces and torques act on link 1 and link 2. The force exerted by base the on link 1 is: fi = mia C j i + R 2 f 2 (miZi/2 + m,2d2)9\ + 29\d2m,2 0 (5.17) -(mih/2 + m2d2)9\ + m2d2 The torque exerted by the base on link 1 is: r i = R 2 r 2 - fi x n , c i + (Rff 2) x r2,ci + hai + u>i x 0 (7i + mi/ 2 /4 + m2d$)b\ + 2m,2d29\d2 (5.18) 0 From the above we can obtain the dynamic equations of the two-link RP robot as: [h + mi(Zi/2) 2 + m,2dl\b\ + 2m2d2d29\ = n m,2d2 - m2d292 — F2 (5.19) (5.20) Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 62 The moment of inertia of link 1 about the origin of frame Fo is: I 0 i = J i + 7 7 i 1 ( / 1 / 2 ) 2 (5.21) and d2(t) = r1+r(t) (5.22) Substituting Equation (5.21) and Equation (5.22) into Equation (5.19), (5.20) we obtain: [hi + ™,2{r + ri)2](9i + 2m2(r + ri)r0\ = r : (5.23) m2r - m2(r + n)9j = F2 (5.24) The dynamic equations in state space form are: xi = x2 (5.25) • n - 2m2(.T3 + ri)x2x4 X2 = ; ; (5.26) loi +m2(x3 +riY x3 = x4 (5.27) .4 = F2 + m,2(xz+n)xl ( 5 2 8 ) m2 where xi = 6i(t) x2 = 6\(t) (5.29) .x3 = r(t) X i = r(t) (5.30) 5.2 The Exact Minimum Time Solution The initial conditions of the robot are: .Ti(0) = .x 1 0 x2(Q) = 0 (5.31) a*(0) = x20 .r4(0) = 0 (5.32) The final conditions are: xi(tf) - x2(tf) = x3(tf) = x4(tf) = 0 (5.33) Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 63 where tf is the final time. The absolute values of the control inputs are bounded: In | < rlmax (5.34) 1^ 21 < F2max (5.35) The cost functional is: J = / ldt (5.36) Jto The Exact Minimum Time Control (EMTC) problem of the two-link planar Revolute and Prismatic (RP) can be stated as follows: For the robot governed by the differential Equations (5.25 - 5.28) find an admissible control T\ and F2 satisfying the constraints in Equation (5.34) and Equation (5.35) which transfer the robot from initial position to the fixed final position in minimum time, i.e., such that the cost functional J defined in Equation(5.36) is minimized. Referring to Equations (5.25 - 5.28), we define V i = n - 2 m 2(x3+ r 1)x2X 4 V2 = ^ + m 2 (x 3 + r 1 ) x l m,2 The Hamiltonian is then H(x, n,F2, A) = 1 + A I . T 2 + X2Vi + A3 .T4 + A4V2 (5.39) where X\, X2 , A 3 , A 4 are the costate variables. The necessary conditions for the EMTC can be written as: State equation: x = — = / (x , r 1 , F 2 , t ) (5.40) Costate equation: Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 64 7 T = - A (5-41) ax Stationary Condition: H(x\ A*, T I * , F2*) < (x*, A*, n , F 2 ) (5.42) Where rj* and F£ are the optimal controls, x* and A* are the corresponding optimal state and costate trajectories. Substituting Equation (5.39) into Equation (5.42), we can see the optimal controls are dependent on the costate variables A 2 and A 4 . TImax if A 2 < 0 — Tlmax if A 2 > 0 Tl*(t) = F2*(t) F2max if A 4 < 0 —F2max if A 4 > 0 By using the time normalization technique shown in Chapter 4, we can transform EMTC problem into a fixed time interval problem. The equivalent Two Point Boundary Value Problem (TPBVP) for the two link RP robot is: ~dT dx2 ~dT dx3 dT dx± ~dT d\i IT d\2 dT dh dT dX^ IT dz dT = zx2 = zVi = 2.T4 = zV2 = 0 dvx dv2 dx3 dx3 — = 0 (5.43) (5.44) (5.45) (5.46) (5.47) (5.48) (5.49) (5.50) (5.51) Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 65 where T G [0,1] for t 6 [0, tf], and Let V2 = Then dVi dx2 dVi dx3 dVi dx^ dV2 dx2 dV2 dx3 The boundary conditions are: *(T) = tf T = tf U_ V ri - 2m2(x3 + ri)x2x± hi + m2(xz + r i ) 2 F2 +m,2(x3 + r{)x\ m2 -2m,2(x3 + ri)x4 V -2m,2x2x4V - 2m,2(x3 + r{)U V2 -2m,2(x3 + ri)x2 V 2x2(x3 + ri) r 2 x2 (5.52) (5.53) (5.54) (5.55) (5.56) (5.57) (5.58) (5.59) (5.60) xi (0) = .Tio •^ 3(0) = x20 xi(l) = 0 xs(l) = 0 x2(0) = 0 .T4(0) = 0 .T2(l) = 0 .T4(l) = 0 (5.61) From the above equations, we can see the boundary conditions for the costate variables are unknown. In order to get the exact minimum time solution, we have to solve the above TPBVP. We can use subroutine MUSN to solve the TPBVP. The convergence of the solution Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 66 depend on the initial guess of the unknown parameters. In this case we have to give the initial guesses for 5 unknown parameters, that is Ai , A 2 , A 3 , A 4 and the final time z. The convergence of the solution is very sensitive to the unknown initial guesses. For this reason, it is very hard to get the exact minimum time solutions using the traditional calculus of variation method. 5.3 Zero-Net-Work Solution Next we will design the zero-net-work controller for above two-link Revolute and Prismatic (RP) robot manipulator. The basic algorithm of the zero-net-work method has been given in Chapter 3. For the RP manipulator in this chapter, we assume the final positions and velocities of each link are equal to zero. For zero-net-work method the control torque for link 1 is bang-bang and has one switch and the control force for link 2 is also bang-bang and has one switch. We can use the results from Newton-Euler formulation of the dynamic equations in Section 5.1 to calculate the work done by all the forces/torques for each link. Al l the work will be calculated in base frame Fo. For link 1 the work done by all the forces/torques can be calculated as follow: As shown in Figure 5.3, there are one torque T\ and four forces ( / i x , / i 2 , —f2x, ~hz) act on it. In frame Fo, the displacement of point o is always zero. If we move all the forces to point o, forces fix,fiz, —f2z pass through point o, so the work done by these forces is always zero. The displacement of 02 in x direction in frame Fo is: Sx = 6[(r± + r)cos6i] (5.62) The displacement of o2 in y direction in frame Fo is: 8y = 6[(ri + r)sin9i] (5.63) Reaction force: hx = m 2[(ri + r)6i + 20if 1] (5.64) Work done by all the forces/torques for link 1 is: Worh = TISOI - f2x(ri + r)86i (5.65) Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 67 For link 2 as shown in Figure 5.3, there are two forces f2x,f2z act on it. f2z = F2 is the control input for link 2. Work done by all the forces for link 2 is: Work2 = (-f2xsindi + F2cos9i)6x + (f2xcos9x + F2sin9{)8y (5.66) Since we assume the control input for each link is alway bang-bang and switch once. For different physical parameters and motion configurations, we can not always find the solution that both link reach the final position at the same time using maximum or minimum control input only. Following assumption must be made: once the link reaches the final position, the control input will be changed to the torque/force needed to hold the link at the final position. For the two link planar RP manipulator there is only two possibilities: Case 1: Link 1 reaches the final position first. This means: 01 = 0; 0i = 0; 9\ = 0 (5.67) In order to hold link 1 at it's final position, from Equation (5.23) the control input for joint 1 is: TI = 0 (5.68) This means the movement of link 2 has no effect on link 1, once link 1 reaches the final position. When link 1 reaches the final position, we can turn the control input n off. Case 2: Link 2 reaches the final position first. This means: r = 0 ; r = 0 ; f = 0 (5.69) In order to hold link 2 at the final position, from Equation (5.24) the control input for joint 2 is: F2 = -m2rx9\ (5.70) Once the link 2 reaches the final position, the control input for link 2 is changed to —m.2r\9\. Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 68 We first set the switching positions of link 1 and link 2 to zero, this means the control inputs do not switch until the final point is reached. So each link can reach the final position, but the work done by all the forces and torques for each link is not equal to zero at the final point. We will then increase the switching position for each link, and calculate the work done by all the forces and torques at the final position. When the work done for each link at final position is zero, then we have found the zero-net-work switching point for each link. After we find the zero-net-work switching point for each link, we have specified the zero-net-work controller for the RP robot manipulator. We use the position sensor to obtain the position of the robot arm at every moment. For link 1, when 6\(t) < 6\sw the control input is + T i m a x ; when Q\(t) > 9isw the control input is — T i m a x . For link 2, when r(t) < rsw the control input is +F2max', when r[t) > rsw the control input is — F2max- The FORTRAN program which is used to find the zero-net-work switching positions is shown in Appendix A.4. FORTRAN subroutine DDASSL shown in Appendix B.2 is used to integrate the dynamic equations of the robot system. 5.4 Computer Simulation The proposed zero-net-work method will now be simulated for the two-link Revolute and Pris-matic (RP) manipulator, and its performance will be compared with the exact minimum time solution. We will use the zero-net-work method to two different parameter sets of the RP manipulator. For each parameter set, we will perform simulations for two different motion configurations. Table 5.2 shows the two different parameter set for the simulations. Set Tlmax F2max n h 777,1 777,2 h N.m N m m Kg Kg kgm? 1 2 2 0.6 0.9 3 1 0.81 2 3 2.5 0.4 1.2 4 1.5 1.92 Table 5.2: Physical parameters of two-link revolute and prismatic manipulator We use Simulink to perform the simulation. The system's S-function can be found in Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 69 Relational Absolute Operator! poston error Constantl 0 P e r a t 0 r 2 Control inpull Control input2 Control trajectories F2 = -m2*f1*x(2)A2 Relational DS.vflhi'?9 Point thetasw • Muxl ol link 1 Mux2 of link 2 Figure 5.4: Case 1: Simulation block diagram Appendix A.6. The parameter set 1 shown in Table 5.2 is used for the simulation case 1 and case 2, the parameter set 2 is used for the simulation case 3 and case 4. The simulation results for different parameters and motion configurations are presented in the following: 5.4.1 Simulation Case 1: Case 1 00 ro rsw t-lsw ^2sw * 2 / error Zero-net-work 7T/4 0 0.3927 0.6778 1.3449 6.74 % EMTC TT / 4 0 0.6308 0.3510 0.9094 1.2600 Table 5.3: Case 1: Simulation results of two-link revolute and prismatic manipulator In this case we will move the manipulator from initial state 0, 0, 0) to final state (0, 0, 0, 0). The FORTRAN program using zero-net-work method to find the switching position for Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 70 Casel: Joint Position Trajectories ot Link 1 c 0-4 I Zero-work EMTC \ \ \ V \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ . : \ \ \ : \ v \ \ \ ^ \ N . N. . . . 7s 1 ' 0 0.2 0.4 0.6 0.8 1 1.2 1.4 Time (second) Figure 5.5: Case 1: State trajectories of link 1 Casel: Joint Position.Trajectories of Link 2 0.05 0 -0.15 -0.2 Zero-work EMTC N \ \ \ S / / / \ \ \ \ \ / / / / \ \ \ \ f 1 1 1 1 \ \ \ \ / \ / : / /. / : i 0 0.2 0.4 0.6 0.8 1 1.2 1.4 Time (second) Figure 5.6: Case 1: State trajectories of link 2 Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 71 Casel: Control Trajectories of Link 1 5 0.5 I 1 1 ! 1 I j Zero-work - - EMTC : I : 1 1 1 1 : I • I 1 : 1 : 1 - *. X : I ; I 1 1 1 - : : ' T " : l : I 1 1 : 1 _ : :'T ' : I • I : I I I : I ' I I : I : I : I i i i i i i " 0 0.2 0.4 0.6 0.8 1 1.2 1.4 Time (second) Figure 5.7: Case 1: Control trajectories of link 1 Casel: Control Trajectories of Link 2 g- 0.5 ¥ = 0 o c o O-0.5 i i Zero-work EMTC I ; i ; \ • — i.. • 1 1 1 I 1 1 1 ! \ 1 1 1 1 1 , 1 ; l I I I I I I I 0 0.2 0.4 0.6 0.8 1 1.2 1.4 Time (second) Figure 5.8: Case 1: Control trajectories of link 2 Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 72 Casel: Work done by all the forces and torques of R&P two-link robot 0.6 0.5 0.2 0.1 0 I ! ! Link 1 - - Link 2 / K \ / \ \ / ' / • y / y ' N \ N ; \ N \ • S 0 0.2 0.4 0.6 0.8 1 1.2 1.4 Time (second) Figure 5.9: Case 1: Work done by forces/torques for each link (zero-net-work solution) each link is shown in Appendix A.4 . The simulation results are shown in Table 5.3. For link 1, #o is the initial position, 6SW is the switching position, tXsw is the switching time, t\f is the final time. For link 2, rn is the initial position, rsw is the switching position, t2SW is the switching time, t,2f is the final time. In this case, the initial velocity and position of link 2 are zero. So there is no minimum to maximum switch for link 2. The control input for link 2 start from the beginning is the force needed to hold link 2 at the final position while link 1 is moving. Control input for link 1 has one switch: 0isw = 0.3927rad. From offline calculation we find the switching positions, we can then use these values in simulation block diagram shown in Figure 5.4 and use Simulink run the simulation. Exact minimum time solution can be obtained by solving the Two Point Boundary Problem (TPBVP). The FORTRAN program for exact minimum time solution of the two-link revolute and prismatic manipulator can be found in Appendix A.5. The exact minimum time is tf exact = 1.2600sec, zero-net-work time is tfzw = l-3449sec. The time difference between the Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 73 proposed zero-net-work control method and Exact Minimum Time Control (EMTC) is 6.74%. Thus we can say zero-net-work control method leads to a near minimum time solution. Figure 5.5 shows the state trajectories of link 1, Figure 5.6 shows the state trajectories of link 2. Figure 5.7 shows the control trajectories of link 1, Figure 5.8 shows the control trajectories of link 2. The work done by external and reaction forces/torques for zero-net-work method are shown in Figure 5.9. 5.4.2 Simulation Case 2: Case 2 i~-2sw t2f error Zero-net-work 0.2 0.3 0.1083 0.1426 0.3709 0.4007 0.7383 0.7810 1.69 % EMTC 0.2 0.3 0.0170 0.4020 0.3999 0.7680 Table 5.4: Case 2: Simulation results of two-link revolute and prismatic manipulator In this case we will move the manipulator from initial state (0.2, 0, 0.3, 0) to final state (0, 0, 0, 0). The simulation results are shown in Table 5.4. Control input for link 1 has one switch: 0\sw = 0.1083rad. Control input for link 2 has one switch: rsw — 0.1426m. From offline calculation we find the switching positions, we can then use these values in simulation block diagram shown in Figure 5.10 and use Simulink run the simulation. The exact minimum time is tfexact = 0.7680sec, zero-net-work time is tfzw = 0.7810sec. The time difference between the proposed zero-net-work control method and EMTC is 1.69%. Figure 5.11 shows the state trajectories of link 1, Figure 5.12 shows the state trajectories of link 2. Figure 5.13 shows the control trajectories of link 1, Figure 5.14 shows the control trajectories of link 2. The work done by external and reaction forces/torques for zero-net-work method are shown in Figure 5.15. Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 74 Operaiorl position error! H 3 Relational D^wltchino Operators P o l n l *<>taswl Operate position error2 State trajectories Figure 5.10: Case 2: Simulation block diagram 0.35 0.25 0.2 0.15 0.05 h -0.05 Case2: Joint Position Trajectories of Link 1 I I ! Jero-work EMTC — = ^ -0.1 0.1 0.2 0.3 0.4 0.5 Time (second) 0.6 0.7 Figure 5.11: Case 2: State trajectories of link 1 Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator Case2: Joint Position Trajectories of Link 2 ! j l Zero-work EMTC < i 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 Time (second) Figure 5.12: Case 2: State trajectories of link 2 Case2: Control Trajectories of Link 1 5 0.5 z 3 -' H I — Zero-work - EMTC I j I i ! 1 ! t i A I ; ! i i " 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 Time (second) Figure 5.13: Case 2: Control trajectories of link 1 Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 76 Case2: Control Trajectories of Link 2 Q. = o g C O " - 0 . 5 1 1 Zero-work - - EMTC : 1 ! I j 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 Time (second) Figure 5.14: Case 2: Control trajectories of link 2 Case2: Work done by all the forces and torques of R&P two-link robot 0 1 1 / I — Linki - - Link 2 / / : / \ \ \ / / / : / \ \ \ ' / :/ / / : V ' " \ \ \ '• I ' ' : / / \ \ \ \ / '; / / '•' / \ " A \ \ \ / '• / / X / : \ : V A ^ \ ; \ / r i i ^ \ x : \ 1 i — 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 Time (second) Figure 5.15: Case 2: Work done by forces/torques for each link (zero-net-work solution) Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator Relational Operatorl Control inputl Control inputs Relational t—1 c^ SSrti ° P e r a , o r 2 Control trajectories - < ^ * < ^ 5 4 -F2 = -m2*rl*x(2)*2 Absolute position errorl Switching Point thetasw State trajectories of link 1 State trajectories ot link 2 Figure 5.16: Case 3: Simulation block diagram Case3: Joint Position Trajectories of Link 1 ^0 .3 Zero- work EMTC Vs. \ \ \ \ \ \ \ \ \ \ V \ \ \ \ \ \ \ . \ . \ \ \ \ >v 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 Time (second) Figure 5.17: Case 3: State trajectories of link 1 Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 78 Case3: Joint Position Trajectories of Link 2 0.05 0 -0.2 -0.25 1 ?ero-work EMTC •v. V \ \ \ / / / \ \ \ \ / / / / . \ \ \ t 1 1 I \ \ \ \ \ / / / / \ \ s. / • j l I I 1 I I I I I 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 Time (second) Figure 5.18: Case 3: State trajectories of link 2 Case3: Control Trajectories of Link 1 2 -2 -3 — Zero-work cn trm ; i i i i • " I " i i i ; i i i ! 1 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 Time (second) Figure 5.19: Case 3: Control trajectories of link 1 Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 79 Case3: Control Trajectories of Link 2 2 3 -1 -2 -3 1 r Zero-work - - EMTC ! i I i I I \ I i i I I I I j 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 Time (second) Figure 5.20: Case 3: Control trajectories of link 2 Case3: Work done by all the forces and torques of R&P two-link robot 1.2 0.8 0.2 0 ! — l — Linki - - Link 2 i i ' 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 Time (second) Figure 5.21: Case 3: Work done by forces/torques for each link (zero-net-work solution) Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 80 Case 3 Oo Qsw rsw tlsw t-2sw hf * 2 / error Zero-net-work TT/4 0 0.3927 0.7520 1.5005 2.63 % EMTC TT/4 0 0.7250 0.3850 1.0816 1.4620 Table 5.5: Case 3: Simulation results of two-link revolute and prismatic manipulator 5 . 4 . 3 S i m u l a t i o n C a s e 3 : In this case we will move the manipulator from initial state (j, 0, 0, 0) to final state (0, 0, 0, 0), parameter set 2 of the RP manipulator in Table 5.2 will be used for simulation. The simulation results are shown in Table 5.5. For link 1, f?o is the initial position, 9SW is the switching position, t\sw is the switching time, t\f is the final time. For link 2, ro is the initial position, rsw is the switching position, t2sw is the switching time, t2f is the final time. Control input for link 1 has one switch: Q\sw = 0.3927rad. From offline calculation we find the switching positions, we can then use these values in simulation block diagram shown in Figure 5.16 and use Simulink run the simulation. The exact minimum time is tfexact = 1.4620sec, zero-net-work time is tfzw — 1.5005sec. The time difference between the proposed zero-net-work control method and EMTC is 2.63%. Figure 5.17 shows the state trajectories of link 1, Figure 5.18 shows the state trajectories of link 2. Figure 5.19 shows the control trajectories of link 1, Figure 5.20 shows the control trajectories of link 2. The work done by external and reaction forces/torques for zero-net-work method are shown in Figure 5.21. 5 . 4 . 4 S i m u l a t i o n C a s e 4 : In this case we will move the manipulator from initial state (0.2, 0, 0.3, 0) to final state (0, 0, 0, 0), parameter set 2 of the RP manipulator in Table 5.2 will be used for simulation. The simulation results are shown in Table 5.6. For link 1, 6Q is the initial position, 6SW is the switching position, t\sw is the switching time, t\f is the final time. For link 2, ro is the initial position, rsw is the switching position, t2sw is the switching time, t2f is the final time. Control Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 81 Case 4 00 rsw t\sw t-2sw *2/ error Zero-net-work 0.2 0.3 0.1050 0.1441 0.3997 0.4363 0.7915 0.8449 2.04 % EMTC 0.2 0.3 0.0250 0.4465 0.4350 0.8280 Table 5.6: Case 4: Simulation results of two-link revolute and prismatic manipulator input for link 1 has one switch: 9\Sw = 0.1050rad. Control input for link 2 has one switch: rsw = 0.1441m. From offline calculation we find the switching positions, we can then use these values in simulation block diagram shown in Figure 5.22 and use Simulink run the simulation. The exact minimum time is tf exact = 0.8280sec, zero-net-work time is tfzw = 0.8449sec. The time difference between the proposed zero-net-work control method and EMTC is 2.04%. Figure 5.23 shows the state trajectories of link 1, Figure 5.24 shows the state trajectories of link 2. Figure 5.25 shows the control trajectories of link 1, Figure 5.26 shows the control trajectories of link 2. The work done by external and reaction forces/torques for zero-net-work method are shown in Figure 5.27. Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 82 Operator 1 position error! H 3 Rotational J-witcNriQ nnarotnr? Point thetasw Control trajectories <3< H3 Point thetasv/l Relational Absolute OperatoM posibooerrora Slate trajectories State trajectories Mux! o(Pnk2 Figure 5.22: Case 4: Simulation block diagram 0.3 0.25 0.2 Case4: Joint Position Trajectories of Link 1 I Z6 - - Er ro-work \hjc X X. X \ \ \ . . . . \ . . . . x. \ \ X . X X . X >v X 0.05 -0.05 0.1 0.2 0.3 0.4 0.5 Time (second) 0.6 0.7 0.8 0.9 Figure 5.23: Case 4: State trajectories of link 1 Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator 83 Case4: Joint Position Trajectories of Link 2 0.3 0.25 r-0.2 h 0.05 h Oh — z - - E =ro-work MTC 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Time (second) Figure 5.24: Case 4: State trajectories of link 2 Case4: Control Trajectories of Link 1 3 2 -2 -3 Zero-work - - EMTC ; I ; | ; I ! 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Time (second) Figure 5.25: Case 4: Control trajectories of link 1 Chapter 5. Development of the Zero-Net-Work Controller for a Cylindrical Manipulator Case4: Control Trajectories of Link 2 3 2 -2 -3 1 — Zero-work FMTr 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Time (second) Figure 5.26: Case 4: Control trajectories of link 2 Case4: Work done by all the forces and torques of R&P two-link robot I" 0.25 - Link 1 - Link 2 1 /. 1 1 / * \ \ / / / \ \ \ / / / 1 \ \ \ 1 1 . 1 1 \ \ \ 1 1 1 \ \ \ 1 / 1 / /. . / \ \ . . . \ / / / / . \ \ \ \ \ ^ \ / / \ \ — 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Time (second) Figure 5.27: Case 4: Work done by forces/torques for each link (zero-net-work solution) Chapter 6 Development of the Zero-Net-Work Controller for a Revolute Manipulator 6.1 Dynamic Model Figure 6.1: Sketch of two-link revolute manipulator In this chapter we will investigate the control of a Two-Link Revolute Planar Manipulator (TLRPM) as shown in Figure 6.1. The T L R P M consists of two horizontal rigid links, and is controlled by torque inputs r± and r 2 . The first degree of freedom is the angular rotation 9\ of the inner link (shoulder), and the second degree of freedom is the angular rotation f?2 of the outer link (elbow). The remaining degrees of freedom used for positioning the end effector are 85 Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 86 neglected. The absolute value of the control torque for each link is bounded: \T\\ < r i m a i , \ T 2 \ < T2max-Let us give the notations as follow: For i = 1, 2, qi denotes the joint angle, which also serves as a generalized coordinate, m,i is the mass of link i , Zj denotes the length of link i , lci is the distance from the joint i to the center of mass of link i , 7j is the moment of inertia of link i about the center of mass of link i, and M is the mass of end effector with load. The base frame Fo, frame F\ and frame F 2 are chosen using D-H convention as shown in Figure 6.1. The D-H parameters are shown in Table 6.1,where * means variable. Link a.i CXi di Oi 1 h 0 0 2 h 0 0 Q*2 Table 6.1: D-H parameters of two-link revolute manipulator The homogeneous transformation from frame FQ to frame Fi is: Hi 01 cosqi —sinqi 0 l\cosq\ sinqi cosqi 0 l\sinq\ 0 0 1 0 0 0 0 1 The homogeneous transformation from F\ to F 2 is: cosq2 —sinqi 0 l2Cosq2 H 1 2 = smq2 cosq2 0 0 0 0 1 0 0 1 (6.1) (6.2) Chapter 6. Development of the Zero-Net-Work Controller for a R.evolute Manipulator 87 So the homogeneous transformation from FQ to F2 is: H02 = H01H12 co.s(qx + q 2 ) -svn{q\ + q2) 0 l x c o s q x + l2cos(qi + q2) sin(qx + q 2 ) cos{q} + q 2 ) 0 l x s i n q x + l 2 s i n ( q x + q2) 0 0 1 0 0 0 0 1 (6.3) Next we will use the Newton-Euler method shown in Chapter 3 to obtain the dynamic equations of the T L R P M . We begin with the forward recursion to express the various velocities and accelerations in term of q \ , q 2 and their derivatives. The angular velocity and acceleration of link 1 and link 2 are: W] = q\k, oci = q\k u2 = (q\ + i x r ] ] C i ) = q}k x Z r l i + qxk x (chk x / c l i ) -ic\2 from Equation 6.5 we have: aC,2 = (R?) 7 a e > 1 + u2 x r2,F:2 + u>2 x (u>2 x r 2 x 2 ) = ( R ? ) T a e J + (<7, + cy2)k x lc:A + (ry, + 2 (m,2 + M)(-hq2cosq2 + hq\sinq2) - (m,2lc2 + Ml2)(q\ + q2f (m2 + M)(l\q2sinq2 + liq\cosq2) + (m 2/ c2 + Ml2)(q\ + q2) 0 (6.13) (6.14) The torque exerted by link 1 on link 2 is: r2 = / 2 « 2 + UJ2 x (I2u2) - f2 x r2,c2 + f3 x r3i, c2 Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 90 = I2a2 + u>2 x {I2u:2) - f2 x lc2i + f3 x (lc2 - l2)'\ = I2a2 - m,2&c,2 x tai - Ma C i 2 x Z2i = h{q\ + q2)k +m,2lc2[liqfsinq2 + hq\cosq2 + lc2(q\ + q2)]k +Ml2[l±qfsinq2 + l\q\cosq2 + Z 2 ( < ? 1 + = q isw If qi < qisw ul= uiholding lfql = 0, q2>0 u2 = u2holding lfql>0, q2=0 ui = 0 If q l =0,q2 = 0 Tout = Tout + Tstep Integrate one time step Call D D A S S L q2sw = q2sw + qstep qisw = 0 qisw =qlsw + qstep Calculate Worki Decrease qstep Reset qisw Yes Yes T ^ Done ^ Figure 6.4: Flowchart of offline calculation Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 100 State Feedback Figure 6.5: Simulation block diagram of two-link revolute manipulator If there is model error or large disturbance, PD controller will be used to guarantee asymp-totic tracking. Because there is no gravitational term in the dynamic equations of the manip-ulator, we can prove that PD control can achieve zero steady state error. In case there are gravitational terms present in the dynamic equations, there will be a steady state error, PID controller will be used. To show PD control alone can achieve zero steady state error for the Two-Link Revolute Planar Manipulator (TLRPM). Consider the dynamic equations in matrix form: D(q)q + C(q, q)q = u (6.56) PD control scheme in vector form is: u = KpCi - iCjq (6.57) q — q — q is the difference between the desired joint position q and the actual joint position q. Kp, Kd are proportional and derivative gains, respectively. Consider Lyapunov Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 101 function candidate: V = ^ q T D ( q ) q + i q T X p q (6.58) The derivative of V is: V = qTD(q)q + ^qTD(q)q - qTKjA (6.59) From Equation (6.56), we have: D(q)q=u-C(q,q)q (6.60) Substitute above equation into Equation (6.59): V = q T (u - Kpq) + iqT(D(q) - 2C(q, q))q (6.61) Using the fact that D — 2C is skew symmetric and substitute the PD control law for u into above equation yields: V = - q T i Q q < 0 (6.62) LaSalle's Theorem: Suppose a Lyapunov function candidate V is found such that, along solution trajectories V < 0, system is asymptotically stable if the only solution of the system satisfying V = 0 is the null solution. From Equation (6.62), if V = 0, we have q = 0 and hence q = 0. From the equation of motion with PD control: D(q)q + C(q, q)q = Kpq - iY d q (6.63) We must then have iv"pq = 0, which implies that q = 0, q = 0. Lasalle's Theorem then implies that the system is asymptotically stable. 6.4 Computer Simulation The proposed zero-net-work method will now be simulated for two-link revolute manipulator, and its performance will be compared with the exact optimal solution. We will use the zero-net-work method to three different parameter sets of two-link revolute manipulator. For each Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 102 parameter set, we will perform simulations for different motion configurations. Simulink will be used to perform the simulation. The system's S-function can be found in Appendix A.9. The simulation results for different parameters and motion configurations are presented in the following: 6.4.1 Simulation Case 1: Case 1 QlO 920 Qisw Q2sw t'lsw t'2sw t2f error Zero-net-work 0.975 0 0.4880 0.5746 1.1440 5.59 % Exact 0.975 0 0.5417 0.0875 0.5872 1.0834 PD 0.975 0 3.8865 3.8865 258.73 % Table 6.2: Case 1: Simulation results of two-link revolute manipulator Physical parameters for the IBM 7535B 04 robot are used for simulation in this case: h = 2lcX = 0.4m l2 = 2lc2 = 0.25m mi = 29.58% m 2 = 15.00% M = 6.0kg (6.64) h = 0.416739%m2 I2 = 0.205625%m2 uimax = 25NM u2max = 9NM This example is a rest to rest motion of the manipulator from straight to straight configu-rations. The initial and the final conditions of the states in [rati] and [rad/s] are: x(0) = [0.975 0.0 0.0 0.0]T (6.65) x(t/) = [0 0.0 0.0 0.0] r (6.66) The FORTRAN program using zero-net-work method to find the switching positions for each link is shown in Appendix A.7 . The simulation results are shown in Table 6.2. For i = 1, 2, qio is the initial position of link i , Qisw is the switching position of link i, tiSW is the switching time of link i , and Uf is the final time of link i . In this case, the initial velocity and position of link Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 103 2 are zero. So there is no minimum to maximum switch for link 2. The control input for link 2 start from the beginning is the torque needed to hold link 2 at the final position while link 1 is moving. Control input for link 1 has one switch: q\sw = 0.4880ra\ = 27, Kp2 = 300, Kp>2 = 100. Kp\ and K p 2 are the proportional gain for link 1 and link 2, respectively. KDI and Kp>2 are the derivative gain for link 1 and link 2, respectively. From the simulation results, we can see the fastest response of PD control (tfPD = 3.8865sec) is still far slower than the response of the proposed zero-net-work method (tfzw — 1.1440sec). Figure 6.6 shows the state trajectories of link 1, Figure 6.7 shows the state trajectories of link 2. Figure 6.8 shows the control trajectories of link 1, Figure 6.9 shows the control trajectories of link 2. The work done by external and reaction forces/torques for each link are shown in Figure 6.10 and Figure 6.11. 6.4.2 Simulation Case 2: Physical parameters for the IBM 7535B 04 robot for case 1 is also used in this case. This example is a rest to rest motion of the manipulator from broken to straight configurations. The initial and the final conditions of the states in [rad] and [rad/s] are: x(0) = [0.76 0 0.2618 0.0]T x(tf) = [0.0 0.0 0.0 0.0]T (6.67) (6.68) Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 104 Casel: Joint Position Trajectories of Link 1 £• 0.6 h -0.2 Zero-work - - EMTC — PD 0 0.5 1 1.5 2 2.5 3 3.5 4 Time (second) Figure 6.6: Case 1: State trajectories of link 1 Casel: Joint Position Trajectories of Link 2 / / / \ \ ! i lero-work EMTC / / / / \ \ I \ I / 1 _ . . J " ' 1 1 1 1 1 1 1 1 \ \ \ \ 1 1 1 1 \ t I \ 1 1 1 \ \ \ ; ; 0.8 £ . 0.6 a £ 0.4 0.2 -0.2 0.5 1 1.5 2 2.5 Time (second) 3.5 Figure 6.7: Case 1: State trajectories of link 2 Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 105 Casel: Control Trajectories of Link 1 1 i 1 Zero-work \ - - i EMTC ! ; -/ / .< / / / / / / / 0.5 1.5 2 2.5 Time (second) 3.5 Figure 6.8: Case 1: Control trajectories of link 1 Casel: Control Trajectories of Link 2 10 z 5 §- 0 -10 I / U Zero-work EMTC PD 0.5 1.5 2 2.5 Time (second) 3.5 Figure 6.9: Case 1: Control trajectories of link 2 Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator Casel: Work done by all the forces and torques of TLRPM of Link 1 3 2.5 2 0.5 ... . 1 — Zero-work - EMTC ' ' " " ' A / \ I \ I / / / / s / 1 : \ / ' • \ / ' \ / 1 : \ / 1.,: \ I / 1 • \ / ' ' \ / ' • \ 1 : \ \ . : V . — I I I I / \ \ \ \ / 1 1 s 1 / \ \ \ N - -0 0.2 0.4 0.6 0.8 1 1.2 Time (second) Figure 6.10: Case 1: Work done by forces/torques for link 1 Casel: Work done by all the forces and torques of TLRPM of Link 2 7 6 5 0 1 K : ' "v ' . \ A . i 1 Zero-work - - EMTC ' / \^ ' / ' / : \ \ ' / v ' / ; V ' / : V ' / V ./../ \. 1 1 / \ t 1 / / / / / / / / / / / / \\ \ \ \ \ ; / / 1 y/ / 1 \ \ 1^ . 0 0.2 0.4 0.6 0.8 1 1.2 Time (second) Figure 6.11: Case 1: Work done by forces/torques for link 2 Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 107 Case 2 710 720 Qisw Q2sw tlsw t2sw t2f error Zero-net-work 0.76 0.2618 0.4139 0.0388 0.5302 0.2127 1.0552 0.2485 3.08 % Exact 0.76 0.2618 0.5119 0.4625 0.9508 1.0237 Table 6.3: Case 2: Simulation results of two-link revolute manipulator The simulation results are shown in Table 6.3. In this case, Control input for link 2 has 1 minimum to maximum switch: q2sw — 0.0388raii. Link 2 reaches the final position at t2f = 0.2485sec. The control input for link 2 switches from maximum value to the torque needed to hold link 2 at the final position while link 1 is moving for t > 0.2485sec. Control input for link 1 has one switch: qisw — 0.4139rari. The exact minimum time is tfexact = 1.0237sec, zero-net-work time is tfzw = 1.0552sec. The time difference between the proposed zero-net-work control method and EMTC is 3.08%. In this case the proposed zero-net-work solution is very close to the exact minimum time. Figure 6.12 shows the state trajectories of link 1, Figure 6.13 shows the state trajectories of link 2. Figure 6.14 shows the control trajectories of link 1, Figure 6.15 shows the control trajectories of link 2. The work done by external and reaction forces/torques for zero-net-work method are shown in Figure 6.16. 6.4.3 Simulation Case 3: Case 3 710 720 Qisw Q2sw tlsw t-2sw t2f error Zero-net-work 0.376 0 0.1880 0.6340 1.2649 2.58 % Exact 0.376 0 0.6165 0.0396 0.6645 1.2331 Table 6.4: Case 3: Simulation results of two-link revolute manipulator Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 108 Case2: Joint Position Trajectories of Link 1 0.8 '8 a. £ 0.4 0.2 0 I I I — Zer - - EM >-work rc N . \ N. \ N N N N • 0 0.2 0.4 0.6 0.8 1 Time (second) Figure 6.12: Case 2: State trajectories of link 1 Case2: Joint Position Trajectories of Link 2 0.2 0 £ -0.4 -0.8 -1 — Zero-work - - EMTC \ \ \ \ \ / / / / \ \ \ \ \ . . / * * / / / / \ \ \ / / / / . / \ \ \ \ / / / / / 0 0.2 0.4 0.6 0.8 1 Time (second) Figure 6.13: Case 2: State trajectories of link 2 Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 109 Case2: Control Trajectories of Link 1 1 Zero-work — i 1 - - EMTC • ! ! ; I I i ; ; ; I -0 0.2 0.4 0.6 0.8 1 Time (second) Figure 6.14: Case 2: Control trajectories of link 1 Case2: Control Trajectories of Link 2 10 -5 -10 Zero-work - - EMTC ; I i ; | I \\ 0 0.2 0.4 0.6 0.8 1 Time (second) Figure 6.15: Case 2: Control trajectories of link 2 Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 110 Case2: Work done by all the forces and torques of TLRPM 6 5 4 2 0 1 1 i\ Linki Link 2 i \ i \ i \ / \ / \ / / / \ \ \ \ \ 1 1 1 1 \ \ \ \ \ / / / / ' y \ \ V \ \ / / / / \ \ s 0.2 0.4 0.6 0.8 1 Time (second) Figure 6.16: Case 2: Work done by forces/torques for each link (zero-net-work solution) Following physical parameters are used for simulation in this case: lx = 2lcl = 0.4m l2 = 2lc2 = 0.25m mi = 0.245% m 2 = 0.15315% M = 0.5% (6.69) h = 3.2688e - 3%m 2 I2 = 0.7986e - 3%m 2 uimax = 0.25 NM u2max = 0.10NM This example is a rest to rest motion of the manipulator from straight to straight configu-rations. The initial and the final conditions of the states in [rad] and [ra,d/s] are: x(0) = [0.376 0.0 0.0 0.0]T (6.70) x(t/) = [0.0 0.0 0.0 0.0]T (6.71) The simulation results are shown in Table 6.4. The exact minimum time is tfexact = 1.2331sec, zero-net-work time is tfzw = 1.2649sec. The time difference between the proposed zero-net-work control method and EMTC is 2.58%. Figure 6.17 shows the state trajectories Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 111 Case3: Joint Position Trajectories of Link 1 o 1 l ! 1 1 Zero-work - - EMTC - \ \ \ \ \ \ \ ^ \ \ : \ \ : \ \ \ \ \ x \ : \ : \ \ \ \ \ \ : \ : \ --' u l 1 1 1 1 1 1 1 0 0.2 0.4 0.6 0.8 1 1.2 1.4 Time (second) Figure 6.17: Case 3: State trajectories of link 1 of link 1, Figure 6.18 shows the state trajectories of link 2. Figure 6.19 shows the control trajectories of link 1, Figure 6.20 shows the control trajectories of link 2. The work done by external and reaction forces/torques for zero-net-work method are shown in Figure 6.21. 6 . 4 . 4 S i m u l a t i o n C a s e 4 : Case 4 710 720 Qisw Q2sw ^ l S U ! 1-2sw f'2f error Zero-net-work 0.8 0.2 0.4471 0.0142 0.9572 0.2801 1.7026 0.2998 5.07 % Exact 0.8 0.2 0.1018 0.9120 0.4322 1.4980 1.6204 Table 6.5: Case 4: Simulation results of two-link revolute manipulator Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 112 Case3: Joint Position Trajectories of Link 2 ! \ Zero-work EMTC / / / / / \ \ \ \ / / / / V \ : \ : V 1 1 : \ • \ \ : \ 1 1 1 1 \ \ \ s... / / / _ . . / \ \ \ ; / i \ \ \ i 0.6 0.5 0.3 0.2 -0.1 0.2 0.4 0.6 0.8 Time (second) 1.4 Figure 6.18: Case 3: State trajectories of link 2 Case3: Control Trajectories of Link 1 0.25 0.2 0.15 0.1 0.05 o -0.05 -0.1 -0.15 -0.2 -0.25 Zero-work EMTC 0.2 0.4 0.6 0.8 Time (second) 1.2 1.4 Figure 6.19: Case 3: Control trajectories of link 1 Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 113 Case3: Control Trajectories of Link 2 0.1 0.05 I I Zero-work EMTC I 1 ; ] ; ; \ ; | ; i i 0 0.2 0.4 0.6 0.8 1 1.2 1.4 Time (second) Figure 6.20: Case 3: Control trajectories of link 2 Case3: Work done by all the forces and torques of TLRPM 8 7 6 5 3 2 0 1 / \ / * — Linki - - Link 2 1 1 1 , " \ \ \ \ 1 1 1 / \ : \ \ \ / / / / \ \ : \ ; \ / 1 1 ;\ : \ \ \ / / / / \ \ \ / \ v / / / / " " \ \ \ \ \ \ / ^y i \ X 0 0.2 0.4 0.6 0.8 1 1.2 1.4 Time (second) Figure 6.21: Case 3: Work done by forces/torques for each link (zero-net-work solution) Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 114 Following physical parameters are used for simulation in this case: lx = 2/ c l = 1.0m l2 = 2Zc2 = 0.625m mi = 0.61261% m 2 = 0.38288% M = 1.25% h = 51.0547e - 3%m 2 I2 = 12.4660e - 3%m 2 (6.72) = 3.9ATM ^2max = 1.5WM This example is a rest to rest motion of the manipulator from broken to straight configura-tions. The initial and the final conditions of the states in [rad] and [rad/s] are: The simulation results are shown in Table 6.5. The exact minimum time is tf exact — 1.6204sec, zero-net-work time is tfzw = 1.7026sec. The time difference between the proposed zero-net-work control method and EMTC is 5.07%. Figure 6.22 shows the state trajectories of link 1, Figure 6.23 shows the state trajectories of link 2. Figure 6.24 shows the control trajectories of link 1, Figure 6.25 shows the control trajectories of link 2. The work done by external and reaction forces/torques for zero-net-work method are shown in Figure 6.26. x(0) = [0.8 0.0 0.2 0.0] r x(t/) = [0.0 0.0 0.0 0.0]T (6.73) (6.74) Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator Case4: Joint Position Trajectories of Link 1 1.2 1 0.6 o o_ o ~* 0.4 0.2 0 -0.2 r ^ - - ^ ! 1 — , 1 1 i : — Zero-work : - - EMTC / / / / \ : Y : \ : \ / / / / \ \ \ \ \ : \ : \ • N ; \ \ N . : \ : : : \ . : \ : : \ . : \ 7>s_ . A _ \ . : \ 1 1 1 1 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (second) Figure 6.22: Case 4: State trajectories of link 1 Case4: Joint Position Trajectories of Link 2 -2 ! \ ~ ~ - V L \ \ 1 ! : Zero-work : - - EMTC " \ \ \ \ \ \ \ - 1 : / : ; ; V: V :\ : \ : \ \ / / . / : / I / \ \ \ \ \ / / / : / • / / i • • \ \ \ / • • i i 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (second) Figure 6.23: Case 4: State trajectories of link 2 Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 116 Case4: Control Trajectories of Link 1 -5 i ! i i i. 1 . : Zero-work : - - EMTC : : l : I ; ; ! : 1 : 1 : I ! ; ; : : I : 1 . ! ; ; : : l : 1 . ; \ \ ! \ ; ; ; ; ; • i i i 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (second) Figure 6.24: Case 4: Control trajectories of link 1 Case4: Control Trajectories of Link 2 -1 -2 : Ze — Eiv o-work TC ; ; ! | ; : ; : 1 :1 1 : i 1 1 ! 1 1 1 : i : 1 : l : J -.1 ; 1 : | 1 :1 1 :1 ' ' "1 _ ' i i i i 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time (second) Figure 6.25: Case 4: Control trajectories of link 2 Chapter 6. Development of the Zero-Net-Work Controller for a Revolute Manipulator 117 Case4: Work done by all the forces and torques of TLRPM 0.3 0.1 0 - Linki - Link 2 / . \ / 1 / / / / / / \ \ \ \ \ \ \ \ / / / / / \ \ \ \ \ / / / / \ \ \ \ N / \ S N 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 Time (second) Figure 6.26: Case 4: Work done by forces/torques for each link (zero-net-work solution) Chapter 7 Conclusions and Future Work 7.1 Conclusions In this thesis a near minimum time zero-net-work control algorithm has been developed to control the point to point motion of a robot manipulator. This proposed zero-net-work method has the basic structure of the exact minimum time control. The zero-net-work method assumes for each link the control input is bang-bang and switches from its minimum to maximum value only once, also the solution satisfies all the constraints (control input, boundary condition and work-energy constraints). Based on this idea, we have designed zero-net-work controllers for one-link manipulators, a cylindrical and a revolute manipulator. From the simulation results, we can see if the exact minimum time solution is bang-bang, and has one switch for each link, using the zero-net-work method we can find the exact minimum time. If the exact minimum time solution has more than one switch for each link, using the zero-net-work method we can find near minimum time solution. The main advantage of the zero-net-work method is that it is computationally efficient and does not require a good initial guess for the unknown initial boundary conditions. Because the zero-net-work method uses the maximum control input magnitude most of the time, it is much faster than a traditional PID controller. Also, the zero-net-work method does not need to solve the Two Point Boundary Value Problem (TPBVP), it can generate the control input much faster than the exact minimum time method. So we can say the proposed method provides a practical solution to control the motion of a robot manipulator near the minimum time. Currently, the zero-net-work method has two stages: offline calculation and online application. If we further improve the efficient of the algorithm, it can be used online. 118 Chapter 7. Conclusions and Future Work 119 7.2 Suggestion for Further Research The limitations of the zero-net-work method are: a) Like all model based control algorithm, the zero-net-work method requires a very good model of the plant. It can not deal with large model mismatch or large disturbances. Currently, a PID controller is used as a backup controller to guarantee the manipulator reaches its final position. A better plant model could be of benefit in future work. b) In this thesis we assume the control constraints for each link is — ^ m a 3 ; ^ iii ^ W j m a 3 ; . In practice, most actuators do not allow to switch from minimum to maximum value in very short period of time. More constraints on control input must be defined. We can set the actuator constraints as a saturation limit and a slew rate limit for example. Further research includes: a) Improving the efficiency of the zero-net-work method. Currently, the searching start point for each link is at its final position, and using a fixed step size. Computation time can be reduced by using variable searching step size. First we can use a large step size to reduce the searching range, and then use a smaller step size to find the exact switching position. b) Practical implementation of the algorithm should be investigated. c) A more practical model of the actuator should be used. A smooth switching period function should be defined and used to search for the solution. d) A robust controller for large model mismatch or disturbances need to be developed. Bibliography [1] Bobrow, J.E. 1982. "Optimal control of robotic manipulators", Ph.D. dissertation, University of California, Los Angeles. [2] Bobrow, J.E., Dubowsky,S. and Gibson,J.S. 1983 (San Franciso). "On the optimal control of robotic manipulators with actuator constraints", Proc. American Control Conference 2 :pp. 782-787. [3] Bobrow, J.E., Dubowsky, S.,and Gibson,J.S. 1985. "Time optimal control of robotic manipulators along specified paths", Int. J. Robot. Res. 4(3): pp. 3-17. [4] Bobrow,J.E. 1988. "Optimal robot path planning using the minimum-time criterion", IEEE J. Robotics and Automation, Vol. RA-4(4), pp. 443-450. [5] Broyson, A.E. , Jr., and Ho, Y-C 1975. Applied optimal control - optimization, estima-tion, and control, Hemisphere, Washing, D.C. . [6] Chen,C.C. 1989. "On the structure of the time-optimal controls for robotic manipula-tors", IEEE Transactions on Automation Control, Vol.34, No.l , January. [7] Chen,Y.B. 1988. "Minimum-time control of robotic manipulators", Ph.D. Dissertation, Rensselaer Polytechnic Institute, Troy, NY. [8] Chen,Y.B., and Desrochers, A.A. 1988. "Time-optimal control of Two-Degree of Free-dom Robot Arms", Proc. IEEE Conf. Robot, and Autom ., Philadelphia,PA: pp. 1210-1215. [9] Chen,Y.B. and Desrochers, A.A. 1989. "The Structure of Minimum-Time Control Law for Robotic Manipulators with Constrained ", Proc.IEEE Int. Conf. Robotics and Autom., Scottsdale, AZ, May, pp. 970-975. [10] Chen,Y.B. and Desrochers, A.A. 1989. "Minimum-time control laws for robotic ma-nipulators", Proc. of IEEE 28th Conference on Decision and Control , pp. 2494-2499. [11] Chen,Y.B. and Desrochers, A.A. 1990. "A proof of the structure of the minimum-time control law of robotic manipulators using a Hamiltonian formulation", IEEE Trans. Robotics and Automation, Vol. 6, pp. 388-393. [12] Chen,Y.B. and Huang, J. 1992. "An efficient computation of time-optimal control trajectory for robotic point-to-point motion", Proc. 4th Conf. on Intelligent Robot. Syst. for Space Exploration(Troy,NY), Sept. . [13] Chen,Y.B. and Desrochers, A.A. 1993. "Minimum-time control laws for robotic ma-nipulators" , International Journal of Control, Vol. 57, pp. 1-27. 120 Bibliography 121 [14] Chen,Y.B. , Huang, J. and Wen, J.T.Y. 1993. "A continuation method for time-optimal control synthesis for robotic point-to-point motion", Processing of the 32nd Conferance on Decision and Control, San Antonio,Texas. December: pp. 1628-1663. [15] Dahl, O., 1994. "Path-constrained robot control with limited torques - experimental evaluation" , IEEE Transactions on Robotics and Automation, Vol. 10, No. 5. October 1994, pp. 658-669. [16] Formal sky, A . M . and Osipov,S.N. 1990. "On the problem of time-optimal manipulator-arm turning", IEEE Trans. Automatic Control, Vol. 35, pp. 714-719. [17] Fotouhi-O, R., 1996. " Time-optimal control of two-link manipulators", PhD thesis, University of Saskatchewan, Saskatoon, Canada. [18] Fu, K.S., Gonzalez, R.C., Lee, C.S., Lee, 1987. Robotics: control, sensing, vision, intelligence, McGraw-Hill. [19] Geering , H.P., Guzzella,L., Hepner, S.A.R. and Onder, C H . 1986. "Time-optimal motions of robots in assembly tasks", IEEE Trans. Automatic Control AC-31(6): pp. 512-518. [20] Galicki, M . , 1998. "The planning of robotic optimal motions in the presence of ob-stacles" , The International Journal of Robotics Research, Vol 17, No 3, March 1998, PP248-259. [21] Hoi, C.W.J. , L.G. van Willigenburg, E.J. van Henten and G. van Straten 2001. "A new optimization algorithm for singular and non-singular digital time-optimal control of robots ", Proceedings of the 2001 IEEE International Conference on Robotics and Automation, Seoul, Korea. May 21-26 pp. 1136-1141. [22] Kahn,M.E. 1970. "The near-minimum time control of open-loop articulated kinematic chains", Ph.D. dissertation, Stanford University. [23] Kahn, M.E., and Roth,B. 1971. "The near-minimum-time control of open-loop articu-lated kinematic chains", ASME J. Dyn. Sys., Meas., Contr., Sept., pp. 164-172. [24] Kim, B.K. and Shin,K.G. 1985. "Suboptimal control of industrial manipulators with weighted minimum-time-fuel criterion", IEEE Trans. Aut. Cont. , Vol. AC-30, pp. 1-10. [25] Kirk, D.E. 1970. Optimal control theory, an introduction, Prentice-Hall, Englewood Cliffs,N.J. . [26] Lin, S.-Y. 1992. "A hardware implementable two-level parallel computing algorithm for general minimum-time control", IEEE Trans. Automatic Control, Vol. 37, pp. 589-603. [27] Luh, J.Y.S., and Walker,W.M. 1977(New Orleans). "Minimum-time along the path for a mechanical arm", Proc. 1977 IEEE Conf. Decision Contr., 2: pp.755-759. Bibliography 122 [28] Luh,J.Y.S., and Lin, C.S. 1981. "Optimal path planning for mechanical manipulators", ASME J. Dyn. Sys.,Meas., Contr., Vol.2, pp. 330-335, June 1981. [29] McCarthy, J.M. and J.E. Bobrow, 1992. "The number of saturated actuators and con-straint forces during time-optimal movement of a general robotic system", Proceeding of the 1992 IEEE International Conference on Robotics and Automation, Nice, France - May 1992, pp. 542-546. [30] Meier,E-B., and Bryson, A.E.Jr. 1990. "Efficient algorithm for time-optimal control of a two-link manipulator" , J.Guidance Control Dynam. 13(5): pp. 859-866. [31] Niv, M . , and Anslander, D.M. 1984. "Optimal control of a robot with obstacles", Proc. American Contr. Conf. (San Diego, CA), June. : pp. 280-287. [32] Pfeiffer,F. and Johanni, R., 1987. "A concept for manipulator trajectory planning", IEEE J. Robotics and Automation. [33] Rajan, V.T. . 1985. "Minimum time trajectory planning", Proc. of IEEE Conf. on Robotics and Automation, pp. 759-764. [34] Sahar, G. and Hollerbach, J.M., 1985. "Planning of minimum-time trajectories for robot arms", Proc. of IEEE Int. Conf. on Robotics and Automation, St.Louis, MO. March, pp. 751-758. [35] Sahar, G. and Hollerbach, J.M., 1986. "Planning of minimum-time trajectories for robot arms" Int. J. Robot. Res. Vol. 5, No.3 :pp. 90-100. [36] Sato, 0.,et.al, 1983. "Minimum-time control of a manipulator with two-degree of free-dom" Bui. of JSME , Vol. 26, No.218, August: pp. 1404-1410. [37] Sciavicco, L. and Siciliano, B. 1996. Modeling and control of robot manipulators, Mc-Graw Hill, New York. [38] Shiller, Z. and Dubowsky, S. 1985. "On the optimal control of robotic manipulators with actuator and end-effector constraints" , Proc. IEEE Int. Robotics and Autom. : pp. 614-620. [39] Shiller, Z., and Dubowsky,S. 1987. "Time optimal paths and acceleration lines of robotic manipulators", 26th IEEE Conf. on Decision and Control. Los Angeles, Dec. . [40] Shiller, Z., and Dubowsky,S. 1988. "Global time optimal motion of robotic manipula-tors in the presence of obstacles", Pros, of IEEE Conf. Robotics and Automation , pp. 370-375. [41] Shiller, Z., and Dubowsky,S. 1991. "On computing the global time optimal motions of robotic manipulators in the prescence of obstacles", IEEE Trans. Robotics and Automation, Vol.7, pp. 759-764. Bibliography 123 [42] Shiller, Z., Chang, H. and Wong, V., 1996. "The practical implementation of time-optimal control for robotic manipulators" , Robotics &; Computer-Integrate Manufac-turing, Vol. 12, No. 1, pp. 29-39. [43] Shin,K.G. and Mckay,N.D. 1983. "An efficient robot arm control under geometric path constraints", IEEE Conf. on Decision and Control , San Franciso,CA. [44] Shin.K.G. and Mckay,N.D. 1984(San Diegp). "Open-loop minimum-time control of mechanical manipulators and its application", Proc. American Contr. Conf. : pp. 296-303. [45] Shin,K.G., and Mckay,N.D. 1985. "Minimum-time control of robotic manipulators with geometric path constraints", IEEE Trans. Automatic Control. Vol. AC-30,No.6: pp. 531-541. [46] Singh,S. and Leu, M.C. , 1987. "Optimal trajectory generation for robotic manipulators using dynamic programming", ASME Trans. J. Dynamic Sys., Meas. ,and Control, Vol.109, June, pp. 88-86. [47] Sontag,E.D. and Sussmann 1986. "Time-optimal control of manipulators", Proc. IEEE Int. Conf. on Robotics and Automation, San Franciso,CA,April, pp. 1692-1697. [48] Slotine, J-J, E., and Yang, H.S., 1989. "Improving the efficiency of minimum-time path-following algorithms", IEEE Trans. Robotics and Automations, Vol.5, No.l. . [49] Spong, M.W. and Vidyasagar, M. , 1989. Robot dynamics and control, John Wiley &: Sons. [50] Wen, J. and Desrochers,A.A. 1986. "Sub-time-optimal control statrategies for robotic manipulators", Proc. IEEE Int. Conf. on Robotics and Automation. (San Franciso), April, pp. 402-406. [51] Weinreb, M.W. and Orin,D.E.. 1982. "Efficient dynamic computer simulation of robot mechanisms", ASME J. Dynam. Sys. Measurement Control 104: pp. 205-211. [52] Weinreb, A. and Bryson,A.E. . 1985. "Optimal control of systems with hard control bounds", IEEE Trans. Automatic Control Vol.30, pp. 1135-1138. [53] Weinreb, A. and Bryson,A.E. . 1985. "Minimum-time control of a two-link robot arm" IFAC Control Application of Nonlinear Programming and Optimization, Capri,Italy. [54] Yang,H.S. and Slotine, J-J.E. 1993. "Efficient algorithms for near-minimum-time con-trol of robot manipulators", M.I.T. Report NSL-931001, Nonlinear System Laboratory. [55] Yang, H.S., and Slotine,J-J.E. 1994. "Fast algorithms for near-minimum-time control of robot manipulators", Int. J. Robot. Res. 13(6): pp. 521-532. Appendix A Simulation Program Listings A . l S-function of One-link Horizontal Manipulator %V:I:I:I:I:I:I:I:I:I:I^^ % File name onel.m V, % Chapter 4 '/, One link horizontal manipulator '/. This is s-function for simulink function[sys,x0] =robot2(t,x,tau,flag,x01,x02) 1=10; if abs(flag)==l a=[x(2); tau/I] ; sys=a; elseif abs(flag)==3 sys=x; elseif abs(flag)==0 sys=[2,0,2,l,0,l]; x0=zeros(2,1); x0(l)=x01; x0(2)=x02; else sys=[] ; end A.2 S-function of One-link Vertical Manipulator '/. Chapter 4 % Onelink vertical manipulator '/, This is s-function for simulink function[sys,x0]=robot2(t,x,tau,flag,xOl,x02) if abs(flag)==l 1=0.3; m=5; I=l/3*m*l"2; % I = 0.1500 g=9.8; a=[x(2); (tau-m*g*l/2*cos(x(l)))/I]; sys=a; elseif abs(flag)==3 sys=x; elseif abs(flag)==0 sys=[2,0,2,l,0,l]; x0=zeros(2,l); x0(l)=x01; x0(2)=x02; else sys=[] ; end 124 Appendix A. Simulation Program Listings A.3 Exact Minimum Time Solution of One-link Vertical Manipulator cy:/:///:///:/://///://///:/:/:/:/:/.,/.,/:/.,/.y.'/.,/:/:/.'/.,/.y. C'/. File name: onelkMT.for */. cy//:///:/:///://///:/.y.y.y.y.y.y.y.y.,/.y.y.y.y.y.y.y.y.y.y. C'/. Chapter 4 C Main program onelkMT.for C C***BEGIN PROLOGUE onelkMT.for C***DATE WRITTEN 030501 (YYMMDD) C***REVISI0N DATE 030601 (YYMMDD) C***AUTH0R TA0 FAN C***PURP0SE SOLVING TPBVP PROBLEM IN CHAPTER 4. C***DESCRIPTI0N C This is FORTRAN program used to solve exact minimum time control C problem of one-link vertical robot manipulator C in chapter 4 of the thesis. C Subroutine "MUSN" is used to solve C the nonlinear TPBVP. C***REFERENCES (NONE) C***R0UTINES CALLED MUSN, FDIF, X0T C ROUTINES FDIF is used to define the differential equations of the system C ROUTINES X0T is used to evaluate the initial approximation C X0(t) of the solution. C Description of variables used in the program: C Taumax is maximum control inputs C Tau is control inputs C***END PROLOGUE onelkMT.for C IMPLICIT DOUBLE PRECISION (A-H.0-Z) DIMENSION ER(5),TI(12),X(5,12),Q(5,5,12),U(15,12),D(5,12), 1 PHIREC(15,12),W(360),UGR(20) INTEGER IU(60) EXTERNAL FDIF,X0T,G C C C N is the order of the system. C C NU is one of the dimension of U and PHI. C NU must be greater than or equal to N * (N+l) / 2. C C NTI is one of the dimension of TI, X, S, Q, U en PHI. C NTI must be greater than or equal to the total number of C necessary output points +1 (i.e. if the entry value for C NRTI > 1, NTI may be equal to the entry value of NRTI + 1) C C LW is the dimension of W. LW >= 7*N + 3*N*NTI + 4*N*N C C LIW is the dimension of IW. LIW >= 3*N + NTI C C ER(3) must contain the machine precision C C LWG is the dimension of WGR. LWG >= (total number of grid points) C / 5. The minimum number of grid points between 2 succesive output C points is 5, so the minimum value for LWG is the number of actually C used output points. Initially a crude estimate for LWG has to be made C C ER(1) must contain the required tolerance for solving the differential Appendix A. Simulation Program Listings C equation. C ER(2) must contain the initial tolerance with which a first aproximate C solution will be computed. This approximation is then used as an C initial approximation for the computation of an solution with an C tolerance ER(2)*ER(2) and so on until the required tolerance is C reached. As an initial tolerance max(ER(l),min(ER(2),1.d-2)) will C be used C G A,B the two boundary points C C NRTI is used to specify the output points. There are 3 ways to C specify the output points: C 1) NRTI = 0, the output points are determined automatically using AMP. C 2) NRTI = 1, the output points are supplied by the user in the array TI. C 3) NRTI > 1, the subroutine computes the (NRTI+1) output points TI(k) by C TI(k) = A + (k-1) * (B - A) / NRTI ; C so TI(1) = A and TI(NRTI+1) = B. C Depending on the allowed increment between two succesive output points, C more output points may be inserted in cases 2 and 3. C On exit NRTI contains the total number of output points. C AMP must contain the allowed increment between two output C points. AMP is used to determine output points and to assure that the C increment between two output points is at most AMP*AMP. A small value C for AMP may result in a large number of output points. C Unless 1 < AMP < .25 * sqrt(ER(l)/ER(3)) the default value C .25 * sqrt(ER(l)/ER(3)) is used. C ITLIM maximum number of allowed iteration C IERR0R=1, diagnostics will be printed during computation N = 5 NU = 15 NTI = 12 LU = 360 LIW = 60 ER(3) = 1.1D-15 LWG = 20 ER(1) = l.D-6 ER(2) = l.D-2 A = 0.D0 B = 1.D0 NRTI=10 AMP=0.01D0 ITLIM=20 IERR0R=1 OPEN(FILE='datalMT.m',UNIT=6,status='unknown') WRITE(6,30) 30 FORMAT(2X,'% This is the results of TPBVP') CALL MUSN(FDIF,X0T,G,N,A,B,ER,TI,NTI,NRTI,AMP,ITLIM,X,Q,U,NU,D, 1 PHIREC,KPART,U,LW,IW,LIW,WGR,LWG,IERR0R) WRITE(6,*) * MUSN: IERR0R =',IERR0R WRITE(6,200) A,B,ER(1),ER(2),ER(4),ER(5),KPART 200 FORMAT(' A = ',F8.4,3X,'B = *,F8.4,/,' REQUIRED TOLERANCE = ',1P, 1 D12.5.3X,'START TOLERANCE = '.D12.5,/, 2 ' CONDITION NUMBER = \D12.5,3X, 3 'AMPLIFICATION FACTOR = '.D12.5,/,' K-PARTITIONING =',I2,/) IF (IERR0R.NE.0) GOTO 3000 WRITE(6,215) 215 FORMAT( ' I ' ,4X,'T',9X,'XI' , 12X,'X2 ' , 12X,'X3' , 12X,'X4', 12X,'X5' , 1 /) DO 2200 K = 1 , NRTI Appendix A. Simulation Program Listings WRITE(6,220) K.TKK) , (X(J,K) , J=1,N) 2200 CONTINUE 220 FORMAT(' ',12,IX,F6.4,IP,5(2X,D12.5)) 3000 CONTINUE STOP END SUBROUTINE FDIF(T,Y,F) c C IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION Y(5),F(5) DOUBLE PRECISION zm,zl,zI,zg,Taumax,Tau * X1,X2,P1,P2,Z X1=Y(1) X2=Y(2) P1=Y(3) P2=Y(4) Z=Y(5) C Physical parameters of the manipulator zm=5.0D0 zl=0.3D0 zI=0.15D0 zg=9.8D0 Taumax=8.ODO C Decided the control input IF(P2.LT.0.0D0) THEN Tau=Taumax ELSE Tau=-Taumax END IF F(l) = Z*X2 F(2) = Z*(Tau-zm*zg*zl*DC0S(Xl)/2.0D0)/zI F(3) = -Z*zm*zg*zl*P2*DSIN(Xl)/(2.0D0*zI) F(4) = -Z*P1 F(5) = O.ODO RETURN END OF FDIF END SUBROUTINE XOT(T.X) c C IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION X(5) C C Casel X(l) = l.DO X(2) = O.ODO X(3) = 0.2446D0 X(4) = 0.0125D0 X(5) = 0.6268D0 C Case2 C X(l) = 1.57D0 C X(2) = O.ODO Appendix A. Simulation Program Listings C X(3) = 0.2051D0 C X(4) = 0.O187D0 C X(5) = 0.7525D0 RETURN C END OF XOT END SUBROUTINE G (N, XA, XB, FG, DGA, DGB) c C IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION XA(N),XB(N),FG(N),DGA(N,N),DGB(N,N) C DO 1100 I = 1 , N DO 1100 J = 1 , N DGA(I.J) = O.DO DGB(I.J) = O.DO 1100 CONTINUE DGA(1,1) = l.DO DGA(2,2) = l.DO DGB(1,1) = l.DO DGB(2,2) = l.DO C Case 1 FG(1) = XA(1) - l.DO FG(2) = XA(2) FG(3) = XB(1) FG(4) = XB(2) C Case 2 C FG(1) = XA(1) - 1.57D0 RETURN C END OF G END A.4 Zero-net-work Solution of Two-link Revolute and Prismatic Manipulator cy//////:/.y////////://///////////.7.,/.y.,/.,/.y.y.'/.y.y.,/.y.y. C'/. File name: RP21ink.for '/. cy.y////////////.yx/x/x/x/.y//:/x///x///.y.y.y.y. C/. Chapter 5 C Main program RP21ink.for C C***BEGIN PROLOGUE RP21ink C***DATE WRITTEN 030501 (YYMMDD) C***REVISION DATE 030601 (YYMMDD) C***AUTHOR TAO FAN C***PURPOSE OFFLINE CALCULATION OF ZERO WORK METHOD IN CHAPTER 5. C C***DESCRIPTION C This is FORTRAN program used to solve near-minimum time control C problem of two-link planar R&P arm in chapter 5 of the thesis. C The first link is revolute, the second is prismatic. C The algorithm is based on the zero work C method presented in the thesis. C Subroutine "ddassl" is used to solve C the ordinary differential equations with initial conditions. Appendix A. Simulation Program Listings 129 C***ROUTTNES CALLED DDASSL,DRES C ROUTINES DRES is used to define the differential equations of the system C C Description of variables used in the program: C rl, 11, ml, m2, J are physical parameters of the manipulator C X0(4) initial states, Xf(4) final states C XPRIME0C4) initial values for XPRIME(4) C Xsw0(2) initial guess for switch positions C Xsw(2) switch positions, Tsw(2) switch times C XswOstep initial position step size C Xswstep position step size C Tstep time step C Nsw(2) switch indicator, it's used to indicate if the link have C switched, Nsw=-1 before switch, Nsw=+1 after switch C Nfinal(2) final position indicator, initial value=-l, C if link reach it's final position, C Nfinal=+1, control input changes to holding force/torque. C Tau0(2) initial values for control inputs C Taumax(2) maximum control inputs C Tau(2) control inputs C Work(2) work done by all forces/torques for each link C RCforce reaction force between two link C Xlast(4) store previous state values used to calculate work C Tfinal(2) final times for each link C Tstop stop time for simulation C Xerror absolute position error, C Werror absolute work error C Xdeta, Ydeta displancement in X,Y direction C***END PROLOGUE RP21ink IMPLICIT DOUBLE PRECISION(A-H,0-Z) COMMON Tau(2), rl, 11, ml, m2, J EXTERNAL DRES DIMENSION X(4), XPRIME(4), DELTA(4),INF0(15),RW0RK(300),IW0RK(60) DOUBLE PRECISION rl, 11, ml, m2, J DOUBLE PRECISION X0(4), Xf(4),XPRIME0(4), Xsw0(2),Xsw(2),Tsw(2), * Xswstep,XswOstep,Tstep,Nsw(2),Nfinal(2), * Taumax(2),Tau0(2), Uork(2), RCforce, Xlast(4), Tfinal(2), * Xerror,Werror, Xdeta, Ydeta, Tstop PARAMETER (PI=3.14159D0) C LRW — Set it to the declared length of the RWORK array. C You must have LRW .GE. 40+CMAX0RD+4)*NEQ+NEQ*2 LRW=300 C LIW — Set it to the declared length of the IWORK array. C You must have LIW .GE. 20+NEQ LIW=60 C Number of differential equations NEQ=4 C Physical parameters for simulation case 1,2 C rl=0.6D0 C length of the first link 11=0.9D0 C mass of the first link ml=3.D0 Appendix A. Simulation Program Listings 130 C mass moment of inertia of the linkl J=0.81D0 C mass of the second link m2=l.D0 C Maximum torque of linkl Taumax(l)=2.D0 C Maximum force of link2 Taumax(2)=2.D0 C Physical parameters for simulation case 3,4 C C rl=0.4D0 C length of the first link C 11=1.2D0 C mass of the first link C ml=4.D0 C mass moment of inertia of the linkl C J=1.92D0 C mass of the second link C m2=1.5D0 C Maximum torque of linkl C Taumax(l)=3.D0 C Maximum force of link2 C Taumax(2)=2.5D0 C Initial position for case 1,3 X0(1)=PI/4.D0 X0(2)=0.D0 X0(3)=0.D0 X0(4)=0.D0 C Initial position for case 2,4 C X0(1)=0.2D0 C X0(2)=0.D0 C X0(3)=0.3D0 C X0(4)=0.D0 C Final position Xf(1)=0.D0 Xf(2)=0.D0 Xf(3)=0.D0 Xf(4)=0.D0 C Xerror=1.0D-4 Uerror=3.0D-4 Tstop=1.5D0 C Initial torque IF (X0(1).LT.Xerror) THEN Tau0(l)=0.D0 E1SE TauO(1)=-Taumax(1) END IF IF (XO(3).LT.Xerror) THEN Tau0(2)=-m2*rl*X0(2)**2 E1SE TauO(2)=-Taumax(2) END IF C Initial velocity and accelaration Appendix A. Simulation Program Listings 131 XPRIMEO(1)=X0(2) XPRIMEO(2)=(TauO(1)-2.DO*m2*(XO(3)+r1)* & X0(2)*X0(4))/(J+m2*(X0(3)+rl)**2) . XPRIMEO(3)=X0(4) XPRIMEO(4)=(TauO(2)+m2*(X0(3)+rl)*X0(2)**2)/m2 C Initial step size XswOstep=0.001DO Xswstep=XswOstep Tstep=0.01DO C Initial vaules for switch positions XswO(l)=Xf(1) XswO(2)=Xf(3) C Start of a new integration problem C Initialized the switch position 3 Xsw(l)=XswO(l) Xsw(2)=XswO(2) 5 CONTINUE C Reset the switch indicator Nsw(l)=-1.0D0 Nsw(2)=-1.0D0 Nfinal(l)=-1.0D0 Nfinal(2)=-1.0D0 Tfinal(l)=0.0D0 Tfinal(2)=0.0D0 Tsw(l)=0.0D0 Tsw(2)=0.0D0 C Dimension of INFO(N) DO 7 1=1,15 INF0(I)=0 7 CONTINUE C Do you want the solution only at C TOUT (and not at the next intermediate step) . C Yes - Set INFO(3) = 0 C No - Set INF0(3) = 1 **** C Do you want the code to decide C on its own maximum stepsize? C Yes - Set INFO(7)=0 C No - Set INF0(7)=1 C and define HMAX by setting C RW0RK(2)=HMAX **** C Do you want the code to define C its own initial stepsize? C Yes - Set INFO(8)=0 C No - Set INF0(8)=1 C and define HO by setting C RW0RK(3)=H0 **** INF0(3)=1 INF0(7)=1 INF0(8)=1 C maximum stepsize RW0RK(2)=0.0001D0 C initial stepsize RWORK(3)=0.001DO C Relative tolerance RT0L=1.0D-11 Appendix A. Simulation Program Listings C Absolute tolerance AT0L=1.0D-8 C Initialize the parameters T=0.D0 DO 10 1=1,NEQ X(I)=X0(I) 10 XPRIME(I)=XPRIMEO(I) C Initial value of workdone by external force C of each link DO 20 1=1,2 20 Work(I)=0.D0 C Open file "datal.m' to store data of Linkl OPEN(FILE='datal.m',UNIT=6,status='unknown') URITE(6,30) 30 FORMAT(2x, ''/.'/, This is the simulation result of Linkl') WRITE(6,40) 40 FORMAT(lOx, ''/, Time' ,9x, 'Position' ,5x, 'Velocity' ,4x, 'Force', * 9x,'Uork') WRITE(6,50) 50 FORMAT(2x,'output1=[') C Open file "data2.m' to store data of Link2 OPEN(FILE='data2.m',UNIT=7,status='unknown') WRITE(7,60) 60 FORMAT(2x,'*/.'/. This is the simulation result of Link2') WRITE(7,40) WRITE(7,80) 80 F0RMAT(2x,'output2=[') C Decide the control input Tau 90 CONTINUE DO 100 1=1,2 IF((X(2*I-1) .GE. Xsw(I))) THEN Tau(I)=-Taumax(I) Tsw(I)=T IF (X0(2*I-1).LT.Xerror) THEN Tsw(I)=0.0D0 END IF ELSE Tau(I)=+Taumax(I) Nsw(I)=1.0D0 C Nsw indicates the link has switched END IF 100 CONTINUE C Linkl reaches final position IF (((X(l)-Xf(l)).LT.Xerror).OR.(Nfinal(l).GT.O.DO)) THEN IF (Nfinal(l).LT.O.DO) THEN Tfinal(l)=T END IF Tau(l)=0.D0 Nfinal(l)=+1.0D0 END IF C Link2 reaches final position IF (((X(3)-Xf(3)).LT.Xerror) .OR.(Nfinal(2).GT.O.DO)) THEN Appendix A. Simulation Program Listings IF (Nfinal(2).LT.O.DO) THEN Tfinal(2)=T END IF Tau(2)=-m2*rl*X(2)**2 Nfinal(2)=+1.0D0 END IF C Both links reach final position IF (((X(l)-Xf(1)).LT.Xerror).AND.((X(3)-Xf(3)).LT.Xerror)) THEN Tau(l)=O.DO Tau(2)=0.D0 END IF C Output the data WRITE(6.,110) T, (X(I) ,1=1,2) ,Tau(l) ,Work(l) 110 F0RMAT(7x,6(F10.6,3x)) WRITE(7,111) T,(X(I),1=3,4),Tau(2),Work(2) 111 F0RMAT(7x,6(F10.6,3x)) C Reaction force between linki and link2 RCforce=m2*((X(3)+rl)*XPRIME(2)+2*X(2)*X(4)) DO 120 1=1,4 120 Xlast(I)=X(I) C Integrate one time step TOUT=T+Tstep CALL DDASSL (DRES, NEQ, T, X, XPRIME, TOUT, INFO, RTOL, ATOL, * IDID, RWORK, LRW, IWORK, LIW, RPAR, IPAR.JAC) C Calculate work for each link C Workd of linki Work(l)=Work(l)+(X(l)-Xlast(l))*Tau(l) & -(X(l)-Xlast(1))*RCforce*(rl+Xlast(3)) C C Displacement in x direction Xdeta=(X(3)+rl)*DC0S(X(l))-(Xlast(3)+rl)*DC0S(Xlast(1)) C Displacement in y direction Ydeta=(X(3)+rl)*DSIN(X(l))-(Xlast(3)+rl)*DSIN(Xlast(l)) C C Work of link2 Work(2)=Work(2)+Tau(2)*(DSIN(Xlast(1))*Ydeta+DC0S(Xlast(1))*Xdeta) & - RCforce*DSIN(Xlast(l))*Xdeta+RCforce*DCOS(Xlast(l))*Ydeta IF (((ABS(Tau(l))).LT.1.0D-8).AND.((ABS(Tau(2))).LT.1.0D-8) * .OR.(T.GT.Tstop)) THEN C Final point, check for works IF ((((ABS(X(1))).LT.Xerror).AND.((ABS(X(3))).LT.Xerror)).AND. * (((ABS(Work(l))).LT.Werror).AND.((ABS(Work(2))).LT.Werror))) * THEN GOTO 126 END IF IF (Xsw(l) .LT.XO(D) THEN C Increase switch position for link 1 and restart integration Xsw(l)=Xsw(l)+Xswstep PRINT*,'Updating Xswl=',Xsw(l) CLOSE(6,STATUS='DELETE') CLOSE(7, STATUS=' DELETE' ) GOTO 5 END IF Appendix A. Simulation Program Listings IF (Xsw(2).LT.X0(3)) THEN C Increase switch position for link 2 and restart integration Xsw(l)=XswO(l) Xsw(2)=Xsw(2)+Xswstep PRINT*,'Updating Xsw2=',Xsw(2) CLOSE ( 6, STATUS=' DELETE' ) CLOSE(7,STATUS='DELETE') GOTO 5 ELSE C Reduce step size by half Xswstep=Xswstep/2.0D0 PRINT*,'Updating Xswstep=',Xswstep CLOSE(6,STATUS='DELETE') CLOSE(7,STATUS='DELETE') GOTO 3 END IF ELSE C Continue integration GOTO 90 END IF 126 WRITE(6,110) T,(X(I),1=1,2),Tau(l),Work(l) WRITE(7,111) T,(X(I),1=3,4),Tau(2),Work(2) WRITE(6,130) WRITE(7,130) 130 FORMAT(2x,']; ') WRITE(6,140)XswO(l) 140 FORMAT(3x, '"/, The initial guess of XswO(l) = ',F10.6) WRITE(6,150)Xsw(l) 150 FORMAT(3x, ''/, The switch position of linkl Xsw(l) = ',F10.6) WRITE(6,170)Tsw(l) 170 FORMAT(3x, "/. The switch time of linkl Tsw(l) = ' ,F10.6) WRITE(6,190)Tfinal(l) 190 FORMAT(3x,'"/. The final time of linkl Tfinal(l) = ',F10.6) WRITE(6,240)XswO(2) 240 FORMAT(3x,'/. The initial guess of Xsw0(2) = ',F10.6) WRITE(6,250)Xsw(2) 250 FORMAT(3x,'/. The switch position of link2 Xsw(2) = ',F10.6) WRITE(6,270)Tsw(2) 270 FORMAT(3x,'/. The switch time of link2 Tsw(2) = ',F10.6) WRITE(6,290)Tfinal(2) 290 F0RMAT(3x, 7. The final time of link2 Tfinal(2) = *,F10.6) WRITE(6,300) XswOstep.Xswstep 300 FORMAT(lx, '*/, XswOstep=',F9.5,3x,'Xswstep=',F10.6) END SUBROUTINE DRES(T,X,XPRIME,DELTA,IRES,RPAR,IPAR) IMPLICIT DOUBLE PRECISION(A-H.O-Z) DIMENSION X(4), XPRIME(4), DELTA(4) DOUBLE PRECISION rl, 11, ml, m2, J COMMON Tau(2), rl, 11, ml, m2, J DELTA(1)=XPRIME(1)-X(2) Appendix A. Simulation Program Listings 135 DELTA(3)=XPRIME(3)-X(4) DELTA(2)=(J+m2*(X(3)+rl)**2.0D0)*XPRIME(2)+ & 2.0D0*m2*(X(3)+rl)*X(2)*X(4)-Tau(l) DELTA<4)=m2*XPRIME(4)-m2*(X(3)+rl)*X(2)**2.D0-Tau(2) RETURN END A.5 Exact Minimum Time Solution of Two-link Revolute and Prismatic Manip-ulator cy//.y////////.v//.r///////:///.•/.'/.•/.•/.•/.•/.•/.•/.•/.•/.•/.•/.'/.'/. C/. File name: RP21inkMT.for "/. cy//j////.y.y////.y////.y//////.%y.y.y.y.y.y.y.y.y.,/.y.y.y. C'/. Chapter 5 C Main program RP21inkMT.for C C***BEGIN PROLOGUE RP21inkMT.for C***DATE WRITTEN 030501 (YYMMDD) C***REVISI0N DATE 030601 (YYMMDD) C***AUTH0R TA0 FAN C***PURP0SE SOLVING TPBVP PROBLEM IN CHAPTER 5. C***DESCRIPTION C This is FORTRAN program used to solve exact minimum time control C problem of two-link planar R&P arm in chapter 5 of the thesis. C The first link is revolute, the second is prismatic. C Subroutine "MUSN" is used to solve C the nonlinear TPBVP. C***REFERENCES (NONE) C***R0UTINES CALLED MUSN, FDIF, X0T C ROUTINES FDIF is used to define the differential equations of the system C ROUTINES XOT is used to evaluate the initial approximation C X0(t) of the solution. C Description of variables used in the program: C Taumax is maximum control inputs C Tau is control inputs C***END PROLOGUE RP21inkMT.for C IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION ER(5),TI(12),X(5,12),Q(5,5,12),U(15,12),D(5,12), 1 PHIREC(15,12),W(900),WGR(20) INTEGER IW(60) EXTERNAL FDIF,XOT,G C C C N is the order of the system. C C NU is one of the dimension of U and PHI. C NU must be greater than or equal to N * (N+l) / 2. C C NTI is one of the dimension of TI, X, S, Q, U en PHI. C NTI must be greater than or equal to the total number of C necessary output points +1 (i.e. if the entry value for C NRTI > 1, NTI may be equal to the entry value of NRTI + 1) C C LW is the dimension of W. LW >= 7*N + 3*N*NTI + 4*N*N C C LIW is the dimension of IW. LIW >= 3*N + NTI Appendix A. Simulation Program Listings 136 c C ER(3) must contain the machine precision C C LWG is the dimension of WGR. LWG >= (total number of grid points) C / 5. The minimum number of grid points between 2 succesive output C points is 5, so the minimum value for LWG is the number of actually C used output points. Initially a crude estimate for LWG has to be made C C ER(1) must contain the required tolerance for solving the differential C equation. C ER(2) must contain the initial tolerance with which a first aproximate C solution will be computed. This approximation is then used as an C initial approximation for the computation of an solution with an C tolerance ER(2)*ER(2) and so on until the required tolerance is C reached. As an initial tolerance max(ER(l),min(ER(2),1.d-2)) will C be used C C A,B the two boundary points C C NRTI is used to specify the output points. There are 3 ways to C specify the output points: C 1) NRTI = 0, the output points are determined automatically using AMP. C 2) NRTI = 1, the output points are supplied by the user in the array TI. C 3) NRTI > 1, the subroutine computes the (NRTI+1) output points TI(k) by C TI (k) = A + (k-1) * (B - A) / NRTI ;. C so TI(1) = A and TI(NRTI+1) = B. C Depending on the allowed increment between two succesive output points, C more output points may be inserted in cases 2 and 3. C On exit NRTI contains the total number of output points. C AMP must contain the allowed increment between two output C points. AMP is used to determine output points and to assure that the C increment between two output points is at most AMP*AMP. A small value C for AMP may result in a large number of output points. C Unless 1 < AMP < .25 * sqrt(ER(l)/ER(3)) the default value C .25 * sqrt(ER(l)/ER(3)) is used. C ITLIM maximum number of allowed iteration C IERR0R=1, diagnostics will be printed during computation N = 9 NU = 45 NTI = 12 LW = 900 LIW = 60 ER(3) = 1.1D-15 LWG = 20 ER(1) = l.D-6 ER(2) = l.D-2 A = 0.D0 B = 1.D0 NRTI=10 AMP=0.01D0 ITLIM=20 IERR0R=1 OPEN(FILE='datalMT.m',UNIT=6,status='unknown') WRITE(6,30) 30 FORMAT(2X, ''/, This is the results of TPBVP') CALL MUSN(FDIF,X0T,G,N,A,B,ER,TI,NTI,NRTI,AMP,ITLIM,X,Q,U,NU,D, 1 PHIREC,KPART,W,LW,IW,LIW,WGR,LWG,IERR0R) WRITE(6,*) ' MUSN: IERR0R =',IERR0R WRITE(6,200) A,B,ER(1),ER(2),ER(4),ER(5),KPART Appendix A. Simulation Program Listings 200 FORMAT (' A = ',F8.4,3X,'B = * ,F8.4,/,' REQUIRED TOLERANCE = ',1P, 1 D12.5,3X,'START TOLERANCE = >,D12.5,/, 2 ' CONDITION NUMBER = ',D12.5,3X, 3 'AMPLIFICATION FACTOR = ',D12.5,/,' K-PARTITIONING =',I2,/) IF (IERROR.NE.O) GOTO 3000 WRITE(6,215) 215 FORMAT(' I ',4X,'T',9X,'XI',12X,'X2',12X,'X3',12X,'X4',12X,'X5', 1 /) DO 2200 K = 1 , NRTI WRITE(6,220) K,TI(K),(X(J,K),J=l,N) 2200 CONTINUE 220 FORMAT(' ',12,IX,F6.4,IP,5(2X,D12.5)) 3000 CONTINUE STOP END SUBROUTINE FDIF(T,Y,F) IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION Y(9),F(9) DOUBLE PRECISION rl,11,ml,m2,I,Taumax(2),Tau(2), * X1,X2,X3,X4,P1,P2,P3,P4,Z,V1,V2,V12,V13,V14,V22,V23 X1=Y(1) X2=Y(2) X3=Y(3) X4=Y(4) P1=Y(5) P2=Y(6) P3=Y(7) P4=Y(8) Z=Y(9) C Physical parameters for simulation case 1,2 C C C C C C C rl=0.6D0 length of the first link 11=0.9D0 mass of the first link ml=3.D0 mass moment of inertia of the linkl I=0.81D0 mass of the second link m2=l.D0 C C Maximum torque of linkl Taumax(l)=2.DO Maximum force of link2 Taumax(2)=2.D0 C C C C C C C C C C Physical parameters for simulation case 3,4 rl=0.4D0 length of the first link 11=1.2D0 mass of the first link ml=4.D0 mass moment of inertia of the linkl I=1.92D0 mass of the second link Appendix A. Simulation Program Listings C m2=1.5D0 C Maximum torque of linkl C Taumax(l)=3.D0 C Maximum force of link2 C Taumax(2)=2.5D0 C Decided the control input IF(P2.LT.0.0D0) THEN Tau(1)=Taumax(1) ELSE Tau(1)=-Taumax(1) END IF IF(P4.LT.0.0D0) THEN Tau(2)=Taumax(2) ELSE Tau(2)=-Taumax(2) END IF U = Tau(l)-2.0D0*m2*(X3+rl)*X2*X4 V = I+m2*(X3+rl)**2 VI = U/V V12 = (-2.0D0*m2*(X3+rl)*X4)/V V13 = (-2.0DO*m2*X2*X4*V-2.0DO*U*m2*(X3+rl))/V**2.0DO V14 = -2.0D0*m2*X2*(X3+rl)/V V2 = (Tau(2)+m2*(X3+rl)*X2**2.0D0)/m2 V22 = 2.0D0*X2*(X3+rl) V23 = X2**2 F(l) = Z*X2 F(2) = Z*V1 F(3) = Z*X4 F(4) = Z*V2 F(5) = 0.0D0 F(6) = -Z*(P1+P2*V12+P4*V22) F(7) = -Z*(P2*V13+P4*V23) F(8) = -Z*(P3+P2*V14) F(9) = 0.0D0 RETURN C END OF FDIF END SUBROUTINE XOT(T.X) c C IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION X(5) C Case 1,3 X(l) = 3 14D0/4.0D0 X(2) = 0 0D0 X(3) = 0 ODO X(4) = 0 ODO X(5) = 1 ODO X(6) = 1 ODO X(7) = 1 ODO X(8) = 1 ODO X(9) = 1 ODO Appendix A. Simulation Program Listings Case 2 4 X(l) = 0 2D0 X(2) = 0 ODO X(3) = 0 3D0 X(4) = 0 ODO X(5) 1 ODO X(6) = 1 ODO X(7) = 1 ODO X(8) = 1 ODO X(9) = 1 ODO RETURN C END OF XOT END SUBROUTINE G(N,XA,XB,FG,DGA,DGB) c C IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION XA(N),XB(N),FG(N),DGA(N,N),DGB(N,N) C DO 1100 I = 1 , N DO 1100 J = 1 , N DGA(I,J) = 0. DGB(I.J) = 0. CONTINUE DGA(1,1) = 1 DO DGA(2,2) = 1 DO DGA(3,3) = 1 DO DGA(4,4) = 1 DO DGB(1,1) = 1 DO DGB(2,2) = 1 DO DGB(3,3) = 1 DO DGB(4,4) = 1 DO C Case 1,3 FG(1) = XA(1) - 3.14D0/4.0D0 FG(2) = XA(2) FG(3) = XA(3) FG(4) = XA(4) FG(5) = XB(1) FG(6) = XB(2) FG(7) = XB(3) FG(8) = XB(4) C Case 2,4 c FGCl) = XA(1) - 0.2D0 c FG(2) = XA(2) c FG(3) = XA(3)- 0.3D0 c FG(4) = XA(4) c FG(5) = XB(1) c FG(6) = XB(2) c FG(7) = XB(3) c FG(8) = XB(4) c RETURN END OF G END Appendix A. Simulation Program Listings A.6 S-function of Two-link Revolute and Prismatic Manipulator y:///:/:///;/:/:/:/;/:/.y.7.'/.,/.'/.y.'/.7.,/.,/.'/.'/.,/.'/:/:/.v.y. '/• File name: RPsimfunl.m '/, y://///:/:/:/:///.,.y.y.y.y.y.y.y.y.y.y.y.y.y.y.,/.y.y.y.y.y. '/, Chapter 5 '/, This is the s-function of two link planar revolute and % prismatic manipulator. '/, The first link is revolute, the second is prismatic function [sys,x0,str,ts] = robot2(t,x,u,flag.xOl,x02,x03,x04) switch flag case 0 sys = [4, 0, 4, 2, 0, 0, 1] x0(l)=x01 x0(2)=x02 x0(3)=x03 x0(4)=x04 str = [] ; % number of continuous states % number of discrete states '/, number of outputs % number of inputs y, reserved must be zero 7, direct feedthrough flag '/, number of sample times '/, Initialization ts [0 0]; '/, sample time: [period, offset] case 1 '/, Derivatives % Physical parameters for simulation case 1,2 rl=0.6; 11=0.9; */. length of the first link ml=3; % mass of the first link m2=l; % mass of the second link I=l/3*ml*ll'2; % mass moment of inertia of the linki % Physical parameters for simulation case 3,4 •/.rl=0.4; '/.11=1.2; */. length of the first link %ml=4; % mass of the first link ym2=1.5; % mass of the second link y,I=l/3*ml*ll~2; '/, mass moment of inertia of the linki a=[x(2); (u(l)-2.*m2.*(x(3)+rl).*x(2).*x(4))/(I+m2.*(x(3)+rl)."2); x(4); (u(2)+m2.*(x(3)+rl).*x(2).~2)/m2 ;]; sys=a; case 2 sys = [] ; '/, do nothing Discrete state update case 3 sys = x; case 9 sys = [] ; '/. do nothing '/, Terminate otherwise error(['unhandled flag = ',num2str(flag)]); end Appendix A. Simulation Program Listings A.7 Zero-net-work Solution of Two-link Revolute Manipulator c y ; / / / / / / / / / . y / / / / / / . y : / / / / / / / / / / / / / / / / / r / / / : / / / / / x / . • / . • / . C'/. File name: TLRPM.for '/. c y / / : / / / / / / / / / ; / / / / / : / / / / / : / / / / / : / : / . y . y . y . y . y x / . y . y . y . y . y . C/. Chapter 6 C Main program TLRPM.for C C***BEGIN PROLOGUE TLRPM.for C***DATE WRITTEN 030501 (YYMMDD) C***REVISI0N DATE 030601 (YYMMDD) C***AUTH0R TAO FAN C***PURP0SE OFFLINE CALCULATION OF ZERO WORK METHOD IN CHAPTER 6 . C***DESCRIPTION C This is FORTRAN program used to solve near-minimum time control C problem of two-link planar revolute arm in chapter 6 of the thesis. C The algorithm is based on the zero work C method presented in the thesis. C Subroutine "ddassl" is used to solve C the ordinary differential equations with initial conditions. C* PREFERENCES (NONE) C***ROUTINES CALLED DDASSL,DRES C ROUTINES DRES is used to define the differential equations of the system C C Description of variables used in the program: C X0(4) initial states, Xf(4) final states C XPRIME0(4) initial values for XPRIME(4) C Xsw0(2) initial guess for switch positions C Xsw(2) switch positions, Tsw(2) switch times C XswOstep initial position step size C Xswstep position step size C Tstep time step C Nsw(2) switch indicator, it's used to indicate if the link have C switched, Nsw=-1 before switch, Nsw=+1 after switch C Nfinal(2) final position indicator, initial value=-l, C if link reach it's final position, C Nfinal=+1, control input changes to holding force/torque. C Tau0(2) initial values for control inputs C Taumax(2) maximum control inputs C Tau(2) control inputs C Work(2) work done by all forces/torques for each link C RCforce reaction force between two link C Xlast(4) store previous state values used to calculate work C Tfinal(2) final times for each link C Tstop stop time for simulation C Xerror absolute position error, C Werror absolute work error C Xdeta, Ydeta displancement in X,Y direction C***END PROLOGUE TLRPM IMPLICIT DOUBLE PRECISION(A-H,0-Z) COMMON Tau(2),11, lcl, 12,lc2,ml,m2,II,12,M,cl,c2,c3 DOUBLE PRECISION 11, lcl, 12,lc2,ml,m2,II,12,M,cl,c2,c3 EXTERNAL DRES DIMENSION X(4), XPRIME(4), DELTA(4),INFO(15),RW0RK(300),IW0RK(60) DOUBLE PRECISION X0(4), Xf(4),XPRIME0(4), Xsw0(2),Xsw(2),Tsw(2), Appendix A. Simulation Program Listings 142 * Xswstep,XswOstep,Tstep,Nsw(2),Nfinal(2), * TaumaX(2),TauO(2), Work(2), RCforce, Xlast(4), Tfinal(2), * Xerror.Werror, Xdeta, Ydeta, Tstop PARAMETER (PI=3.14159D0) C LRW — Set it to the declared length of the RWORK array. C You must have LRW .GE. 40+(MAX0RD+4)*NEQ+NEq*2 LRW=300 C LIW — Set it to the declared length of the IWORK array. C You must have LIW .GE. 20+NEQ LIW=60 C Number of differential equations NEQ=4 C Physical parameters for Case 1 and Case 2 C length of the link 1 11=0.4D0 lcl=ll/2.OD0 C length of the link 2 12=0.25D0 Ic2=12/2.0D0 C mass of the first link ml=29.58D0 C mass of the second link m2=15.00D0 C mass moment of inertia of the linki I1=0.416739D0 C mass moment of inertia of the link2 I2=0.205625D0 C mass of end effector and load M=6.0D0 C maximum torque for link 1 Taumax(l)=25.0D0 C maximum torque for link 2 Taumax(2)=9.0D0 C Physical parameters for Case 3 C length of the link 1 C 11=0,4D0 C lcl=ll/2.0D0 C length of the link 2 C 12=0.25D0 C Ic2=12/2.0D0 C mass of the first link C ml=0.245D0 C mass of the second link C m2=0.15315D0 C mass moment of inertia of the linki C Il=3.2688D-3 C mass moment of inertia of the link2 C I2=0.7986D-3 C mass of end effector and load C M=0.5D0 C maximum torque for link 1 C Taumax(l)=0.25D0 C maximum torque for link 2 Appendix A. Simulation Program Listings 143 C Taumax(2)=0.1D0 C Physical parameters for Case 4 C length of the link 1 C 11=1.0D0 C lcl=ll/2.0D0 C length of the link 2 C 12=0.625D0 C Ic2=12/2.0D0 C mass of the first link C ml=0.61261D0 C mass of the second link C m2=0.38288D0 C mass moment of inertia of the linki C 11=51.0547D-3 C mass moment of inertia of the link2 C 12=12.4660D-3 C mass of end effector and load C M=1.25D0 C maximum torque for link 1 C Taumax(1)=3.90D0 C maximum torque for link 2 C Taumax(2)=1.56D0 cl=Il+ml*lcl**2.0D0+(m2+M)*11**2.0D0 c2=I2+m2*lc2**2. 0D0+M*12**2. 0D0 c3=m2*ll*lc2+M*ll*12 C Initial position for case 1 X0(1)=0.975D0 XO(2)=0.DO X0(3)=0.D0 XO(4)=0.DO C Initial position for case 2 C X0(1)=0.76D0 C X0(2)=0.D0 C X0(3)=0.2618D0 C X0(4)=0.D0 C Initial position for case 3 C X0(1)=0.376D0 C X0(2)=0.D0 C X0(3)=0.D0 C XO(4)=0.DO C Initial position for case 4 C X0(1)=0.8D0 C X0(2)=0.D0 C X0(3)=0.2D0 C X0(4)=0.D0 C Final position Xf(1)=0.D0 Xf (2)=0.D0 Xf(3)=0.D0 Xf(4)=0.D0 C Xerror=1.0D-4 Appendix A. Simulation Program Listings 144 Werror=1.0D-4 Tstop=2.0D0 C Initial torque TauO (1) =-Taumax (1) Tau0(2)=-Taumax(2) IF (X0(1).LT.Xerror) THEN TauO(1)=(c2+c3*DC0S(XO(3)))*TauO(2)/c2-c3*DSIN(X0(3))*X0(4)**2.ODO END IF IF (X0(3).LT.Xerror) THEN Tau0(2)=(c2+c3)*Tau0(l)/(cl+c2+2*c3) END IF IF ((X0(1).LT.Xerror).AND.(X0(3).LT.Xerror)) THEN TauO(1)=0.ODO Tau0(2)=0.0D0 END IF C Initial velocity and accelaration XPRIHE0(1)=X0(2) XPRIMEO(2)=(c2*(TauO(1)-TauO(2)+c3*(XO(2)+ * X0(4))**2.0D0*DSIN(X0(3))) * -c3*(Tau0(2)-c3*(X0(2)**2.0D0)*DSIN(X0(3)))*DC0S(X0(3))) * /(cl*c2-c3**2.0D0*(DC0S(X0(3)))**2.0D0) XPRIME0(3)=X0(4) XPRIME0(4)=((cl+c3*DC0S(X0(3)))* * (TauO(2)-c3*X0(2)**2.OD0*DSIN(XO(3))) * -(c2+c3*DC0S(X0(3)))*(Tau0(l)-Tau0(2) * +c3*(XO(2)+XO(4))**2.0DO*DSIN(XO(3)))) * /(cl*c2-c3**2.ODO*(DCOS(X0(3)))**2.ODO) C Initial step size Xsw0step=0.01D0 Xswstep=XswOstep Tstep=0.001DO C Initial guess for switch positions XswO(l)=Xf(1) XswO(2)=Xf(3) C Start of a new integration problem C Initialized the switch position 3 Xsw(l)=XswO(l) Xsw(2)=XswO(2) 5 CONTINUE C Reset the switch indicator Nsw(l)=-1.0D0 Nsw(2)=-1.0D0 Nfinal(l)=-1.0D0 Nfinal(2)=-1.0D0 Tfinal(l)=O.ODO Tfinal(2)=0.0D0 Tsw(l)=O.ODO Tsw(2)=0.0D0 C Dimension of INFO(N) DO 7 1=1,15 INF0(I)=0 Appendix A. Simulation Program Listings 7 CONTINUE C Do you want the solution only at C TOUT (and not at the next intermediate step) ... C Yes - Set INFO(3) = 0 C No - Set INF0(3) = 1 **** C Do you want the code to decide C on its own maximum stepsize? C Yes - Set INFO(7)=0 C No - Set INF0(7)=1 C and define HMAX by setting C RW0RK(2)=HMAX **** C Do you want the code to define C its own initial stepsize? C Yes - Set INFO(8)=0 C No - Set INF0(8)=1 C and define HO by setting C RW0RK(3)=H0 **** INF0(3)=1 INF0(7)=1 INF0(8)=1 C maximum stepsize RW0RK(2)=0.0001D0 C initial stepsize RW0RK(3)=0.001D0 C Relative tolerance RT0L=1.0D-11 C Absolute tolerance AT0L=1.0D-8 C Initialize the parameters T=0.D0 DO 10 1=1,NEQ X(I)=X0(I) 10 XPRIME(I)=XPRIMEO(I) C Initial value of workdone by external force C of each link DO 20 1=1,2 20 Work(I)=0.D0 C Open file "datal.m' to store data of Linki OPEN(FILE='datal.m',UNIT=6,status='unknown') WRITE(6,30) 30 FORMAT(2X,'/,'/, This is the simulation result of Linki') WRITE(6,40) 40 FORMAT(10X,'% Time',9X,'Position',5X,'Velocity',4X,'Force * 9X,'Work') WRITE(6,50) 50 FORMAT(2X,'output 1=[') C Open file "data2.m' to store data of Link2 OPEN(FILE='data2.m',UNIT=7,status='unknown') WRITE(7,60) 60 FORMAT(2X, ''/,'/, This is the simulation result of Link2') WRITE(7,40) WRITE(7,80) Appendix A. Simulation Program Listings 80 F0RMAT(2X,'output2=[') C Decide the control input Tau 90 CONTINUE DO 100 1=1,2 IF((X(2*I-1) .GE. Xsw(I))) THEN Tau(I)=-Taumax(I) Tsw(I)=T IF (X0(2*I-1).LT.Xerror) THEN Tsw(I)=0.0D0 END IF ELSE Tau(I)=+Taumax(I) Nsw(I)=1.0D0 C Nsw indicates the link has switched END IF 100 CONTINUE C Linkl reaches final position IF (((X(l)-Xf(1)).LT.Xerror).OR.(Nfinal(l).GT.O.DO)) THEN IF (Nfinal(l).LT.O.DO) THEN Tfinal(l)=T END IF Tau(l)=(c2+c3*DC0S(X(3)))*Tau(2)/c2-c3*DSIN(X(3))*X(4)**2.0D0 Nfinal(l)=+1.0D0 END IF C Link2 reaches final position IF (((X(3)-Xf(3)).LT.Xerror) .OR.(Nfinal(2).GT.O.DO)) THEN IF (Nfinal(2).LT.O.DO) THEN Tfinal(2)=T END IF Tau(2)=(c2+c3)*Tau(l)/(cl+c2+2.0D0*c3) Nfinal(2)=+1.0D0 END IF C Both links reach final position IF (((X(l)-Xf (D).LT.Xerror).AND. ((X(3)-Xf (3)).LT.Xerror)) THEN Tau(l)=0.D0 Tau(2)=0.D0 END IF C Output the data WRITE(6,110) T,(X(I),1=1,2),Tau(l),Uork(l) 110 F0RMAT(7X,6(F10.6,3X)) WRITE(7,111) T,(X(I),1=3,4),Tau(2),Work(2) 111 FORMAT(7X,6(F10.6,3X)) DO 120 1=1,4 120 Xlast(I)=X(I) C Integrate one time step TOUT=T+Tstep CALL DDASSL (DRES, NEQ, T, X, XPRIME, TOUT, INFO, RTOL, ATOL, * IDID, RWORK, LRW, IWORK, LIW, RPAR, IPAR.JAC) C Calculate work for each link C Work of linkl Work(l)=1.0D0/2.0D0*(ml*lcl**2.0D0+Il)*X(2)**2.0D0 Appendix A. Simulation Program Listings 147 C Work of link2 Work(2)=1.0D0/2.0D0*(I2+m2*lc2**2.0D0)*(X(2)+X(4))**2.0D0 * +1.0D0/2.0D0*m2*ll**2.0D0*(X(2))**2.0D0 * +DC0S(X(3))*X(2)*(X(2)+X(4))*ll*lc2*m2 IF (((ABS(Tau(l))).LT.1.OD-8).AND.((ABS(Tau(2))).LT.1.OD-8) * .OR.(T.GT.Tstop)) THEN C Final point, check for works IF ((((ABS(X(1))).LT.Xerror).AND.((ABS(X(3))).LT.Xerror)).AND. * (((ABS(Work(l))).LT.Werror).AND.((ABS(Work(2))).LT.Werror))) * THEN GOTO 126 END IF IF (Xsw(l) .LT.XO(D) THEN C Increase switch position for link 1 and restart integration Xsw (1) =Xsw (1) +Xswstep PRINT*,'Updating Xswl=',Xsw(l) CLOSE(6,STATUS='DELETE') CLOSE(7,STATUS='DELETE') GOTO 5 END IF IF (Xsw(2).LT.X0(3)) THEN C Increase switch position for link 2 and restart integration Xsw(l)=XswO(l) Xsw(2)=Xsw(2)+Xswstep PRINT*,'Updating Xsw2=',Xsw(2) CLOSE(6,STATUS='DELETE') CLOSE(7,STATUS='DELETE') GOTO 5 ELSE C Reduce step size by half Xswstep=Xswstep/2.0D0 PRINT*,'Updating Xswstep='.Xswstep CLOSE(6,STATUS='DELETE') CLOSE(7,STATUS='DELETE') GOTO 3 END IF ELSE C Continue integration GOTO 90 END IF 126 WRITE(6,110) T,(X(I),1=1,2),Tau(l),Work(l) WRITER,111) T, (X(I),I=3,4),Tau(2) ,Work(2) WRITE(6,130) WRITE(7,130) 130 FORMAT(2X,']; ') WRITE(6,140)Xsw0(l) 140 FORMAT(3X, "/, The initial guess of Xsw0(l) = ',F10.6) WRITE(6,150)Xsw(l) 150 FORMAT(3X,'/. The switch position of linki Xsw(l)=' ,F10.6) WRITE(6,170)Tsw(l) 170 FORMAT(3X,'"/, The switch time of linki Tsw(l) = ' ,F10.6) WRITE(6,190)Tfinal(l) 190 FORMAT(3X, '*/, The final time of linki Tfinal(l) = ',F10.6) Appendix A. Simulation Program Listings 148 WRITE(6,240)XswO(2) 240 FORMAT(3X, "/. The initial guess of Xsw0(2)=',F10.6) WRITE(6,250)Xsw(2) 250 FORMAT(3X, 7. The switch position of link2 Xsw(2) = ',F10.6) WRITE(6,270)Tsw(2) 270 FORMAT(3X, "/. The switch time of link2 Tsw(2)=',F10.6) WRITE(6,290)Tfinal(2) 290 FORMAT(3X, ''/, The final time of link2 Tfinal(2) = ',F10.6) URITE(6,300) XswOstep.Xswstep 300 FORMAT(IX,7. XswOstep=',F9.5,3X,'Xswstep=',F10.6) END SUBROUTINE DRES(T,X,XPRIME,DELTA,IRES,RPAR,IPAR) IMPLICIT DOUBLE PRECISION(A-H.O-Z) DIMENSION X(4), XPRIME(4), DELTA(4) COMMON Tau(2),ll, lcl, 12,lc2,ml,m2,II,12,M,cl,c2,c3 DOUBLE PRECISION 11, lcl, 12,lc2,ml,m2,II,12,M,cl,c2,c3 DELTA(1)=XPRIME(1)-X(2) DELTA(3)=XPRIME(3)-X(4) DELTA(2)=XPRIME(2)-(c2*(Tau(l)-Tau(2)+c3*(X(2)+ * X(4))**2.0D0*DSIN(X(3))) * -c3*(Tau(2)-c3*(X(2)**2.0D0)*DSIN(X(3)))*DC0S(X(3))) * /(cl*c2-c3**2.ODO*(DCOS(X(3)))**2.ODO) DELTA(4)=XPRIME(4)-((cl+c3*DC0S(X(3)))* * (Tau(2)-c3*X(2)**2.0D0*DSIN(X(3))) * -(c2+c3*DC0S(X(3)))*(Tau(l)-Tau(2) * +c3*(X(2)+X(4))**2.0D0*DSIN(X(3)))) * /(cl*c2-c3**2.ODO*(DCOS(X(3)))**2.ODO) RETURN END A.8 Exact Minimum Time Solution of Two-link Revolute Manipulator c7//:///:///:/://///://///:/://///:/:/:/:/:/.'/.,/.'/.•/.•/.•/.'/.,/. Cl File name: TLRPM_MT.for '/. cy.y.y.y//.v.r/.v.M,/.,/.,/.,/.,/.v.,/.v.y.,/.y.y.,/.,/.'/.%"/.y.y. C/, Chapter 6 C Main program TLRPM_MT.for C C***BEGIN PROLOGUE TLRPM_MT.for C***DATE WRITTEN 030501 (YYMMDD) C***REVISION DATE 030601 (YYMMDD) C***AUTH0R TAO FAN C***PURP0SE SOLVING TPBVP PROBLEM IN CHAPTER 6. C***DESCRIPTION C This is FORTRAN program used to solve exact minimum time C problem of two-link planar revolute arm in chapter 6 of C Subroutine "MUSN" is used to solve C the nonlinear TPBVP. C***REFERENCES (NONE) C***ROUTINES CALLED MUSN, FDIF, XOT C ROUTINES FDIF is used to define the differential equations C ROUTINES XOT is used to evaluate the initial approximation control the thesis. of the system Appendix A. Simulation Program Listings C XO(t) of the solution. C Description of variables used in the program: C Taumax is maximum control inputs C Tau is control inputs C***END PROLOGUE TLRPM_MT.for C IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION ER(5),TI(12),X(5,12),Q(5,5,12),U(15,12),D(5,12), 1 PHIREC(15,12),W(900),UGR(20) INTEGER IW(60) EXTERNAL FDIF,XOT,G C C C N is the order of the system. C C NU is one of the dimension of U and PHI. C NU must be greater than or equal to N * (N+1) / 2. C C NTI is one of the dimension of TI, X, S, Q, U en PHI. C NTI must be greater than or equal to the total number of C necessary output points +1 (i.e. if the entry value for C NRTI > 1, NTI may be equal to the entry value of NRTI + 1) C C LW is the dimension of W. LW >= 7*N + 3*N*NTI + 4*N*N C C LIW is the dimension of IW. LIW >= 3*N + NTI C C ER(3) must contain the machine precision C C LWG is the dimension of WGR. LWG >= (total number of grid points) C / 5. The minimum number of grid points between 2 succesive output C points is 5, so the minimum value for LWG is the number of actually C used output points. Initially a crude estimate for LWG has to be made C C ER(1) must contain the required tolerance for solving the differential C equation. C ER(2) must contain the initial tolerance with which a first aproximate C solution will be computed. This approximation is then used as an C initial approximation for the computation of an solution with an C tolerance ER(2)*ER(2) and so on until the required tolerance is C reached. As an initial tolerance max(ER(l),min(ER(2),1.d-2)) will C be used C C A,B the two boundary points C C NRTI is used to specify the output points. There are 3 ways to C specify the output points: C 1) NRTI = 0, the output points are determined automatically using AMP. C 2) NRTI = 1, the output points are supplied by the user in the array TI. C 3) NRTI > 1, the subroutine computes the (NRTI+1) output points TI(k) by C TI(k) = A + (k-1) * (B - A) / NRTI ; C so TI(1) = A and TI(NRTI+1) = B. C Depending on the allowed increment between two succesive output points, C more output points may be inserted in cases 2 and 3. C On exit NRTI contains the total number of output points. C AMP must contain the allowed increment between two output C points. AMP is used to determine output points and to assure that the C increment between two output points is at most AMP*AMP. A small value C for AMP may result in a large number of output points. Appendix A. Simulation Program Listings 150 C Unless 1 < AMP < .25 * sqrt(ER(l)/ER(3)) the default value C .25 * sqrt(ER(l)/ER(3)) is used. C ITLIM maximum number of allowed iteration C IERR0R=1, diagnostics will be printed during computation N = 9 NU = 45 NTI = 12 LW = 900 LIW = 60 ER(3) = 1.1D-15 LWG = 20 ER(1) = l.D-6 ER(2) = l.D-2 A = 0.D0 B = 1.D0 NRTI=10 AMP=100.D0 ITLIM=20 IERR0R=1 0PEN(FILE='datalMT.m',UNIT=6,status='unknown') WRITE(6,30) 30 FORMAT(2X, This is the results of TPBVP') CALL MUSN(FDIF,XOT,G,N,A,B.ER.TI,NTI,NRTI,AMP,ITLIM,X,Q,U,NU,D, 1 PHIREC,KPART,W,LW,IW,LIW,WGR,LWG,IERROR) WRITE(6,*) ' MUSN: IERROR =',IERROR WRITE(6,200) A,B,ER(1),ER(2),ER(4),ER(5),KPART 200 FORMAT (' A = ',F8.4,3X,'B = \F8.4,/,' REQUIRED TOLERANCE = \1P, 1 D12.5,3X,'START TOLERANCE = '.D12.5,/, 2 ' CONDITION NUMBER = ',D12.5,3X, 3 'AMPLIFICATION FACTOR = '.D12.5,/,' K-PARTITIONING =',I2,/) IF (IERROR.NE.O) GOTO 3000 WRITE(6,215) 215 FORMAT(' I ',4X,'T',9X,'XI',12X,'X2',12X,'X3',12X,'X4',12X,'X5', 1 /) DO 2200 K = 1 , NRTI WRITE(6,220) K.TKK) , (X(J,K) , J=1,N) 2200 CONTINUE 220 FORMAT(' ',12,IX,F6.4,IP,5(2X,D12.5)) 3000 CONTINUE STOP END SUBROUTINE FDIF(T,Y,F) c C IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION Y(9),F(9) DOUBLE PRECISION 11,lcl,12,lc2,II,12,M,Taumax(2),Tau(2), k Cl,c2,c3,c4,all,al2,al3,a22,al4,a24,s(2),deta C f22,f24,f32,f34,f42,f44 C DOUBLE PRECISION XI,X2,X3,X4,P1,P2,P3,P4,Z X1=Y(1) X2=Y(2) X3=Y(3) X4=Y(4) P1=Y(5) P2=Y(6) Appendix A. Simulation Program Listings 151 P3=Y(7) P4=Y(8) Z=Y(9) C Physical parameters for Case 1 and Case 2 C length of the link 1 11=0.4D0 lcl=ll/2.0D0 C length of the link 2 12=0.25D0 Ic2=12/2.0D0 C mass of the first link ml=29.58D0 C mass of the second link m2=15.00D0 C mass moment of inertia of the linki I1=0.416739DO C mass moment of inertia of the link2 I2=0.205625D0 C mass of end effector and load H=6.0D0 C maximum torque for link 1 Taumax(l)=25.0D0 C maximum torque for link 2 Taumax(2)=9.0D0 C Physical parameters for Case 3 C length of the link 1 C 11=0.4D0 C lcl=ll/2.0D0 C length of the link 2 C 12=0.25D0 C Ic2=12/2.0D0 C mass of the first link C ml=0.245DO C mass of the second link C m2=0.15315D0 C mass moment of inertia of the linki C Il=3.2688D-3 C mass moment of inertia of the link2 C I2=0.7986D-3 C mass of end effector and load C M=0.5D0 C maximum torque for link 1 C Taumax(l)=0.25D0 C maximum torque for link 2 C Taumax(2)=0.1D0 C Physical parameters for Case 4 C length of the link 1 C 11=1.0D0 C lcl=ll/2.0D0 C length of the link 2 C 12=0.625D0 C Ic2=12/2.0D0 C mass of the first link C ml=0.61261D0 Appendix A. Simulation Program Listings 152 c mass of the second link m2=0.38288D0 mass moment of inertia of the linki 11=51.0547D-3 mass moment of inertia of the link2 12=12.4660D-3 mass of end effector and load M=1.25D0 C C C C C C C C C C C maximum torque for link 1 Taumax(1)=3.90D0 maximum torque for link 2 Taumax(2)=1.56D0 cl=Il+ml*lcl**2.0D0+(m2+M)*ll**2.0D0 c2=I2+m2*lc2**2.0D0+M*12**2.0D0 c3=m2*ll*lc2+M*ll*12 c4=ml*lc1+(m2+M)*11 c5=m2*lc2+M*12 all=cl+c2+2.0D0*c3*DC0S(X3) al2=c2+c3*DCDS(X3) al3=c3*DSIN(X3) a22=c2 al4=c4*DC0S(XI)+c5*DCQS(X1+X3) a24=c5*DCQS(Xl+X3) deta=cl*c2-c3**2.0D0*(DCOS(X3))**2.0D0 C Decided the control input S(l) = (P2*a22-P4*al2)/deta S(2) = (-P2*al2+P4*all)/deta IF(S(1).LT.O.ODO) THEN Tau(1)=Taumax(1) ELSE Tau(l)=-Taumax(l) END IF IF(S(2).LT.O.ODO) THEN Tau(2)=Taumax(2) ELSE Tau(2)=-Taumax(2) END IF f21=-1.0D0 f22=-2.0D0*al3*(al2*X2+a22*X4)/deta f24=2.0D0*al3*(all*X2+al2*X4)/deta f32=-l.0D0*((al2-a22)*(a22*(2.0D0*X2+X4)*X4+al2*X2**2.0D0)-al3**2.0D0 * *X2**2.0D0+al3*Tau(2))/deta + 2.0D0*al3*(al2-a22)* * (al3*(a22*(2.0D0*X2+X4)*X4+al2*X2**2.0D0) * +a22*Tau(l)-al2*Tau(2))/deta**2.0D0 f34=((al2-a22)*(al2*(2.0D0*X2+X4)*X4+al1*X2**2.0D0)-al3**2.0D0 & *((X2+X4)**2.0D0+X2**2.0D0)+al3*(2.0D0*Tau(2)-Tau(l)))/deta & -2.0D0*al3*(al2-a22)*(al3*(al2*(2.0D0*X2+X4)*X4 & +all*X2**2.0D0)+al2*Tau(l)-all*Tau(2))/deta**2.0D0 f42=-2.0D0*al3*a22*(X2+X4)/deta f44=2.0D0*al3*al2*(X2+X4)/deta Appendix A. Simulation Program Listings 153 F(l) = Z*X2 F(2) = Z*(al3*(a22*(2.0D2*X2+X4)*X4+al2*X2**2.0D0) & +a22*Tau(l)-al2*Tau(2))/deta F(3) = Z*X4 F(4) = Z*(-al3*(al2*(2.0D2*X2+X4)*X4+all*X2**2.0D0) & -al2*Tau(l)+all*Tau(2))/deta F(5) = O.ODO F(6) = -Z*(-Pl+P2*f22+P4*f24) F(7) = -Z*(P2*f32+P4*f34) F(8) = -Z*(-P3+P2*f42+P4*f44) F(9) = O.ODO RETURN C END OF FDIF END SUBROUTINE XOT(T.X) c C IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION X(5) C C Case 1 X(l) = 0.975D0 X(2) = O.ODO X(3) = O.ODO X(4) = O.ODO X(5) = -0.456692 X(6) = -0.298622 X(7) = -0.093548 X(8) = -0.074258 X(9) = 1.083378 c Case 2 c X(l) = 0.76 c X(2) = O.ODO c X(3) = 0.2618 c X(4) = O.ODO c X(5) = -0.534711 c X(6) = -0.310833 c X(7) = -0.117210 c X(8) = -0.07799 c X(9) = 1.023704 c c Case 3 c X(l) = 0.376 c X(2) = O.ODO c X(3) = O.ODO c X(4) = O.ODO c X(5) = -0.534711 c X(6) = -0.310833 c X(7) = -0.117210 c X(8) = -0.07799 c X(9) = 1.233072 c c Case 4 Appendix A. Simulation Program Listings 154 c X(l) 0.8 c X(2) = O.ODO c X(3) = 0.2 c X(4) = O.ODO c X(5) -0.534711 c X(6) = -0.310833 c X(7) = -0.117210 c X(8) = -0.07799 c X(9) = 1.620396 RETURN C END OF XOT END SUBROUTINE G(N,XA,XB,FG,DGA,DGB) c C IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION XA(N),XB(N),FG(N),DGA(N,N),DGB(N,N) C DO 1100 I = 1 , N DO 1100 J = 1 , N DGA(I.J) = O.DO DGB(I.J) = O.DO 1100 CONTINUE DGA(1,1) = 1 DO DGA(2,2) = 1 DO DGA(3,3) = 1 DO DGA(4,4) = 1 DO DGB(1,1) = 1 DO DGB(2,2) = 1 DO DGB(3,3) = 1 DO DGB(4,4) = 1 DO C Case 1 FG(1) = XA(1) - 0.975D0 FG(2) = XA(2) FG(3) = XA(3) FG(4) = XA(4) FG(5) = XB(1) FG(6) = XB(2) FG(7) = XB(3) FG(8) = XB(4) C Case 2 C FG(1) = XA(1) - 0.76D0 C FG(2) = XA(2) C FG(3) = XA(3)- 0.2618D0 C FG(4) = XA(4) C FG(5) = XB(1) C FG(6) = XB(2) C FG(7) = XB(3) C FG(8) = XB(4) C Case 3 C FG(1) = XA(1) - 0.376D0 C FG(2) = XA(2) C FG(3) = XA(3) C FG(4) = XA(4) C FG(5) = XB(1) C FG(6) = XB(2) Appendix A. Simulation Program Listings c c c c c c c c c c c FG(7) = XB(3) FG(8) = XB(4) Case 4 FG(1) = XA(1) - 0.8D0 FG(2) = XA(2) FG(3) = XA(3)- 0.2D0 FG(4) = XA(4) FG(5) = XB(1) FG(6) = XB(2) FG(7) = XB(3) FG(8) = XB(4) C RETURN END OF G END A.9 S-function of Two-link Revolute Manipulator vx/x/x/x/x/x/x/x/x/x/x/x/x/x/.% '/, File name: TLRPMsimfun.m % mx/x/x/x/x/x/x/x/x/x/x/x/x/:/. % Chapter 6 7. This is the s-function of two-link planar revolute manipulator function[sys,xO]=robotTLRPM(t,x,u,flag,xOl,x02,x03,x04) if abs(flag)==l 7. physical parameters for Case 1 and Case 2 7.11=0.4; */. length of the link 1 '/.lcl=ll/2; */.12=0.25; 7. length of the link 2 7.1c2=12/2; 7jnl=29.58; 7. mass of the first link 7jn2=15.00; 7. mass of the second link 7,11=0.416739; 7. mass moment of inertia of the linkl 7.12=0.205625; 7. mass moment of inertia of the link2 7.M=6.0; 7. mass of end effector and load 7,ulmax=25; 7. maximum torque for link 1 7,u2max=9; 7. maximum torque for link 2 7. physical parameters for Case 3 7.11=0.4; 7. length of the link 1 7.1cl=ll/2; 7.12=0.25; 7. length of the link 2 7.1c2=12/2; %ml=0.245; % mass of the first link 7jn2=0.15315; 7. mass of the second link 7.11=3.2688e-3; 7. mass moment of inertia of the linkl 7.12=0.7986e-3; % mass moment of inertia of the link2 7.M=0.5; 7. mass of end effector and load 7.ulmax=0.25; % maximum torque for link 1 7.u2max=0.1; % maximum torque for link 2 7. physical parameters for Case 4 11=1.0; 7, length of the link 1 lcl=ll/2; 12=0.625; '/. length of the link 2 Appendix A. Simulation Program Listings 156 lc2=12/2; ml=0.61261; '/. mass of the first link m2=0.38288; '/. mass of the second link 11=51.0547e-3; 7. mass moment of inertia of the linkl 12=12.4660e-3; 7. mass moment of inertia of the link2 M=1.25; 7. mass of end effector and load ulmax=3.90; 7. maximum torque for link 1 u2max=1.56; 7. maximum torque for link 2 cl=Il+ml*lcl-2+(m2+M)*ll~2; c2=I2+m2*lc2~2+M*12-2; c3=m2*ll*lc2+M*ll*12; a=[x(2); (c2*(u(l)-u(2)+c3*(x(2)+x(4))-2*sin(x(3)))-c3*(u(2) -c3*(x(2)-2)*sin(x(3)))*cos(x(3)))/(cl*c2-c3-2*(cos(x(3)))-2); x(4); ((cl+c3*cos(x(3)))*(u(2)-c3*x(2)-2*sin(x(3))) -(c2+c3*cos(x(3)))*(u(l)-u(2)+c3*(x(2)+x(4))-2*sin(x(3)))) /(cl*c2-c3-2*(cos(x(3)))~2)] ; sys=a; elseif abs(flag)==3 sys=x; elseif abs(flag)==0 sys=[4.0,0.0,4.0,2.0,0.0,1.0]; x0(l)=x01; x0(2)=x02; x0(3)=x03; x0(4)=x04; else sys=[] ; end A. 10 S-function of Zero-net-work Controller for Two-link Revolute Manipulator mx/x/x/x/x/x/xmvx/x/x/x/x/. 7 . File name: ZWcontroller.m 7 , vx/x/x/x/x/x/x/x/x/x/x/x/x/xa '/. Chapter 6 7. This is the s-function 7. used for generating the controller inputs 7. of two link planar revolute manipulator. function[sys,xO,str,ts]=ZWcontroller(t,control,Q,flag,qisw,q2sw) 7. physical parameters for Case 1 and Case 2 11=0.4; 7. length of the link 1 lcl=ll/2; 12=0.25; 7. length of the link 2 lc2=12/2; ml=29.58; 7. mass of the first link m2=15.00; 7. mass of the second link 11=0.416739; 7. mass moment of inertia of the linkl 12=0.205625; V. mass moment of inertia of the link2 Appendix A. Simulation Program Listings M=6.0; 7. mass of end effector and load ulmax=25; 7. maximum torque for link 1 u2max=9; 7. maximum torque for link 2 7. physical parameters for Case 3 7.11=0.4; 7. length of the link 1 7.1cl=ll/2; 7.12=0.25; 7. length of the link 2 7.1c2=12/2; %ml=0.245; 7. mass of the first link 7«m2=0.15315; 7. mass of the second link 7.11=3.2688e-3; 7. mass moment of inertia of the linki 7.12=0.7986e-3; 7. mass moment of inertia of the link2 7.M=0.5; 7. mass of end effector and load 7.ulmax=0.25; 7. maximum torque for link 1 7.u2max=0.1; 7. maximum torque for link 2 7. physical parameters for Case 4 7.11=1.0; 7. length of the link 1 7.1cl=ll/2; 7.12=0.625; 7. length of the link 2 7.1c2=12/2; 7.ml=0.61261; 7. mass of the first link 7.m2=0.38288; 7. mass of the second link 7.11=51.0547e-3; 7. mass moment of inertia of the linki 7.12=12.4660e-3; 7. mass moment of inertia of the link2 7.M=1.25; 7. mass of end effector and load 7.ulmax=3.90; 7. maximum torque for link 1 7.u2max=l. 56; 7. maximum torque for link 2 error=0.0001; 7. final position error cl=Il+ml*lcl-2+(m2+M)*ll"2; c2=I2+m2*lc2-2+M*12_2; c3=m2*ll*lc2+H*ll*12; switch flag, vx/x/x/x/x/x/x/x/:/. 7. Initialization 7. vx/x/x/x/x/x/x/xa 7. Initialize the states, sample times, and state ordering strings case 0 [sys,x0,str,ts]=mdlInitializeSizes; 7X/X/XIX/XI. 7. Outputs 7. 'IXIXIXIXIXI. 7. Return the outputs of the S-function block, case 3 sys=mdl0utputs(t,control,Q,uimax,u2max,cl,c2,c3,error,qlsw,q2sw) v/x/x/x/x/x/x/x/xa 7. Unhandled flags 7. v/x/x/x/x/x/x/x/xa 7. There are no termination tasks (flag=9) to be handled. 7. Also, there are no continuous or discrete states, 7. so flags 1,2, and 4 are not used, so return an emptyu 7. matrix case { 1, 2, 4, 9 } Appendix A. Simulation Program Listings sys=[] ; v:///://///:/:/:///:///x///:/:/:/:/:/:/:///;///////////////:///:/.v.•/. '/. Unexpected flags (error handling)'/, v.r/.y.y.y.v.v.y.y.y.y.y.v//.v.%v//.r/.v.y.v.,/.,/.'/.y.,/.,/.'/.,/.y.,/.,/.y. '/, Return an error message for unhandled flag values, otherwise error(['Unhandled flag = ',num2str(flag)]); end % end timestwo */. •/. =========================== ============= ======= y, mdllnitializeSizes '/, Return the sizes, initial conditions, and sample times for the S-function. •/. ================ ============== ==== === function [sys,x0,str,ts] = mdllnitializeSizes() sizes = simsizes; sizes NumCont St at e s = 0 sizes NumDisestates = 0 sizes NumOutputs = 2 sizes Numlnputs = 4 sizes DirFeedthrough = 1 sizes NumSampleTimes = 1 sys = simsizes(sizes, ; str = • ; xO = • ; has direct feedthrough ts = [-1 0]; % inherited sample time '/, end mdllnitializeSizes */. •/,==, ========================= =========== == V. mdlOutputs 7, Return the output vector for the S-function •/============= = = = = = = = = = = = = = = = = = = = = = = =========== •/. function sys = mdlOutputs(t,control,Q,ulmax,u2max,cl,c2,c3.error,qisw,q2sw) Q(l) is position of linkl '/• Q(3) is position of link2 if Q(l)>qlsw control(1)= - ulmax; else control(1) = ulmax; end if Q(3)>q2sw control(2)= - u2max; else control(2)= u2max; end if abs(Q(1)<=error)feabs(Q(3)<=error) '/.turn on PD controller */. Kp=100; '/. Kd=20; Appendix A. Simulation Program Listings 159 7. control(l)=-Kp*Q(l)-Kd*Q(2); '/. control(2)=-Kp*Q(3)-Kd*q(4) ; control(1)=0; control(2)=0; elseif abs(Q(l)<=error)&abs(Q(3)>error) control(l)=(c2+c3*cos(Q(3)))*control(2)/c2-c3*sin(Q(3))*Q(4)-2; elseif abs(Q(l)>error)&abs(Q(3)<=error) control(2)=control(l)*(c2+c3)/(cl+c2+c3*2); end sys = [control(l); control(2)]; 7. end mdlOutputs A.11 S-function of Proportional and Derivative (PD) Controller for Two-link Rev-olute Manipulator v//////://///://///:/.•/.•/.•/.'/.7.'/.•/.•/.•/.•/.,/.7.•/.•/.•/.•/.•/.7.•/. % File name: PDcontroller.m 7, 7//:/:/;/:///:/://///://///.y:/.y.7.7.7.7.y.7.7.y.7.7.7.7.7. 7. Chapter 6 7. This is the s-function 7. used for generating the PD control inputs 7. for two-link planar revolute manipulator. function[sys,x0,str,ts]=MTcontroller(t,control,Q,flag,kp,kd) switch flag, 7X/X/.7X/X/.7X/X/X/.7. 7, Initialization 7. 7X/X/X/X/X/X/X/X/.7. 7. Initialize the states, sample times, and state ordering strings, case 0 [sys,xO,str,ts]=mdlInitializeSizes; 7X/X/X/X/X/. 7. Outputs 7. 7X/X/X/X/X/. 7. Return the outputs of the S-function block, case 3 sys=mdlOutputs(t,control,Q,kp,kd); 7X/X/X/X/X/X/X/X/X/. 7« Unhandled flags 7. 7X/X/X/X/X/X/X/X/X/. 7. There are no termination tasks (flag=9) to be handled. 7o Also, there are no continuous or discrete states, 7. so flags 1,2, and 4 are not used, so return an emptyu 7. matrix case { 1, 2, 4, 9 } sys=[] ; 7:///:/.7//.7.y.7.y:/.7.y////:///://///:/:///:/:/.7.7.7.7.7.7.7.7.7.y.7. 7. Unexpected flags (error handling)7. 7//////////:///:/://///:///.7:///://///:/:/:/://///.77.7.7.7.7.7.7.7.7. 7. Return an error message for unhandled flag values, otherwise error(['Unhandled flag = ',num2str(flag)]); Appendix A. Simulation Program Listings end end timestwo •/. % = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = '/. mdllnitializeSizes % Return the sizes, initial conditions, and sample times for the S-function. y= =====================================m function [sys,xO,str,ts] = mdllnitializeSizesO sizes = simsizes; sizes NumContStates = 0 sizes NumDiscStates = 0 sizes NumOutputs = 2 sizes Numlnputs = 4 sizes DirFeedthrough = 1 sizes NumSampleTimes = 1 7. has direct feedthrough sys = simsizes(sizes); str = [] ; xO = []; ts = [-1 0] ; 7. inherited sample time 7. end mdllnitializeSizes */. %==================================== % mdlOutputs 7. Return the output vector for the S-function •/. == 7. function sys = mdlOutputs(t,control,Q,kp,kd) 7. turn on PD controller control(l)=-kp*q(l)-kd*Q(2); control(2)=-kp*q(3)-kd*q(4); sys = [control(l); control(2)]; % end mdlOutputs Appendix B Subroutines Used in This Thesis B . l Subroutine MUSN for Solving Nonlinear T P B V P Subroutine MUSN is written in FORTRAN 77 and is available through Netlib repository (http://www.netlib.org/). The Netlib repository contains freely available software, documents, and databases of interest to the numerical, scientific computing, and other communities. The repository is maintained by AT&T Bell Laboratories, the University of Tennessee and Oak Ridge National Laboratory, and by colleagues world-wide. The collection is replicated 1 at several sites around the world, automatically synchronized, ! to provide reliable and network efficient service to the ! global community. MUSN **************** SPECIFICATION MUSN **************** SUBROUTINE MUSN(FDIF,YOT,G,N,A,B,ER,TI,NTI,NRTI,AMP,ITLIM,Y,Q,U, 1 NU,D,PHI,KP,W,LW,IW,LIW,WG,LUG,IERROR) C C DOUBLE PRECISION A,B,ER(5),TI(NTI),AMP,Y(N,NTI),Q(N,N,NTI), C 1 U(NU,NTI),D(N,NTI),PHI(NU,NTI),W(LW),WG(LWG) C INTEGER N,NTI,NRTI,ITLIM,NU,KP,LW,IW(LIW),LIW,LWG,IERROR C EXTERNAL FDIF,YOT,G **************** t Purpose **************** MUSN solves the nonlinear two-point BVP dy(t)/dt = f(t,y) A <= t <= B or B <= t <= A g(y(a),y(b)) = 0 , where y(t) and f(t,y) are N-vector functions. **************** Method **************** MUSN uses a multiple shooting method for computing an approximate solution of I the BVP at specified output points, which are also used as shooting points. If necessary, more output points (shooting points) are inserted during computa^ tion. For integration a fixed grid is used. Output points are also grid points and th minimum number of grid points between two output points is 5. 161 Appendix B. Subroutines Used in This Thesis 162 **************** Parameters FDIF SUBROUTINE, supplied by the user with specification: SUBROUTINE FDIF(T,Y,F) DOUBLE PRECISION T,Y(N),F(N) where N is the order of the system. FDIF must evaluate the righthand-side of the differential equation, f(t,y) for t=T and y=Y and places the result in F(l) F(N) . FDIF must be declared as EXTERNAL in the (sub)program from which MUSN is called. YOT SUBROUTINE, supplied by the user with specification SUBROUTINE .YOT(T,Y) DOUBLE PRECISION T,Y(N) where N is the order of the system. YOT must evaluate the initial appro-ximation yO(t) of the solution, for any value t=T and place the result in Y(l),...,Y(N). YOT must be declared as EXTERNAL in the (sub)program from which MUSN is called. G SUBROUTINE, supplied by the user with specification SUBROUTINE G(N,YA,YB,FG,DGA,DGB) DOUBLE PRECISION YA(N),YB(N),FG(N),DGA(N,N),DGB(N,N) where N is the order of the system. G must evaluate g(y(A),y(B)) for y(A)=YA and y(B)=YB and place the result in FG(1),...,FG(N). Moreover G must evaluate the Jacobians dg(u,v)/du for u = YA and dg(u,v)/dv for v = YB and place the result in the arrays DGA anb DGB respectively. G must be declared as EXTERNAL in the (sub)program from which MUSN is called. N INTEGER, the order of the system. Unchanged on exit. A,B DOUBLE PRECISION, the two boundary points. Unchanged on exit. ER DOUBLE PRECISION array of dimension (5). On entry: ER(1) must contain the required tolerance for solving the differential equation. ER(2) must contain the initial tolerance with which a first aproximate solution will be computed. This approximation is then used as an initial approximation for the computation of an solution with an tolerance ER(2)*ER(2) and so on until the required tolerance is reached. As an initial tolerance max(ER(l),min(ER(2),1.d-2)) will be used. ER(3) must contain the machine precision. On exit: ER(1), ER(2) and ER(3) are unchanged. ER(4) contains an estimation of the condition number of the BVP. Appendix B. Subroutines Used in This Thesis 163 ER(5) contains an estimated error amplification factor. TI DOUBLE PRECISION array of dimension (NTI). On entry : if NRTI = 1 TI must contain the output points in strict monotone order: A=TI(1) < TI(2) <...< TI(n)=B. On exit TI(j), j=l,...,NRTI contains the output points. NTI INTEGER, NTI is one of the dimension of TI, X, S, Q, U en PHI. NTI must be greater than or equal to the total number of necessary output points +1 (i.e. if the entry value for NRTI > 1, NTI may be equal to the entry value of NRTI + 1). Unchanged on exit. There are 3 ways to NRTI INTEGER. On entry NRTI is used to specify the output points, specify the output points: 1) NRTI = 0, the output points are determined automatically using AMP. 2) NRTI = 1, the output points are supplied by the user in the array TI. 3) NRTI > 1, the subroutine computes the (NRTI+1) output points TKk) by TI(k) = A + (k-1) * (B - A) / NRTI ; so TI(1) = A and TI(NRTI+1) = B. Depending on the allowed increment between two succesive output points, more output points may be inserted in cases 2 and 3. On exit NRTI contains the total number of output points. AMP DOUBLE PRECISION. On entry AMP must contain the allowed increment between two output points. AMP is used to determine output points and to assure that the increment between two output points is at most AMP*AMP. A small value for AMP may result in a large number of output points. Unless 1 < AMP < .25 * sqrt(ER(1)/ER(3)) the default value .25 * sqrt(ER(l)/ER(3)) is used. Unchanged on exit. ITLIM INTEGER, maximum number of allowed iteration. Y DOUBLE PRECISION array of dimension (N.NTI). On exit Y(.,i), i=l,...,NRTI contains the solution at the output points TI(i), i=l,...,NRTI. Q DOUBLE PRECISION array of dimension (N,N,NTI). On exit Q(.,.,i), i=l,...,NRTI contains the orthogonal factors of the incremental recursion. U DOUBLE PRECISION array of dimension (NU.NTI). On exit U(.,i), i=2,...,NRTI contains the upper triangular factors of the incremental recursion. The elements are stored column wise, the j-th column of U is stored in U(nj+1,.),U(nj+2,.),...,U(nj+j,.), where = (j-D * j / 2. NU INTEGER, NU is one of the dimension of U and PHI. NU must be greater than or equal to N * (N+l) / 2. Unchanged on exit. D DOUBLE PRECISION array of dimension (N.NTI). On exit D(.,i) i=2,...,NRTI contain the inhomogeneous terms of the incremental recursion. PHI DOUBLE PRECISION array of dimension (NU.NTI). On exit PHI(.,i), i=l NRTI contains the fundamental solution of the incremental recursion. The fundamental solution is upper triangular and stored in the same way as the upper triangular U. Appendix B. Subroutines Used in This Thesis KP INTEGER, On exit KP contains the dimension of the increasing solution space. W DOUBLE PRECISION array of dimension (LW). Used as work space. LW INTEGER. . LW is the dimension of W. LW >= 7*N + 3*N*NTI + 4*N*N . IW INTEGER array of dimension (LIW). Used as work space. LIW INTEGER. LIW is the dimension of IW. LIW >= 3*N + NTI . WG DOUBLE PRECISION array of dimension (LWG). WG is used to store the integration grid points. LWG INTEGER. LWG is the dimension of WG. LWG >= (total number of grid points) / 5. The minimum number of grid points between 2 succesive output points is 5, so the minimum value for LWG is the number of actually used output points. Initially a crude estimate for LWG has to be made (see also IERROR 219). IERROR INTEGER, error indicator. On entry : if IERR0R=1, diagnostics will be printed during computation. On exit : if IERROR = 0 no errors have been detected. **************** Error indicators IERROR 0 No errors detected. 101 INPUT error: either ER(1) < 0 or ER(2) < 0 or ER(3) < 0. TERMINAL ERROR. 105 INPUT error: either N < 1 or NRTI < 0 or NTI < 3 or NU < N*(N+l)/2 or A=B. TERMINAL ERROR. 106 INPUT error: either LW < 7*N + 3*N*NTI + 4*N*N or LIW < 3*N + NTI. TERMINAL ERROR. 120 INPUT error: the routine was called with NRTI=1, but the given output points in the array TI are not in strict monotone order. TERMINAL ERROR. 121 INPUT error: the routine was called with NRTI=1, but the first given output point or the last output point is not equal to A or B. TERMINAL ERROR. 122 INPUT error: the value of NTI is too small; the number of necessary output points is greater than NTI-1. TERMINAL ERROR. Appendix B. Subroutines Used in This Thesis 123 INPUT error: the value of LWG is less than the number of output points. Increase the dimension of the array WG and the value of LWG. TERMINAL ERROR. 216 This indicates that during integration the requested accuracy could not be achieved. User must increase error tolerance. TERMINAL ERROR. 219 This indicates that the routine needs more space to store the integra-tion grid point. An estimate for the required workspace (i.e. the value for LWG) is given. TERMINAL ERROR. 230 This indicates that Newton failed to converge. TERMINAL ERROR. 231 This indicates that the number of iteration has become greater than ITLIM. TERMINAL ERROR. 240 This indicates that the global error is probably larger than the error tolerance due to instabilities in the system. Most likely the system is ill-conditioned. Output value is the estimated error amplification factor. WARNING ERROR. 250 This indicate that one of the upper triangular matrices U is singular. TERMINAL ERROR. 260 This indicates that the problem is probably too ill-conditioned with respect to the BC. TERMINAL ERROR. **************** Auxiliary routines **************** Calls are made to the MUS library routines: DBCMAV, DCHINC, DCROUT, DCSAOJ, DCSHPO, DFQUS, DFUNRC, DGTUR, DINPRO, DINTCH, DJINGX, DKPCH, DLUDEC, DMATVC, DNEWPO, DPSR, DQEVAL, DQUDEC, DRKF1S, DRKFGG, DRKFGS, DRKFMS, DSBVP, DSOLDE, DSOLUP, DSORTD, DTAMVC, ERRHAN. Remarks **************** MUSN is written by R.M.M. Mattheij and G.W.M. Staarink. Last update: 02-15-88. **************** Example of the use of MUSN **************** Consider the differential equation: u' = .5 u*(w-u) / v v' = -0.5 (w-u) w' = (0.9 - 1000 (w-y) - 0.5 w(w-u)) / x x' = 0.5 (w-u) y' = -100 (y-w) Appendix B. Subroutines Used in This Thesis and the boundary conditions: u(0) = v(0) = w(0) = 1 x(O) = -10 w(l) = y(l) As an initial guess for the solution we take: u(t) = 1 ; v(t) = 1 ; x(t) = -10 ; w(t) = -4.5 t*t + 8.91 t + 1 ; y(t) = -4.5 t*t + 9 t + 0.91 The next program computes and prints the solution for t=0, 0.1,0.2,...,1. This program has been run on a Olivetti M24 PC, operating under MS-DOS V2.ll, using the Olivetti MS-Fortran V3.13 R1.0 compiler and the MS Object Linker V2.01 (large). IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION ER(5),TI(12),X(5,12),Q(5,5,12),U(15,12),D(5,12), 1 PHIREC(15,12),W(315),WGR(20) INTEGER IU(27) EXTERNAL FDIF,YOT,G C C SETTING OF THE INPUT PARAMETERS C N = 5 NU = 15 NTI = 12 LW = 315 LIW = 27 ER(3) = 1.1D-15 LWG = 20 ER(1) = l.D-6 ER(2) = l.D-2 A = O.DO B = 1.D0 NRTI = 10 AMP = 100 ITLIM = 20 CALL MUSN(FDIF,YOT,G,N,A,B,ER,TI,NTI,NRTI,AMP,ITLIM,X,Q,U,NU,D, 1 PHIREC,KPART,W,LW,IW,LIW,WGR,LWG,IERROR) END SUBROUTINE FDIF(T,Y,F) c C IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION Y(5),F(5) C Y3MY1 = Y(3) - Y(l) Y3MY5 = Y(3) - Y(5) F(l) = 0.5D0 * Y(l) * Y3MY1 / Y(2) F(2) = - 0.5D0 * Y3MY1 F(3) = (0.9D0 - 1.D3 * Y3MY5 - 0.5D0 * Y(3) * Y3MY1) / Y(4) F(4) = 0.5D0 * Y3MY1 F(5) = 1.D2 * Y3MY5 RETURN C END OF FDIF END SUBROUTINE YOT(T.X) Appendix B. Subroutines Used in This Thesis Q c IMPLICIT.DOUBLE PRECISION (A-H.O-Z) DIMENSION X(5) C X(l) = l.DO X(2) = l.DO X(4) = -10.DO X(3) = - 4.5D0*T*T + 8.91D0 * T + l.DO X(5) = - 4.5D0*T*T + 9.DO * T + 0.91D0 RETURN C END OF YOT END SUBROUTINE G(N,XA,XB,FG,DGA,DGB) C IMPLICIT DOUBLE PRECISION (A-H.O-Z) DIMENSION XA(N),XB(N),FG(N),DGA(N,N),DGB(N,N) C DO 1100 I = 1 , N DO 1100 J = 1 , N DGA(I.J) = O.DO DGB(I.J) = O.DO 1100 CONTINUE DGA(1,1) = 1-DO DGA(2,2) = l.DO DGA(3,3) = l.DO DGA(4,4) = l.DO DGB(5,3) = l.DO DGB(5,5) = -l.DO FG(1) = XA(1) - l.DO FG(2) = XA(2) - l.DO FG(3) = XA(3) - l.DO FG(4) = XA(4) + 10.DO FG(5) = XB(3) - XB(5) RETURN C END OF G END B.2 Subroutine DDASS for Solving Nonlinear IVP C This is FORTRAN subroutine used to solve ordinary differential equations C with initial conditions. C It is written in FORTRAN 77 and is available through Netlib repository C (http://www.netlib.org/). C It can direct use in the SUN station. When use PC change the C subroutine "dlmach" at the end of the program. SUBROUTINE DDASSL (RES, NEQ, T, Y, YPRIME, TOUT, INFO, RTOL, ATOL, + IDID, RWORK, LRW, IWORK, LIW, RPAR, IPAR, JAC) C***BEGIN PROLOGUE DDASSL C***PURPOSE This code solves a system of differential/algebraic C equations of the form G(T,Y,YPRIME) = 0. C***TYPE DOUBLE PRECISION (SDASSL-S, DDASSL-D) C**KEYWORDS DIFFERENTIAL/ALGEBRAIC, BACKWARD DIFFERENTIATION FORMULAS, C IMPLICIT DIFFERENTIAL SYSTEMS C***DESCRIPTION C***ROUTINES CALLED D1MACH, DDAINI, DDANRM, DDASTP, DDATRP, DDAWTS, C XERMSG Appendix B. Subroutines Used in This Thesis •Usage: EXTERNAL RES, JAC INTEGER NEQ, INFO(N), IDID, LRW, LIW, IWORK(LIW), IPAR DOUBLE PRECISION T, Y(NEQ), YPRIME(NEQ), TOUT, RTOL, ATOL, * RWORK(LRW), RPAR CALL DDASSL (RES, NEQ, T, Y, YPRIME, TOUT, INFO, RTOL, ATOL, * IDID, RWORK, LRW, IWORK, LIW, RPAR, IPAR, JAC) *Arguments: (In the following, all real arrays should be type DOUBLE PRECISION.) RES:EXT This is a subroutine which you provide to define the differential/algebraic system. NEQ:IN This is the number of equations to be solved. T:IN0UT This is the current value of the independent variable. Y(*):IN0UT This array contains the solution components at T. YPRIME(*):INOUT This array contains the derivatives of the solution components at T. TOUT:IN This is a point at which a solution is desired. INFO(N):IN The basic task of the code is to solve the system from T to TOUT and return an answer at TOUT. INFO is an integer array which is used to communicate exactly how you want this task to be carried out. (See below for details.) N must be greater than or equal to 15. RTOL,ATOL:INOUT These quantities represent relative and absolute error tolerances which you provide to indicate how accurately you wish the solution to be computed. You may choose them to be both scalars or else both vectors. Caution: In Fortran 77, a scalar is not the same as an array of length 1. Some compilers may object to using scalars for RTOL,ATOL. IDID:OUT This scalar quantity is an indicator reporting what the code did. You must monitor this integer variable to decide what action to take next. RWORK:WORK A real work array of length LRW which provides the code with needed storage space. LRW:IN The length of RWORK. (See below for required length.) IWORK:WORK An integer work array of length LIW which probides the code with needed storage space. LIW:IN The length of IWORK. (See below for required length.) RPAR,IPAR:IN These are real and integer parameter arrays which you can use for communication between your calling program and the RES subroutine (and the JAC subroutine) JAC:EXT This is the name of a subroutine which you may choose to provide for defining a matrix of partial derivatives Appendix B. Subroutines Used in This Thesis 169 C described below. C C Quantities which may be altered by DDASSL are: C T, Y(*), YPRIME(*), INFO(l), RTOL, ATOL, C IDID, RW0RK(*) AND IWORK(*) C C *Description C C Subroutine DDASSL uses the backward differentiation formulas of C orders one through five to solve a system of the above form for Y and C YPRIME. Values for Y and YPRIME at the initial time must be given as C input. These values must be consistent, (that is, if T,Y,YPRIME are C the given initial values, they must satisfy G(T,Y,YPRIME) = 0 . ) . The C subroutine solves the system from T to TOUT. It is easy to continue C the solution to get results at additional TOUT. This is the interval C mode of operation. Intermediate results can also be obtained easily C by using the intermediate-output capability. C C The following detailed description is divided into subsections: C 1. Input required for the first call to DDASSL. C 2. Output after any return from DDASSL. C 3. What to do to continue the integration. C 4. Error messages. C C C INPUT — WHAT TO DO ON THE FIRST CALL TO DDASSL C C The first call of the code is defined to be the start of each new C problem. Read through the descriptions of all the following items, C provide sufficient storage space for designated arrays, set C appropriate variables for the initialization of the problem, and C give information about how you want the problem to be solved. C C C RES — Provide a subroutine of the form C SUBROUTINE RES(T,Y,YPRIME,DELTA,IRES,RPAR,IPAR) C to define the system of differential/algebraic C equations which is to be solved. For the given values C of T,Y and YPRIME, the subroutine should C return the residual of the defferential/algebraic C system C DELTA = G(T,Y,YPRIME) C (DELTA(*) is a vector of length NEQ which is C output for RES.) C C Subroutine RES must not alter T,Y or YPRIME. C You must declare the name RES in an external C statement in your program that calls DDASSL. C You must dimension Y,YPRIME and DELTA in RES. C C IRES is an integer flag which is always equal to C zero on input. Subroutine RES should alter IRES C only if it encounters an illegal value of Y or C a stop condition. Set IRES = -1 if an input value C is illegal, and DDASSL will try to solve the problem C without getting IRES = -1. If IRES = -2, DDASSL C will return control to the calling program C with IDID = -11. C C RPAR and IPAR are real and integer parameter arrays which C you can use for communication between your calling program C and subroutine RES. They are not altered by DDASSL. If you Appendix B. Subroutines Used in This Thesis 170 C do not need RPAR or IPAR, ignore these parameters by treat-C ing them as dummy arguments. If you do choose to use them, C dimension them in your calling program and in RES as arrays C of appropriate length. C C NEQ — Set it to the number of differential equations. C (NEQ .GE. 1) C C T — Set it to the initial point of the integration. C T must be defined as a variable. C C Y(*) — Set this vector to the initial values of the NEQ solution C components at the initial point. You must dimension Y of C length at least NEQ in your calling program. C C YPRIME(*) — Set this vector to the initial values of the NEQ C first derivatives of the solution components at the initial C point. You must dimension YPRIME at least NEQ in your C calling program. If you do not know initial values of some C of the solution components, see the explanation of INFO(ll). C C TOUT — Set it to the first point at which a solution C is desired. You can not take TOUT = T. C integration either forward in T (TOUT .GT. T) or C backward in T (TOUT .LT. T) is permitted. C C The code advances the solution from T to TOUT using C step sizes which are automatically selected so as to C achieve the desired accuracy. If you wish, the code will C return with the solution and its derivative at C intermediate steps (intermediate-output mode) so that C you can monitor them, but you still must provide TOUT in C accord with the basic aim of the code. C C The first step taken by the code is a critical one C because it must reflect how fast the solution changes near C the initial point. The code automatically selects an C initial step size which is practically always suitable for C the problem. By using the fact that the code will not step C past TOUT in the first step, you could, if necessary, C restrict the length of the initial step size. C C For some problems it may not be permissible to integrate C past a point TSTOP because a discontinuity occurs there C or the solution or its derivative is not defined beyond C TSTOP. When you have declared a TSTOP point (SEE INFO(4) C and RWORK(D), you have told the code not to integrate C past TSTOP. In this case any TOUT beyond TSTOP is invalid C input. C C INF0(*) — Use the INFO array to give the code more details about C how you want your problem solved. This array should be C dimensioned of length 15, though DDASSL uses only the first C eleven entries. You must respond to all of the following C items, which are arranged as questions. The simplest use C of the code corresponds to answering all questions as yes, C i.e. setting all entries of INFO to 0. C C INF0(1) - This parameter enables the code to initialize C itself. You must set it to indicate the start of every C new problem. C Appendix B. Subroutines Used in This Thesis 171 C **** Is this the first call for this problem ... C Yes - Set INFO(l) = 0 C No - Not applicable here. C See below for continuation calls. **** C C INFO(2) - How much accuracy you want of your solution C is specified by the error tolerances RTOL and ATOL. C The simplest use is to take them both to be scalars. C To obtain more flexibility, they can both be vectors. C The code must be told your choice. C C **** Are both error tolerances RTOL, ATOL scalars ... C Yes - Set INF0(2) = 0 C and input scalars for both RTOL and ATOL C No - Set INFO(2) = 1 C and input arrays for both RTOL and ATOL **** C C INFO(3) - The code integrates from T in the direction C of TOUT by steps. If you wish, it will return the C computed solution and derivative at the next C intermediate step (the intermediate-output mode) or C TOUT, whichever comes first. This is a good way to C proceed if you want to see the behavior of the solution. C If you must have solutions at a great many specific C TOUT points, this code will compute them efficiently. C C **** Do you want the solution only at C TOUT (and not at the next intermediate step) ... C Yes - Set INF0(3) = 0 C No - Set INF0(3) = 1 **** C C INF0(4) - To handle solutions at a great many specific C values TOUT efficiently, this code may integrate past C TOUT and interpolate to obtain the result at TOUT. C Sometimes it is not possible to integrate beyond some C point TSTOP because the equation changes there or it is C not defined past TSTOP. Then you must tell the code C not to go past. C C **** Can the integration be carried out without any C restrictions on the independent variable T ... C Yes - Set INF0(4)=O C No - Set INF0(4)=1 C and define the stopping point TSTOP by C setting RWORK(1)=TSTOP **** C C INF0(5) - To solve differential/algebraic problems it is C necessary to use a matrix of partial derivatives of the C system of differential equations. If you do not C provide a subroutine to evaluate it analytically (see C description of the item JAC in the call list), it will C be approximated by numerical differencing in this code. C although it is less trouble for you to have the code C compute partial derivatives by numerical differencing, C the solution will be more reliable if you provide the C derivatives via JAC. Sometimes numerical differencing C is cheaper than evaluating derivatives in JAC and C sometimes it is not - this depends on your problem. C C **** Do you want the code to evaluate the partial C derivatives automatically by numerical differences ... C Yes - Set INFO(5)=0 Appendix B. Subroutines Used in This Thesis 172 C No - Set INF0(5)=1 C and provide subroutine JAC for evaluating the C matrix of partial derivatives **** C C. INFO(6) - DDASSL will perform much better if the matrix of C partial derivatives, DG/DY + CJ*DG/DYPRIME, C (here CJ is a scalar determined by DDASSL) C is banded and the code is told this. In this C case, the storage needed will be greatly reduced, C numerical differencing will be performed much cheaper, C and a number of important algorithms will execute much C faster. The differential equation is said to have C half-bandwidths ML (lower) and MU (upper) if equation i C involves only unknowns Y(J) with C I-ML .LE. J .LE. I+MU C for all 1=1,2 NEQ. Thus, ML and MU are the widths C of the lower and upper parts of the band, respectively, C with the main diagonal being excluded. If you do not C indicate that the equation has a banded matrix of partial C derivatives, the code works with a full matrix of NEQ**2 C elements (stored in the conventional way). Computations C with banded matrices cost less time and storage than with C full matrices if 2*ML+MU .LT. NEQ. If you tell the C code that the matrix of partial derivatives has a banded C structure and you want to provide subroutine JAC to C compute the partial derivatives, then you must be careful C to store the elements of the matrix in the special form C indicated in the description of JAC. C C **** Do you want to solve the problem using a full C (dense) matrix (and not a special banded C structure) ... C Yes - Set INFO(6)=0 C No - Set INF0(6)=1 C and provide the lower (ML) and upper (MU) C bandwidths by setting C IW0RK(1)=ML C IW0RK(2)=MU **** C C C INFO(7) — You can specify a maximum (absolute value of) C stepsize, so that the code C will avoid passing over very C large regions. C C **** Do you want the code to decide C on its own maximum stepsize? C Yes - Set INFO(7)=0 C No - Set INFO(7)=1 C and define HMAX by setting C RW0RK(2)=HMAX **** C C INFO(8) — Differential/algebraic problems C may occaisionally suffer from C severe scaling difficulties on the C first step. If you know a great deal C about the scaling of your problem, you can C help to alleviate this problem by C specifying an initial stepsize HO. C C **** Do you want the code to define C its own initial stepsize? Appendix B. Subroutines Used in This Thesis C Yes - Set INFO(8)=0 C No - Set INF0(8)=1 C and define HO by setting C RW0RK(3)=H0 **** C C INFO(9) — If storage is a severe problem, C you can save some locations by C restricting the maximum order MAXORD. C the default value is 5. for each C order decrease below 5, the code C requires NEQ fewer locations, however C it is likely to be slower. In any C case, you must have 1 .LE. MAXORD .LE. 5 C **** Do you want the maximum order to C default to 5? C Yes - Set INFO(9)=0 C No - Set INFO(9)=1 C and define MAXORD by setting C IW0RK(3)=MAX0RD **** C C INFO(IO) —If you know that the solutions to your equations C will always be nonnegative, it may help to set this C parameter. However, it is probably best to C try the code without using this option first, C and only to use this option if that doesn't C work very well. C **** Do you want the code to solve the problem without C invoking any special nonnegativity constraints? C Yes - Set INF0(10)=0 C No - Set INF0(10)=1 C C INFO(ll) —DDASSL normally requires the initial T, C Y, and YPRIME to be consistent. That is, C you must have G(T,Y,YPRIME) = 0 at the initial C time. If you do not know the initial C derivative precisely, you can let DDASSL try C to compute it. C **** Are the initialHE INITIAL T, Y, YPRIME consistent? C Yes - Set INFO(ll) = 0 C No - Set INFO(ll) = 1, C and set YPRIME to an initial approximation C to YPRIME. (If you have no idea what C YPRIME should be, set it to zero. Note C that the initial Y should be such C that there must exist a YPRIME so that C G(T,Y,YPRIME) = 0.) C C RTOL, ATOL — You must assign relative (RTOL) and absolute (ATOL C error tolerances to tell the code how accurately you C want the solution to be computed. They must be defined C as variables because the code may change them. You C have two choices — C Both RTOL and ATOL are scalars. (INF0(2)=O) C Both RTOL and ATOL are vectors. (INF0(2)=1) C in either case all components must be non-negative. C C The tolerances are used by the code in a local error C test at each step which requires roughly that C ' ABS(LOCAL ERROR) .LE. RT0L*ABS(Y)+AT0L C for each vector component. C (More specifically, a root-mean-square norm is used to C measure the size of vectors, and the error test uses the Appendix B. Subroutines Used in This Thesis 174 C magnitude of the solution at the beginning of the step.) C C The true (global) error is the difference between the C true solution of the initial value problem and the C computed approximation. Practically all present day C codes, including this one, control the local error at C each step and do not even attempt to control the global C error directly. C Usually, but not always, the true accuracy of the C computed Y is comparable to the error tolerances. This C code will usually, but not always, deliver a more C accurate solution if you reduce the tolerances and C integrate again. By comparing two such solutions you C can get a fairly reliable idea of the true error in the C solution at the bigger tolerances. C C Setting AT0L=0. results in a pure relative error test on C that component. Setting RT0L=0. results in a pure C absolute error test on that component. A mixed test C with non-zero RTOL and ATQL corresponds roughly to a C relative error test when the solution component is much C bigger than ATOL and to an absolute error test when the C solution component is smaller than the threshhold ATOL. C C The code will not attempt to compute a solution at an C accuracy unreasonable for the machine being used. It will C advise you if you ask for too much accuracy and inform C you as to the maximum accuracy it believes possible. C C RW0RK(*) — Dimension this real work array of length LRW in your C calling program. C C LRW — Set it to the declared length of the RWORK array. C You must have C LRW .GE. 40+(HAXORD+4)*NEQ+NEQ*2 C for the full (dense) JACOBIAN case (when INF0(6)=0), or C LRW .GE. 40+(MAX0RD+4)*NEQ+(2*ML+MU+l)*NEq C for the banded user-defined JACOBIAN case C (when INF0(5)=1 and INF0(6)=1), or C LRW .GE. 40+(MAX0RD+4)*NEQ+(2*ML+MU+l)*NEQ C +2*(NEQ/(ML+MU+1)+1) C for the banded finite-difference-generated JACOBIAN case C (when INF0(5)=0 and INF0(6)=1) C C IW0RK(*) — Dimension this integer work array of length LIW in C your calling program. C C LIW — Set it to the declared length of the IWORK array. C You must have LIW .GE. 20+NEQ C C RPAR, IPAR — These are parameter arrays, of real and integer C type, respectively. You can use them for communication C between your program that calls DDASSL and the C RES subroutine (and the JAC subroutine). They are not C altered by DDASSL. If you do not need RPAR or IPAR, C ignore these parameters by treating them as dummy C arguments. If you do choose to use them, dimension C them in your calling program and in RES (and in JAC) C as arrays of appropriate length. C C JAC — If you have set INF0(5)=0, you can ignore this parameter C by treating it as a dummy argument. Otherwise, you must Appendix B. Subroutines Used in This Thesis 175 C provide a subroutine of the form C SUBROUTINE JAC(T,Y,YPRIME,PD.CJ,RPAR,IPAR) C to define the matrix of partial derivatives C PD=DG/DY+CJ*DG/DYPRIME C CJ is a scalar which is input to JAC. C For the given values of T,Y,YPRIME, the C subroutine must evaluate the non-zero partial C derivatives for each equation and each solution C component, and store these values in the C matrix PD. The elements of PD are set to zero C before each call to JAC so only non-zero elements C need to be defined. C C Subroutine JAC must not alter T,Y,(*),YPRIMEC*), or CJ. C You must declare the name JAC in an EXTERNAL statement in C your program that calls DDASSL. You must dimension Y, C YPRIME and PD in JAC. C C The way you must store the elements into the PD matrix C depends on the structure of the matrix which you C indicated by INFO(6). C *** INFO(6)=0 — Full (dense) matrix *** C Give PD a first dimension of NEQ. C When you evaluate the (non-zero) partial derivative C of equation I with respect to variable J, you must C store it in PD according to C PD(I,J) = "DG(I)/DY(J)+CJ*DG(I)/DYPRIME(J)" C *** INFO(6)=1 — Banded JACOBIAN with ML lower and MU C upper diagonal bands (refer to INFO(6) description C of ML and MU) *** C Give PD a first dimension of 2*ML+MU+1. C when you evaluate the (non-zero) partial derivative C of equation I with respect to variable J, you must C store it in PD according to C IROW =I-J+ML+MU+1 C PD(IR0W,J) = "DG(I)/DY(J)+CJ*DG(I)/DYPRIME(J)" C C RPAR and IPAR are real and integer parameter arrays C which you can use for communication between your calling C program and your JACOBIAN subroutine JAC. They are not C altered by DDASSL. If you do not need RPAR or IPAR, C ignore these parameters by treating them as dummy C arguments. If you do choose to use them, dimension C them in your calling program and in JAC as arrays of C appropriate length. C C C OPTIONALLY REPLACEABLE NORM ROUTINE: C C DDASSL uses a weighted norm DDANRM to measure the size C of vectors such as the estimated error in each step. C A FUNCTION subprogram C DOUBLE PRECISION FUNCTION DDANRM(NEQ,V,WT,RPAR,IPAR) C DIMENSION V(NEQ),WT(NEQ) C is used to define this norm. Here, V is the vector C whose norm is to be computed, and WT is a vector of C weights. A DDANRM routine has been included with DDASSL C which computes the weighted root-mean-square norm C given by C DDANRM=SQRT((1/NEQ)*SUM(V(I)/WT(I))**2) C this norm is suitable for most problems. In some C special cases, it may be more convenient and/or Appendix B. Subroutines Used in This Thesis C efficient to define your own norm by writing a function C subprogram to be called instead of DDANRM. This should, C however, be attempted only after careful thought and C consideration. C C C OUTPUT — AFTER ANY RETURN FROM DDASSL ' C C The principal aim of the code is to return a computed solution at C TOUT, although it is also possible to obtain intermediate results C along the way. To find out whether the code achieved its goal C or if the integration process was interrupted before the task was C completed, you must check the IDID parameter. C C C T — The solution was successfully advanced to the C output value of T. C C Y(*) — Contains the computed solution approximation at T. C C YPRIME(*) — Contains the computed derivative C approximation at T. C C IDID — Reports what the code did. C C *** Task completed *** C Reported by positive values of IDID C C IDID =1 — A step was successfully taken in the C intermediate-output mode. The code has not C yet reached TOUT. C C IDID =2 — The integration to TSTOP was successfully C completed (T=TST0P) by stepping exactly to TSTOP. C C IDID =3 — The integration to TOUT was successfully C completed (T=TOUT) by stepping past TOUT. C Y(*) is obtained by interpolation. C YPRIMEO) is obtained by interpolation. C C *** Task interrupted *** C Reported by negative values of IDID C C IDID = -1 — A large amount of work has been expended. C (About 500 steps) C C IDID = -2 — The error tolerances are too stringent. C C IDID = -3 — The local error test cannot be satisfied C because you specified a zero component in ATOL C and the corresponding computed solution C component is zero. Thus, a pure relative error C test is impossible for this component. C C IDID = -6 — DDASSL had repeated error test C failures on the last attempted step. C C IDID = -7 — The corrector could not converge. C C IDID = -8 — The matrix of partial derivatives C is singular. C Appendix B. Subroutines Used in This Thesis 177 C IDID = -9 — The corrector could not converge. C there were repeated error test failures C in this step. C C IDID =-10 -- The corrector could not converge C because IRES was equal to minus one. C C IDID =-11 — IRES equal to -2 was encountered C and control is being returned to the C calling program. C C IDID =-12 — DDASSL failed to compute the initial C YPRIME. C C C C IDID = -13,..,-32 — Not applicable for this code C C *** Task terminated *** C Reported by the value of IDID=-33 C C IDID = -33 — The code has encountered trouble from which C it cannot recover. A message is printed C explaining the trouble and control is returned C to the calling program. For example, this occurs C when invalid input is detected. C C RTOL, ATOL — These quantities remain unchanged except when C IDID = -2. In this case, the error tolerances have been C increased by the code to values which are estimated to C be appropriate for continuing the integration. However, C the reported solution at T was obtained using the input C values of RTOL and ATOL. C C RWORK, IUORK — Contain information which is usually of no C interest to the user but necessary for subsequent calls. C However, you may find use for C C RW0RK(3)—Which contains the step size H to be C attempted on the next step. C C RW0RK(4)—Which contains the current value of the C independent variable, i.e., the farthest point C integration has reached. This will be different C from T only when interpolation has been C performed (IDID=3). C C RW0RK(7)—Which contains the stepsize used C on the last successful step. C C IW0RK(7)—Which contains the order of the method to C be attempted on the next step. C C IW0RK(8)—Which contains the order of the method used C on the last step. C C IWORK(l)—Which contains the number of steps taken so C far. C C IW0RKC12)— Which contains the number of calls to RES C so far. C Appendix B. Subroutines Used in This Thesis 178 C IW0RK(13)—Which contains the number of evaluations of C the matrix of partial derivatives needed so C far. C C IW0RK(14)—Which contains the total number C of error test failures so far. C C IW0RK(15)—Which contains the total number C of convergence test failures so far. C (includes singular iteration matrix C failures.) C C C INPUT — WHAT TO DO TO CONTINUE THE INTEGRATION C (CALLS AFTER THE FIRST) C C This code is organized so that subsequent calls to continue the C integration involve little (if any) additional effort on your C part. You must monitor the IDID parameter in order to determine C what to do next. C C Recalling that the principal task of the code is to integrate C from T to TOUT (the interval mode), usually all you will need C to do is specify a new TOUT upon reaching the current TOUT. C C Do not alter any quantity not specifically permitted below, C in particular do not alter NEQ,T,Y(*),YPRIME(*),RW0RK(*),IW0RK(*) C or the differential equation in subroutine RES. Any such C alteration constitutes a new problem and must be treated as such, C i.e., you must start afresh. C C You cannot change from vector to scalar error control or vice C versa (INFO(2)), but you can change the size of the entries of C RTOL, ATOL. Increasing a tolerance makes the equation easier C to integrate. Decreasing a tolerance will make the equation C harder to integrate and should generally be avoided. C C You can switch from the intermediate-output mode to the C interval mode (INFO(3)) or vice versa at any time. C C If it has been necessary to prevent the integration from going C past a point TSTOP (INF0(4), RWORK(D), keep in mind that the C code will not integrate to any TOUT beyond the currently C specified TSTOP. Once TSTOP has been reached you must change C the value of TSTOP or set INFO(4)=0. You may change INFO(4) C or TSTOP at any time but you must supply the value of TSTOP in C RWORK(l) whenever you set INF0(4)=1. C C Do not change INF0(5), INF0(6), IWORK(1), or IW0RK(2) C unless you are going to restart the code. C C *** Following a completed task *** C If C IDID = 1, call the code again to continue the integration C another step in the direction of TOUT. C C IDID = 2 or 3, define a new TOUT and call the code again. C TOUT must be different from T. You cannot change C the direction of integration without restarting. C C *** Following an interrupted task *** C To show the code that you realize the task was Appendix B. Subroutines Used in This Thesis 179 C interrupted and that you want to continue, you C must take appropriate action and set INF0(1) = 1 C If C IDID = -1, The code has taken about 500 steps. C If you want to continue, set INF0(1) = 1 and C call the code again. An additional 500 steps C will be allowed. C C IDID = -2, The error tolerances RTOL, ATOL have been C increased to values the code estimates appropriate C for continuing. You may want to change them C yourself. If you are sure you want to continue C with relaxed error tolerances, set INF0(1)=1 and C call the code again. C C IDID = -3, A solution component is zero and you set the C corresponding component of ATOL to zero. If you C are sure you want to continue, you must first C alter the error criterion to use positive values C for those components of ATOL corresponding to zero C solution components, then set INF0(1)=1 and call C the code again. C C IDID = -4,-5 Cannot occur with this code. C C IDID = -6, Repeated error test failures occurred on the C last attempted step in DDASSL. A singularity in the C solution may be present. If you are absolutely C certain you want to continue, you should restart C the integration. (Provide initial values of Y and C YPRIME which are consistent) C C IDID = -7, Repeated convergence test failures occurred C on the last attempted step in DDASSL. An inaccurate C or ill-conditioned JACOBIAN may be the problem. If C you are absolutely certain you want to continue, you C should restart the integration. C C IDID = -8, The matrix of partial derivatives is singular. C Some of your equations may be redundant. C DDASSL cannot solve the problem as stated. C It is possible that the redundant equations C could be removed, and then DDASSL could C solve the problem. It is also possible C that a solution to your problem either C does not exist or is not unique. C C IDID = -9, DDASSL had multiple convergence test C failures, preceeded by multiple error C test failures, on the last attempted step. C It is possible that your problem C is ill-posed, and cannot be solved C using this code. Or, there may be a C discontinuity or a singularity in the C solution. If you are absolutely certain C you want to continue, you should restart C the integration. C C IDID =-10, DDASSL had multiple convergence test failures C because IRES was equal to minus one. C If you are absolutely certain you want C to continue, you should restart the Appendix B. Subroutines Used in This Thesis 180 C integration. C C IDID =-11, IRES=-2 was encountered, and control is being C returned to the calling program. C C IDID =-12, DDASSL failed to compute the initial YPRIME. C This could happen because the initial C approximation to YPRIME was not very good, or C if a YPRIME consistent with the initial Y C does not exist. The problem could also be caused C by an inaccurate or singular iteration matrix. C C IDID = -13,..,-32 Cannot occur with this code. C C C *** Following a terminated task *** C C If IDID= -33, you cannot continue the solution of this problem. C An attempt to do so will result in your C run being terminated. C C C ERROR MESSAGES C C The SLATEC error print routine XERMSG is called in the event of C unsuccessful completion of a task. Most of these are treated as C "recoverable errors", which means that (unless the user has directed C otherwise) control will be returned to the calling program for C possible action after the message has been printed. C C In the event of a negative value of IDID other than -33, an appro-C priate message is printed and the "error number" printed by XERMSG C is the value of IDID. There are quite a number of illegal input C errors that can lead to a returned value IDID=-33. The conditions C and their printed "error numbers" are as follows: C C Error number Condition C C 1 Some element of INFO vector is not zero or one. C 2 NEQ .le. 0 C 3 MAXORD not in range. C 4 LRW is less than the required length for RWORK. C 5 LIW is less than the required length for IWORK. C 6 Some element of RTOL is .It. 0 C 7 Some element of ATOL is .It. 0 C 8 All elements of RTOL and ATOL are zero. C 9 INF0(4)=1 and TSTOP is behind TOUT. C 10 HMAX .It. 0.0 C 11 TOUT is behind T. C 12 INF0(8)=1 and H0=0.0 C 13 Some element of WT is .le. 0.0 C 14 TOUT is too close to T to start integration. C 15 INF0(4)=1 and TSTOP is behind T. C 16 —( Not used in this version )— C 17 ML illegal. Either .It. 0 or .gt. NEQ C 18 MU illegal. Either .It. 0 or .gt. NEQ C 19 TOUT = T. C C If DDASSL is called again without any action taken to remove the C cause of an unsuccessful return, XERMSG will be called with a fatal C error flag, which will cause unconditional termination of the C program. There are two such fatal errors: Appendix B. Subroutines Used in This Thesis c C Error number -998: The last step was terminated with a negative C value of IDID other than -33, and no appropriate action was C taken. C C Error number -999: The previous call was terminated because of C illegal input (IDID=-33) and there is illegal input in the C present call, as well. (Suspect infinite loop.) C c C***END PROLOGUE DDASSL