RSD-TR-22-86 Control of Constrained Hamiltonian Systems and Applications to Control of Constrained Robots by N. Harris McClamrocht Anthony M. Blocht tDepartment of Aerospace Engineering and Department of Electrical Engineering and Computer Science tDepartment of Mathematics The University of Michigan Ann Arbor, MI 48109 October 1086 CENTER FOR RESEARCH ON INTEGRATEGRATED MAN'UFACTURING Robot Systems Division COLLEGE OF ENGINEERING THE UNIVERSITY OF MICHIGAN ANN ARBOR, MICHIGAN 48109-1109

RSD-TR-22-86 TABLE OF CONTENTS 1. Introduction............................................................................................ 1 2. Controlled Hamiltonian Systems With Constraints................................ 2 3. Decomposition Of Controlled Hamiltonian Systems With Constraints........................................ 5 4. Constrained Hamiltonian Systems As Singular Systems......................... 9 5. Control Problems For Constrained Hamiltonian Systems....................... 11 6. Control Of Robot Tasks Defined By Holonomic Constraints.................. 13 7. References............................................................................................... 14

RSD-TR.22-86 ABSTRACT It is shown that the theory of Hamiltonian control systems can be extended in a very natural manner to a theory of Hamiltonian control systems with con-.straints. In particular, these problems may be formulated either in terms of the original (symplectic) manifold or in terms of the constraint manifold. The analysis of such problems is shown to be essentially equivalent to the analysis of systems of controlled singular (differential-algebraic) equations. Certain robotic control problems, defined by task constraints, are shown to fall within the defined theoretical framework

RSD-TR-22-86 1. Introduction In recent years there has been great interest in Hamiltonian control systems [6,9,20], control systems which preserve the natural Hamiltonian structure. A great number of important mechanical systems are of this type. There has also been much interest recently in the theory of constrained Hamiltonian systems. Such systems have turned out to be of importance in the theory of integrable Hamiltonian systems; for example see [10]. In addition, there has been a recent realization that such problems provide a suitable framework for a number of interesting engineering applications. In this paper, we develop a framework so that the theories of Hamiltonian control systems and constrained Hamiltonian systems can be combined in a very natural way to produce a theory for constrained Hamiltonian control systems. We show how constrained Hamiltonian control systems may be formulated either on the original symplectic manifold or on the constraint manifold. Control theoretic problems may be formulated in either context, but an explicit formulation in terms of the constraint manifold may be most suitable for purposes of analysis and control design. Decomposition methods for obtaining such a formulation are presented. We briefly mention results on feedback stabilization and optimal control for constrained Hamiltonian control systems which have been developed elsewhere. A controllability result is stated, the proof following from previous controllability results and from the framework established for conHamiltonian Systems 1

RSD-TR-22-86 strained Hamiltonian systems. Constrained Hamiltonian control systems can also be viewed as singular sets of differential equations; in this case they are sets of coupled differential and algebraic equations in control theoretic form. The connection of constrained Hamiltonian control systems with a special class of controlled singular systems is thereby established. Finally, the importance of such theoretical problems in applications is shown. The emphasis is on the critical role of constraints in properly defining an important class of robotic control problems associated with the application of robots to advanced tasks involving interaction with their environment. 2. Controlled Hamiltonian Systems WVith Constraints Hamiltonian control systems were first introduced in [6] and developed in [4],[9] and [20], for example. We consider here the case of affine Hamiltonian control systems with constraints. Let M be a differentiable manifold with local coordinates (ql,.., q n) on M and let T*M denote the cotangent bundle of M with local coordinates (q,'', *, qp,, p ) on T*M. T*M is a symplectic manifold and its symplectic form is given locally by w = Edpi Adqi. (1) i We suppose that we have a Hamiltonian function H: T*M -+ R given by H(q,p) = T(q,p) + V(q) where T(q,p) is a kinetic energy function on T*M that 2 Hamiltonian Systems

RSD-TR-22-86 is a positive definite quadratic form in p,. *, pn and V(q) is a potential energy function on M. We denote by XH the Hamiltonian vector field associated with H(q,p). We also assume that there are functions G T*M -+ R that define control vector fields Xi on T*M,j=1,',r, and functions Obi: T*M -- R that define constraint vector fields Yi on T*M,j=1, *, 2m. Thus the equations for a constrained Hamiltonian control system are given by x = XH(z) + E, Xi (X) + CEX Ye (X), (2) i.~~ i 0i () = 0, i=,., 2m, (3) or in local form q =O(p) + yuj aGi(,p) ai+ ~ ~ (') i *=1,, n, (4) OH(q p) OGCY(q,p) + Ex a (q, p) -; = aH(_) - OG'(q) >9q,=1,.,n, (5) q (q,p) = 0, i=1, ~ _,2m. (6) The (u a,,, ) are the control inputs and the (X1,, 2m ) are the multipliers corresponding to the constraints. Note that the control inputs and the constraint multipliers enter the equations in precisely the same manner, although their meanings are quite different; the control inputs are viewed as arbitrarily specified external inputs whereas the multipliers are viewed as implicitly specified by the requirement that the constraints be satisfied. Hamiltonian Systems 3

RSD-TR-22-86 Notice that this formulation is very much in the spirit of Dirac's theory of constrained Hamiltonian systems. In [111 the system is driven by a total Hamiltonian HT(q,p,X)= H(q,p) + EX Q'(q,p). (7) In our more general context the system is driven by a total Hamiltonian HT(q,p,X,u) HC(qp,u) + EX q (q,p) (8) where H (q,p,u) -- H(q,p) + yuj G' (q,) (0) is the drift Hamiltonian together with the control Hamiltonians. We now distinguish between holonomic and nonholonomic constraints as folb lows: Definition 1. A set of constraints fi, j=1,, m on T*M is holonomic if the forms d4i, j=1,''',m define an integrable distribution on T*M. Otherwise, the constraints are called nonholonomic. Note that the definition is a slight generalization of the classical definition of holonomic constraints: where the /i are functions of the variables q', i=1,'', n, only. In this case i = i-, j=1,, m and, j=m+l,, 2m. 4 Hamiltonian Systems

RSD-TR-22-86 3. Decomposition Of Controlled Hamiltonian Systems With Constraints In this section we show how the control of a Hamiltonian system with constraints may be viewed as the control of a Hamiltonian system on the constraint manifold. Suppose that the zero set of the constraints defines a submanifold N of T*M, We assume that the matrix [((i, il}] is nondegenerate, where (F,G} is the Poisson bracket on T*M. Thus N is symplectic. Now we can generalize the arguments in [14] to the control situation as follows. Let Z denote the Hamiltonian vector field corresponding to Hc(q,p,u) = H(q,p) + Sui Gi(q,p). Since N is'-a symplectic submanifold of T*M at every point n of N, the tangent space of T*ll at n can be decomposed as T T*Mn = T N, e TNn, (10) where i denotes orthogonal complement. Then the vector field Z has the decomposition Z = ZN E ZNj. (11) If v is any tangent vector to N then w(ZN,v) = w(Z,v) = < dHC,v> = <dHC I N,v>. (12) Hence ZN is the Hamiltonian vector field on N corresponding to HC N.the restriction of HC to N, relative to the restricted symplectic form w N. Hamiltonian Systems 5

RSD-TR-22-86 Now let Yj denote the Hamiltonian vector field corresponding to i, j=l,.. *,2m. Since the Oi = 0 on N, the restricted control vector fields yi satisfy yNN= O, and the Yj, j=l,'', 2m, thus form a basis of TN-. Hence zNj.=EXi, for some scalars Xl,...,X2m,andZN=Zt where ZT is the Hamiltonian vector field corresponding to HT (q,p,X,U). Now along N, Z T,K =(HT,'k )=O. Hence (HC, bi }-= Xi {(i, bi }, j } l, j., 2m. (13) Since the matrix of Poisson brackets [{4i, 5i }] is nonsingular at each point of N, there is a unique solution for X1, * e,X2m. Thus we have the following. Theorem 1. The full Hamiltonian HT constraints the Hamiltonian control system to the constraint manifold N, and if the matrix [{I', P' }] is nonsingular on N we can solve uniquely for the multipliers X1, e', X2m X We see from the above development that the constraint multipliers X1, X,X 2m depend on the drift vector field, the control vector fields and the constraint vector fields. It is this intrinsic coupling that significantly complicates the analysis of control problems. Nevertheless, the Hamiltonian structure is preserved on the constraint manifold, as has been shown. Now if T*M = R 2, we can follow the arguments in [10] to show that the Poisson bracket on the constraint manifold N, (F-G}N is related to the Poisson bracket on R 2, {F,G}, by ~~~~~~~~6 ~ ~Hamiltonian Systems

RSD-TR-22-86 (F,G}N = {F,G} - {F,'i }')cf1{qi,G} (14) i,i where ci-I is the ijth entry of the inverse of C = [{Wi,0i }1. We thus have Theorem 2. The equations of motion of the Hamiltonian control system constrained to N are q' {qB, HT}, i=l-, n (15) P {P. HT}, i=1 ~, (16) or, equivalently, q ={q' HC}N, i=-1, n (17) pi ={pH N i=1, * ~,)N (18) Thus we can regard the system as evolving on R 2n under the full Hamiltonian HT with respect to the original bracket-structure or as evolving on the constraint manifold N C R2, under the control Hamiltonian HC with the constraint manifold bracket structure. We also have the following theorem which follows from [21]. Theorem 3. Suppose Hi, j=1,,, m are a set of independent holonomic constraints on T*M with m < n. Then there is a local canonical transformation g: T*M D U - T*M such that the transformed constraint functions Hi = iJ og = Q2, j=1, * *, m, if and only if sb, j=1,, m are in involution, i.e. Hamiltonian Systems 7

RSD-TR-22-86 {Y, Ii } = o, i, j=1. ~, m. (19) Here Q i, j=1, n ~, n are the transformed configuration coordinates. Thus, after a canonical transformation, locally the constraints can be written as Q1 Q2=.. = m 0. (20) Note that for constraints in the classical form the involution condition is automatically satisfied, and only the linear independence condition is required to satisfy the assumptions of Theorem 3. For a system on T*M = R 2, with holonomic constraints Jl (q) = 0 j=1,, m, we can write this transformation quite explicitly as follows. We partition the n-vectors q, p and transformed n-vectors Q, P as: q iQQ P i Q Q1 P P,: (21P1 q q2 P q2 (21)2 2 where q 1 is an m-vector and q2 is an n-m vector, etc. Then it is possible to find a function 6(q2) such that b(56(q2), q2) = 0 is satisfied locally. Now consider the canonical transformation given by the generating function F(q, P) = (q - 6(q2))P2 + q 1P2- (22) Then, the transformation is given by [13] Q1 = ql - 6(q2) Pi = P1 (23) Q2= H2 P2 = P2ta PS a laamiltonian SystemrHs

RESD.-TR-22-88 and the transformed equations are Qi = {QiH },i i n (24) pi _ {pi H H }9 i=-1 — ne (25) Q' =0, i = 1, m, (28) where the transformed Hamiltonian is H r(Q, P, X) = H c(Q, P, i Q (27) ial and HE(Q, P, u) = Hc (q,p,u). Since Q' —, i=1,...,m the first m of equations (24) can be used to express P1 in terms of (P2, Q2,); the first m of equations (25) can be used to express X in terms of (P2, Q2,u). Thus the last n-m of equations (24) and of equations (25) can be written as ordinary differential equations in (P2, Q2) and u, which characterize the dynamics on the constraint manifold. 4. Constrained Hamiltonian Systems As Singular Systems It is clear from the above development that analysis of control problems constrained to a given manifold involves both differential equations and algebraic equations. In fact, these coupled differential and algebraic equations can be represented as a singular set of differential equations. This can be seen explicitly by writing our constrained Hamiltonian control equations, using vector notation, in local form as Hamiltonian Systems 9

RSD-TR-22-86 _aH(_ _GI_ GI(q,p)7\ _ _,_) 9H(q,p) + q -u aOt_(q,p) (28+) 01 0 0 ~ - q aq i q 0 0 (q,P) or equivalently OHT (q, p,X,u) 10 01 Op OHT(q,p,X,u) I 10 p (29) 0 00 OHT(q,p,X,u) OX ad In this form, we have 2n + 2m coupled equations: 2n differential equations and 2m algebraic equations, in the 2n + 2m variables (q,p,X). These equations are inherently coupled through the constraint multipliers that appear in the differential equations. Note that the constraint multipliers do not appear in the algebraic equations, so the algebraic equations cannot be explicitly used to solve for the multipliers. Thus the differential and algebraic structure of the equations is of an especially complex form. Such singular systems have been studied in recent years [7,8] but there are few theoretical results that can be applied to the class of systems of interest here. However, it should be mentioned that there have been a number of results obtained for the numerical solution of initial value problems [2,12,18] for singular systems of the class considered here. 10 Hamiltonian Systems

RSD-TR-22-86 From the theoretical analysis of Section 3 and the previous work in [15,17j, it becomes clear that there are two distinct approaches to the analysis of control theoretic problems for constrained Hamiltonian control systems expressed in singular form. One method is to solve, in algebraic form, for the multiplier vector X from equation (13), and to substitute into equations (15) and (16) to obtain a differential equation on R 2. The alternate approach is to decompose the system to obtain nonsingular differential equations which give a representation of the motion on the constraint manifold. Theorem 3 is the key for carrying out this procedure. One makes a canonical transformation so that the constraints are in the simple form given by equation (20). We can thus obtain a set of 2n - 2m differential equations on the constraint manifold. Note that the basic approaches suggested here each depend on the use of the Hamiltonian structure. Our view is that consideration of general nonlinear singular systems is not a tractable approach, but that progress can be made for the class of systems of interest here by exploiting the HIamiltonian relationships. 5. Control Problems For Constrained Hamiltonian Systems The two approaches to control of constrained Hamiltonian control systems discussed in Section 4 have been shown to be useful in different ways. The first approach, that of solving for the multipliers, has, for example, proved useful in optimal planning problems for constrained robot manipulators Hamiltonian Systems 11

RSD-TR-22-86 [151. There that approach was used to develop a numerical procedure to solve a time optimal control problem for a control system constrained to follow a given path and with a given contact force (multiplier) vector. On the other hand, the second approach based on canonical transformations to simplify the constraints has been used in the analysis of stability of closed loop constrained robot manipulators [17]. Closed loop stability on the constraint manifold (such that the motion and multipliers satisfy a regulation or tracking property) is guaranteed in terms of conditions on the feedback controller; both global and local modifications of the computed torque controller used in robotics are included. Although the development in these papers was carried out in a Lagrangian formulation, these results can be restated in the Hamiltonian form considered here. Detailed statements of these results can be found in the indicated references. We state here a controllability result which follows from our previous development. We say that the system (2) and (3) is controllable if for all x and y in T*MF there exists an admissible control and a time T > O such that there is a solution of equations (2) and (3) satisfying x(O) - z, z( T) = y. Theorem 4. Suppose we are given the constrained Hamiltonian control system defined by equations (2) and (3). Suppose that the control constraints u (t) _ 1, i=1,, r (30) 12 Hamiltonian Systems

RSD-TR-22-86 are imposed, and the constraint manifold N is compact. Then the system is controllable on the constraint manifold N if and only if the vector fields {XN, Yx,. o,9 yN} satisfy the accessibility rank condition, where XN is the drift Hamiltonian vector field restricted to the manifold N and yN are the control vector fields restricted to the manifold N, j=, -, r. Proof: Recall that the accessibility rank condition for {XN, yN,..., yN} is that the Lie algebra generated by (XN, yjN..., Vy spans the tangent space of N at each point in N. Necessity then follows from the results of Sussman and Jurdjevich in [19] and sufficiency from the results of Bonnard in [5], since, by our earlier analysis, XN is Hamiltonian on N which is compact and hence the set of points which are Poisson stable with respect to XN is dense in N. (Poisson stability is discussed in [5].) 6. Control Of Robot Tasks Defined By HIolonomic Constraints Finally we describe several robot tasks which give rise to the imposition of holonomic constraints of the above form. Additional details, including the specific form of the equations in Lagrangian form, are given in [161. Such holonomic constraints naturally arise in cases where the end effector of the robot interacts with its environment in a way that should be reflected in the dynamics of the robot. We mention three cases here. If the end effector of the robot is to pick up an object (where the dynamics of the object are not neglibible), then the system d-ynamics are defined by the dynamics of the robot and the dynamics of the object constrained so that the end effector of the robot grasps the object. If Hatnmiltonian Systems 13

RSD-TR-22-86 the end effector of the robot is to move along a specified (rigid and noncompli~ ant) surface, then the system dynamics should reflect the robot dynamics plus the contact force required to maintain satisfaction of contact between the end effector and the surface. If two robots are to cooperatively grasp a rigid object, then the system dynamics are defined by the dynamics of each robot and by the constraint that they are grasping a common object. These robot tasks are beyond the capability of most current industrial robots; there is no existing method for control of the robots to cause them to carry out the desired task. It is suggested here that the proper way to view such advanced robot problems is through a formulation involving constrained Hamiltonian control systems. 7. References [11] V.I. Arnold, Mathematical Methods of Classical Mechanics, Springer-Verlag, 1978. [2] J. Baumgarte, "Stabilization of Constraints and Integrals of Motion in Dynamical Systems," Computer Methods in Applied Mechanics and Engineering, 1, 1972, 1-16. [3] G.D. Birkhoff, Dynamical Systems, AIMS Publications, 1927. [4] A.M. Bloch, "Left Invariant Control Systems on Infinite-Dimensional Homogeneous Spaces," Proceedings of the 24th IEEE Conference on Decision and Control, 1985, 1027-1030. 14 Hamiltonian Systems

RSD-TR-22-88 [5] B. Bonnard, "Controllabilite de Systemes Mechaniques sur le Groups de Lie," SIAM J. Control and Optimization, 22, 1984, 711-722. [6] R.W. Brockett, "Control Theory and Analytical Mechanics," in Geometric Control Theory, eds. C. Martin and R. Hermann, 3, Math. Science Press, 1977, 1-46. [7] S.L. Campbell, Singular Systermse of Differential Equations, Pitman Publishing Co., 1980 and 10982. [8] S.L. Campbell, "Nonlinear Time-varying Generalized State-space Systems: An Overview," Proceedings of the 23rd IEEE Conference on Decision and Control, 1984, 268-273. [9] P. Crouch and M. Irving, "On Finite Volterra Series which Admit Hamiltonian Realizations," Math. Systems Theory, 17, 1984, 329-333. [10] P. Deift, F. Lund and E. Trubowitz, "Nonlinear Wave Equations and Constrained Harmonic Motion," Communications in Mathematical Physics, 74, 1080, 141-188. [111 P.A.M. Dirac, Lectures on Quantum Mechanics, Belfer Graduate School of Science Monographs, No. 3, Yeshiva University, 1964. [12] C.W. Gear, G.K. Gupta, and B. Leimkuhler, "Automatic Integration of Euler Lagrange Equations with Constraints," J. Comp. and Applied Math (to appear). [13] H. Goldstein, Classical Mechanics, Addison-Wesley, 1950. Hamiltonian Systems 15

UNI vER8 OF MICHIGAN RSD-TR-22-86 is r a earo 1111s 3 9015 03465 8131 [14] V. Guillemin and S. Sternberg, Symplectic Techniques in Physics, Cambridge University Press, 1984. [15] H.P. Huang and N.H. McClamroch, "Time Optimal Control for a Robotic Contour Following Problem," Robot Systems Division, RSD-TR-24-86, University of Michigan, October, 1986. [16] McClamroch, N.H., "Singular Systems of Differential Equations as Dynamic Models for Constrained Robot Systems," Proceedings of IEEE Conference on Robotics and Automation, San Francisco, 1986. [17] N.H. McClamroch and D. Wang, "Feedback Stabilization and Tracking of Constrained Robots," Robot Systems Division, RSD-TR-25-86, University of Michigan, September, 1986. [18] L.R. Petzold, "Differential/Algebraic Equations are not ODE's," SIAM J. Sci. Stat. Comp., 3, 1982, 367-384. [19] H.J. Sussman and V. Jurdjevcich, "Controllability of Nonlinear Systems," J. Differential Equations, 12. 1972, 95-116. [20] A.J. van der Schaft, "Hamiltonian Dynamics with External Forces and Observations," Math. Systems Theory, 15, 1982, 145-168. [21] R.W. Weber, "Hamiltonian Systems with Constraints and their Meaning in Mechanics," Archive for Rational Mechanics and Analysis, 91, 1986, 309-335. 18 3[Hamiltonian Systems