Formulation of Dynamics, Actuation, and Inversion of a Three-Dimensional Two-Link Rigid Body System ••••••••••••••••• •••••••••••••• Hooshang Hemami Department of Electrical Engineering The Ohio State University Columbus, Ohio 43210 Behzad Dariush Honda Research Institute Suite 300 Mountain View, California 94041 Kamran Barin College of Medicine The Ohio State University Columbus, Ohio 43210 Received 19 July 2002; accepted 19 May 2005 In this paper, three issues related to three-dimensional multilink rigid body systems are considered: dynamics, actuation, and inversion. Based on the Newton-Euler equations, a state space formulation of the dynamics is discussed that renders itself to inclusion of actuators, and allows systematic ways of stabilization and construction of inverse sys- tems. The development here is relevant to robotic systems, biological modeling, human- oid studies, and collaborating man-machine systems. The recursive dynamic formulation involves a method for sequential measurement and estimation of joint forces and couples for an open chain system. The sequence can start from top downwards or from the ground upwards. Three-dimensional actuators that produce couples at the joints are included in the dynamics. Inverse methods that allow estimation of these couples from the kinematic trajectories and physical parameters of the system are developed. The formulation and derivations are carried out for a two-link system. Digital computer simulations of a two- rigid body system are presented to demonstrate the feasibility and effectiveness of the methods. © 2005 Wiley Periodicals, Inc. Journal of Robotic Systems 22(10), 519–534 (2005) © 2005 Wiley Periodicals, Inc. Published online in Wiley InterScience (www.interscience.wiley.com). • DOI: 10.1002/rob.20083 520 • Journal of Robotic Systems—2005 1. INTRODUCTION theless, preliminary studies have demonstrated the feasibility of this approach.9,10 In many cases, the con- Systems of interconnected rigid bodies for robotic tribution of sensory modalities can be summarized in and natural systems have been the subject of studies 1–3 the form of measured angles and angular velocities. for many years. In developing the equations of mo- Similarly, the motor control mechanisms can be rep- tion of rigid body structures, the robotics community resented by actuators that generate joint torques. has especially focused on the problem of computa- The single rigid body case is considered first tional efficiency, with many of the most efficient al- where free motion, constrained motion, and actuators gorithms based on a recursive Newton-Euler 4 are discussed. The two-link system is considered in formulation. Indeed computational efficiency is im- Section 3. Stability and inverse plant are addressed in portant for the simulation and control of increasingly Sections 4 and 5. Simulations in Section 6, and dis- complex mechanisms of interconnected rigid bodies. cussion and conclusions in Section 7 conclude the However, equally important is developing formula- paper. tions which are easy to implement and capable of ad- dressing specific needs of not only robotic systems, but also natural systems for the purpose of simulat- ing mechanisms of human motor control. 2. SINGLE RIGID BODY This paper addresses some of the basic issues re- lated to dynamics and control of rigid body systems. In this section the Newton-Euler equations are for- This is especially true for natural systems, where the mulated. In the Newton-Euler formulation, the equa- complexity of both body dynamics and motor control tions are not expressed in terms of independent vari- mechanisms have so far prevented widespread appli- ables and involve constraint forces, which must be cation of these models to physiological and clinical eliminated in order to explicitly describe the input questions. Although the theoretical results of this pa- output relationships and arrive at closed-form equa- per are very general, the focus is to provide the ana- tions. Arithmetic operations are needed to derive the lytical framework for systematic study of underlying closed-form dynamic equations and reduce the di- neural control mechanisms that mediate walking and mensionality of the system to a minimum. This rep- quiet-standing in human. For example, current con- resents a complex procedure which requires physical ceptual models of the way humans maintain their up- intuition. Systematic methods have previously been right stability are based on intuitive and empirical in- developed to project the dynamic equations of a terpretation of experimental data. One such model is larger dimensional state space to a closed form re- described by Nashner and his associates.5 Their ap- duced state space.11 One contribution of this paper is proach is based on partitioning the movement space to introduce systematic methods to recursively into discrete regions. Placement of the body in any of project the dynamic equations in order to eliminate these regions generates a preprogrammed muscle ac- joint reaction forces and redundant state variables. tivation pattern that attempts to restore upright sta- Alternatives to the Newton-Euler formulation of bility. At least three different control strategies have rigid body dynamics are the Lagrangian and Kane’s been identified for maintaining postural stability in formulations,12 which describe the behavior of a dy- the sagittal plane: ankle strategy, hip strategy, and namic system in terms of work and energy stored in stepping strategy.6 Some investigators have ex- the system rather than of forces and momenta of the pressed skepticism about the application of rigid- individual members involved. The constraint forces body dynamics to the strategy-oriented model of pos- involved in the system are automatically eliminated tural control.7 However, it is demonstrated that the in the formulation and the closed-form dynamic inverted pendulum model of postural dynamics can equations can be derived systematically in any coor- be used to test the strategy-oriented hypotheses of dinate system. Closed form formulations based on control mechanisms.8 Lagrangian formulation, Kane’s formulations, and Addition of feedback elements that represent Newton-Euler with projections,11 have certain advan- sensory and motor mechanisms to the three- tages in control; however with the exception of the dimensional model described here can provide the latter, these methods have sometimes been criticized framework to apply these models to clinical and by those in the area of biomechanics and human physiological questions. This is not a trivial step. The movement science because most joint reaction forces feedback elements are most likely nonlinear, perhaps are automatically eliminated early in the process of discrete, and in many cases highly simplified. None- deriving dynamics equations. In addition, for mod- Hemami et al.: Formulation of Dynamics • 521 eling high degree of freedom systems, recursive ICS, is given in Appendix A. The resulting torque methods are simpler to implement and considerably from the action of ⌳ is faster. First, we present the dynamics of a single rigid R˘ AЈ⌳. body and introduce the notation. The formulation is then extended to two rigid bodies. Let vectors of force G and H be, respectively, the gravity vector and the resultant vector of other 2.1. Free Motion forces acting on the body. The torque N1 is the result of all couples and moment of all the remaining Before the state space of one rigid body and its for- forces, except ⌳, operating on the body. The diago- mulation are presented, a transformation that con- nal moment of inertia matrix of the rigid body, along verts cross product of vectors to a matrix transfor- its principal axes, is given by J, where mation is presented. The introduction of a skew symmetric matrix allows the representation of vector ͓ ͔ J = diag j1,j2,j3 . cross products as a transformation. This is more con- venient for computer simulation and calculations. To The nonlinear vector f͑⍀͒ is defined in Appendix A. illustrate this issue, let a vector R represent a posi- An additional matrix B͑⌰͒ defines the relation be- tion and a vector F, acting at the tip of vector R, tween ⍀ and ⌰˙ . Matrix B is specified in Appendix A. represent a force. The torque of the force F is With these definitions, the state space equations of described by the cross product of R and F. The cal- motion of the single rigid body are culation of this torque can be carried out as a transformation: ⌰˙ = B͑⌰͒⍀, R ϫ F = R˘ F. J⍀˙ = f͑⍀͒ + N + R˘ AЈ⌳, The skew symmetric 3ϫ3 matrix R˘ is given in Ap- 1 pendix A. With these preliminaries, consider a single rigid body.13 Let ⌰ and ⍀ be, respectively, the Euler angles X˙ = V, and the angular velocity vector of the body, ex- pressed in the body coordinate system ͑BCS͒ mV˙ = G + H + ⌳. ͑1͒ ⌰ ͓␪ ␪ ␪ ͔Ј = 1, 2, 3 , The first two equations above describe the rotational motion of the rigid body. The last two equations de- ⍀ ͓␻ ␻ ␻ ͔Ј = 1, 2, 3 . scribe the translation of the body in the ICS. The sequence of angles above corresponds to roll, pitch, and yaw, respectively. Let X and V be the 2.2. Constrained Motion translational vectors of position and velocity of the The inverted three-dimensional pendulum can be center of gravity of the body in the inertial coordi- used to model the human torso, the human head or ͑ ͒ ⌳ nate system ICS . Let be a 3-vector of force acting certain robots. The equations of the inverted pendu- at a point c1 on the body whose coordinates are vec- lum can be systematically derived from the free mo- tor R in BCS. This force could correspond to the vec- tion equations above as shown below. We consider ⌳ tor of ground reaction forces. Let vector be speci- connection of the rigid body either to a stationary fied in the ICS. In connection with vector R,we point or to a moving platform. Suppose the rigid define the 3ϫ3 matrix R˘ .