RSD-TR- 13-84 DYNAMICS OF A CLOSED CHAIN MANIPULATOR1 N. Harris McClamroch Department of Aerospace Engineering The University of Michigan Ann Arbor, Michigan 48109-1109 Han-Pang Huang Department of Electrical Engineering and Computer Science The University of Michigan, Ann Arbor, Michigan 48109-1109 September 1984 CENTER FOR ROBOTICS AND INTEGRATED MANUFACTURING Robot Systems Division COLLEGE OF ENGINEERING THE UNIVERSITY OF MICHIGAN ANN ARBOR, MICHIGAN 48109-1109 1This work was supported by the Air Force Office Office of Scientific Research/AFSC, United States Air Force under AFOSR contract number F49620-82-C-0089.

TABLE OF CONTENTS 1. INTRODUCTION............................................................................... 2 2. MODELLING A CLOSED CHAIN MANIPULATOR....................... 2 3. DIRECT DYNAMICS AND INVERSE DYNAMICS.......................... 7 4. CONCLUSIONS.................................................................................. 12 5. REFERENCE...................................................................................... 14

RSD-TR- 13-84 ABSTRACT In many manipulator configurations, where the end effector of the manipulator is in contact with a fixed object, a complete mathematical model for the manipulator dynamics should include the effects of the resulting contact force between the end effector and the fixed object. Equations for such a closed chain manipulator are developed, where the end effector constraint is defined by a smooth manifold. These equations are shown to be complete in the sense that the direct dynamics problem and the inverse dynamics problem are well-posed. This formulation suggests a new approach to planning and tracking control for closed chain manipulators. Closed Chain 1

RSD-TR-1 3-84 1. INTRODUCTION There are numerous applications where the end effector of a robot manipulator is in contact with a fixed external object. These applications include the use of manipulators for carrying out assembly and machining tasks. However, the most common models of manipulator dynamics do not take into account the contact forces between the end effector of the manipulator and the fixed object [5,6,11]. If the contact force can be directly sensed, then it is possible to explicitly eliminate the contact force from the model, and this forms the basis for several control approaches [8,9,10,12]. However, direct and accurate sensing of the contact force is not always possible so that a complete dynamic model of the manipulator, including the effect of the contact force, is required. In this paper we develop such a model and indicate two important dynamics problems, each of which we show to be well-posed. By imposing a contact constraint on the end effector, the manipulator links form a so-called closed chain; hence, we refer to a closed chain manipulator. Closed chain mechanical manipulators have been studied in [1,3,7], but the treatment on which our results are based is in [3]. 2. MODELLING A CLOSED CHAIN MANIPULATOR Let pER n denote the position vector of the end effector of the manipulator, in terms of a fixed workspace coordinate system. Suppose that constraints on the end effector are given as Closed Chain 2

RSD-TR-13-84 0(p)= O (1) where q:R n -OR m is twice continuously differentiable. A closed chain system is formed through continuous contact of the tip of the manipulator with the manifold defined by the constraints. Let S be a frictionless manifold defined by the constraints S= { pER:..(p)=O, i=l,...,m} (2) We also assume that the gradient vectors Vo1(P),..., Vtqm(P) are linearly independent for all pES, so that the constraints define an m dimensional smooth manifold. If p o is a point on S, then we can define the normal space of S at po as N(p ) p:p = E aVi (p o), i1=,...,m (3) and the tangent space of S at p o as T(po) = {p:<p,y> = 0, yEN(po)} (4) N(po ) and 1po ) are subspaces of R" and are orthogonal complements of each other so that R = T(p 0) G N(p 0) (5) In order that the manipulator does not lose contact with the constraint manifold, it is required that the velocity of the manipulator end effector lie in the 3 Closed Chain

RSD-TR-13-84 tangent space of S and the contact force lie in the normal space of S. We refer to the constraints on the end effector velocity and force due to contact as the natural constraints. Let qER n denote the vector of robot joint coordinates [5,6]. The relation between robot coordinates and workspace coordinates can be expressed as p = H(q) (6) where H: R n -OR n is twice continuously differentiable. We also assume that the manipulator is nonredundant, viz., the Jacobian matrix OH(q) is nonsingular and square. It is convenient to define the manipulator equations of motion in terms of the joint coordinates [4]. Let rER" be the generalized joint torque vector required to maintain satisfaction of the path constraints; then the dynamic equations for a closed chain manipulator, taking into account the contact force, can be written as [2] M(q)q + F(q,q)= T + r (7) Here TER' is the generalized input joint torque vector. M(q) denotes the inertial matrix which is symmetric and nonsingular. F(q,4) comprises Coriolis terms, centrifugal terms, and gravitational terms. Since no work is done by the contact torque r in a virtual displacement 6q, Closed Chain 4

RSD-TR-13-84 n 3r 6q1 = 0 l-1 or equivalently in workspace coordinates n E3t 6p, = 0 1 —1 where Sq,..., qn ( sp 1,..., 6p, ) represent scalar virtual displacements of the end effector in robot (workspace) coordinates.? is the generalized force vector, due to contact, in workspace coordinates. Using (6), we have n n n O9HI (H(q) = 1 i=.= l1 q Thus a H:(q) Ti; --, (i=1,...,n) From the constraints we know that the virtual displacements bp 1,*, S6Pn must satisfy Soj (P) = E ( Pl'P= O, (j=l,..., m) We now introduce Lagrange multipliers X1,... Xm; multiply this equation by Xi to obtain 5 Closed Chain

RSD-TR-13-84 xi aqj(p) i = 0, (j=l,...,m) Sum up these m equations; using a previous equation obtain n' m Oi(p) ] E' - E~ Pt bpi =.0 Since,(P), are linearly independent vectors, X1,. can ap ap be chosen such that j =1i F), (1/,...,n) The corresponding contact force components, computed in robot coordinates, are n m aOi (p) OHt (q) =l — 1 -apt aq; Let Jacobian matrices be defined as a oH(q) -4q) aq D(p) O( then the complete set of equations of motion can be written, using vector notation, as Closed Chain 6

RSD-TR-13-84 M(q)q + F(q, )= T + r (8) r - JT(q)D T (p)X (9) P = H(q) (10) +(p)= 0 (11) or equivalently as M(q)t' + F(q,) = T + JT(q)f (12) f DT(p)X (13) p = H(q) (14)' q(p)= O (15) where X = (X1,..,X, ) T 3. DIRECT DYNAMICS AND INVERSE DYNAMICS Given a specified motion of the manipulator, satisfying the imposed constraints, can we compute the input joint torque vector which would generate this specified motion and the corresponding contact force? This problem is usually referred to as the direct dynamics problem. Similarly, the inverse dynamics problem considers the following: given the input joint torque vector, what is the manipulator motion, satisfying the imposed constraints, and the corresponding contact force necessary to maintain satisfaction of the constraints? Calculation 7 Closed Chain

RSD-TR-13-84 of the contact forces plays an important role in these problems. The existence (and uniqueness) of the manipulator motion and joint torques is also addressed. Throughout this section, we assume that the constraint manifold is frictionless. The desired motion of a manipulator is often specified in workspace coordinates instead of robot coordinates. Using the coordinate transformation given by (6) define J(q,q) t [d fq(t)] then velocity and acceleration of the end effector in workspace coordinates are p_ J(q,4)q + J(q)q Assume the motion p(t)ER ", to<t<tf, given in workspace coordinates, satisfies (15). The corresponding contact forces and the system dynamics are implicitly defined by (12),(13),(14). Note that At) is defined in workspace coordinates. We now make the following assumptions: (Al) The inertial matrix M: Rn -+R' is nonsingular. F: R n X Rn - Rn is bounded and Lipschitz continuous. (A2) q:R n — R m is twice continuously differentiable. (A3) J(q) is nonsingular for all qER n. D(p) has rank m fqr all pER" (A4) The m X m matrix A(q) -= D(H(q))q)M-l(q)J T (q) D T (H(q)) Closed Chain 8

RSD-TR-13-84 is nonsingular. Two basic propositions are derived below. Proposition 1: Suppose that the torque vector T(t)ERn is piecewise continuous on to<t<tf. Given T(t) and initial values q(to) = qo, q(to) = qo, satisfying H(qo)ES, J(qo)qoET(H(qo)). Then with assumptions (A1)'(A4) there exists a unique contact force f(t) such that the solution of (12) satisfies the path constraints (15), assuming there is no finite escape time, for to<t< t Proof: Our objective is to obtain an expression for the contact force which guarantees. satisfaction of the path constraints. To this end, suppose that 0(p)=O so that dO(p) = D(p)p = o dt d2~() = D(p)p + Dt(p,p)p o dt2 where D(p,Pj) dt D(p(t)). Since M(q) is nonsingular, 4 is obtained as 4 = M -(q)[ T - F q,q) ] + M-(q)JT (q)f From (13) obtain A(q)X = D(Iq))J(q)M-l(q)[ F(q,) - T] - [D(H(q))J(q,q) + D(H(q),J(q)q)J(q) J 9 Closed Chain

RSD-TR-13-84 Note that D(p) is not a square matrix in general. These are m linear equations in m unknowns XERm; Since A(q) is nonsingular, X can be uniquely determined as a linear function of q,q, T X = g(q,j,T) where ( q, T) A -'(q)D(H(q))J(q)M-l(q)[ F(gq,) - T] - A -'(q)[ D(H(q))J(q,O) + D(H(q),J(q))J(q) ]q Hence, the resulting contact force is f = D T(H(q))g(q,, T ) Eqns. (12), (13) reduce to the following initial value problem. M(q) Q + F(q,q) = T + JT(q)D T (H(q))g(q,O, T) q(to) = qo, q(to)-= o This initial value problem has a unique solution q(t) defined for to0 t to + 6 for some 6>0. Assuming there is no finite escape time, the solution is defined for to< t<t. We now show that the constraints are satisfied throughout the motion. For the contact force X = g(q,,T 7), to0t<tf,it can be verified that p(t)=H(q(t)) satisfies Closed Chain 10

RSD-TR-13-84 d2'(p) =0 to<t<t1 dt2 But, since H(qo)ES, J(qo)4oET(H(qO)) it follows that do(p(t o)) q(p(t o)) = 0, d = dt Thus necessarily (p(t))= 0, t 0< t< t Proposition 2: Given motion q(t), satisfying q(H(q(t))) = 0, to<t_<t, which is twice continuously differentiable. Under assumptions (AI).(A3), there exist input joint torque vector T(t) and corresponding contact force vector At)EN(H(q(t))) satisfying (12) for t o< tf t Pro of: From (12), (13), we have M(q) Q + Fq, ) = T + J T (q) D T (H(q))X Given q(t), to<t<tf, the left hand side of the equation is determined. Clearly there are many T(t), X(t), ttt satisfying the equation. 11 Closed Chain

RSD-TR-13-84 As indicated in the proof, there are many input joint torque vector functions and contact force function which generate the same given motion. For a specified contact force function, the torque T(t) is uniquely defined. The case of an open chain manipulator corresponds to the assumption that the contact force At)=O, to<t<tf, so that the torque T(t) is uniquely determined. 4. CONCLUSIONS We have carefully developed a general model for a closed chain manipulator, and we have shown that the direct and inverse dynamics problems are well-posed. We believe that the closed chain manipulator model is the appropriate model to use in control design and analysis, where the manipulation task involves contact between the manipulator end effector and a fixed object. Numerous approaches to control of manipulators, not including a closed chain constraint, have been developed [5,6,11]. It is likely that those control approaches can be suitably modified to apply to the closed chain case, but the modifications and extensions have not yet been developed. It is possible to eliminate the path constraint and the contact force from the dynamic model [3], by a suitable elimination of variables. However, the resulting equations are usually extremely complicated; moreover, elimination of the contact force from the model may not be most desirable if the contact force represents a variable to be controlled. Thus an interesting challenge is to develop suitable control approaches, based on the complete equations (12)-(15) Closed Chain 12

RSD-TR-13-84 developed here. In our development, a number of specific assumptions have been made. Important extensions would include incorporation of friction effects on the constraint set, allowance of nonsmooth constraint sets, and consideration of constraint sets defined in terms of inequalities so that contact between the end effector and the fixed object need not be continuously maintained. 13 Closed Chain

RSD-TR-1 3-84 5. REFERENCE [1] Draganoiu, G.,et al., "Computer Method for Setting Dynamical Model of an Industrial Robots with Closed Kinematic Chains." The 12 th Int. Symp. on Industrial Robots, June 1982, Paris, France. [2] Goldstein, H., "Classical Mechanics." Cambridge, Massachusetts: AddisonWesley Press, 1950. [3] Hemami, H., and Wyman, B.F., "Modelling and Control of Constrained Dynamic Systems with Appl. to Biped Locomotion in the Frontal Plane." IEEE Trans. on Automatic Control. AC-24(4):526-535, August 1979. [4] Lee, C.S.G., "Robot Arm Kinematics, Dynamics, and Control." Computer. 62-80, Dec. 1982. [5] Luh, J.Y.S., "Conventional Controller Design for Industrial Robots —a Tutorial." IEEE Trans. on Systems, Man, and Cybernetics, SMC13(3):298-316,1983. [6] Luh, J.Y.S., "An Anatomy of Industrial Robots and their Controls." IEEE Trans. on Automatic Control, AC-28(2):133-153, 1983. [7] Orin, D.E., and Oh, S.Y., "Control of Force Distribution in Robotic Mechanisms Containing Closed Kinematic Chains." ASME J. of Dynamic Systems, Meas., and Control. 102:134-141, June 1981. [8] Paul, R.P., and Shimano, B., "Compliance and Control." 1976 Joint Automatic Control Conf., 694-699. [9] Raibert, M.H., and Craig, J.J., "Hybrid Position/ Force Control of Manipulators." ASME J. of Dynamic Systems, Meas., and Control. 102:126-133, June 1981. [10] Salisbury, J.K., "Active Stiffness Control of a Manipulator in Cartesian Coordinates." IEEE 1980 Decision and Control Conf., 95-100. [11] Vukobratovic, M., and Stokic, D., "Control of Manipulation Robots — Theory and Appl." New York: Springer-Verlag, 1982. Closed Chain 14

UNIVERSITY OF MICHIGAN 1 III iliill ili 111111111 I l l iii11111 11111 1111111111111 RSD-'TR'13 050846 3 9015 03465 7992 2WP Manip'ltor Compliance based o Joi nt Torque;11 Paul, R. nIManipulator R. iO-nand ContrOl SS94 Co121 trol Pro. of the 1980 Conf. on Decision and Control, -94. Closed Chain 15