UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

Hybrid constraint space position/force control Wong, Roger 1996

Your browser doesn't seem to have a PDF viewer, please download the PDF to view this item.

Item Metadata

Download

Media
831-ubc_1996-0440.pdf [ 8.06MB ]
Metadata
JSON: 831-1.0087230.json
JSON-LD: 831-1.0087230-ld.json
RDF/XML (Pretty): 831-1.0087230-rdf.xml
RDF/JSON: 831-1.0087230-rdf.json
Turtle: 831-1.0087230-turtle.txt
N-Triples: 831-1.0087230-rdf-ntriples.txt
Original Record: 831-1.0087230-source.json
Full Text
831-1.0087230-fulltext.txt
Citation
831-1.0087230.ris

Full Text

HYBRID CONSTRAINT SPACE POSITION/FORCE CONTROL By Roger Wong B. A . Sc. (Mechanical Engineering) University of British Columbia A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF T H E REQUIREMENTS FOR T H E D E G R E E OF M A S T E R OF APPLIED SCIENCE in T H E FACULTY OF GRADUATE STUDIES MECHANICAL ENGINEERING We accept this thesis as conforming to the required standard T H E UNIVERSITY OF BRITISH COLUMBIA June 1996 © Roger Wong, 1996 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 reference 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 MgCHArOlCAU <5 /Ofc (Kj&g^/OG FACULTY O P A P P U c 7 ] > i C i e ^ C S The University of British Columbia Vancouver, Canada DE-6 (2/88) Abstract This thesis documents the conceptual development of the author's contribution, the hy-brid constraint space position/force controller for robotic manipulator control in con-strained environments. This method is built upon a constraint space dynamic model, where the model parameters are displacement along the constraint trajectory and nor-mal force between the manipulator end-effector and environment. This dynamic model is constructed by transforming conventional joint space manipulator dynamics into their constraint space equivalents through the application of mapping functions, which relate differential displacements and velocities in the constraint space coordinate system to the joint space coordinate system. Conventional PD controllers may then be applied to the simplified dynamic structure of the constraint space equations of motion, in order to produce a vector of manipulator joint torques which will satisfy both position and force requirements along the environmental constraint. Actuator constraints and momentum compensating techniques are also used to ensure that the position and force control problems are completely decoupled from one another. This modelling technique is then applied to the control problem for a two degree of freedom prismatic robot as an illus-trative example. Simulation of this specific controller is carried out with respect to three different constraint surfaces, a planar, a concave circular and a convex circular environ-ment. The results of these simulations show that the hybrid constraint space controller provides excellent position and force trajectory tracking for the planar case study. This thesis is intended to be the forerunner to future work which will develop in detail, the application of hybrid constraint space control to highly nonlinear manipulator/constraint models. u Table of Contents Abstract ii List of Figures vi List of Tables ix Nomenclature x Acknowledgements xii 1 Introduction 1 1.1 Opening Remarks 1 1.2 Motivation 2 1.3 Contributions of the Thesis • • • 3 1.4 Outline of the Thesis 4 2 Literature Review 5 2.1 Stiffness Control 6 2.2 Hybrid Position/Force Control . 8 2.3 Hybrid Impedance Control 11 2.4 Reduced State Position/Force Control 12 2.5 Summary 14 3 Hybrid Constraint Space (HCS) Control 16 3.1 Definition of a Constraint Space Coordinate System 19 i i i 3.2 Derivation of the Conventional Joint Space Dynamics 20 3.3 Derivation of Mapping Functions 21 3.4 Formulation of the Constraint Space Dynamics 22 3.5 Actuator Constraints Required for Maintaining End-Effector Trajectory . 24 3.6 Derivation of a Position Control Law 28 3.7 Derivation of a Force Control Law 29 3.8 Superposition 30 4 Derivation of an HCS Controller for a 2DOF Prismatic Robot 31 4.1 Definition of a Constraint Space Coordinate System 31 4.2 Derivation of the Conventional Joint Space Dynamics . . . 32 4.3 Derivation of Mapping Functions 33 4.4 Formulation of the Constraint Space Dynamics . 34 4.5 Actuator Constraints Required for Maintaining End-Effector Trajectory . 36 4.6 Derivation of a Position Control Law 38 4.7 Derivation of a Force Control Law 39 4.8 Superposition 41 5 Simulation Results for a 2DOF Prismatic Robot with HCS Control 42 5.1 Planar Constraint Surface 42 5.2 Concave Circular Constraint Surface 46 5.3 Convex Circular Constraint Surface • • • 52 5.4 Comparative Discussion 59 6 Conclusions & Summary 61 6.1 Conclusions . . . 61 6.2 Summary 61 i v 6.3 Future Work 62 Bibliography 63 Appendix A Software Coding for Simulations Performed in Matlab 65 v List of Figures 1.1 Research Tree in the Field of Robotics 2 2.2 Stiffness Control:SDOF Manipulator 6 2.3 Hybrid Position/Force Control: 2DOF Manipulator 9 3.4 Procedural Flow Chart for a Hybrid Constraint Space Controller 17 3.5 General Constraint Space Coordinate System 19 3.6 nDOF Manipulator Constrained to Arbitrary Surface 20 3.7 Generalized Constraint Forces Qs and Qn 25 3.8 Momentum Compensation for Curved Surfaces 27 4.9 2DOF Planar Robot with Arbitrary Constraint 32 5.10 Planar Constraint Surface . . : 43 5.11 Task Space Simulation Results for the 2DOF Prismatic Robot Constrained to a Planar Constraint Surface with Position Control 44 5.12 Constraint Space Simulation Results for the 2DOF Prismatic Robot Con-strained to a Planar Constraint Surface with Position Control 45 5.13 Contact Force Simulation Results for the 2DOF Prismatic Robot Con-strained to a Planar Constraint Surface with Position Control 45 5.14 Task Space Simulation Results for the 2DOF Prismatic Robot Constrained to a Planar Constraint Surface with Position/Force Control 47 5.15 Constraint Space Simulation Results for the 2DOF Prismatic Robot Con-strained to a Planar Constraint Surface with Position/Force Control . . . 47 vi 5.16 Contact Force Simulation Results for the 2DOF Prismatic Robot Con-strained to a Planar Constraint Surface with Position/Force Control . . . 48 5.17 Concave Circular Constraint Surface 49 5.18 Task Space Simulation Results for the 2DOF Prismatic Robot Constrained to a Concave Circular Constraint Surface with Position Control 50 5.19 Constraint Space Simulation Results for the 2DOF Prismatic Robot Con-strained to a Concave Circular Constraint Surface with Position Control 51 5.20 Contact Force Simulation Results for the 2DQF Prismatic Robot Con-strained to a Concave Circular Constraint Surface with Position Control 51 5.21 Task Space Simulation Results for the 2DOF Prismatic Robot Constrained to a Concave Circular Constraint Surface with Position/Force Control . . 52 5.22 Constraint Space Simulation Results for the 2DOF Prismatic Robot Con-strained to a Concave Circular Constraint Surface with Position/Force Control . . . 53 5.23 Contact Force Simulation Results for the 2DOF Prismatic Robot Con-strained to a Concave Circular Constraint Surface with Position/Force Control 53 5.24 Convex Circular Constraint Surface 54 5.25 Task Space Simulation Results for the 2DOF Prismatic Robot Constrained to a Convex Circular Constraint Surface with Position Control 55 5.26 Constraint Space Simulation Results for the 2DOF Prismatic Robot Con-strained to a Convex Circular Constraint Surface with Position Control . 56 5.27 Contact Force Simulation Results for the 2DOF Prismatic Robot Con-strained to a Convex Circular Constraint Surface with Position Control . 56 5.28 Task Space Simulation Results for the 2DOF Prismatic Robot Constrained to a Convex Circular Constraint Surface with Position/Force Control . . 57 vii 5.29 Constraint Space Simulation Results for the 2DOF Prismatic Robot Con-strained to a Convex Circular Constraint Surface with Position/Force Con-trol 58 5.30 Contact Force Simulation Results for the 2DOF Prismatic Robot Con-strained to a Convex Circular Constraint Surface with Position/Force Con-trol 58 vin List of Tables 5.1 Simulation Parameters for the 2DOF Prismatic Robot with HCS Control Constrained to a Planar Surface 43 5.2 Simulation Parameters for the 2DOF Prismatic Robot with HCS Control Constrained to a Concave Circular Surface 50 5.3 Simulation Parameters for the 2DOF Prismatic Robot with HCS Control Constrained to a Convex Circular Constraint Surface 54 i x Nomenclature The following symbolic convention is with respect to the author's own material contained within Chapter 3, Chapter 4 and Chapter 5. s Constraint Space Position Coordinate n Constraint Space Force Coordinate Nr Contact Force at the End-Effector Manipulator Joint Space L Manipulator Lagrangian Potential Function Q Manipulator Joint Torques M. General Mass Matrix C General Coriolis/Centripetal Matrix G General Gravity Matrix U Differential Kinematic Mapping Function 9i Geometric Mapping Function Qs Constraint Space Position Generalized Force Qn Constraint Space Force Generalized Force ms Constraint Space Equivalent Mass Term cs Constraint Space Equivalent Coriolis/Centripetal Term 9s Constraint Space Equivalent Gravity Term F Qs as a Function of Manipulator Joint Torques k Joint Space Acceleration as a Function of s and s K Joint Space Acceleration as a Function of s X Fi Mass Coefficient for ith Manipulator Joint Equation of Motion Fij Kinematic Manipulator Joint Torque Ratio Constraint Qimc Momentum Compensation for ith Manipulator Joint Torque Qikin Position Control Component of Joint Torques kpy Position Controller Velocity Gain kpp Position Controller Position Gain kfv Force Controller Velocity Gain kfp Force Controller Position Gaiii ke Environmental Stiffness fm Measured Contact Force Qinor Force Control Component of Joint Torques Hi Component of ith Joint Torque Required for Force Control Qiact Superposed Position/Force ith Joint Torque x i Acknowledgements First and foremost, I would like to recognize the guidance and support I have received from my advisor, Dr. D. B. Cherchas throughout the last two years. I would also like to thank Mai Lai and John for their patience, love, and encour-agement. To my friends and the entire department of mechanical engineering here at U . B . C . , thank you for listening. Finally, for NSERC and ASI B .C . , thank you for the financial assistance and spon-sorship which made this project possible. xn Chapter 1 Introduction 1.1 Opening Remarks Research in the area of robotics is typically driven by two concerns; the desire to ex-pand robotics applications and the desire to improve robot performance. The former usually involves interdisciplinary collaboration, and consists of automating industrial or commercial processes through the application of existing robot technology. The latter however, is primarily concerned with advances in manipulator technology itself. To-gether, the two fuel a technological revolution which has produced marvels ranging from automated assembly lines in the automotive industry to state-of-the-art robot surgeons in the operating room. It is difficult to say which of these two research groups has wagered more heavily in the flourishing growth of the technology, for both have played an integral role. With the visionary research done in the area of applications, robotics continues to justify and magnify the funding which it receives from the various segments of industry, commerce and government. On the other hand, with the innovative research performed in the area of fundamentals, robotics is constantly increasing its adaptability and suitability as a transferable technology. The only certainty is continued success for the field in the years to come. 1 Chapter 1. Introduction 2 ( ROBOTICS A RESEARCH J A^PPLICATIONS ^ F^UNDAMENTALS ^ ( PROCESS ^ f SYSTEM ^ AIECHANICALA /CONTROLLER^ ANALYSIS J \^ DESIGN J \^ DESIGN J y DESIGN J MODELLING CONTROL Figure 1.1: Research Tree in the Field of Robotics 1.2 Motivation The author's work has the singular objective of advancing robot performance through the introduction of a new manipulator controller designed especially for use in constrained environments, thereby falling within the category of robotics research dedicated to fun-damentals. A constrained environment is where the manipulator end-effector is restricted to movement along some arbitrary work space surface, a situation typically characterized by simultaneous position and force requirements. An industrial milling operation is a good example of a constrained environment. Do constrained environments warrant such attention? Absolutely. The majority of industrial or commercial processes which may benefit through the application of robotic automation usually consist of constrained environments. Common but essential tasks such as assembly, cutting or grinding all require the tool to follow a specific path on the workpiece surface while simultaneously maintaining a prescribed normal interaction Chapter 1. Introduction 3 force with that same surface. Hence, improvements in the area of constrained manipu-lator modelling and control will most certainly lead to advanced applications for robotic manipulators in even the most demanding industrial or commercial setting. 1.3 Contributions of the Thesis Conventional approaches to the problem of constrained manipulator modelling and con-trol are often devised in the joint space coordinate system of the robot in question. This joint space formulation is typically used because in actuality, the manipulator joints are the recipients of the control signals, and it is convenient to have the system dynamic model and the physical reality share a common coordinate system. However, when a manipulator is operating within a constrained environment, the joint space formulation is no longer appropriate, for two reasons. First, the joint space formulation is usually redundant. Second, the joint space formulation is inconsistent with the system control parameters, which usually consist of constraint surface variables such as displacement along the constraint surface or the normal force of interaction between the manipulator end-effector and the constraint surface. The major theoretical contribution of hybrid constraint space control (HCS control), is a conceptual approach which addresses these two issues by solving the manipulator modelling and control problem for constrained environments with a constraint space dynamic model. Chapter 1. Introduction 4 1.4 Outline of the Thesis The purpose of this thesis; is to illustrate in detail the concept of hybrid constraint space control. Towards that end, the thesis will be structured as follows. The first chap-ter hereafter, entitled Literature Review will document the conceptual origins of hybrid constraint space control by surveying past research relevant to the work. The chapter entitled Hybrid Constraint Space Control will then discuss the method in a generalized fashion, with respect to arbitrary manipulators constrained to arbitrary surfaces. Next, the chapter entitled Derivation of an HCS Controller for a 2DOF Prismatic Robot will illustrate the general concepts discussed previously with a specific example, a two degree of freedom prismatic robot. The controller devised in this chapter will then be simulated for varying constraint environments within the following chapter, entitled Simulation Re-sults for a 2DOF Prismatic Robot with HCS Control. Finally, conclusions regarding the work, and suggestions for future study will be presented through the chapter entitled Conclusions, to be followed by a bibliography of reference work and a list of supporting appendices. Chapter 2 Literature Review Hybrid constraint space control is unique because it employs a constraint space dynamic model. In other words, the model parameters deal exclusively with constraint conditions, such as end-effector displacement along the constraint surface or normal force levels between the manipulator end-effector and the environment. In hindsight, the employment of a constraint based model seems completely reasonable, since a process involving a constrained environment is almost always governed by constraint conditions. If so, why did researchers first begin with joint space formulations for their manipulator models? When the first industrial manipulators were designed, robotic automation was only being applied to positioning processes such as spray-painting and welding. Because joint space solutions to these positioning problems were readily available through inverse kine-matics, researchers came to perceive the joint space formulation as an undisputedly prac-tical procedure. Consequently, when robotic automation began its integration with force control applications, researchers persisted in applying joint space formulations. Gradually however, the preference for joint space approaches weakened as researchers began to recognize the important role played by the constraint surface in position/force control problems for constrained environments. The purpose of this literature review is to highlight the key conceptual developments of this progression, which ultimately lead to the development of hybrid constraint space control. Towards that end, this chapter 5 Chapter 2. Literature Review 6 Figure 2.2: Stiffness Control:SDOF Manipulator will include brief discussions on stiffness control, hybrid position/force control, hybrid impedance control and reduced state position/force control, as well as closing remarks regarding the influence of these methods in the author's formulation of hybrid constraint space control. 2.1 Stiffness Control The concept of stiffness control was introduced to the literature by Salisbury and Craig in 1980, Salisbury and Craig (1980), and is important because it represented a turn-ing point in the direction of robotics research at the time. Previously, researchers had been primarily concerned with the robot position control problem. Stiffness control was the first method to formally address the force control problem, and consequently, the predescessor to modern constrained environment control strategies. The concept of stiffness control may be clearly illustrated by considering the force control problem for a single degree of freedom manipulator as shown within Figure 2.2. Chapter 2. Literature Review 7 In this example, the manipulator is required to establish a contact force of /<* with a rigid environment. Assuming the presence of a simple PD control algorithm and steady-state conditions, the control force r, which is equal to the contact force, will depend only upon the position error. However, the high environmental stiffness will prevent the control force from producing any significant surface deformation. As a result, the position error will persist, and the steady-state contact force will obey the following expression. fss = kp(xd-xe) (2.1) Note that the term kp represents the controller position gain. By considering Equation 2.1 and its similarity to Hooke's Law, the concept of stiffness control may be clearly summarized. By varying the proportional control gain kp, which represents the effective stiffness of the closed-loop system, fss may be tuned to match any reasonable value of fd. This statement may be generalized to include higher order manipulators, as the principles of operation remain unchanged. Although the stiffness controller is important in that it represents the first successful attempt at robot manipulator force control, it is quite limited with respect to applicable environments. For instance, the controller is only capable of set-point force control, unless proportional control gains are altered in mid-operation, which is bad practice and cumbersome as a tool. In addition, the issue of simultaneous position/force control is not addressed by this method. In short, the stiffness controller is a pure force controller employing a joint space dynamic model. As such, its limitations rendered it inadequate for dealing with constrained environments. Chapter 2. Literature Review 8 2.2 Hybrid Position/Force Control The ability to enforce simultaneous position/force control is an important requirement for any successful manipulator controller in a constrained environment. Consequently, research efforts following the development of the stiffness controller sought exclusively to attain this goal. The concept of hybrid position/force control, introduced by Raibert and Craig in 1981, Raibert and Craig (1981), accomplished this end through task space decoupling. Raibert and Craig postulated that if the position and force control prob-lems were decoupled, then independent, dedicated controllers could be designed for each subtask. These controllers would then act in parallel, with the net effect being a system under simultaneous position/force control. In order to illustrate the operating principles of hybrid position/force control, consider the following dynamics formulation for an arbitrary n-link manipulator. T=M(q)q + C(q,q)q + G(q)+T^on(l) (2.2) Note that the vectors r , q and / represent respectively, the joint torques, the joint displacements and the end-effector contact forces. Furthermore, "the vector Zam represents contact forces at the end-effector transformed into equivalent disturbances for the joint degrees of freedom. Now, suppose it is known that a vector of desired joint trajectories and q^ will produce the desired end-effector position and contact force histories. Then, a computed torque controller for the vector of joint torques r would take the following form. . r = M ( g ) ( ^ + ^ ( ^ - £ ) + y ^ - £ ) ) + C( £ , £ )g + G ( i ) + r c o n (2.3) Note again, that the control formulation of Equation 2.3 is with respect to the joint coordinate system q. Chapter 2. Literature Review 9 Figure 2.3: Hybrid Position/Force Control: 2DOF Manipulator A hybrid position/force controller for the dynamics system of Equation 2.2 would differ from the controller of Equation 2.3 in that the hybrid controller would be based upon a decoupled task space coordinate system x as opposed to the joint coordinate system q. A decoupled task space coordinate system is one where the basis vectors of the coordinate system are aligned in directions of pure position or force control. A n example of a decoupled task space coordinate system for a two degree of freedom manipulator is shown within Figure 2.3. In order to formulate a controller in terms of the decoupled task space, it is necessary to incorporate task space components into Equation 2.2 through a transforming function from task space to joint space. x = h(q) (2.4) In addition, the first and second time derivatives of the task space basis vectors are also required. x = J(q)q x = J(q)q+J(q)q (2.5) Note that the term J(q) is the manipulator Jacobian. Substitution of Equation 2.5 into Chapter 2. Literature Review 10 Equation 2.2 will yield the following expression. T = M(q)J-\q)(x-J(q)q) + C(q4)q+G(q)+zcon (2.6) Note that Equation 2.6 may be used as the basis for another computed torque controller where the desired trajectories are now a^, x^ and x^ as opposed to q^, q^ and q^. T = M(2)J _ 1(£)(id + ^ ( i a - i ) + i P ( ^ - ^ ) - ^ ( £ ) i ) + C(q1q)i+G{q) + Tcon (2.7). Clearly, Equation 2.7 represents PD control with respect to decoupled position and force subtasks, since the vector x is a decoupled task space coordinate system. The remaining terms with respect to the joint space may be fed-forward from sensory data. However, Equation 2.7 should not be confused with a constraint space dynamic model. The dynamic model of Equation 2.6 upon which the computed torque controller of Equa-tion 2.7 is built, is actually a joint space dynamic model. Task space kinematic quan-tities are merely introduced into the joint space model so that a controller may receive task space references as input. This method operates by aligning the task space with constraint space conditions. The constraint space is not actually incorporated into the system dynamic model. As a result, complicated constraint trajectories will introduce difficulties for the method, as these constraints may no longer be aligned with a single, stationary task space coordinate system. The controller would require a position and time varying Jacobian function to relate the constantly changing task space system. Nevertheless, hybrid position/force control successfully implements simultaneous po-sition/force control for simple, constrained environments. Furthermore, by decoupling the control problem into purely position or force subtasks, the control of either is clearly isolated. Because of these qualities, hybrid position/force control, even today, remains a cornerstone for robot force control theory. Chapter 2. Literature Review 11 2.3 Hybrid Impedance Control With the introduction of hybrid position/force control as a viable solution to control problems for constrained environments, the applicability of robot manipulators to indus-try increased significantly. As a result, subsequent work in the area sought to extend the method to its Emits, focussing pn the various applications of hybrid position/force control, as opposed to other, more effective alternatives. As a result, the next major advance in robot force control theory did not appear in the literature until 1987, Hogan (1987), in the form of hybrid impedance control by Hogan. Where hybrid position/force control was built upon distinct separations between the position and force subtasks, hybrid impedance control postulated that for any given task space direction, position and force were inseparable quantities. Instead of defining a task space direction as position or force exclusive, Hogan proposed that control for each task space direction should be derived on the basis of the desired dynamic behaviour for the components of position and force in that direction. Because the control parameters were relationships between position and force, Hogan's approach earned the name impedance control. In practice, the hybrid impedance controller may often be implemented with a com-puted torque controller framework similar to that of the hybrid position/force controller as shown within Equation 2.7. Instead of using a PD controller represented by the terms X j , Xj and xj, the desired task space response is derived from inverse Laplace transforms of the desired transfer function response for position and force in that particular direction. Hence, the control laws for position and force predominant directions are respectively, as follows. r = M(gJJ-\q)(L-1{s(x4s)-Z-\s)f(s))}-J(^ Chapter 2. Literature Review 12 C ( i , i ) i + % ) + rc, •on (2.8) r = M ( £ ) J - 1 ( 2 ) ( L - 1 { ^ - 1 W ( / ^ ) - / W ) } - % ) ? ) + (2.9) The transfer functions in Equation 2.8 and Equation 2.9 are chosen by applying the duality principle, which assigns complementary transfer functions to the manipulator closed loop model depending upon the environmental impedance in that particular task space direction. Nevertheless, hybrid impedance control is also based upon a joint space dynamic model. Like hybrid position/force control, it employs a task space transformation to incorporate constraint conditions into the system dynamics. Consequently, the method also suffers from the limitations of a stationary task space when applied to complicated constraint trajectories. However, the value of hybrid impedance control, and the reason for its inclusion in this discussion, is the landmark it represents for conceptual develop-ments in the area of robot force control. Previously, little concern was given to the role of the environment in the determination of control laws for a given manipulator task. Hybrid impedance control represents the first environment sensitive control algorithm. 2.4 Reduced State Position/Force Control Following the development of hybrid impedance control, researchers began to seriously consider the role of the environment in the formulation of constrained environment control strategies. Introduced to the literature in 1988, reduced state position/force control by McClamroch and Wang, McClamroch and Wang (1988), was a novel approach to the position/force control problem, differing substantially from the joint space methodologies Chapter 2. Literature Review -13 of earlier work such as stiffness control. McClamroch and Wang postulated that when a manipulator is brought into contact with a constraint surface, its degrees of positional freedom are reduced while degrees of force freedom are introduced into the system. Consequently, the design of a controller for a given manipulator and environment configuration should be based upon a reduced control coordinate set, representing the true degrees of freedom of the manipulator and constraint pairing. However, McClamroch and Wang did not identify a procedure for isolating this reduced control coordinate set. Their work proved the existence of reduced coordinates," and made reference to them in the formulation of the following computed torque controller. ; Consider the previous n-link manipulator, and assume that a functional relationship exists between the manipulator joint space q and the reduced control coordinates z. q = k{z) (2.10) Furthermore, taking the first and second time derivatives of Equation 2.10 will yield the following expressions. -;/;- i='K(z)z q = K(z)z + K(z)z K(z),= ^ - , •• , (2,11)-yln* addition, the vector may be related to a ivector of force degrees of freedom - A, • representing the contact forces between the manipulator and the environment'. rcm = J-\z)\ (2.12) Noteithatrthe degrees of freedom.for z combined with A equal-the.original joint degrees; of. freedom. Substituting theresults of Equation 2.11 and.Equation 2.12.into-the n-link, manipulator dynamics will yield a reduced state dynamics formulation. ~ ;,. ,:. t:=M*(K{z)z + K(z)z) + C*{z,z)K(z)z +G^z) + J-^W (2-13) Chapter 2. Literature Review 14 Applying PD control terms, the equivalent computed torque controller may then be devised. r = M * U ) i ^ ( i ) ( i i + ^ ( i d - i ) + i p ( ^ - z ) ) + M*(z)ir(z)i + C*(z,z)K(z)z + G*{z) + J-'iAa + kfikd-*)) (2-14) UnUke the previous three controllers discussed, the reduced state position/force controller operates with respect to an optimal coordinate system z , which fully recognizes the fact that the manipulator and the constraint are an integrated system with degrees of freedom which are different from the manipulator's alone. However, McClamroch and Wang have not identified a procedure for identifying Equation 2.10, which is the crux of the method, if it is to have any practical application. Despite this setback, reduced state position/force control is still a major achievement> for it represents the first fully documented attempt at building a constraint space dynamic model for manipulator control in a constrained environment. .2.5 Summary This literature review has considered four different controllers in an attempt to chart the progression of concepts which ultimately lead to the author's contribution, hybrid con-straint space control. The first method considered, stiffness control, was a joint space, set-point force controller, therefore, not actually a true constrained environment con-troller. Nevertheless, stiffness control was included in the discussion, as it represented the first attempt at force control. The second, hybrid position/force control, was yet another joint space formulation, although this method was capable of simultaneous posi-tion/force control. In addition, this hybrid method made use of a decoupling task space coordinate system in order to separate the position and force problems into independent Chapter 2. Literature Review 15 subtasks. However, the method employed stationary task space systems, making it inap-propriate for complicated constraint trajectories. The third, hybrid impedance control, was also a joint space formulation, but was also the first method to recognize the influence of the environment in controller design. Finally, there was reduced state position/force control, which sought formulation of the system dynamics in an optimal reduced control coordinate system, markedly different from the previous joint space methods. Hybrid constraint space control goes one step further than reduced state position/force control by actively defining the optimal control coordinates. In addition, this optimal control coordinate system is constraint based, as well as being decoupling in nature. Hence, hybrid constraint space control is actually the culmination of concepts from hy-brid position/force control and reduced state position/force control, with the end result being a distinctively, constraint based approach to system modelling and control for constrained environments. Chapter 3 Hybrid Constraint Space (HCS) Control Hybrid constraint space modelling is a new approach in formulating control strategies for constrained robot manipulators. The technique essentially combines the concept of task space decoupling from hybrid position/force control with the concept of optimal state representation from reduced state position/force control, in order to introduce a unique iconstraint space coordinate system in which to model the system dynamics. There are two distinct advantages in employing hybrid constraint space control. First, the method completely decouples the position and force constraints into independent subtasks. Sec-ond, the control equations are explicitly expressed in terms of constraint parameters of practical concern, such as displacement along the constraint surface and normal inter-action force between the manipulator end-effector and environmental constraint surface. As a result, the constrained system is decoupled and represented with an optimal, non-redundant model. Because of these qualities, hybrid constraint space control has the potential to become a truly viable alternative to existing methods for constrained ma-nipulator control. The formulation of a hybrid constraint space controller consists of eight distinct steps. This procedure is illustrated pictorally in Figure 3.4. In short, hybrid constraint space control is a modelling technique where the system dynamic model is constructed with re-spect to a constraint based coordinate system. Therefore, the first step is the definition of 16 Chapter 3. Hybrid Constraint Space (HCS) Control 17 D E F I N E C O N S T R A I N T S P A C E C O O R D I N A T E S D E R I V E JOINT S P A C E D Y N A M I C S -6-F O R M U L A T E C O N S T R A I N T S P A C E D Y N A M I C S A C T U A T O R CONSTRAINTS D E R I V E M A P P I N G F U N C T I O N S D E R I V E POSITION C O N T R O L L A W D E R I V E F O R C E C O N T R O L L A W • • • • • • • • • SUPERPOSITION Figure 3.4: Procedural Flow Chart for a Hybrid Constraint Space Controller Chapter 3. Hybrid Constraint Space (HCS) Control 18 such a coordinate system with respect to the constraint surface of the particular problem. Having denned a constraint space, the next immediate goal becomes the formulation of a system dynamic model based upon these coordinates. In order to construct a constraint space dynamic model, it is first necessary to con-struct a manipulator joint space dynamic model as a starting point. Next, kinematic relationships are formulated between the constraint space degrees of freedom and the joint space degrees of freedom. These relationships or mapping functions are then used to transform the manipulator joint space dynamic model into an equivalent, but con-straint space based dynamic model. The end-result is a manipulator dynamic model based upon the previously defined constraint space coordinate system. It is important to note that this constraint space dynamic model is a model where the manipulator end-effector implicitly travels along the constraint trajectory, provided that certain kine-matic constraints and momentum compensating terms are applied to the manipulator joint torques, which is the fifth step. Through the application of these first five steps, the manipulator end-effector has been constrained to a trajectory consistent with the environmental constraint surface. As a result, the position and force problems have become decoupled. Furthermore, the position and force control actuator requirements may be linearly superposed to achieve simultaneous position and force control. Therefore, the sixth and seventh steps involve the application of respectively, appropriate position and force control laws. Finally, the eighth step consists of the summation of the required manipulator actuator forces from the separate position and force subtasks into a unifying actuator force vector responsible for hybrid position and force control. In order to clarify this procedure, each of these eight steps will now be discussed Chapter 3. Hybrid Constraint Space (HCS) Control 19 x-axis Figure 3.5: General Constraint Space Coordinate System in detail with respect to a general n-degree of freedom manipulator constrained to an arbitrary surface in 3-space. 3.1 Definition of a Constraint Space Coordinate System The first step in devising a hybrid constraint space controller is the definition of a con-straint space coordinate system. This constraint space coordinate system is unique and consists of a single positional degree of freedom, s, along the constraint trajectory to-gether with a single force degree of freedom, n, normal to the constraint trajectory. For the purposes of illustration, consider the constraint trajectory on the arbitrary curved constraint surface shown within Figure 3.5. Consider now, the case where an arbitrary n-degree of freedom manipulator is con-strained to the previously shown constraint surface, a situation illustrated in Figure 3.6. For all intents and purposes, the desired kinematics and dynamics of the constrained sys-tem may be completely described by s and n alone. This premise holds true, because for Chapter 3. Hybrid Constraint Space (HCS) Control 20 x-axis z-axis Figure 3.6: nDOF Manipulator Constrained to Arbitrary Surface practical applications such as cutting or grinding, the control variables relate primarily to conditions on the workpiece. Since the workpiece surface is represented by the constraint surface, a process where the system consists of a manipulator constrained to a work sur-face may be completely controlled through the specification of the variables s and n. In other words, the coordinates of the constraint space coordinate system contain all the in-formation required to specify interactive behaviour between the manipulator end-effector and the constraint surface. It is this property of the constraint space coordinate system which makes it an optimal reference frame for modelling the system dynamics. 3.2 Derivation of the Conventional Joint Space Dynamics Having identified a constraint space coordinate system, it now becomes necessary to construct a model of the constrained manipulator dynamics with respect to these coor-dinates. The starting point in the construction of this model is the conventional joint space dynamics formulation of the manipulator. More specifically, the Lagrangian po-tential function and the equations of motion for the manipulator with respect to its joint Chapter 3. Hybrid Constraint Space (HCS) Control 21 vector coordinates are required. Referring to the n-degree of freedom manipulator of Figure 3.6, the potential function and equations of motion take the following form. L = L(q,q) (3.1) Q = M(q)q + C(q,q)q+G(q) (3.2) Once this potential function has been derived, it may be transformed into an equivalent constraint space representation through mapping functions, and subsequently used to produce the constraint space equations of motion through the application of Lagrange's equation. The joint space equations of motion on the other hand, will be used as the basis for kinematic constraints, to be discussed later. 3.3 Derivation of Mapping Functions In order to transform the Lagrangian potential function into its constraint space equiv-alent, mapping functions between the joint space and the constraint space coordinate system are required. These mapping functions may be derived using differential kine-matics, and take the following form. qi = fi{s,s) = 92 = / 2(s,i) = (3.3) qn = fn(s,s) = These results require an exact knowledge of the differential relation between a differential displacement along the constraint trajectory and subsequent differential displacements of the manipulator joints. For reasonable constraint trajectories which may be represented with analytic functions, these differential relations are easily derived. Chapter 3. Hybrid Constraint Space (HCS) Control 22 However, in the case of highly nonlinear robots, differential kinematics alone are insufficient to produce the constraint space equations of motion. Geometric functions must also be derived. 9i = 9i(s) * = * W ,3.4) 9n = 9n(s) Nevertheless, it is not the intent of this work to deal with the cases of highly nonlinear robots. It is the intent of the work to illustrate the concept of hybrid constraint space control. Hence, it is sufficient to say at this point, that differential kinematics alone are adequate for deriving the mapping functions of example manipulators treated onwards. Nonlinear robots will complicate the derivation of the mapping functions, but will not limit the applicability of the method. One further point of notice, is the case of redundancy. For a constraint trajectory in 2-space, only two degrees of manipulator freedom are required. Likewise, for a con-straint trajectory in 3-space, only three degrees of manipulator freedom are required. As a result, the method assumes that only the bare minimum of the required joints are active. The remaining joints are locked, and removed from consideration. Therefore, the mathematical developments shown previously, should be restructured so that only active joint degrees of freedom are present. Note that these statements have been made ignoring orientation requirements. 3.4 Formulat ion of the Constraint Space Dynamics Once the mapping functions have been identified, the constraint space dynamics formula-tion may be attained. The mapping functions are first used to transform the Lagrangian Chapter 3. Hybrid Constraint Space (HCS) Control 23 potential function from joint space to constraint space coordinates. L = L(q,q) = L(s,s) (3.5) The constraint space potential function may then be used through Lagrange's method to provide the constraint space dynamics. Q - — (—] - — (3 6) * dt \5s J 8s Qs = ms(s)s + cs(s,s)s + gs{s) (3.7) Note that the constraint space dynamics consist of a single equation of motion in s. This phenomena holds true, because for a constrained manipulator end-effector, the only two relevant parameters are the location of the end-effector along the constraint trajectory and the magnitude of the normal force of interaction between the end-effector and the constraint surface. Since s represents motion along the constraint trajectory, the normal force involves no dynamics in the ideal case of a rigid environmental constraint. The normal force is simply a static force balance. Qn = Nr (3.8) Having formulated these two equations, which represent the constraint space dynamics, it is now necessary to define expressions which relate the generalized forces Qs and Qn to the joint actuator torque vector Q. Again, Q in this context, refers to the active manipulator joints. The relationship between Qs and Q may be derived through the principle of virtual work, stated as follows. Q, = Sl (3.9) Chapter 3. Hybrid Constraint Space (HCS) Control 24 Given a differential displacement along s, SW is the dot product between the active joint torque vector and the differential displacements of the active joints. SW = QMi + Q2h2 + ••• + QJqn (3-10) Since the differential relations between s and the active joint vectors are known, the following expression may be formed. Q. = F{QUQ2---Qn) (3.11) However, this single equation is insufficient to provide a constraining solution to the active joint torque vector Q. The results of the virtual work formulation only guarantee the magnitude of the equivalent force at the end-effector acting tangential to the constraint trajectory. Additional expressions which ensure kinematic consistency with the constraint trajectory are also required. The relationship between Qn and Q may be dealt with later, as hybrid constraint space control completely decouples the effects of Qs and Qn. Once kinematic constraints have been introduced to provide a set of actuator joint torques which ensure desired positional action, the required torques to satisfy normal force considerations may be included through linear superposition. 3 . 5 Actuator Constraints Required for Maintaining End-Effector Trajectory Having constructed a formulation of the constraint space dynamics, it is useful to in-terpret the physical meaning of the generalized forces Qs and Qn. Consider Figure 3.7 where these two generalized forces are shown. For a minimally active joint manipulator constrained to this surface, the joint motion will be unique, given that the joint servo motors are unpowered, and that a tangential Chapter 3. Hybrid Constraint Space (HCS) Control 25 Constraint Figure 3.7: Generalized Constraint Forces Qs and Qn force Qs together with a normal force Qn are applied to the manipulator end-effector. From a cursory inspection, it would appear that Qs is responsible for controlling motion in the constraint trajectory direction, while Qn is responsible for providing the desired normal force of interaction. This statement is mostly true. The force Qs is used to control position and the force Qn is used to control normal force, but Qn is also used to introduce kinematic constraints as well as momentum com-pensation. Consequently, it is now necessary to illustrate the derivation of the kinematic constraints which help define the position controlling component of Qn. In order to define the kinematic constraints on the active joint vector, the existing differential kinematics must be differentiated once further with respect to time. q\ = £fi{s) = hi(3,s) q\ = f/aCi) = h2{s,s) qn = £fn{s) = hn(s,s) Note that these second time derivatives will have two components. The first component will be with respect to i while the second component will be with respect to I . The Chapter 3. Hybrid Constraint Space (HCS) Control 26 first term is a coriolis and centripetal term, and is nonlinear. The second term deals with tangential acceleration, and is linear. Since the kinematic constraint is concerned with maintaining a desired tangential acceleration on the constraint surface, only the tangential components are used at this point. q\ = hl(s) q2 = hUs) H 2 K } (3.13) Returning to the joint space equations of motion derived previously, these second time derivatives may be substituted. Given that the manipulator is linear and that only minimally active joints are present, the joint space equations of motion will consist of either two equations for a planar trajectory or three equations for a 3-space trajectory. Assuming a constraint trajectory in 3-space, the equations of motion will be linear and take the following form. Qi = F1(m1,m2)s Q2 = F2(m1,m2)s (3.14) Q3 = F3(m1,m2)s The terms m\ and m2 represent the local slopes to define the orientation of the tangent plane to the constraint surface at any given point. Note that these slopes are themselves, analytic functions dependent solely upon s. Regardless, these three equations of motion may be used to form a pair of force ratios, and together with the previous virtual work expression, used to solve a 3 X 3 system of equations to provide unique values for the active joint actuator forces. Qs = F(QltQ2lQ3) T ^ 1 = F12(m1,m2) H2 Chapter 3. Hybrid Constraint Space (HCS) Control 27 Constraint Trajectory V iewpoin t is normal to the plane defined by s and n! ! ! Figure 3.8: Momentum Compensation for Curved Surfaces 9 i Qs F2z(mi,m2) (3.15) With a unique solution for the active joint actuator forces already derived, it would seem that all the physical aspects of the constraint space method have been addressed. However, consider the case of a n-degree of freedom manipulator traversing a curved constraint surface, shown in detail within Figure 3.8. It is clear that equations which focus on the desired end-effector tangential force along with a kinematic component of the normal force will always produce acceleration parallel to the constraint trajectory at any point. However, when rounding a curved path, the manipulator will have momentum which is normal to the constraint surface. If this momentum is not compensated, the end-effector will drift into the constraint surface. When drifting occurs, the kinematic and static force components of Qn will have overlapping effects, with the consequence being a coupled position and force problem. In order to avoid this situation, momentum compensation must be included in the required values of the active joint torque actuators. The computation of the required amounts of compensation are simple, and involve Chapter 3. Hybrid Constraint Space (HCS) Control 28 employing the nonlinear terms of the second time derivatives computed for the kinematic constraints. These nonlinear terms may be substituted into the joint space equations of motion to directly give the required values of momentum compensation. Qimc = G1(m1,m2)s <?2mc = G 2 ( m i , m 2 ) i (3.16) Qzmc - G 3 ( m 1 , m 2 ) i These terms may then be added to the previous solutions of Equation 3.15 to provide a set of active joint torque actuator forces which give exact end-effector position control. Qlkin = Ql + Q lmc <?2fc,n = <?2 + <?2mc ( 3 1 7 ) Qzkin = Qz + Q,Zmc Note that for non-curved surfaces, the momentum compensation terms are zero, and do not factor into the model. 3.6 Derivation of a Position Control Law The discussion up until this point has assumed that a value for the generalized constraint space force Q s has been available for the virtual work expression, which is then used to aid in the solution of the active joint torque forces. In order to actually generate an appropriate value of Q s depending upon the status of the end-effector compared to its desired status upon the constraint trajectory, it is necessary to implement a position control law. Fortunately, the dynamics for the coordinate s consist of a single equation of motion. Because of this simplicity, which is a natural consequence of employing the decoupling action of the hybrid constraint space controller, any number of conventional control laws may be employed. The author has chosen to apply computed torque control, Chapter 3. Hybrid Constraint Space (HCS) Control 29 Lewis, Abdallah, and Dawson (1993). Consequently, the controller for Qs may be formed as follows. Qs = ms(s)(sd + kpy^d - k p p ( s d - s)) + cs(s,s)s•+gs{s) (3.18) This controller is essentially a PD law with respect to the Unear portion of the dynamics and a feed-forward mechanism with respect to the nonlinear portion. Note that sensory equipment can readily provide the required states s and s which are exactly measured along the constraint trajectory. Other control laws may be used, as long as they provide satisfactory convergence. The aim of this section is simply to provide a functioning and consistent method for generating Qs. 3.7 Derivation of a Force Control Law As mentioned previously, the generalized force Qn in actuality, consists of two compo-nents. The first component deals with kinematic constraints and momentum compensa-tion, and has had its effects implicitly incorporated into the active joint torque forces. Because of this incorporation, the position and force control problems may be completely decoupled. As a result, there is no control required for the second component of the gen-eralized force Qn in the ideal case of a rigid environment. The second component may simply be set equal to the desired contact force. Qn = h (3-19) However, in the case of a non-rigid environment, albeit a very stiff environment, a com-pliant force control law of the following form may be applied. Qn = fd~ kfvkj + kfp{fd -fn) (3.20) Chapter 3. Hybrid Constraint Space (HCS) Control 30 Note that ke represents the environmental stiffness, 8 represents the rate of change of deformation and fm represents the desired normal force. Regardless of the choice for either Equation 3.19 or Equation 3.20 it is necessary to compute the required active joint torque forces which when superposed with the kinematic requirements, produces the desired normal force of interaction. These requirements may be found again, through the principles of virtual work. Qlnor = tfl(Qn) Qlnor = H2(Qn) • ' . (3-21) Qz-nor = H3(Qn) Another method is to simply treat the active joint actuators as a coordinate space, and to find the representation of the normal force vector with respect to this joint space. 3.8 Superposition With the position and normal components completely isolated, the final step for imple-mentation consists of the linear superposition of terms. Qlact = Qlkin + Q lnor Qlact = . <?2fcm + <?2nor (3.22) Qzact — Qzkin 4" Qznor 'Note that this development has not considered the effects of end-effector orientation. With various applications, the angle of approach is very important, and to address this concern, it is sufficient to say that angle of approach will only introduce additional kine-matic constraints. Otherwise, the method of hybrid constraint space control is universally , applicable. Chapter 4 Derivation of an HCS Controller for a 2DOF Prismatic Robot In order to illustrate the conceptual discussion from the previous chapter, consider the planar robot shown within Figure 4.9. This planar robot is shown with an arbitrary constraint surface, as the purpose of this section is the development of a hybrid constraint space controller for a general constraint. 4.1 Definition of a Constraint Space Coordinate System The definition of a constraint space coordinate system for the arbitrary constraint shown within Figure 4.9 is very simple. The coordinate s is essentially the line integral along the constraint curve. The normal force coordinate n is essentially the local normal direction to s for any given value of the position coordinate. It is interesting to note that this definition differs from hybrid position/force control in that the the decoupling coordinate system is a moving coordinate system. The position coordinate s is a displacement along the constraint surface, not a displacement in some prespecified, fixed axis direction. In the same vein, the force coordinate n is the local normal to s, and as such, is also not in a prespecified, fixed axis direction. Furthermore, the method is different from reduced state position/force control in that an explicit and unique formulation for selecting the optimal system coordinates has been employed. 31 Chapter 4. Derivation of an HCS Controller for a 2D0F Prismatic Robot 32 Viewpoint is normal to the plane defined by s and n!!! s / Constraint Trajectory Figure 4.9: 2DOF Planar Robot with Arbitrary Constraint 4.2 Derivation of the Conventional Joint Space Dynamics Having identified the constraint space coordinates s and n, it is now necessary to formu-late the constraint space dynamics in terms of these two variables. The first step however, is the derivation of the joint space dynamics. With respect to the planar manipulator in question, the Lagrangian potential function may be written in terms of the kinetic energies of the two linkages. L = KEi + KE2 (4.1) KE2 = -m2(il + i22) L = - ( m i + m 2 ) / i + -m2i\ (4.2) (4.3) In addition, the joint space equations of motion should also be derived at this point, as they are required for the kinematic constraints. i_ (8L\ S ± _ Q dt \8q J 8q — 1 = ' h Fx ' Q = (4.4) (4.5) Chapter 4. Derivation of an HCS Controller for a 2D0F Prismatic Robot 33 Fx = (mi + m2)h F2 = m2i2 (4.6) Note that these results have been derived ignoring the effects of gravity. Furthermore, Equation 4.3 and Equation 4.6 correspond to respectively Equation 3.1 and Equation 3.2 from the previous chapter. 4 . 3 D e r i v a t i o n o f M a p p i n g F u n c t i o n s Having derived the joint space dynamics formulation, it is necessary to find mapping functions from joint space to constraint space. Differential kinematics may be employed to derive these relations. Consider the general differential displacements shown within Figure 4.9. With respect to this illustration, several expressions may be found. 9 = tan'1™ (4.7) Note that the term m represents the local slope of the constraint surface. With respect to a planar trajectory, a single value of slope is all that is required to define the orientation of the constraint. Using this slope, differential relations between s and the joint vector may be found. ^ = sin(tan~lrm) ^ = cos^tan^m) (4-8) as as Before the actual computation of the first time derivatives, recall the definition of the time derivative, being the sum of all partials, the partials consisting of derivatives taken with respect to all parameters which the function consists. / = / (*! , x 2 - - - x n ) (4.9) df _ df^dx^ ^ df^dx^ ^ ^ df dxn (4 10) dt dx\ dt dx2 dt dxn dt Chapter 4. Derivation of an HCS Controller for a 2D0F Prismatic Robot 34 Because the constraint space coordinate system uniquely defines the constrained system in an optimal fashion, for constrained motion, the joint vector must be a function of the constraint space coordinates. Note that there has been a state reduction as a consequence of constraining the end-effector to the constraint surface. h=h(s) l2 = k(s) (4.11) As a result, it may be said that joint space representation for a constrained system is redundant. Using these results, the differential kinematic relations may be defined as follows, ' i = £ ' = £8 = s i n t t a n - 1 ™ ^ k = t = = cos(tan- 1 m)i (4.12) — p n c ( f o n ^ m \ f l dt ds dt The mapping functions for this manipulator and constraint system are then easily sum-marized. U = sinftan - 1 m)s (4-13) l2 = cos(tan _ 1 m)s Note that Equation 4.13 corresponds to Equation 3.3. Furthermore, there is no equivalent to Equation 3.4 because this prismatic robot is linear. 4.4 Formulation of the Constraint Space Dynamics Having derived the mapping functions, it is now possible to construct the constraint space dynamics through substitution. Recall the joint space Lagrangian potential function. = -(wii + m2) s in 2 ( tan _ 1 m ) i 2 H—m 2 cos 2 ( tan - 1 m ) i 2 (4-14) 2 2 L = -(mi s in 2 ( tan - 1 m) + m 2 ) i 2 (4-15) 2 Chapter 4. Derivation of an HCS Controller for a 2D0F Prismatic Robot 35 The constraint space Lagrangian has now been formed, and the constraint space equation of motion may be found by applying Lagrange's equation with respect to the constraint degree of freedom s. d (8L\ 8L = " 7 7 <4-16> For clarity, some intermediate computations are now shown. SL — = (mi s in 2 ( tan _ 1 TO) + m2)s (4-17) 5s — ( — ] = (mi s in 2 ( tan _ 1 TO) -\-m2)s + 2mi sin(tan _ 1 m) cos(tan _ 1 TO) - m i (4.18) dt yds J 1 + m2 dm . ,. m = —s (4.19) ds — = mi sin(tan _ 1 m) cos(tan _ 1 m) - m i (4.20) os 1 + m z Summarizing these results produces the constraint space equation of motion. Qs = (mi s in 2 ( tan _ 1 m) + m 2 ) i + mi sin(tan _ 1 m) cos(tan - 1 m) - m i (4.21) 1 + TO Note that Equation 4.21 corresponds to Equation 3.7. This constraint space equation of motion may now be used as the basis for a control algorithm governing the kinematic behaviour of the manipulator end-effector as it traverses the constraint surface. However, it must be possible to define Qs as a function of the joint actuator forces, or in other words, a relation must be found between F\ and F2 which ensures that a generalized force of Qs is present in the direction of s. This relation may be derived through the principles of virtual work. SW . _ FxSh + F 2 £l 2 ^ s ~ Ss Fi s in(tan - 1 m)Ss + F2 cos(tan _ 1 m)Ss Is Qs = F i sin(tan _ 1 TO) + F2 cos(tan _ 1 TO) (4.23) Chapter 4. Derivation of an HCS Controller for a 2D0F Prismatic Robot 36 Equation 4.23 is the analog to Equation 3.11 from the previous chapter. Note that this relationship does not uniquely define Fi and F2, since the relation only specifies what Fi and F2 must be as a pair to ensure an end-effector equivalent force of Qs in the direction of s. 4.5 Actuator Constraints Required for Maintaining End-EfFector Trajectory Since the relationship derived from virtual work for the constraint force Qs does not uniquely define i* \ and F2, it becomes necessary to provide another relationship. Recall the previous unconstrained joint space dynamics. Fi — (roi + m2)li F2 = m 2 l 2 (4.24) If the second time derivatives were computed with respect to the constraint space coordi-nate time derivatives, and substituted into the equations of motion, a second kinematic constraining equation may be formed. Zi = h(s, s) = cos(tan - 1 m)~rj—j^ni + sin(tan _ 1 m)5 (4-25) l2 = Z 2(i, s) = — sin(tan _ 1 m) ~ms + cos(tan - 1 m)s (4.26) 1 + m Note that Equation 4.25 and Equation 4.26 are equivalent to Equation 3.12. Using knowledge from particle differential kinematics, it is known that the first terms are nor-mal accelerations while the latter are tangential accelerations. Considering solely the tangential accelerations the second time derivative relations are as follows. Zi = s in( tan _ 1 m)5 (4-27) Z2 = cos(tan _ 1 m)s (4.28) Chapter 4. Derivation of an HCS Controller for a 2DOF Prismatic Robot 37 These equations may then be substituted into the joint space dynamics, and divided to form a new relation. Fi (mi + m 2 ) sin(tan _ 1 m)s (mi + m 2 )m ^ ^ F2 m 2 cos(tan _ 1 m)s m 2 Now, assuming the presence of some form of controller providing values for Qs, the following system of equations may be solved for Fi and F2, the required joint actuator forces for desired motion along the constraint surface. Q3 = Fi sin(tan _ 1 m) + F2 cos(tan - 1 m) (4.30) = (mi + m 2 )m F2 m 2 Note that the system of equations in the variables Fi and F2 shown by Equation 4.30 and Equation 4.31 are equivalent to the general expression from Equation 3.15. Although a pair of equations have been derived for the solution of F\ and F2, these equations do not take into account the need for momentum compensation if the end-effector is swinging around a curved surface. Consider this description of the mechanics involved. The constraint space relationship derived from virtual work will ensure a certain tangential force Q3 in the direction of s. If the end-effector is lagging behind, Qs may be increased, if is leading ahead, Qs may be reduced. But, with the two actuators F\ and F2, there exist any infinite pair of compensation which can produce Qs. Hence, a second constraint is required, that is the acceleration of the joints l\ and l2 must be such that the acceleration of the end-effector is in the direction of s. However, when the end-effector is tracing a curved surface, normal acceleration is also required. In attempting to decouple the position and force problems completely, it is desirable to compute the required normal acceleration to maintain end-effector tracking, and com-pensate, so that the end-effector will follow the constraint surface exactly, without the Chapter 4. Derivation of an HCS Controller for a 2D0F Prismatic Robot 38 need for contact force to maintain tracking. The normal acceleration has already been computed previously when considering the kinematic constraints. 11 = cos(tan _ 1 m) -ms (4-32) 1 + m 12 = — sin(tan _ 1 m) -ms (4.33) 1 + m? Substituting these acceleration components into the joint space equations of motion will yield the following. Fimc = {mi + m2) cos(tan _ 1 rn)—-—-ms (4-34) F2mc = — m 2 sin(tan _ 1 m) -ms (4.35) 1 + m2 Note that Equation 4.34 and Equation 4.35 are equivalent to Equation 3.16. Likewise, Equation 4.36 and Equation 4.37 are equivalent to Equation 3.17. These compensating forces may be added to the results of the systems solution to generate the required actuator forces for position control. Flkin = Fx + Flmc (4.36) F2kin = F2 + F2mc (4.37) Note that for non-curved surfaces, m = 0 and no compensation is required. 4.6 Derivation of a Position Control Law The previous discussion has shown how the actuator forces may be computed, given the factors Qs, s, s, m and m. The slope and slope time derivatives depend on the constraint geometry and are simple to compute as a function of end-effector location. The position and velocity along the constraint surface, may be measured. However, a control law is Chapter 4. Derivation of an HCS Controller for a 2D0F Prismatic Robot 39 required to compute the desired values of Qs. For this task, it is necessary to return to the constraint space dynamics formulation derived earlier. Qs = (mi sin 2(tan _ 1 m) + ra2).s + mi sin(tan _ 1 m) cos(tan _ 1 m)- -ms (4.38) The computed torque controller can then be applied to this expression. Qs = (misir^^an^m) + m2)(sd +^(sd — s) + kpp(sd — s)) + vn (misin(tan~1m)cos(tan~lm) - ) i (4.39) 1 + m Qs = Qs(s,s,m,rh) (4.40) Note that Equation 4.39 is equivalent to Equation 3.18. Furthermore, the required system data shown in summary form by Equation 4.40 are easily attainable through sensors at the end-effector and an a priori knowledge of the constraint surface. Hence, this controller, which uses PD control on the linear dynamics and feed-forward control on the non-linear dynamics, is readily transferable to most practical, constrained environments. 4.7 Derivation of a Force Control Law As mentioned previously, the position control law in conjunction with the kinematic con-straints and momentum compensation, has been formulated such that the end-effector exactly traces the constraint surface. The end-effector may not initially be at the correct point on the surface, but it will always remain on the constraint surface while it attempts to track the desired trajectory. Furthermore, this tracking has been achieved with kine-matic constraints and momentum compensation, such that the end-effector contacts the constraint surface without any actual interaction force. Therefore, for force control, the desired force may simply be treated as a linear superposition to the position controlling Chapter 4. Derivation of an HCS Controller for a 2D0F Prismatic Robot 40 actuator forces. In the case of an ideal, rigid environment, no force control law is re-quired. The joints may simply apply a desired contact force, which when superimposed with the position control components of the joint torques, produce an equal and opposite reaction from the environment, hence, the static force balance shown in Equation 3.19. However, when a non-rigid environment is encountered, a force control law is required, and will take exactly the form of Equation 3.20. Qn = fd- kfvKS + kfp(fd - fm) (4.41) Equation 4.41 does not differ from Equation 3.20 because these equations do not contain dynamic terms which are manipulator dependent, such as Equation 4.21. The relationship between the actuator forces Fx and F2, and the desired contact force may be derived through virtual work. (4.42) Q„ = (4.43, Sli = — <£racos(tan - 1 m) 5l2 = ^rasin(tan _ 1 m) (4.44) Qn = —Fi cos(tan _ 1 TO) + F2 sin(tan _ 1 m) (4.45) Combining this result with the virtual work expression for Qs will yield the following system of equations. Qs — Fx sin(tan _ 1 m) + F2 cos(tan _ 1 TO) (4.46) Qn = -Fx cos(tan _ 1 m) + F2 sin(tan _ 1 TO) (4.47) This system may be rewritten with the constraint space forces as the independent quan-tities. Fx — Qs sin(tan _ 1 TO) — <5ncos(tan_ 1 TO) (4.48) F2 — Qs cos(tan _ 1 TO) + Qn sin(tan _ 1 TO) (4.49) Chapter 4. Derivation of an HCS Controller for a 2D0F Prismatic Robot 41 The components of Fx and F2 responsible for the normal force of interaction may then be simply found. Fxnor = - Q „ c o s ( t a n _ 1 m) (4.50) F2nor = Qnsin(tan _ 1 m) (4-51) Note that Equation 4.50 and Equation 4.51 are equivalent to the general expression of Equation 3.21. 4.8 Superposition Having computed both the kinematic and normal force controlling components of the actuator forces Fx and F2, these components may be linearly superposed, since the hybrid constraint space controller is a linearly decoupling mechanism. Flsup = F\kin + Fxnor (4-52) F2sup = F2kin + F2nor (4.53) Finally, note that Equation 4.52 and Equation 4.53 are equivalent to Equation 3.22. Chapter 5 Simulation Results for a 2 D O F Prismatic Robot with HCS Control In the previous chapter, a hybrid constraint space controller was developed for a two degree of freedom prismatic robot. This controller will now be used to govern the same robot as it is constrained to three different environmental surfaces: a planar surface, a concave circular surface and a convex circular surface. The computer simulations used to generate results for this section are written with respect to Matlab m-file and s-function format. 5.1 Planar Constraint Surface The first simulation environment to be considered is the simple planar constraint surface, where the constraint trajectory is a straight line, as shown within Figure 5.10. The physical parameters used for this simulation are summarized within Table 5.1. The term ke represents the environmental stiffness, and is required since the manipulator end-effector is assumed to be making contact with the constraint surface. Furthermore, these simulations assume zero vector initial conditions. The first simulation with respect to the planar constraint, is the case of pure posi-tion control. The results for this simulation are shown in Figure 5.11, Figure 5.12 and Figure 5.13. Figure 5.11 shows a superposition of the constraint surface in task space 42 Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 43 Task Space x-axis (m) Figure 5.10: Planar Constraint Surface rrti m 2 k "'pv kpp kfp /(*) 2 kg 1 kg 1000 N / m 5 10 0.075 0.001 y = x Table 5.1: Simulation Parameters for the 2DOF Prismatic Robot with HCS Control Constrained to a Planar Surface Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 44 0.9 0.8 0.7 .£0.6 I gO.S CO S. en * 0.4 0.3 0.2 0.1 0 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 Task Space x-axis (m) Figure 5.11: Task Space Simulation Results for the 2DOF Prismatic Robot Constrained to a Planar Constraint Surface with Position Control with the controlled end-effector position trajectory in task space. Clearly, the hybrid constraint space controller is capable of enforcing a trajectory that is consistent with the environmental constraint surface in question. However, maintaining a trajectory on the constraint surface alone is insufficient. The end-effector must also maintain the desired kinematic response on the constraint surface. The constraint space simulation results are shown within Figure 5.12 and demonstrate convergence between desired and actual con-straint space position within two seconds. Finally, the hybrid constraint space controller must be capable of enforcing this position control without violating the boundaries of the environmental constraint surface. This condition is necessary because hybrid constraint space control is a position/force decoupling controller, such that the end-effector trajec-tory is maintained without the need for contact forces from the environment. Figure 5.13 confirms this condition by demonstrating that the contact force is zero for the duration of this first simulation. —*— Constraint — End-Effector Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 45 Figure 5.12: Constraint Space Simulation Results for the 2D0F Prismatic Robot Con-strained to a Planar Constraint Surface with Position Control 0.5i 1 1 1 1 1 1 1 r 0.4-0 2 f 0.1 & 2 o 0 5 -0 1 o -0 2 -0.4 -_ o s l 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 Simulation Time (s) Figure 5.13: Contact Force Simulation Results for the 2DOF Prismatic Robot Con-strained to a Planar Constraint Surface with Position Control Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 46 Having confirmed the effectiveness of the hybrid constraint space controller for the case of pure position control, consider now the case of position/force control for the planar constraint surface. The results for this simulation are shown in Figure 5.14, Figure 5.15 and Figure 5.16. Figure 5.14 shows a superposition of the constraint surface with the controlled end-effector trajectory in task space, and again, the two are consistent. The environment has been modelled as a hard surface with high stiffness, so that no significant deformation occurs as the result of commanding a contact force between the end-effector and the constraint surface. Figure 5.15 confirms the decoupling nature of the hybrid constraint space controller as the constraint space kinematic response is unchanged from the case of pure position control. Finally, the contact force is shown within Figure 5.16, illustrating convergent behaviour towards the desired value, well within the simulation time of two seconds. The simulation results for the planar constraint show that the hybrid constraint space controller performs very well, providing decoupled and convergent control for both posi-tion and force trajectories on the constraint surface. However, the flexibility and useful-ness of the hybrid constraint space controller are much more evident when the technique is applied to curved constraint surfaces. 5.2 Concave Circular Constraint Surface The second simulation environment to be considered in this chapter is the concave circular constraint surface. Again, the constraint trajectory is a fine, but a circular one, as shown within Figure 5.17. The physical parameters used for this simulation are summarized within Table 5.2. Once more, zero initial conditions are assumed. In addition, the hybrid constraint space controller for the simulations in this and the following section have made Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 47 Figure 5.14: Task Space Simulation Results for the 2DOF Prismatic Robot Constrained to a Planar Constraint Surface with Position/Force Control Figure 5.15: Constraint Space Simulation Results for the 2DOF Prismatic Robot Con-strained to a Planar Constraint Surface with Position/Force Control Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 48 0.8 0.6 T3 I 1 1 1 1 1 - / — Desired "F 77777 Actual "F" ,1/ , , , , , 1 1 1 , 1 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Simulation Time (s) Figure 5.16: Contact Force Simulation Results for the 2DOF Prismatic Robot Con-strained to a Planar Constraint Surface with Position/Force Control use of momentum compensation, since the constraint surfaces exhibit curvature. The case of pure position control for the circular concave constraint is considered first, with the simulation results shown within Figure 5.18, Figure 5.19 and Figure 5.20. Figure 5.18 shows the superposition of the constraint surface with the task space end-effector trajectory. Similar to the previous planar constraint case, the hybrid constraint space controller is capable of maintaining the end-effector on the constraint surface. Note though, that this simulation is conducted away from the singular region of infinite slope where the task space x-axis goes to one. In practice, a hybrid constraint space controller may be used to trace an entire circle, but the controlling equations must be restructured to avoid singularities associated with infinite slope. In the case of this two degree of freedom prismatic robot, the restructuring may be performed by switching the joint degrees of freedom with one another, such that a near infinite slope becomes a near zero slope. Figure 5.19 shows the kinematic response in constraint space coordinates, Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 49 1, 1 1 , , , , , 1 Task Space x-axis (m) Figure 5.17: Concave Circular Constraint Surface and again, the hybrid constraint space controller enforces convergence with the desired trajectory. With respect to the decoupling issue, Figure 5.20 shows a zero contact force for the duration of this pure position control simulation. Consider now a second simulation with the concave circular constraint surface, where position/force control is required. The results for this trial are shown within Figure 5.21, Figure 5.22 and Figure 5.23. Figure 5.21 is a superposition of the constraint and the controlled end-effector trajectory, and shows a complete correlation between the two. Any deformation into the environment surface due to an applied normal force is once more negligible, as the constraint surface is modelled with a high stiffness. Figure 5.22 shows a constraint space coordinate kinematic response which is convergent, and identical to the corresponding pure position control plot, demonstrating again, the decoupling nature of the hybrid constraint space controller. Finally, Figure 5.23 illustrates the contact force history for this simulation, which is very similar to the one for the planar constraint dealt Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 50 mi ra2 le kpp kfP m 2 kg 1 kg 1000 N / m 5 10 0.1 0.001 y = 1 — y/l — x2 Table 5.2: Simulation Parameters for the 2DOF Prismatic Robot with HCS Control Constrained to a Concave Circular Surface 1 0.9 0 6 0.7 E 2 0.6 m l g0.5 9 a. w * 0.4 to to I- 0.3 0.2 0 1 0 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 Task Space x-axis (m) Figure 5.18: Task Space Simulation Results for the 2DOF Prismatic Robot Constrained to a Concave Circular Constraint Surface with Position Control Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 51 Figure 5.19: Constraint Space Simulation Results for the 2DOF Prismatic Robot Con-strained to a Concave Circular Constraint Surface with Position Control 0.5 04 0.3 0.2 C -0.2 -0.3 -0.4 -0.5 0.2 0 4 0.6 0.8 1 1.2 1.4 1.6 1.8 Simulation Time (s) Figure 5.20: Contact Force Simulation Results for the 2DOF Prismatic Robot Con-strained to a Concave Circular Constraint Surface with Position Control Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 52 OS 0.8 0.7 •2 0 6 ?0.5 CC CL O) ^ 0.4 wCO t— 0.3 0.2 0.1 0 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 Task Space x-axis (m) Figure 5.21: Task Space Simulation Results for the 2DOF Prismatic Robot Constrained to a Concave Circular Constraint Surface with Position/Force Control with previously. Therefore, for linear and curved surfaces, the hybrid constraint space controller is capable of decoupling the position and force problems, as well as providing convergent control for both. 5.3 Convex Circular Constraint Surface The final simulation environment to be considered is the convex circular constraint sur-face. Like the previous two constraint trajectories already considered, this trajectory is also a planar Une, as shown within Figure 5.24. The physical parameters used for this simulation are summarized within Table 5.3. As before, zero initial conditions are assumed. The case of pure position control for the convex circular constraint is treated first, the simulation results being shown within Figure 5.25, Figure 5.26 and Figure 5.27. The Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 53 Figure 5.22: Constraint Space Simulation Results for the 2DOF Prismatic Robot Con-strained to a Concave Circular Constraint Surface with Position/Force Control Figure 5.23: Contact Force Simulation Results for the 2DOF Prismatic Robot Con-strained to a Concave Circular Constraint Surface with Position/Force Control Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 54 Task Space x-axis (m) Figure 5.24: Convex Circular Constraint Surface mi m 2 k kpp kfv kfP m 2 kg 1 kg 1000 N / m 5 10 0.075 0.001 y = y/l-(x- l ) 2 Table 5.3: Simulation Parameters for the 2DOF Prismatic Robot with HCS Control Constrained to a Convex Circular Constraint Surface Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 55 Figure 5.25: Task Space Simulation Results for the 2DOF Prismatic Robot Constrained to a Convex Circular Constraint Surface with Position Control superposition of the constraint surface with the end-effector path is shown by Figure 5.25, which demonstrates an almost exact correlation between the two. Similar to the previous case, the region of singular slope near the origin is avoided within this simulation. If the entire circular path is required, the singular regions may be dealt with as mentioned previously for the case of the concave circular constraint. Figure 5.26 shows the kine-matic response in constraint space coordinates, and again, there is convergence between the desired history and the actual history. Figure 5.27 demonstrates once more, that the hybrid constraint space controller is decoupling, since for this pure position control problem, the contact force history is again zero. Consider now the final simulation in this chapter, where the prismatic robot is simu-lated for position/force control while restricted to the convex circular constraint surface. The results of this run are shown within Figure 5.28, Figure 5.29 and Figure 5.30, and like the previous, demonstrate the effectiveness of the hybrid constraint space controller. Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 56 i i l [ 1 Desired "s" Actual "s" ' : -"O 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Simulation Time (s) Figure 5.26: Constraint Space Simulation Results for the 2 D O F Prismatic Robot Con-strained to a Convex Circular Constraint Surface with Position Control 0.51 1 1 1 1 1 1 1 1 1 0.4-0.3-0.2 -I o.i - ! \ \ I ! \ i -to y .9 -0.2 --0.3 - J J i i | | i --0.4 --0.51 ' < 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 Simulation Time (s) Figure 5.27: Contact Force Simulation Results for the 2 D O F Prismatic Robot Con-strained to a Convex Circular Constraint Surface with Position Control Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 57 0.9 0.8 _0.7 e | 0.6 >. o> 0.5 0.3 0.2 0.1 0 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 Task Space x-axis (m) Figure 5.28: Task Space Simulation Results for the 2DOF Prismatic Robot Constrained to a Convex Circular Constraint Surface with Position/Force Control Figure 5.28 shows that the controlled end-effector perfectly traces the outline of the convex circular constraint surface. Furthermore, Figure 5.29 shows that the end-effector maintains a convergent kinematic response while tracing the surface. Finally, Figure 5.30 shows that the contact force is also convergent to the desired level. Hence, for the planar, concave circular and convex circular constraint surfaces, the prismatic robot under hybrid constraint space control is completely decoupling with re-spect to the position and force control problems, as well as capable of providing convergent control to the desired position and force trajectories. Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 58 0.3 Desired "s" Actual "s" 0.5 1 Simulation Time (s) Figure 5.29: Constraint Space Simulation Results for the 2DOF Prismatic Robot Con-strained to a Convex Circular Constraint Surface with Position/Force Control 0.5 1 Simulation Time (s) 1.5 Figure 5.30: Contact Force Simulation Results for the 2DOF Prismatic Robot Con-strained to a Convex Circular Constraint Surface with Position/Force Control Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 59 :5.4 Comparative Discussion Having performed various simulations for a two degree of freedom prismatic robot under hybrid constraint space control, it is evident that the method provides very good simul-taneous position and force regulation in a constrained environment. Now, how does this performance compare to previous methods, for example, the earlier work discussed in Chapter 2? A qualitative assessment may be made as follows. First, with respect to the stiffness controller, the hybrid constraint space controller is clearly better. The constraint space method gives simultaneous position and force control for arbitrary trajectories, while the stiffness controller is only useful for setpoint force control. On the other hand, the stiffness controller was included in the literature review more as a historical benchmark, than a performance benchmark. A more fair comparison, would be the relative performance of a hybrid position/force controller to that of the constraint space controller. Like the constraint space method, hybrid position/force control is also capable of enforcing simultaneous position and force trajectories. However, the hybrid position/force controller was developed with respect to stationary task space coordinates. In other words, for a hybrid position/force controller i to operate with a curved constraint surface, a time-varying Jacobian function relating the changing task space coordinates with the curvature of the environment would be required. Hybrid constraint space control avoids this problem by employing a constraint space dynamic model. By using differential kinematics, the constraint space dynamic model is capable of tracking along curving constraint trajectories without the need for establishing time-varying Jacobian transformations from task to joint space. A comparison with hybrid impedance control yields similar results, as the impedance Chapter 5. Simulation Results for a 2D0F Prismatic Robot with HCS Control 60 controller is basically a more realistic version of the hybrid position/force controller which recognizes that position and force are not independent quantities for any specific direction i n task space given the compliance of practical constraint environments. Nevertheless, position and force relations in the same task space degree of freedom is not the issue here. Like hybrid position/force control, the impedance controller employs stationary task space coordinates when transforming from task to joint space. As a result, it too requires time-varying Jacobian transformations. Finally, it is noteworthy to compare the operating principles of the reduced state position/force controller with that of the hybrid constraint space controller. In princi-ple, both methods are very similar, with the intent of formulating constrained system dynamics with respect to a set of optimal coordinates. However, the reduced state posi-tion/force controller does not actually detail a specific method for deriving these optimal coordinates. Hybrid constraint space control though, has an established method for de-riving an optimal dynamics formulation, which may be used to successfully implement simultaneous position and force control, as shown by the simulation results from this chapter. Consequently, the author feels that hybrid constraint space control may offer a viable alternative to conventional means of position and force control for constrained environment control problems. Chapter 6 Conclusions & Summary 6.1 Conclusions The hybrid constraint space controller, as simulated for the two degree of freedom pris-matic robot, performed very well. In every instance, the controller was capable of provid-ing decoupled control of both position and force. Furthermore, the actual position and force trajectories consistently converged to the desired position and force trajectories. 6.2 Summary This thesis has considered in full, the origins, conceptual detail and initial simulation of the hybrid constraint space control technique developed by the author. The literature review has charted the development of the method with respect to previously published work, citing strong conceptual ties with the task space decoupling approach of hybrid constraint space position/force control and the optimal state representation approach of reduced state position/force control. The method for constructing a hybrid constraint space controller with respect to a general manipulator and constraint pair were then discussed, with the emphasis placed on the physical concepts justifying each step in the technique. Subsequently, a two degree of freedom prismatic robot was used as an illustrative example for the construction of a hybrid constraint space controller. Finally, 61 Chapter 6. Conclusions & Summary 62 simulation trials were performed for this specific hybrid constraint space controller under the restriction of three constraint surfaces, a planar, a concave circular and a convex circular environment. The simulation results for these trials were very promising. 6.3 Future Work It is the author's intent to extend these results for nonlinear manipulator configurations in a future publication. Specific attention will be given to the case of the two degree of freedom rotary manipulator. Furthermore, the prismatic case will be completely gen-eralized to include a three degree of freedom prismatic manipulator constrained to a constraint surface in 3-space. Bibliography [1] Arnold, V . I., (1978) Mathematical Methods of Classical Mechanics. New York: Springer-Verlag. [2] Astrom, Kar l Johan, and Bjorn Wittenmark, (1989) Adaptive Control. Reading, Massachusetts: Addison-Wesley Publishing Company. [3] Chiaverini, Stefano, and Lorenzo Sciavicco, (1993) "The Parallel Approach to Force/Position Control of Robotic Manipulators." IEEE Transactions on Robotics and Automation, Vol.9, No.4. [4] Fu, Gonzalez, and Lee, (1987) Robotics: Control, Sensing, Vision, and Intelli-gence. New York: McGraw-Hill Book Company. [5] Goldstein, Herbert, (1980) Classical Mechanics: 2nd Edition. Reading, Mas-sachusetts: Addison-Wesley Publishing Company. [6] Grabbe, Carroll, Dawson, and Qu, (1992) "Robust Control of Robot Manipu-lators During Constrained and Unconstrained Motion." Proceedings of the 1992 IEEE International Conference on Robotics and Automation. [7] Greenwood, Donald T., (1965) Principles of Dynamics. Englewood Cliffs, New Jersey: Prentice-Hall, Inc. [8] Hogan, N . , (1987) "Stable Execution of Contact Tasks using Impedance Con-trol." Proceedings of the 1987 IEEE International Conference on Robotics and Automation. [9] Houshangi, N . , (1992) "Position/Force Control of a Robot Manipulator Over an Unknown Surface." Proceedings of the 1992 IEEE International Conference on Systems, Man and Cybernetics. [10] Kankaanranta, R., and H . N . Koivo, (1986) "A Model for Constrained Motion of a Serial Link Manipulator." Proceedings of the 1986 IEEE International Con-ference on Robotics and Automation. [11] Lewis, Abdallah, and Dawson, (1993) Control of Robot Manipulators. New York: Macmillan Publishing Company. 63 Bibliography 64 [12] Lipschutz, Seymour, (1964) Theory & Problems of Set Theory & Related Topics. New York: McGraw-Hill Book Company. [13] Liu, Jing-Sin, (1991) "Hybrid Control for a Class of Constrained Mechanical Systems." Proceedings of the 30th Conference on Decision and Control. [14] McClamroch, N . Harris, and Danwei Wang, (1988) "Feedback Stabilization and Tracking of Constrained Robots." IEEE Transactions on Automatic Control, Vol.33, No.5. [15] Raibert, M . , and J . Craig, (1981) "Hybrid Position/Force Control of Manipula-tors." Journal of Dynamic Systems, Measurement, and Control, Vol.102. [16] Salisbury, J . , and J . Craig, (1980) "Active Stiffness Control of Manipulator in Cartesian Coordinates." Proceedings of the 19th IEEE Conference on Decision and Control. [17] Su, Chun-Yi, and Yury Stepanenko, (1994) "Robust Motion/Force Control of Mechanical Systems with Classical Nonholonomic Constraints." IEEE Transac-tions on Automatic Control, Vol.39, No.3. [18] Takahashi, Rabins, and Auslander, (1970) Control and Dynamic Systems. Read-ing, Massachusetts: Addison-Wesley Publishing Company. [19] Van de Vegte, John, (1990) Feedback Control Systems: 2nd Edition. Englewood Cliffs, New Jersey: Prentice Hall. [20] Wang, Danwei, and N . Harris McClamroch, (1993) "Position and Force Con-trol for Constrained Manipulator Motion: Lyapunov's Direct Method." IEEE Transactions on Robotics and Automation, Vol.9, No.3. [21] Yun, Xiaoping, (1988) "Dynamic State Feedback Control of Constrained Robot Manipulators." Proceedings of the 27th IEEE Conference on Decision and Con-trol. Appendix A Software Coding for Simulations Performed in Matlab 65 C O N S T R A I N T S U R F A C E #1 function[sys,xO]=surfl (t,x,u,flag,l l_dot,l 1,12_dot,12) ifabs(flag)== 1 ml=2; m2=l; kpv=5, kpp=10; kfv=0.075; kfp=0.001; sd_dd=u(l); sd_d=u(2); sd=u(3); Fd=u(4); % Assuming a slope of m=l; st=sin(atan( 1)), ct=cos(atan( 1)); % Determine equivalent point on surface. % x ( l ) = ll_dot = y_dot %x(2) = l l =y % x(3) = 12_dot = x_dot % x(4) = 12 = x xs=(x(4)+x(2))/2, xs_d=(x(3)+x(l))/2; ys=xs; % Use equivalent surface point to determine constraint position. s=xs/ct; s_d=xs_d/ct; % Determine position controller. Qs=(m 1 * st A2+m2) * (sd_dd+kpv* (sd_d-s_d)+kpp * (sd-s)); F2=Qs/(((ml+m2)/m2)* 1 *st+ct), Fl=F2*((ml+m2)/m2)*l; % Determine force controller. ke=1000; delta=sqrt((xs-x(4))A2+(ys-x(2))A2); delta_d=-x(l)*ct+x(3)*st; if((x(4)>xs)&(x(2)<ys)) global Fm; Fm=ke* delta, else global Fm; Fm=0; end Qn=Fd+kfp*(Fd-Fm)-kfv*ke*delta_d; % Superpose the position and force controllers. F1=F1-Qn*ct; F2=F2+Qn*st; % Compute the manipulator dynamics. A=[ 0 0 0 0 1000 0 0 0 0 00 10]; B=[ (Fl+Fm*ct)/(ml+m2) 0 (F2-Fm*st)/m2 0]; sys=A*x+B; elseif abs(flag) == 3 global Fm; if t=0 Fm=0; end sys=[x(l) x(2) x(3) x(4) Fm]; elseif abs(flag) = 0 sys=[4 0 5 4 0 1]; x0=zeros(4,l), x0(l)=ll_dot; x0(2)=ll; x0(3)=12_dot; x0(4)=12; else sys=[]; end 68 S I M U L A T I O N #1 function [ret,xO,str,ts,xts]=finalsiml(t,x,u,flag); %FINALSIM1 is the M-file description of the SIMULINK system named FINALS IM1. % The block-diagram can be displayed by typing: FINALSIM1. % % SYS=FINALSIM1(T,X,U,FLAG) returns depending on F L A G certain % system values given time point, T, current state vector, X, % and input vector, U. % F L A G is used to indicate the type of output to be returned in SYS. % % Setting FLAG=1 causes FINALSIM1 to return state derivatives, FLAG=2 % discrete states, FLAG=3 system outputs and FLAG=4 next sample % time. For more information and other options see SFUNC. % % Calling FINALSHvIl with a F L A G of zero: % [SIZES]=FINALSIM1([],[],[],0), returns a vector, SIZES, which % contains the sizes of the state vector and other parameters. % SIZES( 1) number of states % SIZES(2) number of discrete states % SIZES(3) number of outputs % SLZES(4) number of inputs % SIZES(5) number of roots (currently unsupported) % SIZES(6) direct feedthrough flag % SIZES(7) number of sample times % % For the definition of other parameters in SIZES, see SFUNC. % See also, TRIM, LINMOD, LINSIM, EULER, RK23, RK45, ADAMS, GEAR. % Note: This M-file is only used for saving graphical information; % after the model is loaded into memory an internal model % representation is used. % the system will take on the name of this mfile: sys = mfilename; new_system(sys) simver(1.3) if (0 == (nargin + nargout)) set_param(sys,'Location',[217,259,1158,980]) open_system(sys) end, set_param(sys,'algorithm', 'RK-23') set_param(sys,'Start time', '0.0') set_param(sys,'Stop time', '2.5') set_param(sys,'Min step size', '0.01') 69 set_param(sys,'Max step size', '0.01') set_param(sys,'Relative error',' 1 e-3') set_param(sys,'Return vars', ") add_block('built-in/Mux',[sys,7,,'Mux2']) set_param([sys,'/VMux2'],... 'position',[320,321,375,584]) add_block('built-iri/S-Function',[sys,'/VDynamics']) set jaram([sys,7','Dynamics'],... 'function name Vsurfl',... 'parameters','0,0,0,0',... 'position',[535,267,695,373]) add_block('built-in/Demux',[sys,7','Demux']) set_param([sys,7VDemux'],... 'outputs',^',... *position',[725,296,775,344]) % Subsystem 'XY Graph'. new_system([sys,'/','XY Graph']) set_param([sys,7VXYGraph'],'Location'>[8,52,282,245]) add_block('built-in/Inport',[sys,'/VXYGraph/y']) set_param([sys,'/','XY Graph/y'],... ' 'PortV2',... 'position',[10,100,30,120]) add_block('built-in/Inport',[sys,7,,,XY Graph/x']) set_param([sys,'/','XY Graph/x'],... 'position*,[10,30,30,50]) add_block('built-in/Mux',[sys,'/','XY Graph/Mux*]) set_param([sys,7','XY Graph/Mux'],... 'inputs','2',... 'positional 00,61,130,94]) add_block('built-in/S-Function',[sys,7*,['XY Graph/S-function', 13,'M-file which plots',13,'lines',13,"]]) set_param([sys,7',['XY Graph/S-function', 13,'M-file which plots', 13,'lines', 13,"]],... 'function name','sfunxy',... 'parameters','ax, st',... 'position',[185,70,235,90]) 70 add_line([sys,7VXY Graph'],[35,110,70,110;70,85,95,85]) add_line([sys,7','XY Graph'],[35,40;70,40;70,70,95,70]) add_line([sys,7VXY Graph'],[135,80;180,80]) set_param([sys,7','XY Graph'],... 'Mask Display','plot(0,0,100,100,[ 12,91,91,12,12],[90,90,45,45,90],[51,57,65,75,80,79,75,67,60 ,54,51,48,42,34,28,27,31,42,51],[71,68,66,66,72,79,83,84,81,77,71,60,54,54,58,65,71,74 ,71])*) set_param([sys,7','XY Graph'],... 'Mask Type','XY scope.',... 'Mask Dialogue','XY scope using M A T L A B graph window.\nFirst input is used as time baseAnEnter plotting ranges. |x-min:|x-max:|y-min:|y-max:') set_param([sys,7','XY Graph'],... 'Mask Translate'/ax = [@1, @2, @3, @4];st=-l 'Mask Help','This block can be used to explore limit cycles. Look at the m-file sfunxy.m to see how it works.',... 'Mask Entries','-1V3V-1V3V) % Finished composite block 'XY Graph'. set_param([sys,'/yXY Graph'],... 'position',[660,69,755,156]) add_block('built-in/Derivative',[sys,7','Acceleration']) setjparam([sys,7','Acceleration'],... 'position',[225,334,295,376]) add_block('built-in/Derivative',[sys,7VVelocity']) set_param([sys,7','Velocity'],... 'position',[ 130,399,200,441 ]) add_block('built-in/Sine Wave',[sys,7','Position']) set_param([sys,7','Position'],... 'amplitude',' 10',... 'frequency','1/16',... 'position',[20,452,105,518]) add_block('built-in/Constant',[sys,7','Fd']) set_param([sys,7','Fd'],... 'position',[250,532,285,568]) add_block('built-in/Mux',[sys,7','Mux4']) set_param([sys,7','Mux4'],... 'inputs','2',... 71 'position',[590,479,650,561]) addJ)lock('built-in/Mux',[sys,7','Mux5*]) set_param([sys,7','Mux5'],... 'inputs','?,... 'position',[855,289,895,356]) add_block('built-in/Fcn',[sys,'/','Fcn']) set_param([sys,7','Fcn'],... 'orientation',2,... 'ExprV(u[2]+1 *u[ 1 ])/((1+1 A2)*cos(atan( 1)))',... 'position',[630,413,690,457]) add_block('built-in/To Workspace',[sys,7','To Workspace2']) set_param([sys,7','To Workspace2'],... 'mat-name','hnkl',... *position',[850,142,900,158]) add_block('built-in/To Workspace',[sys,'/','To Workspacel']) set_param([sys,'/','To Workspacel'],... 'mat-name','link2',... 'position',[850,62,900,78]) % Subsystem 'Position_Compare'. new_system([sys,'/','Position_Compare']) set_param([sys,VVPosition_Compare'],'Location',[0,59,274,252]) add_block('built-in/Inport',[sys,'/','Position_Compare/x']) set_param([sys,'/','Position_Compare/x'],... 'position',[65,55,85,75]) add_block('built-in/S-Function',[sys,'/',['Position_Compare/S-function',13,'M-file which plots',13,'lines',13,"]]) set_param([sys,'/',['Position_Compare/S-function', 13,'M-file which plots', 13,'lines', 13,"]],... 'function name','sfunyst',... 'parameters','ax, color, npts, dt',... •position',[130,55,180,75]) add_line([sys,'/','Position_Compare'],[90,65;125,65]) set_param([sys,'/','Position_Compare'],... 'Mask Display7plot(0,0,100,100,[83,76^ 4,72,80,84,74,65,65,65,90,40,40,90,90])',... 'Mask Type','Storage scope.') 72 set _param([sys, 7','Position_Compare'],... 'Mask Dialogue','Storage scope using M A T L A B graph windowAnEnter plotting ranges and line type.|Initial Time Range: |Initial y-min: |Initial y-max:|Storage pts.:|Line type (rgbw-.:xo):') set_param([sys,'/VPosition_Compare'],... 'Mask Translate','npts = @4, color = @5; ax = [0, @1, @2, @3]; dt=-l,') set_param([sys,7','Position_Compare'],... 'Mask Help','This block uses a M A T L A B figure window to plot the input signal. The graph limits are automatically scaled to the min and max values of the signal stored in the scope"s signal buffer. Line type must be in quotes. See the M-file sfunyst.m.') set_param([sys,7','Position_Compare'],... •Mask Entries','5 V-10V10V10000V"g-/r-/c- ./w: /m*/ro/b+'V) % Finished composite block 'PositionCompare'. set_param([sys,7','Position_Compare'],... 'positiori,[720,489,765,551]) add_block('built-in/Mux',[sys,7',*Mux6']) set_param([sys,77Mux6'],... •inputs','2',... 'position',[815,494,855,561]) % Subsystem 'Force'. new_system([sys,7','Force']) set_param([sys,7',"Force'],'Location',[0,59,274,252]) add^lockCbuilt-in/S-Function^sysZ/'^'Force/S-function',! 3,'M-file which plots', 13,'lines', 13,"]]) set_param([sys,7',['Force/S-function', 13,'M-file which plots', 13,'lines', 13,"]],... 'function name','sfunyst',... 'parameters','ax, color, npts, dt',... 'position',[130,55,180,75]) add_block('built-in/Inport',[sys,7';Force/x']) set_param([sys>7','Force/x'],... 'position',[65,55,85,75]) add_line([sys,7',Torce'],[90,65,125,65]) set_param([sys,7','Force'],... 73 'Mask Display','plot(0,0,100,100,[83,76,63,52,42,38,28,16,11,84,11,11,11,90,90,11 ],[75,58,47,5 4,72,80,84,74,65,65,65,90,40,40,90,90])',... 'Mask Type','Storage scope.') set_param([sys,V,'Force'],... 'Mask Dialogue','Storage scope using M A T L A B graph window.\nEnter plotting ranges and line type. |Initial Time Range: |Initial y-min:|Initial y-max: |Storage pts.:|Line type (rgbw-.:xo):') set_param([sys,7','Force'],... 'Mask Translate*,'npts = @4, color = @5; ax = [0, @1, @2, @3]; dt=-l;') set_param([sys,7','Force'],... 'Mask HelpVThis block uses a M A T L A B figure window to plot the input signal. The graph limits are automatically scaled to the min and max values of the signal stored in the scope"s signal buffer. Line type must be in quotes. See the M-file sfunyst.m.') set_param([sys,7,'Force'],... 'Mask Entries','2V-lVlVl0000V"b-/r-/c-./w:/m*/ro/b+"V) % Finished composite block 'Force'. set_param([sys,7','Force'],... 'position',[885,529,930,591]) add_line(sys,[l 10,485;315,485]) add_line(sys,[205,420,315,420]) , add_line(sys,[205,420;210,420,220,355]) add_line(sys,[300,355,315,355]) add_line(sys,[l 10,485; 115,485; 125,420]) add_line(sys,[700,320,720,320]) add_line(sys,[655,520;715,520]) add_line(sys,[780,310,815,310,815,250;635,250;635,135;655,135]) add_line(sys,[290,550;315,550]) addjine(sys,[780,330;805,330,805,35;645,35;655,90]) add_line(sys,[815,310;840,310;850,305]) add_line(sys,[805,330,840,330;850,340]) add_line(sys,[900,325,910,325;910,435,695,435]) add_line(sys,[625,435;565,435;565,500;585,500]) add_line(sys,[805,35;805,70;845,70]) add_line(sys,[765,250;765,150;845,150]) add_line(sys,[220,485,220,605;505,605;505,540;585,540]) add_line(sys,[380,455,450,455;450,320;530,320]) add_line(sys,[780,340;785,340;785,510;810,510]) add_line(sys,[860,530,865,530;865,560;880,560]) add_line(sys,[290,550;300,550;300,595;545,595;545,570;790,570;790,545;810,545]) drawnow % Return any arguments, if (nargin | nargout) % Must use feval here to access system in memory if (nargin > 3) if (flag = 0) eval(['[ret,xO,str,ts,xts]-,sys,'(t,x,u,flag);']) else eval(['ret =>, sys,'(t,x,u,flag);']) end else [ret,xO,str,ts,xts] = feval(sys); end else drawnow % Flash up the model and execute load callback end C O N S T R A I N T S U R F A C E #2 mnction[sys,x0]=surf2(t,x,u,flag,ll_dot,ll,12_dot,12) ifabs(flag) = 1 ml=2; m2=l; kpv=5, kpp=10; kfv=0.1; kfp=0.001, sd_dd=u(l); sd_d=u(2); sd=u(3); Fd=u(4); % Determine equivalent point on surface. %x(l) = ll_dot = y_dot % x(2) = 11 = y % x(3) = 12_dot = x_dot % x(4) = 12 = x m_star=(x(2)-l)/x(4); xs= 1 /sqrt( 1 +m_star A2); ys=l-sqrt(l-xsA2), m=xs/sqrt( 1 -xsA2), m_d=x(3)/(l-xsA2)A(3/2); st=sin(atan(m)), ct=cos(atan(m)); % Use equivalent surface point to determine constraint position. s=asin(xs); s_d=x(3)/sqrt(l-xsA2); % Determine position controller. Qs=(m 1 * stA2+m2) * (sd_dd+kpv* (sd_d-s_d)+kpp * (sd-s)); F2=Qs/(((ml+m2)/m2)*m*st+ct); Fl=F2*((ml+m2)/m2)*m; F2=F2-m2*st*(m_d/(l+mA2))*s_d; Fl=Fl+(ml+m2)*ct*(m_d/(l+mA2))*s_d; % Determine force controller. ke=1000, delta=sqrt((xs-x(4))A2+(ys-x(2))A2), delta_d=-x( 1) * ct+x(3) * st, if((x(4)>xs)&(x(2)<ys)) global Fm, Fm=ke*delta, else global Fm, Fm=0, end Qn=Fd+kfp*(Fd-Fm)-kfV*ke*delta_d; % Superpose the position and force controllers. F1=F1-Qn*ct; F2=F2+Qn*st, % Compute the manipulator dynamics. A=[ 0 0 0 0 1 0 0 0 0 0 0 0 00 10], B=[ (Fl+Fm*ct)/(ml+m2) 0 (F2-Fm*st)/m2 0]; sys=A*x+B, elseif abs(flag) = 3 global Fm; if t=0 Fm=0; end sys=[x(l) x(2) x(3) x(4) Fm], elseif abs(flag) == 0 sys=[4 0 5 4 0 1]; x0=zeros(4,l), x0(l)=ll_dot; x0(2)=ll; x0(3)=12_dot; x0(4)=12; else sys=[]; end 77 S I M U L A T I O N #2 function [ret,x0,str,ts,xts]=finalsim2(t,x,u,flag), %FINALSIM2 is the M-file description of the SIMULINK system named FINALSIM2. % The block-diagram can be displayed by typing: FINALSIM2. % % SYS=FINALSIM2(T,X,U,FLAG) returns depending on F L A G certain % system values given time point, T, current state vector, X, % and input vector, U. % F L A G is used to indicate the type of output to be returned in SYS. % % Setting FLAG=1 causes FINALSIM2 to return state derivatives, FLAG=2 % discrete states, FLAG=3 system outputs and FLAG=4 next sample % time. For more information and other options see SFUNC. % % Calling FINALSIM2 with a F L A G of zero: % [SIZES]=FINALSIM2([],[],[],0), returns a vector, SIZES, which % contains the sizes of the state vector and other parameters. % S IZES( 1) number of states % SIZES(2) number of discrete states % SIZES(3) number of outputs % SIZES(4) number of inputs % SIZES(5) number of roots (currently unsupported) % SIZES(6) direct feedthrough flag % SIZES(7) number of sample times % % For the definition of other parameters in SIZES, see SFUNC. % See also, TRIM, LINMOD, LIN SIM, EULER, RK23, RK45, ADAMS, GEAR. % Note: This M-file is only used for saving graphical information, % after the model is loaded into memory an internal model % representation is used. % the system will take on the name of this mfile: sys = mfilename, new_system(sys) simver(1.3) if (0 = (nargin + nargout)) set_param(sys,'Location', [268,217,1240,93 8]) open_system(sys) end, set_param(sys,'algorithm', 'RK-23') set_param(sys,'Start time', '0.0') set_param(sys,'Stop time', '2') set_param(sys,'Min step size', '0.01') 78 set_param(sys,'Max step size', '0.01') set_param(sys,'Relative error',' le-3') set_param(sys,'Return vars', ") add_block('built-in/Mux',[sys,'/','Mux2']) set_param([sys,'/','Mux2'],... 'position',[320,321,375,584]) add_block('built-in/S-Function',[sys,'/VDynamics']) set jaram([sys,'/','Dynarnics'],... 'function name','surf2',... 'pararneters','0,0,0,0*,... 'position',[535,267,695,373]) add_block('built-in/Demux',[sys,'/','Demux']) set_param([sys,'/','Demux'],... 'outputs','5',... 'position',[725,296,775,344]) % Subsystem 'XY Graph'. new_system([sys,'/','XY Graph']) set_param([sys,'/','XYGraph'],'Location',[8,52,282,245]) add_block('built-in/Inport',[sys,'/',*XY Graph/y']) set_param([sys,7','XY Graph/y'],... 'Port','2',... 'position',[10,100,30,120]) add_block('built-in/Inport',[sys,'/','XY Graph/x']) set_param([sys,7','XY Graph/x'],... 'position',[10,30,30,50]) add_block('built-in/Mux',[sys,'/*,'XY Graph/Mux']) set_param([sys,7','XY Graph/Mux'],... 'inputs','2',... 'position',[ 100,61,130,94]) add_block('built-in/S-Function',[sys,7',['XY Graph/S-function', 13,'M-file which plots', 13,'lines', 13,"]]) set_param([sys,7',['XY Graph/S-ainction',13,'M-file which plots',13,'lines',13,"]],... 'function name','sfunxy',... 'parameters','ax, st',... 'position',[l 85,70,23 5,90]) 79 add_line([sys,7','XY Graph'],[35,l 10;70,110;70,85;95,85]) addJine([sys,7',*XYGraph'],[35,40;70,40;70,70,95,70]) addJine([sys,7',*XY Graph'],[ 135,80; 180,80]) set_param([sys,7',*XY Graph'],... 'Mask Display','plot(0,0,100,100,[12,91,91,12,12],[90,90,45,45,90],[51,57,65,75,80,79,75,67,60 ,54,51,48,42,34,28,27,31,42,51],[71,68,66,66,72,79,83,84,81,77,71,60,54,54,58,65,71,74 ,71])') set_param([sys,7','XY Graph'],... 'Mask Type','XY scope.',... 'Mask Dialogue','XY scope using M A T L A B graph window.\nFirst input is used as time baseAnEnter plotting ranges. |x-min:|x-max:|y-min:|y-max:') set_param([sys,7','XY Graph'],... 'Mask Translate','ax = [@1, @2, @3, @4];st=-l;',... 'Mask Help','This block can be used to explore limit cycles. Look at the m-file sfunxy.m to see how it works.',... 'Mask Entries','-1V3V-1V3V) % Finished composite block 'XY Graph'. set_param([sys,7','XY Graph'],... •position',[660,69,755,156]) add_block('built-in/Derivative',[sys,7','Acceleration']) setjparam([sys,7','Acceleration'],... 'position',[225,334,295,376]) add_block('built-in/Derivative',[sys,7','Velocity']) set_param([sys,7','Velocity'],... 'position'^ 130,399,200,441 ]) add_block('built-in/Sine Wave',[sys,7','Position']) set_param([sys,7','Position'],... 'amplitude','10',... 'frequency','1/16',... 'position', [20,452,105,518]) add_block('built-in/Constant*,[sys,7','Fd']) set_param([sys,7','Fd'],... 'position*,[250,532,285,568]) add_block('built-in/Mux',[sys,7','Mux4']) set_param([sys,7','Mux4'],... 'inputs','2',... 80 'position',[590,479,650,561]) add_blockCbuilt-in/Fcn,,[sys,,/,,,Fcn']) set_param([sys,7','Fcn'],... 'orientation'^,... 'Expr','asin( 1 /sqrt( 1 +((u[ 1 ]-1 )/u[2])A2))',... 'position', [630,413,690,45 7]) add_block('built-in/To Workspace', [sys,V,'To Workspace2']) set_param([sys,'/','To Workspace2'],... 'mat-name','link 1',... 'position',[850,142,900,158]) add_block('built-in/To Workspace',[sys,'/','To Workspace 1']) set_param([sys,'/','To Workspace 1'],... 'mat-name','link2',... •position',[850,62,900,78]) % Subsystem 'PositionCompare'. new_system([sys,'/','Position_Compare']) set_param([sys,'/','Position_Compare'],'Location',[0,59,274,252]) add_block('built-in/Inport',[sys,'/','Position_Compare/x']) set_param([sys,'/','Position_Compare/x'],... 'position',[65,55,85,75]) add_block('built-in/S-Function',[sys,'/',['Position_Compare/S-function', 13,'M-file which plots', 13,'lines', 13,"]]) set_param([sys,'/',['Position_Compare/S-function',l3,'M-file which plots', 13,'lines', 13,"]],... 'function name','sfunyst',... 'parameters','ax, color, npts, dt',... 'position',[130,55,180,75]) add_line([sys,'/','Position_Compare'],[90,65,125,65]) set_param([sys,'/','Position_Compare'],... 'Mask Display','plot(0,0,100,100,[83,76,63,52,42,38,28,16,11,84,11,11,11,90,90,11 ],[75,58,47,5 4,72,80,84,74,65,65,65,90,40,40,90,90])',... 'Mask Type','Storage scope.') set_param([sys,'/','Position_Compare'],... 'Mask Dialogue'/Storage scope using M A T L A B graph windowAnEnter plotting ranges and line type.|Initial Time Range: jlnitial y-min: |Initial y-max:|Storage pts.:|Line type (rgbw-.:xo):') set_param([sys,'/','Position_Compare'],... 81 'Mask Translate'/npts = @4; color = @5, ax = [0, @1, @2, @3]; dt=-l;') set_param([sys,'/','Position_Compare'],... 'Mask Help',This block uses a M A T L A B figure window to plot the input signal. The graph limits are automatically scaled to the min and max values of the signal stored in the scope"s signal buffer. Line type must be in quotes. See the M-file sfunyst.m.') set_param([sys,VVPosition_Compare'],... 'Mask Entries';5V-10V10V10000V"g-/r-/c-./w:/m*/ro/b+"V') % Finished composite block 'PositionCompare'. set_param([sys,'/','Position_Compare'],... 'position',[720,489,765,551]) addJMock('built-in/Mux',[sys,7',*Mux5']) set_param([sys,'/','Mux5'],... 'inputs','?,... 'position',[840,289,900,371]) add_block('built-in/Mux',[sys,'/','Mux6']) set_param([sys,'/','Mux6'],... 'inputs','?,... 'position',[805,519,865,601]) % Subsystem 'Force'. new_system([sys,'/','Force']) set_param([sys,'/',Torce'],'Location*,[0,59,274,252]) add_block(*built-iri/S-Function',[sys,'/',['Force/S-function,,13,,M-file which plots*,13,'lines',13,"]]) set_param([sys,'/*,['Force/S-function', 13,'M-file which plots', 13,'Iines*, 13,"]],... 'function name','sfunyst',... 'parameters','ax, color, npts, dt',... 'position',[130,55,180,75]) add_block('built-in/Inport',[sys,'/','Force/x']) set_jparam([sys,'/','Force/x'],... 'position',[65,55,85,75]) add_line([sys,*/','Force'],[90,65,125,65]) set_param([sys,'/','Force'],... 82 'Mask Display','plot(0,0,100,100,[83,76,63,52,42,38,28,16,11,84,11,11,11,90,90,11 ],[75,58,47,5 4,72,80,84,74,65,65,65,90,40,40,90,90])',... 'Mask Type','Storage scope.') set_param([sys,'/','Force'],... 'Mask Dialogue','Storage scope using M A T L A B graph windowAnEnter plotting ranges and line type. [Initial Time Range: [Initial y-min:|Initial y-max:|Storage pts.:|Line type (rgbw-.:xo):') set_param([sys,'/','Force'],... 'Mask Translate','npts = @4; color = @5; ax = [0, @1, @2, @3], dt=-1;') set_param([sys,'/','Force'],... 'Mask Help','This block uses a M A T L A B figure window to plot the input signal. The graph limits are automatically scaled to the min and max values of the signal stored in the scope"s signal buffer. Line type must be in quotes. See the M-file sfunyst.m.') set_param([sys,'/','Force'],... 'Mask Entries','2V-lVlV10000V"b-/r-/c-./w:/m*/ro/b+"V) % Finished composite block 'Force'. set_param([sys,'/','Force'],... 'position',[905,554,950,616]) :ne(sys,[110,485;315,485]) ne(sys,[205,420,315,420]) ne(sys,[205,420;210,420;220,3 55]) ne(sys,[300,355,315,355]) ine(sys,[l 10,485,115,485,125,420]) ine(sys, [700,3 20;720,3 20]) ne(sys,[655,520;715,520]) ne(sys,[780,310;815,310;815,250;635,250;635,135,655,135]) ne(sys,[290,550;315,550]) ne(sys,[780,330;805,330;805,35,645,35,655,90]) ine(sys,[625,435;565,435;565,500;585,500]) ne(sys,[805,35;805,70;845,70]) ne(sys,[765,250,765,150;845,150]) ine(sys,[220,485,220,605;505,605,505,540;585,540]) ne(sys,[380,455,450,455;450,320,530,320]) ine(sys,[815,310,83 5,310]) ne(sys,[805,330;825,330;835,350]) ne(sys,[905,330;915,330;915,435,695,435]) ne(sys,[780,340;785,340,785,540;800,540]) ne(sys,[300,550;300,605;650,605,650,580;800,580]) ine(sys,[870,560,880,560;880,585;900,585]) add_l add_l addj add_l addj addj addj addj addj addj addj addj addj addj addj addj addj addj addj addj add 1 drawnow % Return any arguments, if (nargin | nargout) % Must use feval here to access system in memory if (nargin > 3) if(flag = 0) eval(['[ret,xO,str,ts,xts]-,sys,'(t,x,u,fiag);']) else evaltf'ret =', sys/(t,x,u,flag);']) end else [ret,xO,str,ts,xts] = feval(sys); end else drawnow % Flash up the model and execute load callback end C O N S T R A I N T S U R F A C E #3 mnction[sys,x0]=surf3(t,x,u,flag,ll_dot,ll,12_dot,12) ifabs(flag) = 1 ml =2; m2=l; kpv=5, kpp=10; kfv=0.075, kfp=0.001; sd_dd=u(l); sd_d=u(2), sd=u(3); Fd=u(4); % Determine equivalent point on surface. %x(l) = ll_dot = y_dot %x(2) = ll =y % x(3) = 12_dot = x_dot % x(4) = 12 = x m_star=-x(2)/(l-x(4)); xs= 1 -(1 /sqrt(m_starA2+1)); ys=sqrt(l-(xs-l)A2), m=( 1-xs)/sqrt( 1-(xs-1 )A2); m_d=-x(3)/( 1 -(xs-1 )A2)A(3/2), st=sin(atan(m)); ct=cos(atan(m)); % Use equivalent surface point to determine constraint position. s=quad('int_convex',0.l,xs); s_d=(l/sqrt(l-(l-xs)A2))*x(3); % Determine position controller. Qs=(m 1 * stA2+m2) * (sd_dd+kpv* (sd_d-s_d)+kpp * (sd-s)); F2=Qs/(((ml+m2)/m2)*m*st+ct); Fl=F2*((ml+m2)/m2)*m, F2=F2-m2*st*(m_d/(l+mA2))*s_d; Fl=Fl+(ml+m2)*ct*(m_d/(l+mA2))*s_d; % Determine force controller. ke=1000; delta=sqrt((xs-x(4))A2+(ys-x(2))A2); delta_d=-x( 1) * ct+x(3) * st; if((x(4)>xs)&(x(2)<ys)) global Fm; Fm=ke*delta, else global Fm; Fm=0, end Qn=Fd+kfp*(Fd-Fm)-kfv*ke*delta_d, % Superpose the position and force controllers. F1=F1-Qn*ct, F2=F2+Qn*st, % Compute the manipulator dynamics. A=[ 0 0 0 0 1 0 0 0 0 0 0 0 00 10]; B=[ (Fl+Fm*ct)/(ml+m2) 0 (F2-Fm*st)/m2 0]; sys=A*x+B; elseif abs(flag) = 3 global Fm; i f t=0 Fm=0, end sys=[x(l)x(2) x(3)x(4) Fm]; elseif abs(flag) = 0 sys=[40 5 4 0 1]; x0=zeros(4,l), x0(l)=ll_dot; x0(2)=ll; x0(3)=l2_dot; x0(4)=12, else sys=[]; end 86 S I M U L A T I O N #3 function [ret,x0,str,ts,xts]=finalsim3(t,x,u,flag); %FINALSIM3 is the M-file description of the SIMULINK system named FINALS IM3 % The block-diagram can be displayed by typing: FINALSIM3. % % SYS=FINALSIM3(T,X,U,FLAG) returns depending on F L A G certain % system values given time point, T, current state vector, X, % and input vector, U. % F L A G is used to indicate the type of output to be returned in SYS % % Setting FLAG=1 causes FINALSIM3 to return state derivatives, FLAG=2 % discrete states, FLAG=3 system outputs and FLAG=4 next sample % time. For more information and other options see SFUNC. % % Calling FLNALSIM3 with a F L A G of zero: % [SIZES]=FrNALSJM3([],[],[],0), returns a vector, SIZES, which % contains the sizes of the state vector and other parameters. % SIZES( 1) number of states % SIZES(2) number of discrete states % SIZES(3) number of outputs % SIZES(4) number of inputs % SIZES(5) number of roots (currently unsupported) % SIZES(6) direct feedthrough flag % SIZES(7) number of sample times % % For the definition of other parameters in SIZES, see SFUNC. % See also, TRIM, LINMOD, LINSEVI, EULER, RK23, RK45, ADAMS, GEAR. % Note: This M-file is only used for saving graphical information; % after the model is loaded into memory an internal model % representation is used. % the system will take on the name of this mfile: sys = mfilename; new_system(sys) simver(1.3) if (0 == (nargin + nargout)) set_param(sys,'Location', [274,209,1215,929]) open_system(sys) end; set_param(sys,,algorithm', 'RK-23') set_param(sys,'Start time', '0.0') set_param(sys,'Stop time', '2') set_param(sys,'Min step size', '0.01') 87 setjparam(sys,'Max step size', '0.01') set_param(sys,'Relative error','le-3') set_param(sys,'Return vars', ") add_block(*built-in/Mux',[sys,'/','Mux2']) set_param([sys,'/','Mux2'],... 'position',[320,321,375,584]) add_block('built-in/S-Function',[sys,'/','Dynamics']) set jaram([sys,'/','Dynamics'],... 'function name','surf3',... •parameters','0,0.43 59,0,0.1',... 'position',[535,267,695,373]) add_block('built-in/Demux',[sys,'/','Demux']) set_param([sys,'/','Demux'],... 'outputs',^',... 'position',[725,296,775,344]) % Subsystem 'XY Graph'. new_system([sys,'/','XY Graph']) set_param([sys,'/*,'XYGraph'],'Location',[8,52,282,245]) add_block('built-in/Inport',[sys,'/','XYGraph/y']) set_param([sys,'/','XY Graph/y'],... 'Port','2',... 'position*, [10,100,3 0,120]) add_block('built-in/Inport',[sys,'/','XY Graph/x']) set_param([sys,'/','XY Graph/x'],... 'position',[10,30,30,50]) add_block('built-in/Mux',[sys,*/VXY Graph/Mux']) set_param([sys,'/','XY Graph/Mux*],... 'inputs','2*,... 'position', [100,61,130,94]) add_block('built-in/S-Function',[sys,'/',['XY Graph/S-function', 13,'M-file which plots',13,'lines',13,"]]) set_param([sys,'/',['XY Graph/S-function', 13,'M-file which plots', 13,'lines', 13,"]],... 'function name','sfunxy',... 'parameters','ax, st',... 'position',[ 185,70,23 5,90]) 88 add_line([sys,77XY Graph'],[35,l 10;70,110;70,85;95,85]) addJine([sys//','XYGraph'],[35,40,70,40;70,70,95,70]) addJine([sys,'/','XY Graph'],[135,80;180,80]) set_param([sys,7','XY Graph'],... 'Mask Display','plot(0,0,100,100,[12,91,91,12,12],[90,90,45,45,90],[51,57,65,75,80,79,75,67,60 ,54,51,48,42,34,28,27,31,42,51],[71,68,66,66,72,79,83,84,81,77,71,60,54,54,58,65,71,74 ,71])') set_param([sys,V','XY Graph'],... 'Mask Type','XY scope.',... 'Mask Dialogue','XY scope using M A T L A B graph window.\nFirst input is used as time baseAnEnter plotting ranges. |x-min:|x-max:|y-min:|y-max:') set_param([sys,'/','XY Graph'],... 'Mask Translate'/ax = [@1, @2, @3, @4];st=-l 'Mask Help','This block can be used to explore limit cycles. Look at the m-file sfunxy.m to see how it works.',... 'Mask Entries',*-1V3V-1V3V) % Finished composite block 'XY Graph*. setjparam([sys,'/','XY Graph'],... 'position',[660,69,755,156]) add_block('built-in/Derivative',[sys,'/','Acceleration']) set_param([sys,'/','Acceleration'],... 'position',[225,334,295,376]) add_block('built-in/Derivative',[sys,'/','Velocity']) set_param([sys,'/','Velocity'],... 'position*,[ 13 0,399,200,441 ]) add_block('built-in/Sine Wave',[sys,'/','Position']) set_param([sys,'/','Position'],... 'amplitude','10',... 'frequency','1/16',... *position',[20,452,105,518]) add_block('built-in/Constant',[sys,'/','Fd"]) set_param([sys,'/','Fd'],... 'position',[250,532,285,568]) add_block('built-in/Mux',[sys,'/','Mux4']) set_param([sys,'/','Mux4'],... 'inputs','2',... 89 'position',[590,479,650,561]) add_block('built-in/To Workspace',[sys,7','To Workspace2']) set_param([sys,7','To Workspace2'],... 'mat-name', 'link 1',... 'position',[850,142,900,158]) add_block(*built-in/To Workspace',[sys,'/','To Workspacel']) set_param([sys,'/','To Workspacel'],... 'mat-name','link2',... *position',[850,62,900,78]) % Subsystem 'PositionCompare'. new_system([sys,'/','Position_Compare']) set_param([sys,'/','Position_Compare'],'Location',[0,59,274,252]) add_block('built-in/Inport',[sys,'/','Position_Compare/x']) set_param([sys,'/','Position_Compare/x'],... 'position',[65,55,85,75]) add_block('built-in/S-Function',[sys,'/',['Position_Compare/S-function',13,'M-file which plots', 13,'lines', 13,"]]) set_param([sys,'/',['Position_Compare/S-function', 13,'M-file which plots', 13,'lines', 13,"]],... 'function name','sfunyst',... 'parameters','ax, color, npts, dt',... 'position',[130,55,180,75]) add_line([sys,'/','Position_Compare'],[90,65;125,65]) set_param([sys,'/','Position_Compare'], .. 'Mask DisplayVplot(0,0,100,100,[83,76,63,52,42,38,28,16,l 1,84,11,11,11,90,90,11],[75,58,47,5 4,72,80,84,74,65,65,65,90,40,40,90,90])',... 'Mask Type','Storage scope.') set_param([sys,'/','Position_Compare'],... 'Mask Dialogue','Storage scope using M A T L A B graph windowAnEnter plotting ranges and line type.|Initial Time Range: jlnitial y-min:|Initial y-max: |Storage pts.:|Line type (rgbw-.:xo):') set_param([sys,7','Position_Compare'],... 'Mask Translate','npts = @4; color = @5; ax = [0, @1, @2, @3], dt=-l;') set_param([sys,7','Position_Compare'],... 'Mask Help','This block uses a M A T L A B figure window to plot the input signal. The graph limits are automatically scaled to the min and max values of the signal stored in the scope"s signal buffer. Line type must be in quotes. See the M-file sfunyst.m.') set j3aram([sys,7','Position_Compare'],... 'Mask Entries','5V-10V10VlOOOOV"g-/r-/c-./w:/m*/ro/b+"V) 90 % Finished composite block 'PositionCompare'. set_param([sys,'/','Position_Compare'],... 'position',[720,489,765,551]) add_block('built-in/MATLAB Fcn',[sys,7','MATLAB Fen']) set_param([sys,'/',*MATLAB Fen'],... 'orientation',2,... 'MATLAB Fcn','quad("int_convex",0. l,u)',... 'position',[630,403,705,447]) add^lockCbuilt-in/Fcn^sysZ/'/Fcn']) set_param([sys,'/','Fcn'],... 'position',[625,600,665,620]) add_blockCbuilt-in/Mux',[sys;/','Mux5']) set_param([sys,'/','Mux5'],... 'inputs','2',... 'position',[800,554,860,636]) % Subsystem 'Force'. new_system([sys,'/','Force']) set_param([sys,'/VForce'],'Location',[0,59,274,252]) add_block('built-in/S-Function',[sys,7',['Force/S-function', 13,'M-file which plots', 13,'lines', 13,"]]) set_param([sys,7',['Force/S-function', 13,'M-file which plots', 13,*lines',l3,"]],... 'function name','sfiinyst',... 'parameters','ax, color, npts, dt',... 'position',[130,55,180,75]) add_block('built-in/Inport',[sys,'/','Force/x']) set_param([sys,'/','Force/x'],... 'position',[65,55,85,75]) add_line([sys,7','Force'],[90,65;125,65]) set_param([sys,7','Force'],... 'Mask Display','plot(0,0,100,100,[83,76,63,52,42,38,28,16,11,84,11,11,11,90,90,11 ],[75,58,47,5 4,72,80,84,74,65,65,65,90,40,40,90,90])',... J 91 'Mask Type','Storage scope.') set_param([sys,'/','Force'],... 'Mask Dialogue','Storage scope using M A T L A B graph window.\nEnter plotting ranges and line type, jlnitial Time Range: |Initial y-min:|Initial y-max:|Storage pts.:|Line type (rgbw-.:xo):') set_param([sys,'/','Force'],... 'Mask Translate','npts = @4; color - @5, ax = [0, @1, @2, @3]; dt=-1,') set_param([sys,'/','Force'],... 'Mask Help','This block uses a M A T L A B figure window to plot the input signal. The graph limits are automatically scaled to the min and max values of the signal stored in the scope"s signal buffer. Line type must be in quotes. See the M-file sfunyst.m.') set_param([sys,'/','Force'],... 'Mask Entries','2V-lVlV10000V"b-/r-/c-./w:/m*/ro/b+"V) % Finished composite block 'Force'. set_param([sys,'/yForce'],... *position',[890,484,93 5,546]) add_line(sys,[205,420;315,420]) add_line(sys,[205,420;210,420;220,3 5 5]) add_line(sys,[300,355;315,355]) add_line(sys,[l 10,485,115,485;125,420]) add_line(sys,[700,320;720,320]) add_line(sys,[655,520,715,520]) add_line(sys,[780,310;815,310,815,250;635,250;635,135;655,135]) add_line(sys,[290,550;315,550]) add_line(sys,[780,330;805,330;805,35;645,35;655,90]) add_line(sys,[625,425;565,425;565,500;585,500]) add_line(sys,[805,35;805,70;845,70]) add_line(sys,[765,250;765,150,845,150]) add_line(sys,[380,455;450,455;450,320;530,320]) add_line(sys,[110,485;120,485,120,530;145,530;315,485]) add_line(sys,[805,330;805,425;710,425]) add_line(sys,[145,530;180,530;180,590;380,590;380,54O,585,540]) add_line(sys,[780,340;785,340,795,575]) add_line(sys,[300,550;300,640;745,640;745,615,795,615]) add_line(sys,[865,595;870,595,870,515;885,515]) drawnow % Return any arguments, if (nargin | nargout) % Must use feval here to access system in memory 92 if (nargin > 3) if (flag == 0) eval([,[ret,xO,str,ts,xts]=,,sys,,(t,x,u,flag);']) else evaltf'ret =', sys,'(t,x,u,flag);']) end else [ret,xO,str,ts,xts] = feval(sys); end else drawnow % Flash up the model and execute load callback end 

Cite

Citation Scheme:

        

Citations by CSL (citeproc-js)

Usage Statistics

Share

Embed

Customize your widget with the following options, then copy and paste the code below into the HTML of your page to embed this item in your website.
                        
                            <div id="ubcOpenCollectionsWidgetDisplay">
                            <script id="ubcOpenCollectionsWidget"
                            src="{[{embed.src}]}"
                            data-item="{[{embed.item}]}"
                            data-collection="{[{embed.collection}]}"
                            data-metadata="{[{embed.showMetadata}]}"
                            data-width="{[{embed.width}]}"
                            async >
                            </script>
                            </div>
                        
                    
IIIF logo Our image viewer uses the IIIF 2.0 standard. To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.831.1-0087230/manifest

Comment

Related Items