Techniques for the Design and Simulation of Running Robots by ...

Techniques for the Design and Simulation of Running Robots by John Robert Ridgely

B.S. (University of California at Berkeley) 1986 M.S. (University of California at Berkeley) 1988

A dissertation submitted in partial satisfaction of the requirements for the degree of Doctor of Philosophy in Engineering – Mechanical Engineering in the GRADUATE DIVISION of the UNIVERSITY OF CALIFORNIA, BERKELEY

Committee in charge: Professor David M. Auslander, Chair Professor Dennis Lieu Professor Robert Full 2001

The dissertation of John Robert Ridgely is approved:

Chair

Date

Date

Date

University of California, Berkeley

2001

Techniques for the Design and Simulation of Running Robots

Copyright 2001 by John Robert Ridgely

Abstract

Techniques for the Design and Simulation of Running Robots by John Robert Ridgely Doctor of Philosophy in Engineering – Mechanical Engineering University of California, Berkeley Professor David M. Auslander, Chair

A functional definition of ballistic running for machines is proposed. Ballistic running is defined as a type of running in which there is a distinct flight phase during which no force or negligible forces are applied to the body by the legs, and in which the duration of the stance phase is sufficiently short that the action of the legs can be represented as applying an impulse to the body. This type of running, then, is a process by which the body moves through space in a series of ballistic trajectories separated by impulses. The definition of ballistic running suggests a hierarchical partitioning of the control of a running system. At the highest level, control consists of determining a set of impulses which will cause a body to move along a desired trajectory. A second level coordinates the actions of multiple legs so that the net impulse provided by all the legs adds up to the impluse desired by the top level controller. If enough legs are available to provide the impulse, the leg coordination can be arranged so that each leg provides only linear forces,

1

not torques, against the body at the hip. The lowest level of control is that applied to each leg, causing the leg to move in such a way as to provide the desired impulse. It is shown that a control system can be designed using such a hierarchical structure; this type of control system is able to control and stabilize a running multi-legged robot in the presence of small disturbances. The modular nature of this control architecture should allow running robots to be designed in a manner which is simpler than many methods used in the past in that less sensors and actuators are required. In order to test this hypothesis, controllers are designed and tested on several types of simulations. Simple simulations which model the dynamics of the robot body but do not model the mass properties of the legs are carried out using traditional NewtonEuler modeling techniques. In order to facilitate more complex simulations which involve the dynamics of multiple legs as well as the body, a novel method of dynamic simulation is introduced. This dynamic simulation method involves the simulation of mechanical systems as sets of masses connected by springs, dampers, and other simple connections.

Professor David M. Auslander Dissertation Committee Chair

2

Contents List of Figures 1 Introduction 1.1 Motivation . . . . . . . . . . . . 1.2 Prior Art . . . . . . . . . . . . . 1.2.1 Purely Active Stability . . 1.2.2 Passive Stability . . . . . 1.3 A Different Philosophy . . . . . . 1.3.1 Motivating Principles . . 1.3.2 The Impulse Model . . . . 1.4 Dynamic Simulation Techniques . 2 An 2.1 2.2 2.3 2.4

2.5

2.6 2.7 2.8

iv . . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

1 2 4 4 5 8 8 10 13

Impulse Model of Running Introduction . . . . . . . . . . . . . . . Pulse vs. Impulse Models . . . . . . . A Control Hierarchy . . . . . . . . . . The Body-Only Model . . . . . . . . . 2.4.1 Simulation Technique . . . . . 2.4.2 Body-Only Results . . . . . . . Control With Multiple Legs . . . . . . 2.5.1 Leg Design . . . . . . . . . . . 2.5.2 Control Strategy . . . . . . . . 2.5.3 Allocation of Impulses . . . . . 2.5.4 Leg and Impulse Results . . . . From Impulses to Pulses . . . . . . . . 2.6.1 Pulse Model Results . . . . . . Pseudo-Rigid Spring-Mass Simulation 2.7.1 Impulse-Leg Results . . . . . . Pseudo-Rigid Model with Leg Springs 2.8.1 Controlling Legs with Springs . 2.8.2 Leg-Spring Model Results . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . .

15 15 17 19 22 23 29 32 32 34 35 38 39 40 43 45 46 49 50

. . . . . . . .

. . . . . . . .

i

3 Pseudo-Rigid Modeling of Mechanical Systems 3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . 3.1.1 Historical Perspective . . . . . . . . . . . . . 3.1.2 Related Work . . . . . . . . . . . . . . . . . . 3.1.3 Introduction to the Software . . . . . . . . . 3.2 Model Components . . . . . . . . . . . . . . . . . . . 3.3 Language Considerations . . . . . . . . . . . . . . . . 3.4 Representing and Displaying Attitudes . . . . . . . . 3.4.1 Rotation Matrices . . . . . . . . . . . . . . . 3.4.2 Euler Angles . . . . . . . . . . . . . . . . . . 3.4.3 Euler Parameters and the Quaternion . . . . 3.4.4 Computing the Rotation Matrix . . . . . . . 3.4.5 Rotation Matrices To and From Euler Angles 3.4.6 Angular Velocity . . . . . . . . . . . . . . . . 3.5 Modeling and Numerical Issues . . . . . . . . . . . . 3.5.1 Systems and Solvers . . . . . . . . . . . . . . 3.5.2 Energy . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

55 55 56 60 63 72 73 75 76 77 79 81 84 88 92 92 94

4 Modeling Software Components 4.1 Utility Software Components . . . . . . . 4.1.1 Container Classes . . . . . . . . . . 4.1.2 Matrix Utility Classes . . . . . . . 4.1.3 ODE Solvers . . . . . . . . . . . . 4.2 Model Components . . . . . . . . . . . . . 4.2.1 Nodes . . . . . . . . . . . . . . . . 4.2.2 Reference Points . . . . . . . . . . 4.2.3 Connections . . . . . . . . . . . . . 4.2.4 Bodies . . . . . . . . . . . . . . . . 4.2.5 Constraint Surfaces . . . . . . . . 4.3 Applying Forces to a Body . . . . . . . . 4.3.1 Regular Masses for Force Transfer 4.3.2 Dummy Masses for Force Transfer 4.3.3 Ghost Points for Force Transfer . . 4.4 Example Problems . . . . . . . . . . . . . 4.4.1 Free Motion of a Satellite . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

97 97 99 100 104 113 114 115 117 121 130 131 133 134 136 144 144

5 Summary 5.1 Running . . . . . . . . . . . 5.2 Simulation Studies . . . . . 5.3 Dynamic System Modeling 5.4 Future Work . . . . . . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

153 153 155 156 157

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

ii

A Source Code for Matlab Simulations 161 A.1 Files for Body-Only Simulation . . . . . . . . . . . . . . . . . . . . . . . . . 161 A.2 Files for Body With Legs Impulse Model Simulation . . . . . . . . . . . . . 169 A.3 Files for Body With Legs Pulse Model Simulation . . . . . . . . . . . . . . 179 B MechSys2 Software Reference B.1 CAttitude Class Reference . . . . . . . . . . . B.2 CBaseAdaptiveSolver Class Reference . . . . B.3 CBaseEulerAngles Class Reference . . . . . . B.4 CBaseFixedSolver Class Reference . . . . . . B.5 CBaseForce Class Reference . . . . . . . . . . B.6 CBaseSolver Class Reference . . . . . . . . . B.7 CBaseSurface Class Reference . . . . . . . . . B.8 CBody Class Reference . . . . . . . . . . . . B.9 CBodyRelativeForce Class Reference . . . . . B.10 CConnection Class Reference . . . . . . . . . B.11 CDummyMass Class Reference . . . . . . . . B.12 CDynaBase Class Reference . . . . . . . . . . B.13 CDynaSolver Class Reference . . . . . . . . . B.14 CDynaSystem Class Reference . . . . . . . . B.15 CEulerAngles X Y Z Class Reference . . . . . B.16 CEulerAngles Z X Z Class Reference . . . . . B.17 CEulerAnglesXwYwZw Class Reference . . . B.18 CEulerSolver Class Reference . . . . . . . . . B.19 CGeneralBody Class Reference . . . . . . . . B.20 CGhostPoint Class Reference . . . . . . . . . B.21 CGroundConstraint Class Reference . . . . . B.22 CLinearDamper Class Reference . . . . . . . B.23 CLinearFriction Class Reference . . . . . . . B.24 CLineSurface Class Reference . . . . . . . . . B.25 CMass Class Reference . . . . . . . . . . . . . B.26 CNode Class Reference . . . . . . . . . . . . . B.27 CPlaneSurface Class Reference . . . . . . . . B.28 CPointSurface Class Reference . . . . . . . . B.29 CRefPoint Class Reference . . . . . . . . . . . B.30 CRK2FixedSolver Class Reference . . . . . . B.31 CRK45AdaptiveSolver Class Reference . . . . B.32 CRK4FixedSolver Class Reference . . . . . . B.33 CRosenbrockAdaptiveSolver Class Reference B.34 CSpring Class Reference . . . . . . . . . . . . B.35 CWorldRelativeForce Class Reference . . . . Bibliography

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

193 193 197 202 207 209 215 224 229 239 241 246 247 248 250 256 259 262 265 266 269 272 274 277 280 284 288 295 298 301 304 305 307 308 310 313 316

iii

List of Figures 2.1 2.2 2.3 2.4 2.5 2.6 2.7 2.8 2.9 2.10 2.11 2.12 2.13

Control System Hierarchy . . . . . . . . . . The Simulated Body . . . . . . . . . . . . . Body-Only Simulation Results . . . . . . . Robot and Leg Configuration . . . . . . . . Robot and Legs Impulse Simulation Results Robot and Legs Pulse Simulation Results . Robot and Legs Pulse Simulation Results . Spring-Mass Body Model . . . . . . . . . . Hopping Height Control of the Body . . . . Spring-Mass Model Layout . . . . . . . . . Drop Test of the Leg Springs . . . . . . . . A Different Hopping Height . . . . . . . . . Changing the Hopping Height . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

. . . . . . . . . . . . .

20 29 31 34 38 41 42 44 46 48 51 52 53

3.1 3.2 3.3 3.4 3.5

Generic Spring-Mass Body . . . . . . Rotations Through Euler Angles . . Rotating a MechSys2 Body . . . . Simulation which Becomes Unstable Spring Hopping with Energy . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

70 78 82 93 95

4.1 4.2 4.3 4.4 4.5 4.6 4.7 4.8 4.9

ODE Solver Classes . . . . . . . . . Arrangement of Masses in Body . . . Body Connected to Plane Constraint Force Transfer by Dummy Mass . . . Force Transfer by Force Pairs . . . . Example Body . . . . . . . . . . . . Components of Angular Velocity . . MechSys2 Model . . . . . . . . . . Results of Satellite Simulation . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

106 123 132 135 142 145 147 149 152

iv

Chapter 1

Introduction This dissertation documents work performed toward the goal of improving the technology of machine running. This work has consisted of two principal parts: • The exploration of a novel model of the running process and the verification through simulation studies of its usefulness • The implementation of a novel technique for the dynamic modeling of mechanical systems, which is well suited for the modeling of running machines This work provides a foundation for further research which may lead to the creation of a unique class of running machines, first in simulation, then as prototypes. The dynamic modeling scheme which is introduced will have applications to many other fields as well, in particular enhancing the educational benefits of dynamic simulation software.

1

1.1

Motivation Mobile robots are receiving increasing attention from the research, industrial, and

military communities. Traditionally, mobility of machines has been achieved through the use of wheels. Wheels, of course, have disadvantages when compared to nature’s most common mechanism of land locomotion, legs. Wheels generally cannot be used to traverse especially rough terrain, even when equipped with tracks. Wheels are not as useful as sensing or probing devices as are legs. Furthermore, wheels usually do more damage to the terrain being traversed than do legs. For these reasons and others, there is a great deal of interest in creating machines which use legs for locomotion. A very large body of research into legged locomotion has focused on walking. Popular wisdom considers walking to be the most basic form of legged locomotion, with running as an enhancement: “You must learn to walk before you can run.” For a given system, walking generally requires less power and places less stress on the components of a system. However, walking has a significant disadvantage, especially when walking machines are compared to their wheeled counterparts: walking is slow. For a system of a given size, the maximum speed vmax at which normal

1

walking can take place is limited [1] by the

length of the legs, l, and the acceleration due to gravity, g: vmax =

p

gl

(1.1)

Thus walking is limited to speeds which are much lower than speeds achievable by wheeled motion – or by running. 1

Here, we use the term “normal” to refer to walking in which the legs are used as inverted pendulums, without significant compression during the stride or flexure of the hips as practiced by race walking athletes.

2

Legged machines which can run therefore have a significant practical advantage over those which are limited to walking motion. Yet there has been comparatively little research into the design of practical running machines. This is unfortunate because in order for practical running machines to be constructed, a number of important problems remain to be solved. There are issues which, in the belief of the author, have been sufficiently solved by contemporary technology. For example, mechanical structures and mechanisms can easily be constructed which will reliably sustain the loads of running, at least for devices constructed on a moderate scale of size – for devices with masses on the order of 10−1 kg to 102 kg. At smaller scales than this, traditional machining techniques are difficult to use, and on larger scales the strength requirements placed on mechanical components may become unachievable. Embedded control computers are widely available which can easily process data quickly enough to control the sensors and actuators which are expected to be used to control a running machine: multiple control loops running at sub-millisecond rates are commonplace. Sensors which can measure forces, accelerations, distances, angles, and velocities are easily available and only modestly expensive in comparison to other components of a typical robot. Actuators with sufficient power density, for example electromagnetic and pneumatic actuators, are common. Other significant problems, however, remain to be solved before running machines become commonplace. One of the most critical is that a practical methodology for the design of running control systems has yet to be devised.

3

1.2

Prior Art Methods of controlling the running process – how the legs move and apply forces

in order to cause the desired motion of the body – have so far been developed in a somewhat ad hoc fashion, with specific types of actions being programmed in order to generate specific types of movement. Although some of these programs have generated impressive results, they serve only as first steps toward more complete control programs for running robots.

1.2.1

Purely Active Stability For example, Raibert and colleagues at the MIT Leg Lab created a series of run-

ning robots during the 1980’s[26],[12], [25]. The basic concept was proven with a one-legged hopping machine which could maintain a stable running motion and reject modest disturbances. The concept was extended to two-legged and four-legged machines which used the same algorithm as the one-legged machine to control their movement. The algorithms used were designed specifically to control one particular behavior of a one-legged machine, that of running on level terrain. The mechanical systems of these robots were not designed in such a way as to implement “passive stability.” The idea of passive stability is that the mechanical system is designed such that stable performance is assured by the design of the system, even in the absence of stabilizing feedback control. Many systems are designed using this principle; an example is the traditional design of aircraft, which when properly loaded and trimmed will recover from small disturbances and return to straight and level flight without requiring stabilizing inputs from the pilot. In the absence of stabilizing control inputs, the robots

4

of Raibert’s team were unable to sustain running. Instead, stability of the Raibert robots was provided by the action of the feedback control system. This placed a significant load on the control system, although the control computers available at the time were easily able to handle that load. Sensors were also a significant issue; the basic three-dimensional one-legged hopping machine contained at least ten sensors. The payoff for this complexity was flexibility of operation. With additional programming, hopping robots were able to perform maneuvers such as a sumersault and running up and down simulated stairs.

1.2.2

Passive Stability In the pursuit of more practically useful robot designs, a number of researchers have

taken a different tack, that of seeking passively stable mechanical designs, or at least designs which aid the control system in assuring stability. There appear to be two motivations for this work, both quite pragmatic. The first motivation comes from the recognition that running is very common in nature, even among comparatively unsophisticated creatures such as arthropods. The amount of processing power which a cockroach or an ant devotes to controlling the running process is probably not terribly great, yet these animals are obviously capable of effective locomotion. They can travel over quite rough terrain (in comparison to their body size) quickly and reliably. Biological research into how these animals control their running appears to show that a significant amount of passive stability is built into the animals’ bodies. It would seem sensible for designers of running machines to imitate this successful strategy. The second motivation for the use of passive stability is that it can greatly lower the total cost of designing and building a robot. Less sensors are needed, less actuators are often needed, and the computing hardware need not be as 5

powerful. Furthermore, the control software can be simpler. In some cases, feedback control is not even necessary, as the generation of a pattern is sufficient to cause the appropriate behavior. McGeer [19] demonstrated a scheme by which bipedal running could be made passively stable for an idealized case without the use of any control input. If correctly designed mechanical system built from ideal (frictionless) springs and linkages were placed in the correct initial conditions, it would run indefinitely, rejecting small disturbances. Although this concept cannot be implemented in a practical case – the real world does not often provide conditions amenable to frictionless or nearly frictionless running – it serves as a motivation for passively stable designs. A branch of research has evolved which focuses on designing practical robots based on passively stable running systems which can be driven by open-loop controllers. Successful implementations of this strategy have been designed, built, and tested by Buehler and colleagues [28],[22]. These machines are remarkable in that they use only one actuated degree of freedom per leg, yet are able to perform stable running motion. Key features of these designs are that the input which forces the system to operate in the desired running gait is generated as a function of time only, referred to in the literature as “clock-driven” control; and that the legs are highly compliant, with dynamic properties specifically tuned to stabilize the desired motion. An interesting feature of the development of the aforementioned machines is that some interesting aspects of the robots’ behavior are being discovered after the construction of prototypes. In particular, it was shown in [2] that the legs of the ‘RHex’ robot function as

6

spring loaded inverted pendulums. The method of building a device first, then discovering relevant properties is an effective and often appropriate method of advancing the state of the art in running machine design, given the early stage at which this field of design is currently found. However, as the field matures, it would be hoped that a more complete understanding of machines’ behavior would be achieved prior to construction. A class of running machines under the name “SPRAWL” which have demonstrated impressive performance are based on passively stable mechanical designs, clock-based feedforward running control, and a novel construction technique [5],[6]. These robots incorporate viscoelastic damping into their structures through the use of a construction technique referred to as Shape Deposition Manufacturing in which the electromechanical parts such as servo motors and wires are immersed into a flexible plastic structure. This method appears to show great promise for use on small scales, although it seems unlikely that the ratio of strength to weight of such structures will be sufficient for design on larger scales. It is interesting to note that the legs of these robots function mainly by providing linear forces (referred to as “thrusting” forces by the developers). The hip actuators are used primarily to position the legs in such a way that the linear forces act in the desired direction. This is very similar to the leg function which will be specified in this thesis, as seen in Section 2.3. However, whereas the stability of the running process of the “SPRAWL” machines is provided by the mechanical design and placement of the legs in imitation of the methods which appear to be used in insects, the motion of robots whose design is motivated by this thesis will be stabilized by the action of the controller. This active control should lead to a higher achieveable level of efficiency and performance at the expense of more

7

complex sensors, control hardware, and software.

1.3

A Different Philosophy In this dissertation a design philosophy is presented which seeks to expand upon

previous research into actively stabilized running machines as performed by Raibert et al. This philosophy seeks to create a framework in which a variety of running behaviors can be designed, and within which techniques used to create passive stability can be applied.

1.3.1

Motivating Principles The two branches of running machine design which have been most actively pur-

sued in the past appear to have been motivated in different ways – machine-oriented and biomimetic. The machine-oriented research has sought to design running systems using what can be considered the traditional methods of engineering practice, with the Raibert work being the most prominent. This approach is well suited to taking advantage of available mechanical and electronic componentry and analysis techniques but suffers from the problem that the space of possible designs for the problem “design a machine which runs” is too large. For all but the simplest goals, an effectively infinite number of concepts can be considered. The biomimetic approach seeks to narrow the design space by mimicing concepts which have proven successful in the natural world. This method takes advantage of what has been “learned” during hundreds of millions of years of evolution. A problem with this approach is that many of the components – sensors, computers, and actuators – which are available for engineering design are fundamentally different from those from

8

which natural designs are constructed. The running scheme described in this paper therefore respects the inspiration available from nature but is primarily focused on facilitating the design of engineered systems using the technology which is available at this time. The technology of interest includes mechanical components, electrical and electronic components, sensors, and actuators. Some of the most important features of technology which influence the design scheme follow: • Mechanical components are available with greater absolute properties such as allowable strength • Energy sources with very high power density (such as internal combustion engines) are available • Although comparatively inefficient in comparison to their biological counterparts, technological actuators are able to deliver power with higher absolute precision • The absolute precision of most technological sensors is also higher • Computers capable of performing computations with extremely high precision, negligible error rates, and repeatability are cheap • The mechanical components of technology include prismatic joints and rotary devices that are, at the least, uncommon in nature on a macroscopic scale It can be said that technological systems are not nearly as flexible as their biological counterparts, but within a given narrow range of operation their absolute performance is often much higher. Airplanes don’t feed themselves, but there aren’t many birds breaking the sound barrier, either. 9

The above is not intended as a repudiation of nature’s design strategies but rather a justification for making a significant departure from the currently quite active branch of running machine design which follows natural inspiration more closely.

1.3.2

The Impulse Model The impulse model of running defines a type of running as the motion of a body

through space under the influence of gravity and leg impulses. The motion of the body consists of ballistic arcs separated by force and/or torque impulses which are applied by the legs. The control of the motion of the body consists of the application of the correct sequence of impulses to cause the body to move in the desired path. This model does not intend to precisely model running as it is observed in biological systems, or even some mechanical devices such as “RHex.” It is instead intended to serve as an idealization which is used as a basic structure for design. For example, the idealization of an impulse implies that the duration of the stance phase, when feet are in contact with the ground, is very small with respect to the flight phase when they are not. However, this is often not the case for biological systems: in [16], Kram et al. observed that duty cycle of each leg for running cockroaches was slightly longer than that of the flight phase, resulting in a system in which at least one of the insect’s legs was in contact with the ground at all times. Using the impulse running model, the motion control of a running machine can be partitioned into a three-tiered hierarchy whose elements are as follows: • The control of the movement of the body by the application of a sequence of linear

10

and angular impulses • The partitioning of the impulses needed to control the body into a set of impulses, one for each available leg, which are within the capability of the legs to provide • The control of each leg so as to provide the necessary impulse This partitioning will have significant benefits for the design of the control system as well as the structure of the machine itself. The hierarchical control structure proposed here is somewhat similar to that proposed in [2], in which a dynamic model of a running robot is to be used to map leg actuation forces to their effect on the motion of the body. The distinguishing property of the impulse running model is that the dynamic model is significantly simpler and lends itself to a somewhat different control strategy as well as somewhat different mechanical designs. Although the mechanical designs which are suggested by the impulse model are somewhat more complex than those used in the construction of, for example, “RHex”, these designs are simpler than those of the Raibert robots and their kin and may provide a more optimal balance of simplicity, performance, and flexibility given the current state of the supporting technology. The impulse model was developed with prismatic legs in the Raibert style in mind. However, the work of Saranli and Koditschek[29] suggests that controllers which are designed for use with prismatic legs can also be applicable to systems based on rotary joints. This extends the range of applicability of the current work somewhat. However, prismatic joints are commonly used in the technological world, and they can easily be constructed with high strength and durability in real-world situations. Examples are the shock absorbers used on vehicles – strong, light, and capable of dissipating a large amount of power. 11

Pulses vs. Impulses An impulse is an idealized transfer of momentum in which an infinitely high force is applied to a system during an infinitely short period of time. Such a transfer cannot, of course, happen in a real system. If a machine could be constructed which was capable of moving itself through the application of force pulses which approached zero duration, it would be unable to operate in the real world, as its feet would punch holes into the ground rather than pushing off the surface. Therefore, the impulse idealization is not expected to ever be an exact model nor even a goal of design. The property of impulses which is significant for control design is that because of their short duration, the inter-axis coupling in the dynamics of body motion can be neglected during the application of the impulse. This can be seen from Euler’s Equations: ΣMx = Ixx ω˙ x − (Iyy − Izz )ωy ωz ΣMy = Iyy ω˙ y − (Izz − Ixx )ωz ωx

(1.2)

ΣMz = Izz ω˙ z − (Ixx − Iyy )ωx ωy The rate of change of each angular velocity is of the form ω˙ x =

ΣMx Iyy − Izz + ωy ωz Ix x Ixx

As the impulse imparts a finite change in angular velocity in an infinitesimally short time, the net moment ΣMx approaches infinity. However, second term in the equation includes only strictly finite terms. As the pulse approaches an impulse, then, the second term can more accurately be approximated as zero. For real systems in which the duration of application of the foot force is finite, the coupling term

Iyy −Izz Ixx ωy ωz

will be nonzero, and 12

the use of the impulse approximation will incur errors. However, the worst-case magnitude of these errors can be computed offline, and a control strategy can be devised which is able to reject these errors. If the duration of the stance phase becomes large enough, the dynamics of the body during the stance phase can no longer be neglected and other control schemes will have to be used. However, it is shown in Section2.6 that for some useful running cases, the stance duration will be sufficiently short that the impulse approximation will be useful. For example, running on a hard surface such as concrete allows the use of impulses of sufficiently high force and short duration.

1.4

Dynamic Simulation Techniques The basic behavior of the impulse running model can be explored in an approx-

imate manner using traditional techniques of dynamic modeling. In order to do so, it is necessary to make many simplifying assumptions, not the least of which is that of massless legs. A model is therefore created in which the legs have no mass; this model is used to study the response of the body to impulses from these idealized legs. This model is used to verify that the body’s motion can be controlled as desired given that the legs are able to produce the impulses which are desired. In order to more accurately model the behavior of a physical system, the dynamics of the legs should be incorporated into the model. For a substantial number of legs such as six or eight, this results in the creation of a system which cannot readily be analyzed using classical techniques of dynamic simulation. A numerical technique which makes heavy use

13

of computers is needed. There are several techniques available which can be used for this type of simulation. Commercial dynamic simulation software packages are in widespread use for solving similar problems; probably the most commonly used is Working ModelT M . Somewhat less refined but non-proprietary software with open source code is also available, an example being the Aero package. Commercial finite element analysis software can also be used to analyze dynamic systems. These options are discussed in more detail in Section 3.1.2. Software is under development, as part of this research work, which can be used to simulate dynamic mechanical systems following a somewhat novel philosophy. The newly developed software and modeling technique, here referred to by the name MechSys2, is based on the application of physical laws in their simplest form and uses a simple, consistent set of components to model even complex systems. The components include point masses, springs, dampers, and simple geometric constraints. These components are organized using the techniques of object-oriented programming to form a library of parts which can easily be “assembled” in complex configurations. Simulations are carried out by applying Newton’s laws of motion to the masses and by applying the constituitive equations of springs, dampers, and constraints. The result is that by applying physical principles which can easily be understood by the average University undergraduate, one can implement simulations of systems as complex as a multi-legged robot. The design, implementation, and basic testing of the MechSys2 system is described in Chapter 3.

14

Chapter 2

An Impulse Model of Running This chapter explores a novel approach for the modeling and control of legged running machines. A specific type of running, ballistic running, is considered. This mode of running involves a body moving through space under the influence of gravity and quickly applied forces. The motion is a periodic process with each cycle referred to as a stride cycle. Much of the time is spent in the flight phase during which no part of the body or legs is in contact with the supporting surface such as the ground, or if it is in contact, no significant force is being applied at that time. During a percentage of the time called the stance phase, the legs provide pulses of force which both support the body by pushing it upward and control the body’s attitude and position.

2.1

Introduction Ballistic running is a subset of the many types of running which have been defined.

Running is distinguished from walking in a number of ways:

15

• Walking involves an exchange of gravitational potential and horizontal kinetic energy in which a gait cycle resembles the motion an inverted pendulum, with the maximum kinetic energy out of phase with maximum potential energy[9]. Running involves storing energy in legs, or dissipating and generating energy, in such a way that horizontal kinetic energy and gravitational potential energy fluctuate in phase with one another. • Walking can often be a quasi-static process which can be halted at any time without the body “crashing.” (Some walking machines cannot stop at any arbitrary time, but this is a feature of their design rather than of the walking process.) Running involves motions which generally cannot be halted in process. • The control of walking is a continuous process in that at all times during the cycle of motion, the position and attitude of the body can be (and usually is) controlled. In running motion, there may be times such as the flight phase during which no control action is possible. There are a number of types of running which do not fall under the definition of ballistic running which is proposed in this paper. Some examples are: • “Groucho Running”, a mode of running in which there is no time at which all of the legs are simultaneously off the ground. • Running in which the motion of the body is controlled in a closed-loop fashion during the stance phase. In the ballistic running discussed here, the stance phase is of short duration and the control system applies a predetermined impulse to the body during that time. 16

Ballistic running, then, involves a specific sequence of actions.

The body is

launched into some trajectory of motion by an impulse exerted on the body through the legs. During the flight phase, the motion of the body is monitored; by the time the legs touch the ground at the beginning of the stance phase, a controller has computed the desired impulse to be applied to the body in order to cause the body to move along the correct path in the correct attitude. In the stance phase, the impulse computed by the controller is applied to the body, launching the body onto its next ballistic flight. The control of ballistic running then involves two distinct parts: The computation of the desired impulse which should act on the body during a subsequent stance phase, and the application of the correct impulse to the body. A purpose of this work is to design a control structure which uses this partitioning of the control effort to allow the design of simple and robust running machines and their controllers.

2.2

Pulse vs. Impulse Models The word impulse is used somewhat flexibly in the language of dynamics. It can

be used to describe an idealized event in which a finite change of momentum is imparted to a body during an infinitesimally short period of time; in fact, the “impulse function” is used in many contexts to describe a function of time which has infinitesimally short duration but whose integral is finite. This meaning is often used in dynamics to describe the effect of a collision between rigid bodies. The events which acutally take place while the bodies are in contact, and the forces which are applied between the bodies, are not considered explicitly; instead, an idealization is used in which the duration of the contact approaches zero and

17

the force approaches infinity such that the time integral of force is a finite value equal to the change in momentum of the bodies. A second usage of the word impulse refers to the actual value of the integral of force over time, impulse =

d mv = ∆P = dt

Z t 0

ΣFdt

(2.1)

Here, P is the body’s momentum and ΣF is the sum of the forces acting on the body. It will be necessary to use both meanings of the word impulse in this chapter. The idealized concept of ballistic running involves using the legs to apply impulses of infinitesimally short duration to a body. However, even when the duration of force pulses is large with respect to the period of a running machine’s stride, we will speak of the impulse applied to the body as the integral of force over time. In reality, it is not possible to apply a “perfect” impulse to a body. One cannot design a machine to create nor withstand forces which approach infinity, and even if one could, the machine would punch holes in the ground rather than springing up from it. Real running machines have to make do with non-ideal finite forces. The application of finite forces over finite time to propel the body is referred to as force pulse running. As the forces applied by the legs become smaller, their duration must increase in order to apply the correct impulse (using the second definition of impulse in the preceding paragraph, impulse =

R

ΣF dt). This means that the idealized equations which assume a force impulse

become less and less accurate approximations. At some point, a control system which is based on the approximation of an instant impulse will no longer function correctly. Therefore, in order to design a system which uses the impulse strategy to run, we must have force pulses of sufficiently small duration that the impulse approximation is 18

acceptably accurate but of sufficiently large duration that realistic forces can be applied. It is interesting to note that one factor which may limit appilcations of this design technique has little to do with the technology but is instead a feature of the envoronment: running on soft ground which is unable to support large foot forces will be more difficult for a ballistic runner to accomplish because small forces of comparatively long duration must be used. In a similar fashion, ground which cannot support horizontal foot forces by friction may be difficult for an impulse based machine to handle. However, there is a great deal of firm terrain in the world; furthermore, machines could be designed which alter their strategy when they meet with softer terrain, changing to a different mode of running or even to walking if necessary. Ballistic running should still be a useful technique if used where it is appropriate.

2.3

A Control Hierarchy The impulse based model of running introduced in Section 2.1 suggests a hierar-

chical structure for the control of running. A diagram of the control system hierarchy is shown in Figure 2.1. The body is moving under the influence of impulses generated by a set of legs. This means that the control of the legs can be separated from the control of the body. In this arrangement, there is a body controller which is responsible for the position and attitude of the body. This controller’s output is a net linear impulse and a net angular impulse which are to be applied to the body during the stance phase of a given stride. The body controller need not concern itself with how these impulses are applied; it only needs to have the correct impulses at the correct times.

19

Figure 2.1: Control System Hierarchy

The net impulse which is required by the body is then partitioned among the legs. This action is entirely separate from body control. The output of the partitioning is a set of impulses which are requested from the legs. Again, this controller does not care how the legs generate the impulses which they are asked to produce; it only requires that they do so somehow. In the most general case, each leg may be asked to provide both linear and angular impulses at its hip. However, if there are enough legs present (at least three working during a given stance phase and not in a line), each leg does not have to provide an angular impulse. Instead, the angular impulse can be provided by varying the linear impulses among the legs so that the sum of the impulses provided by the legs adds up to the correct value. This technique is discussed in more detail in Section 2.5.1. The control of the legs takes place in a set of separate controllers, one for each leg.1 Each leg controller is responsible for ensuring that the correct linear impulse is supplied 1

The discussion of separate controllers does not imply that separate computing hardware is used, only that the control algorithms can operate independently except for the exchange of specific information about impulses which must be communicated between them.

20

by that leg. Ideally, a leg controller will be able to apply the correct impulse under all circumstances; however, in real systems it is expected that there will be circumstances in which this is impossible. In such cases, the error in applied impulse may be treated as a disturbance from which the control system as a whole must recover; or a control system may be designed which is able to account for the disturbance explicitly. The latter case is beyond the scope of the present work but may represent a promising direction for increasing the robustness of the control strategy.

21

2.4

The Body-Only Model The first step in studying the behavior of the impulse based running system design

is to investigate the behavior of the highest level of control, the body control. Because the control is divided into a three-level hierarchy, the top level can be subjected to initial testing without having to model the other layers, and a top-level control strategy can be designed. Subsequent investigation, presented in Section 2.5, will involve the lower levels of the system. As studied here, the body-only model uses impulses of negligible duration to control the motion of the body. This impulse based simulation is used to develop a controller which is capable of stabilizing the motion of the body and assuring that the body follows the correct path. It is expected that the controller developed in this way will perform correctly when the system of legs behaves in a manner which is close to that expected by the controller – in other words, when the legs deliver the expected impulse in a short time. When the impulse created by the legs is not of the expected value or the duration of the time in which the legs apply their forces is long, the body will not behave as expected and the controller may be unable to function satisfactorily. One case in which the impulse model may fail is that in which the finite duration of force pulses causes coupling between angular motion about one axis and the change in momentum due to an angular impulse about another. As discussed in Section 1.3.2, this coupling can be neglected in the case of impulses or force pulses of short duration; however, as the duration of the pulses becomes longer the coupling will cause behavior for which the controller may be unprepared. The limitations of the impulse model, and in particular how it breaks down as pulse durations increase, are explored in Section 2.6.

22

2.4.1

Simulation Technique The body simulation is implemented using the standard Newton-Euler method of

modeling a single rigid body. Two fundamental equations are used – Newton’s laws in the familiar form: 1 d v= F dt m

(2.2)

and Euler’s Equations, here repeated from Section 1.3.2: ΣMx = Ixx ω˙ x − (Iyy − Izz )ωy ωz ΣMy = Iyy ω˙ y − (Izz − Ixx )ωz ωx

(2.3)

ΣMz = Izz ω˙ z − (Ixx − Iyy )ωx ωy These equations can be used to give us linear and angular accelerations of the body as a function of the state of the body. In order to construct the simulation, we choose a state vector x which contains the linear and angular position and velocity of the body: #T

"

x=

x vx y vy z vz θx ωx θy ωy θz ωz

(2.4)

Newton’s Laws are used to provide the derivatives of velocity as a function of the current state and the input, which is the net force on the body. Euler’s Equations provide the derivatives as a function of the state and the net torque on the body. The resulting system of ordinary differential equations (or ODE) is solved by a standard ODE solver package; this gives us a time history of the state variables. The system of Newton’s and Euler’s equations applies to the body as it is in the flight phase. At intervals, the body will touch down and go through a stance phase. The

23

impulse approximation requires that the duration of the stance phase approach zero. The stance is modeled using the equations of momentum transfer. For the linear impulse: v1 − v0 =

1 P m

(2.5)

where v0 and v1 are the body’s velocity before and after the impulse respectively, m is the body’s mass, and P is the net impulse applied by all the legs. For the rotational impulse, we apply to each axis i: ω1i − ω0i =

1 Hi Iii

(2.6)

where ωi is an angular velocity component, Iii is a principal moment of inertia, and Hi is a component of the net angular impulse applied to the body. The ODE solver is of course not used to model the effect of impulses during the stance phase. The solver is stopped when the body touches “ground.” In the body-only case, touchdown occurs when the body’s center of mass moves below a specified level. For simplicity, this level is set at zero; the simulation would behave in exactly the same fashion if another level were selected. Simple equations for the effect of the angular impulse using the principal moments of inertia are used because it is assumed that the body’s principal axes of inertia are aligned with the coordinate axes. Research into the performance of the control system when the principal axes of inertia are not so aligned is left for future work. This is justified on the grounds that the current project seeks to demonstrate that control of a body by the impulse method is possible, but not to research the limits of all cases in which it can be done; and if we were really building a robot which had trouble caused by a misalignment of the principal axes of inertia, we could easily employ some strategically positioned weights to solve the 24

problem! A problem arises with the use of a simple ODE system to model a body which is able to rotate in three dimensions. Euler’s Equations provide the rate of change of angular velocities in the ODE. The use of these equations to find a time history of angular velocity is straightforward. However, when we attempt to convert from angular velocity to angles, a problem arises. The “natural” set of variables to choose for representing angles is a set of Euler angles which represent rotations about the coordinate axes. But Euler angles do not behave well as states in a system of differential equations: integrating angular velocities to get angles is not strictly accurate in that the measurement of angular velocities and hence differential angles takes place in a coordinate system attached to the body, while the states in the state vector are intended to represent the position of the body with respect to the world in which it is moving. For large angles neglecting this problem leads to large errors in the time history of angles integrated from angular velocities. If an angle reaches π/2, a representation of the body’s attitude based on Euler angles suffers from a singularity and the representation may be unusable. A method of dealing with this problem is discussed in Section 4.4.1. However, for small angles it is not necessary to employ this method, as the attitude of the body deviates only slightly from the straight and level posture whose stabilization is the purpose of the body controller. Therefore, the simulations discussed in this section are not corrected for angular deviations; instead, Euler angles of the “1-2-3” or “roll-pitch-yaw” set are used to store the attitude of the body in states of the ODE in order to maintain a reasonable degree of simplicity in the simulation software. It should be noted that it will be particularly

25

important to use the methods of Section 4.4.1 to study navigational control of a body. The software is implemented in Matlab. This environment is chosen for several reasons: • Matlab provides an extremely convenient environment for the manipulation of arrays, and this problem is quite full of them. • There is a large set of analysis and visualization tools built into Matlab, as well as a set of tested ODE solvers ready for use. • The scale of the programming effort required to perform these simulations is relatively small, so the organizational tools in a better structured language such as C++ or Java are not badly needed. • The computational effort to solve this problem is not particularly great, so the benefits of a more computationally efficient language (and there are very many of these) are not very important. The source code for the body-only simulations is printed in Section A.1. The system of equations which represent a typical running body are not in general particularly stiff. Therefore, a common nonstiff solver is used to solve the ODE system. The choice for the simulations in this section is the Matlab solver ode45, a simple fourth-order Runge-Kutta solver which has shown itself to efficiently and reliably handle these equations. Stiff solvers were tested with these equations, and as expected, the stiff solvers took several times longer to achieve a solution.

26

Controller Design In keeping with the successful strategy of Raibert and others, the design of the controller for the running machine body is kept extremely simple and straightforward. A simple Proportional-Derivative control law is used to control each of the axes of the body’s position (three positions and three angles), and the axes are all controlled independently. The input to the controller is the body’s state vector, which is assumed to be accurately measured and/or estimated. The output is a sequence of impulses which are requested from the set of legs. If this extremely simple control architecture can be made to work, the feasibility of impulse-based control will have been demonstrated; more complex controllers can be used to improve the performance or flexibility of running systems in the future. The PD control architecture should be practical to implement on real systems. Control inputs consist of linear positions and velocities, and angular positions and velocities. These are quantities which are not particularly difficult nor expensive to measure. A possible set of sensors would include a compass or GPS receiver, several accelerometers, leg position sensors (angular for the hip and linear for the leg extension), and foot contact sensors. The horizontal and vertical position and velocity of the body would be computed by integrating the accelerations measured by accelerometers aligned with each of the body’s axes. Integration, and especially double integration, is highly prone to errors due to “drift” (the integration of small acceleration errors increases over time). Therefore, the position and velocity error would need to be zeroed out periodically. This could be accomplished by using a GPS or other navigational signal to correct the horizontal position, and the foot switches in combination with leg position sensors to detect the vertical position with respect

27

to the ground. Angular accelerometers would measure the attitude of the body in a similar manner. Either a device such as a compass, or another source of navigational information, could be used to correct horizontal attitude; accelerometer-based inclinometers (with appropriate filtering) could detect the position of the body with respect to the gravity vector. In this way, a full set of state information would be available to the controller. When applying impulses directly to the body, hopping height control requires a control strategy which is slightly different from that usually employed. In particular, the objective is to control the height of a hop, given the speed at which the body contacts the ground at the end of the previous hop. Fortunately, a simple calculation relates the height of a hop to the speed of descent from the previous one. We find the takeoff speed v1 from the desired height of a hop h as v1 =

√ 2gh, where g is the acceleration due to gravity. Assuming

the landing speed from the previous hop was v0 , the net vertical impulse delivered to the body should be P = m(−v0 + v1 ); m is the mass of the body and v0 is negative because the √ body has been falling. The form of this equation is convenient: P = m( 2gh − v0 ) can be written as a feedback control equation with the control gain being m, the setpoint

√ 2gh

and the process value v0 . These quantities are then plugged into the derivative portion of the PD controller for body vertical position – note that v0 is the derivative of vertical position – and the proportional gain is set to zero. The result is hopping height control which is optimal in that if the measurements of system parameters and state are correct and the actuator successfully delivers the correct impulse, the next hop will reach exactly the desired height.

28

2.4.2

Body-Only Results Simulations were run using a variety of parameters. The body size and mass

properties were chosen to represent a 1.5 kg object which is the same size as The Control Handbook [17]; the moments of inertia are Ixx = 0.0074 kg · m2 , Iyy = 0.0106 kg · m2 , and Izz = 0.0164 kg · m2 . A rough sketch is shown in Figure 2.2.

This is similar to the body

Figure 2.2: The Simulated Body

configuration of robots such as “RHex” [28] and should be a reasonable configuration for the sort of prototype running machines which one would expect to be built using the impulse design and control method. However, conclusions about the impulse method are relatively independent of the scale of the machine, as long as components with the required strength, functionality, and precision can be fabricated and assembled. The controller gains were hand tuned to achieve reasonable hopping performance and rejection of small disturbances. The hand tuning process was slightly automated in 29

that control gains were made a function of the mass properties of the body; this helps slightly to preserve performance when the body’s properties are altered. However, it does not constitute automatic tuning of any sort; the procedure used in this work should be considered as purely manual tuning. To test the robustness of the control, disturbances were introduced by manipulating the initial conditions of the simulation. The models were tested by applying small but finite disturbances on each axis and in various randomly chosen combinations. The control scheme and gains which were chosen succeeded in stabilizing the hopping motion of the body in the presence of small disturbances. The ability of the body to hop in place is meaningful, but it isn’t quite running because running implies the ability to go somewhere. Turning a hopping controller into a running controller, however, is remarkably simple in this case. The hopping controller is causing the body to hop at a given position setpoint. In order to make the body run, we simply need to move the setpoint, and the body will run after it. This behavior was tested by moving the setpoint in the positive x direction and examining the tracking performance of the body’s motion. It was found that indeed, the body would run to whatever horizontal location the setpoint occupied. This technique of moving setpoint tracking is somewhat reminiscent of Pratt’s virtual model control technique[23],[34], although the mathematical foundations are different. The results of a typical simulation run are shown in Figure 2.3. In this run, the body begins with its position 0.4m away from the y axis and the body’s angle tilted by 10o about the x, or roll, axis. The setpoint is moved away from the starting point at 0.2m/s.

30

Figure 2.3: Body-Only Simulation Results

The simulation results show the body’s attitude tilting rapidly from the starting angle to horizontal and the position error in y being quickly taken care of as the body moves to the x axis. In addition, a stable pattern of running quickly develops as the body follows its moving setpoint.

31

2.5

Control With Multiple Legs In Section 2.4, the top level of control in impulse running was explored. It was

shown that the motion of the body can be stabilized and controlled by applying the correct sequence of net impulses to the body. The next level of control which must be implemented in order to facilitate the design of real running machines is the allocation of the desired net force among a number of legs. In this section, models are created which use sets of legs to control the body.

2.5.1

Leg Design The design of a running system must of course involve a synthesis of hardware and

software. The control scheme which is investigated in this research is designed to permit the use of comparatively simple hardware. This intended hardware includes two actuated degrees of freedom per leg. This is one less than used in the designs of the Raibert team [25] and one more than used in the newer designs of Buehler and colleagues [28],[22]. Thus this technique represents a different compromise between simplicity on the one hand, and flexibility and performance on the other. The amount of sensing and computational power required for this class of designs will also be between that of the designs mentioned above. The leg design which is chosen for this project is that of a telescoping leg with a rotary hip, as shown in Figure 2.4. Both the telescoping and hip rotation may well be provided with energy storing springs as well as actuators which are operated by the hierarchical control system to be presented. However, the impulse running method specifies that any sort of actuation may be used so long as it is capable of causing a leg to deliver

32

the desired impulse to the body, within a given accuracy and duration of time. It is likely that some compliance may be designed into the leg to allow a limited degree of motion perpendicular to the plane in which the legs are rotated. This compliance may be needed to deal with non-ideal motions of the body caused by uneven terrain, disturbances, and the like. This compliance is not modeled in the current research, as it is not needed in the “ideal” case represented in the simulations, and it is expected to have only a very slight effect on the system’s overall performance. An important distinction of this running machine design philosophy is that the rotary hip joints are required to move the legs into the correct orientation at which to supply the needed impulse, but the hip joints are not required to exert a moment through the hip during the stance phase. All control is provided by varying the linear impulse provided by the telescoping legs. This permits the use of a smaller and lighter hip actuator than would otherwise be needed. Furthermore, because the legs only exert forces which are along the leg axes, the legs themselves can be made much lighter than they would have to be if they were required to exert hip torques and thus endure bending loads. The details of the leg design such as materials, type of actuators used, types of sensors, and so on are not considered as part of this research. Instead, it is assumed that legs can be designed with the required properties. The work of Raibert et al., as discussed in Section 1.2.1, has shown clearly that legs which are capable of stabilizing a body can be built and controlled.

33

Figure 2.4: Robot and Leg Configuration

2.5.2

Control Strategy The body motion controller design presented in Section 2.4.1 is modified only

slightly for use in controlling the body with legs. A change needs to be made because the leg design described in Section 2.5.1 is not capable of providing impulses in a lateral direction (parallel to the y, or pitch, axis). Sideways motion of the machine will have to be induced by one of two means: • Turning the body about its yaw axis and then pushing in the body’s local forward or back direction, or • Tilting the body about its roll axis such that the force vector provided by the feet when they push “downward” contains a component along the y axis.

34

The second method is chosen. A primary factor in the choice is that most small running robots are expected to have a greater rotational inertia about their z (yaw) axes than their x (roll) axes, so it will be quicker and easier to turn them in roll than yaw. A second reason is an analogy to aircraft: the banking maneuver just seems to look right. The banking strategy is implemented by creating a control action which responds to an error in the y position of the body by tilting the body. In order to achieve stable performance, this action is controlled by a PD controller, as are the other axes. The banking controller’s gains are hand tuned as are the other gains in the system.

2.5.3

Allocation of Impulses The second level of control, as shown in Figure 2.1, is the allocation of impulses

among the legs. This allocation takes place in the coordinate system of the body, not the world coordinate system. This is an interesting design problem in that given a set of several legs, there are many possible solutions to the problem of allocating forces among the legs in such a way that the net impulse desired by the body is created. (On the other hand, given few enough legs there is only one solution, and given too few there are none.) It would be convenient to devise a scheme which works acceptably for any number of legs, and for legs in any reasonable configuration. Such a scheme would simplify the initial simulation of a new design considerably. Such a scheme was devised to allocate forces among the legs of the machines whose simulations are presented in this section. Linear impulses are allocated in a very simple manner: the desired impulse is distributed equally among all available legs. This seems fair. The moment allocation scheme adds and subtracts linear impulses from those requested from individual legs. It is 35

based on the idea that the legs which are in a position to provide more angular impulse per change in linear impulse should be assigned more of the job than those which are not. For example, if we wish to generate an angular impulse about the x axis, it is desired to assign more of the task to the legs which are farther from the axis than to those which are closer to it. In order to keep the equations simple, the components of angular impulse assigned to a leg are made linearly proportional to the leg’s distance from the axis about which the angular impulse is desired. For a given pattern of legs, this leads to a set of equations in which the positions ri of the N legs with respect to the body’s center of mass are known, and the constants of proportionality a, b, and c are unknown. The equations are as follows: 2 −ΣN i=1 ryi c = Hz N 2 (ΣN i=1 rxi riy )a + (Σi=1 ryi )b = Hx

(2.7)

2 N N (−ΣN i=1 rxi )a + (−Σi=1 rxi ryi )b = Hy + Σi=1 ryi rzi c

where H = (Hx , Hy , Hz ) is the net angular impulse desired by the body controller. Note that no linear impulse in a direction parallel to the y axis is asked from the legs. These equations can easily be solved for the constants a, b, and c online using closed-form solutions. Once these equations have been solved, the changes in linear impulse desired from each leg are computed from the constants and the horizontal positions of the leg with respect to the body’s center of mass: Pxi = cryi Pyi = 0

(2.8)

Pzi = arxi + bryi 36

The components of linear impulse computed here are added to those needed to provide the net linear impulse to the body and sent to the leg controllers. This control method should have some advantages over the method used by Raibert et al. to perform multiple-leg control. Raibert’s method was to program all sets of multiple legs to behave as if they were a single leg, using several fully articulated legs which are capable of providing torque at the hip during stance phase to behave as one such leg. This method is similar except that the several legs which are used need not be of as sophisticated a design as those in Raibert’s robots; they can in fact be a much simpler design and still perform the function of the single virtual leg. Also, since the legs in this system are expected to use active closed-loop impulse control, they should be able to perform in a more robust manner. This advantage, however, depends on details of leg controller performance which are beyond the scope of the current work to investigate. An improvement to this technique might use the properties inherent in the legs to add further stability to the system, following the concept of “preflex” advocated by many mobile robotics researchers. For example, if springs were part of the impulse generating mechanism in the legs, those legs which were closer to the ground due to body tilt would be compressed more and longer, providing more impulse without the need for that impulse to be requested by the controller. However, this sort of behavior is different from that modeled by the impulse model of running, and the control strategy would have to be altered somewhat to accomodate it.

37

2.5.4

Leg and Impulse Results The body and leg model described above was programmed in Matlab and run in

the same fashion as was the body-only model. As would be expected, the behavior of this model was very similar to that of the body-only model. Disturbance rejection and running by setpoint tracking operated in much the same way and with similar control gains. There was one significant difference: the banking scheme used to control lateral position produced a different behavior when moving the body with respect to the lateral (y) axis, as shown in Figure 2.5. Here the body can be seen “banking” – tilting about its x (roll) axis – as it moves the body onto the y axis from its initial disturbed position. With the controller gains

Figure 2.5: Robot and Legs Impulse Simulation Results

38

tuned as shown in the code in Appendix A.2, the body-with-legs impulse based controller performed as expected.

2.6

From Impulses to Pulses The final degree of sophistication added to the Newton-Euler model simulations is

the change from impulse based simulations to simulations in which finite forces are applied by the legs, integrating to a finite impulse in a finite amount of time. This necessitates a change in the modeling technique in that the ODE solver must be run not only during the flight phase but also during the stance phase. For programming convenience, the solver is stopped at the beginning and the end of the stance phase. The way in which forces are applied, and in particular their timing, is a rather open-ended design problem. If legs with springs generate impulses, then a body tilt will cause legs which are closer to the ground to apply their impulses sooner than those which are higher when the lower legs touch down. This greatly complicates the control. Furthermore, there are various ways to modulate the leg forces to produce the different impulses needed from different legs during a given touchdown. The legs can all be given the same force, with some applying the force for a longer time than others; or the time can be the same and the force different. In addition, real legs will not be able to apply a constant force vector throughout the duration of a stance: while the foot is on the ground, the leg is rotating, and so is the force vector. The resulting net impulse is the time integral of the moving force vector. It was decided to begin with the simplest possible case and confirm that the control

39

system still works when the force is applied over a significant time interval without dealing with the other problems mentioned above yet. Simulations in which the legs can act at different times are left for the spring-mass simulation technique (see Section 2.7.) In the simpler case discussed in this section, the direction of the force applied by the legs is allowed to remain constant. This is reasonable because if the state of the body is known, the correct leg direction at touchdown can be computed so that the net linear impulse vector will point in the desired direction after the stance phase is over. This method was successfully used by Raibert for single-leg robot control. All the legs’ forces are turned on at the same time and for the same duration; the magnitudes of the forces are varied by the controller. The legs are turned on at the same point as they are turned on in the impulse model – when the body’s center of gravity reaches the “stance” position. In the pulse model, the body sinks down somewhat – modeling leg compression – during the stance phase.

2.6.1

Pulse Model Results For the most part, the performance of the pulse model is similar to that of the

impulse model as long as the leg forces are kept high enough that the duration of the pulses as a percentage of the stride time is reasonably small. The controller used to control impulse based running works reasonably well to control pulse based running. One important modification which must be made to the controller is a change in the hopping height control. The hopping height for the impulse controller is controlled by computing an impulse which will cause the body to reach a given height midway between takeoff and touchdown. This calculation assumes that the body spends all its time after the impulse is ordered in flight. This scheme does not work well for the pulse based controller because the body does not 40

spend all its time in flight between the times impulses are ordered: it spends a significant percentage of its time in the stance phase, with gravity still acting on it, moving in an arc below the touchdown altitude. A simple solution to this problem is to add extra impulse in the vertical (z) direction, equal to the impulse due to gravity which acts downward during the stance phase. With this modification, the pulse based simulation runs in almost exactly the same way as the impulse based simulation (although the solution runs a good deal more slowly because the ODE solver is working through the stance phase as well as flight phase, reducing its step size to handle the transitions).

Figure 2.6: Robot and Legs Pulse Simulation Results

Output from a simulation of the pulse model with impulse controller is shown in

41

Figure 2.6. The overall behavior can be seen to be very similar to that in Figure 2.5.

In

Figure 2.7: Robot and Legs Pulse Simulation Results

Figure 2.7, the behavior of the hopping height controller and the finite duration of the stance phase are visible. This simulation run had a duty cycle (defined as duration of stance phase divided by duration of stride cycle) which averaged approximately 20%. Longer stance phases were also tried. It was found that at high duty cycles, it was possible to find initial conditions which were not successfully stabilized by the impulse based controller, even though the same initial conditions were stabilized by the same controller acting on the impulse based simulation. This confirms that the impulse based model becomes less usable for purposes of control as the duration of the stance is increased. For duty cycles under

42

30%, no initial conditions which destabilized the system in the pulse based simulation but stabilized it in the impulse based system were found. This supports the conclusion that an impulse based controller can work well to control pulse based running.

2.7

Pseudo-Rigid Spring-Mass Simulation The “pseudo-rigid” spring-mass modeling technique discussed in Chapter 3 is de-

signed for the construction of more realistic models than those which are practical to create by the simpler methods discussed in Sections 2.4-2.6. In particular, models which contain legs of finite mass properties (rather than massless legs) can be constructed, and the mechanism by which force pulses are generated can be modeled in more detail. As a first step, a model which simulates the hopping behavior which is central to ballistic running is designed. The system which is modeled is a body with four hips, the legs being modeled as forces applied at the hips. This model is appropriate at this stage because the legs of robots designed in this work are intended to apply linear but not angular forces at their hips. The body’s mass properties are approximately the same as those of the bodies simulated through the use of Newton-Euler methods in preceding sections, but the body is now modeled as a set of point masses connected by stiff springs rather than as a rigid body. A detailed discussion of the spring-mass modeling technique is given in Chapter 3. A pictorial representation of the spring-mass body model is shown in Figure 2.8. In this figure, the darker spheres represent the masses which implement the mass properties of the body, and the heavy lines between them represent the stiff springs which maintain the

43

body’s rigidity. The lighter spheres are “ghost points”, nodes which are used for transferring forces to the body. The lighter lines are the virtual connections from each ghost point to the body’s main mass; no physical entity is modeled here – ghost points transfer forces applied to them directly to the body as described in Section 4.3.3. In this simulation, the force of gravity is applied to the body at its center of mass node (labeled CG node). Gravity is implemented as a world-relative force, meaning that the direction of the force

Figure 2.8: Spring-Mass Body Model

is not changed when the body’s attitude changes. The point of action of the gravity force moves to remain at the body’s center of mass, however. Leg forces act on the hips and are modeled as body-relative forces. This means that when the body’s attitude changes, the direction of the leg forces changes correspondingly. This follows what would happen if a real body were to tilt, causing its legs to tilt with it. The algorithm used to control the motion of this body very similar to that used in Section 2.5.2 to control the body which is simulated via Newton-Euler methods. In 44

particular, the vertical impulse which is to be applied is calculated at the time of simulated touchdown from the landing velocity of the previous step. A difference is the determination of the duration of the force pulse. The Newton-Euler model was controlled by a controller in which the maximum force was specified and the duration calculated therefrom; the springmass controller uses a fixed duration of pulse and calculates forces from the impulse and duration. These methods are effectively equivalent, given that if the maximum force or duration are specified correctly they cause the same control action to occur.

2.7.1

Impulse-Leg Results In this simulation, simple hopping height control alone is implemented, hopping

being a logical first step in the process of developing ballistic running in the spring-mass model. Figure 2.9 shows a typical result in controlling the body’s hopping height. The simulated “ground” is at z = −0.15m. The body is dropped from a height considerably lower than the desired hopping height; the controller responds by causing the body to hop up to the correct height and continue hopping at that height.

A number of tests were

conducted with this model. All resulted in the sort of performance represented in Figure 2.9. It is not likely that the mechanism which imparts impulses to a real and practical running machine body will act in the manner modeled in this section. The use of forcebased actuators such as voice coils to create the desired forces during each stance phase is unlikely to lead to the most efficient designs. Instead, we can expect that much of the work of hopping will be performed by springs which store the energy of a previous hop and release it to fuel a subsequent hop. The control system actuators will be responsible for 45

Figure 2.9: Hopping Height Control of the Body

reacting to disturbances, supplying energy to replace that lost due to friction, and changing hopping height and course. An exception to this rule would be running on a surface which absorbs a great deal of energy, such as dry sand. Running on the beach, however, is outside the scope of this thesis. The next section discusses a model which simulates more closely a machine which uses leg springs to store and release hopping energy.

2.8

Pseudo-Rigid Model with Leg Springs This model extends that discussed in Section 2.7 by adding rigidly attached legs to

the body. Each leg contains a spring and a force actuator. The leg springs are “nonstiff” in

46

that they are expected to deflect significantly during the simulation, in contrast to the “stiff” springs which maintain the rigidity of the rigid body and enforce constraints. The actuator simply pushes with the desired force. These components are somewhat idealized, but less so than those in the previous models. The stiffnesses used are approximately 102 N/m for the leg springs and 104 - 105 N/m for the rigid body springs. Figure 2.10 shows the layout of the body model. In this three-dimensional plot by Matlab, the circles represent nodes and the lines represent connections between them, most often implemented with springs. The four point masses near the center of the structure which form a pyramidal shape are there to duplicate the inertial properties of the main body, with an additional point for the application of forces at the center of mass. Surrounding the body is a rectangle of points which form four hips, below which are four feet. The motion of the feet with respect to the body is constrained so that each foot may only move along the line of its corresponding leg. This simulates the action of a prismatic leg. Leg rotation is not yet implemented, as this model is designed only to simulate hopping in place. The springs in the legs are necessary to hold the feet: if these springs are not present, the point mass which implements the foot will be free to move wildly when a force is not being applied to the foot – and experience has shown that indeed it will; when the leg force is removed the foot rebounds skyward, launched by the spring which implements contact between the foot and the ground. The ground itself is modeled with four plane surfaces. Each surface is a constraint which forces one foot to remain at or above ground level (y = −0.3m in this example). Four coincident surfaces, one per foot, are required by the design of the software but the effect

47

Figure 2.10: Spring-Mass Model Layout

is the same as if there were one surface constraining all four feet. In each simulation run, the body’s initial conditions place it at the coordinate system origin with no translational velocity. The position of the “ground” is adjusted so that the body may be dropped from this position and fall down until the feet touch the ground. The body can then rebound and begin another hopping cycle. These initial conditions, although somewhat less realistic than the conditions of a body sitting at rest on the ground, are much easier to achieve. Placing the body in a position in which its weight is supported by the feet with all springs at equilibrium is very difficult – computing the equilibrium positions

48

of masses which are attached to several springs is feasible but not considered a particularly productive use of programming and computing resources. Furthermore, releasing the body from a position above the surface reproduces with reasonable accuracy the conditions of a body in the middle of the flight phase.

2.8.1

Controlling Legs with Springs As discussed in Section 2.7.1, the addition of springs to the legs changes the way

in which impulses are imparted to the leg considerably. The springs provide a force, storing the kinetic energy of the body from the descent and releasing it to power the subsequent ascent. In the absence of dissipative elements, a spring will cause the body to rebound with the same speed at which it fell; this behavior is seen in Figure 2.11. In terms of the body motion controller, the springs provide much of the impulse which sustains hopping, and the controller’s job is to add or subtract a bit in order to control hopping height or attitude. The control algorithm used in this section therefore reverts to a simple type of PD controller to control hopping height, rather than one which computes precisely the impulse needed to cause a hop to the correct height. Knowledge of spring and body properties as well as the body’s state would allow the use of a controller which computes the necessary impulse to provide from an analytically derived equation; however, as it is the goal of the current research to demonstrate the effectiveness of a very simple controller, a simple hand-tuned PD controller is used for every axis. The vertical motion controller, then, will compute an impulse based on the difference between the touchdown speed which would create the desired takeoff height after spring rebound and the actual touchdown speed. This is in contrast to the controller for legs with no springs which computes its vertical impulse based 49

on the difference in the vertical velocities – which is the sum of the speeds because the velocity at touchdown is in the opposite direction from the velocity at takeoff. The leg controller used in this simulation is very simple. An actuator is implemented in the C++ class CImpulseActuator, descended from the base class of connection objects, CConnection. When told to “turn on”, this actuator supplies a force of a specified magnitude until the time integral of that force has reached a desired value. Then the impulse actuator turns off until it is activated once again. Such a device could be easily constructed in the real world using hardware which can be made to supply controlled forces, such as a voice coil or electric motor. In order to fully realize the benefits of the hierarchical control system architecture discussed here, feedback control in the leg could be used which actively ensures that the desired impulse is applied to the body. It could, for example, measure the impulse which has been applied during a stance phase, not turning the impulse actuator off until the correct impulse has been delivered (assuming that the actuator is capable of delivering enough impulse, hasn’t stepped into a gopher hole, and so on). This concept is discussed further in the discussion of future work in Section 5.4.

2.8.2

Leg-Spring Model Results The first test of the model with leg springs is conducted by simply dropping the

body from a fixed height of 0.15m above the surface. It should be noted that this height is the distance between position of the body as it is positioned prior to the simulation and the position of the body as the feet first contact the ground with no force in the leg springs; the equilibrium position of the body as it rests on the leg springs under the influence of gravity 50

is lower, the height depending on the stiffnesses of the leg springs. Figure 2.11 shows the response of the body to this “drop test.” As can be expected, the body bounces steadily. Because there are no energy dissipative elements in the system, the hopping height remains constant.

Figure 2.11: Drop Test of the Leg Springs

It is desired to verify that the controller is able to actively stabilize hopping height. This can be done by commanding a height which is substantially different from the height from which the body has been dropped. In Figure 2.12, the body is commanded to hop at a lower height than that from which it was dropped. This demonstrates the response of the system to a type of disturbance which has changed the hopping height from that which

51

was desired. As can be seen in the figure, the height of the hops quickly stabilizes at the correct value.

Figure 2.12: A Different Hopping Height

A further test of the height controller is performed by commanding the controller to change hopping height during a run. In this test, the body is dropped from a height of 0.15m and given a hopping height setpoint of 0.1m. After one second, the setpoint is changed to 0.2m. Because a hand-tuned PD controller is used instead of a control based on precise calculations, it takes a few steps before the hopping height settles down to the correct value. However, it gets there, as shown in Figure 2.13. This figure also shows the forces being applied by the feet as a function of time. The forces for all four feet are shown

52

Figure 2.13: Changing the Hopping Height

on the graph, but as the body is hopping vertically, all the foot forces are the same. The first two impulses are negative, meaning that the forces applied are directed downward, acting against the spring to reduce the hopping height from 0.15m to 0.1m. The next three are directed upward, pushing with the spring to increase the height. The last is downward, as the slightly underdamped controller steers the height to its setpoint. The scope of the pseudo-rigid simulations which have been successfully performed so far is limited by a number of practical issues facing spring-mass simulations. These issues include the following: • Modeling errors due to the pseudo-rigid approximation

53

• Computing speed limitations • Interactions between system dynamics and ODE solvers These issues are discussed in Section 3.5.

54

Chapter 3

Pseudo-Rigid Modeling of Mechanical Systems 3.1

Introduction This chapter introduces a dynamic mechanical system modeling package called

MechSys2. This package uses a comparatively simple modeling technique based on the application of Newton’s laws to point masses and the use of springs and other simple connecting devices to connect the masses together into simulations of rigid bodies. From a comparatively simple basis consisting primarily of masses and springs, we can build modestly complicated mechanical system models with numerous bodies and constraints. An intended use of this modeling system is the simulation of running machines with more complete dynamics than are simulated using more traditional techniques. The MechSys2 software can be used to model the behavior of a system with many legs, actuators, prismatic and

55

rotary joints, and other similar components. The output of such a simulation includes information such as joint forces which is not available in many traditionally conducted analyses.

3.1.1

Historical Perspective The modeling system used in this project represents a decrease in the analytical

complexity and an increase in the computational load with respect to methods which have been used in the past. This section will discuss the historical perspective in which this technique fits, showing why such a computing-intensive structure is appropriate for contemporary use. An example of a problem which has been solved using each of the methods discussed in this section is given in Section 4.4.

Classical Methods The “classical” method of analyzing a dynamic system is through the derivation of equations of motion and the analytical solution of these equations. Classical methods of simulating the motion of dynamic systems in three dimensions generally involve a good deal of complexity in the mathematical representation of the systems. This complexity has been created primarily to deal with the historically high expense of computation: before powerful digital computers became commonplace, it was less expensive to apply cognitive (human) thinking to a problem than to program and use automatic means of crunching numbers. Therefore, the complexity inherent in a mechanical analysis was handled by the reasoning skills of a trained engineer who derived the appropriate equations to represent a particular problem. If possible, these equations would be constructed in a form which could 56

be solved analytically, because the computation involved in solving equations numerically was expensive. A great deal of the training given to mechanical engineers has been in analyzing systems in such a way as to produce equations which lend themselves easily to analytical solution. For example, traditional methods of representing the motion of free bodies in three-dimensional space rely on a complex and clever bag of tricks to produce analytically soluble equations in useful special cases. Equations which represent the motion of a spinning axisymmetric satellite or top in terms of rotation, precession, and nutation are useful only for one particular class of problems. They also rely on a high level of abstraction, creating concepts such as precession to represent a particular behavior of a system. The physical reality of the system consists of elements of mass obeying much simpler laws of motion. The most serious limitation of the classical method is the degree of complexity which can be handled: even modestly complex systems do not lend themselves to a purely analytical solution. An additional difficulty arises in the solution of problems involving geometric constraints. Classical formulations such as Lagrangian mechanics facilitate the solution of some modestly complex problems, but the formulation tends to cancel out terms of significant interest such as forces within the system. We can use Lagrangian mechanics to find the motion of a double pendulum, but the forces in the pivots remain unknown – a significant problem if our job is to design the pivot bearings.

Numerical Solutions By “numerical solutions” we refer to the numerical rather than analytical solution of equations of motion which have been analytically derived. Most often this is accom57

plished through the use of general purpose mathematical codes which permit the numerical solution of equations which do not lend themselves readily (or sometimes at all) to analytical solution. This is important because often the equations of motion of physical systems are not analytically solvable. The creation of equations to model a system in sufficient detail that its equations of motion can be analyzed numerically still requires an intelligent analysis of the specific problem; but the technique can be considered more general than a purely analytical approach in that a wider range of problems can be solved. However, this method is not practically applicable to many problems involving more complex systems. The formulation of the equations of motion of a six-legged running robot would require a dreadfully great deal of effort.

Fully Numerical Methods In recent years, the space of dynamic systems which can be simulated has expanded dramatically as methods which are here described as “fully numerical” have come into common use. The most prominent numerical method used in the simulation of physical systems is the Finite Element Method, or FEM, used in solid mechanics. In this method, a physical body which consists of a continuous piece of material is represented as a set of elements of finite size. Differential constituative equations describe the variation of a quantity such as force, temperature, or electric potential within the continuous solid; these constituative equations are approximated by difference equations relating the values of a quantity at discrete points within the material. A finite element solution is very general in that the constituative equations are the same regardless of variations in material properties such as density and stiffness as well as the geometry of a piece of material. The role of the 58

human operator in performing an FEM analysis is the specification of the properties and geometry of components and the external conditions to which the components are subjected – not the derivation of analytical equations. The cost of the generality of FEM is computing power. The analysis of even a modestly complex system will involve the creation and solution of systems of equation with thousands of variables, and this type of solution can take a long time for even comparatively fast computers. Simulations which take hours and even days to compute are commonplace. Originally, finite element programs were used to analyze simple static systems. The computing power and time required to find the stresses in a statically loaded part were as great as was available. More recently, the exponential rise in computing power has allowed the solution of dynamic problems as well as static ones. In order to solve a dynamic problem, a computing effort comparable to that required to solve the static problem must be made repeatedly as time progresses. Such computing power is readily available at this time, and software is now available [13] which is specifically designed for the solution of dynamic problems. Such software is based on the ability of FEM to model material behavior, and it can provide a more complete analysis than most analytical solutions in that the internal stresses and forces acting on constraints are computed. Because finite element analysis models the internal stresses within a rigid body, it performs a great deal of unnecessary computation if all we’re interested in is the external behavior of the body in a dynamic system. The assumption of a rigid body (rather than an elastic or plastic body whose material properties must be taken into account) greatly simplifies the numerical problem, as the focus of computation is determining external forces

59

and constraints rather than conditions inside the material. A number of software packages have been created expressly to perform rigid body simulations.

3.1.2

Related Work The more distant ancestors of the MechSys2 technique are analytical and semi-

analytical solution methods discussed in Section 3.1.1. Immediate ancestors are several dynamic simulation software packages. A popular dynamic simulation package is Working ModelT M , produced by the MSC.Working Knowledge corporation [14]. This software implements what is referred to by the manufacturer as “physics-based” simulation; the term appears to be used merely to differentiate the product from graphical packages which are based on motion capture or animation. Because this is a proprietary commercial product, the details of the implementation are not freely available. It would appear that the modeling of rigid bodies in this software is based on the standard Newton/Euler equations using the inertia tensor as the basic property of a body. Versions of this software are marketed which are devoted to educational use, reinforcing the notion that, as discussed in Section 3.1.3, dynamic simulation software can be a useful and important tool in the physics and educational cirriculum. A freely available software package which implements rigid body dynamic simulation is Aero [15]. As an academic project, this software was written in C for use under X windows running on UnixT M class computers, and its source code is freely available. Rigid bodies are represented within simulations by their inertia tensors. Connections are modeled using spring and/or damper elements, in much the same way as in the software developed in the current research. Aero can be considered an immediate ancestor of the current software 60

with regard to the dynamic simulation techniques. As with many software applications, the emphasis in Aero is on producing graphic animations which appear realistic rather than on studying the dynamic effects for engineering purposes. However, sufficient information is computed within this package for it to be considered potentially useful as an engineering tool. An interesting feature of the simulation technique underlying Aero is its treatment of collisions. There are two methods available to simulate bodies which collide. The first is for the software to automatically insert springs and dampers between bodies when contact is made; the relative motions of the bodies will cause deformations of the springs and dampers which cause forces to be applied to the bodies, thereby causing an exchange of forces simulating the forces applied to physical bodies in contact. This method is similar to that used in the current research, except that the method by which the forces generated in the springs and dampers are applied to the modeled rigid bodies is different. The second method applies conservation of momentum laws to sets of two bodies in order to compute the bodies’ motions after a collision. This method is more theoretically complex than the spring/damper method and is limited in that it is very difficult to apply in the case of multiple simulaneous collisions. However, it is computationally more efficient. A simulation software package which was developed expressly for use with legged machines is SimSect[27]. This package uses conventional Newton-Euler based dynamic simulation techniques combined with non-stiff ODE solvers. It adds a unique capability in its treatment of the changes of system behavior which occur when a machine’s feet touch or leave the ground. These state changes are modeled explicitly through a formalized and well-

61

defined mechanism. The result is the capability of simulating the discontinuous behavior of a running machine more effectively than other packages which treat state changes in a more ad hoc fashion, including the system discussed in this thesis. The advantage of the system discussed here is that changes of system configuration are handled internally by the simulation and do not require explicit programming effort by the user. In certain situations this advantage is believed to be worth the performance cost. Another ancestor of the current software is a package of code developed by Auslander for use in the class ME39E, “Animating Physics”, taught to freshmen at the University of California at Berkeley. The purpose of this software is to create simulations of interesting dynamic systems using a software base whose simulation equations and application programming interface can be learned by university freshmen in a short time. This software is written in Matlab and is therefore burdened by the slow throughput imposed by that package. However, it is simple and flexible and lends itself well to use by students. It has been used successfully in the teaching of the ME39E class. The role of impulses in running machine simulations discussed in Chapter 2 is reminiscent of a dynamic simulation technique based entirely on impulses, discussed in [21], [20]. This technique models all interactions between rigid bodies as a sequence of impulses. For example, a rock which is sitting upon a road is held in place (as modeled by the computations) by a rapid sequence of impulses which prevent the rock from sinking into the road. Friction between the rock and road is modeled by the application of impulses tangential to the contacting surfaces. This modeling scheme allows many problems which are very difficult to solve using more traditional techniques to be modeled in an efficient and

62

self-consistent manner, and it allows the computation of contact forces by simple integration of impulses over time. However, the underlying model is a bit counterintuitive, and it would be difficult to use a modeling scheme such as this to impart a better understanding of physics into lower-division undergraduates. The term “pseudo-rigid bodies” has been used to describe the rigid body objects which are constructed using the technique discussed in this work. Another concept has been previously given this name, one discussed in a book by Cohen[7]. This previous work, however, deals with a treatment of a rigid body as a continuous body on which is imposed a displacement field which is distinct from the stress and strain used in typical engineering analysis. Whereas the work discussed in this paper seeks to use a nearly rigid model to approximate a rigid body and any internal displacements are treated as errors, the Cohen work seeks to describe the displacements inside the body rather than minimize them.

3.1.3

Introduction to the Software The software developed during the course of this research is referred to as Mech-

Sys2. The design of this software is based upon the idea that any fully rigid body can be modeled to any necessary degree of accuracy by replacing it with a finite set of point masses which are connected together with a set of stiff springs and/or dampers. By “fully rigid body” we mean one whose internal strains are negligible. A proof that any rigid body can be modeled in this manner is given in Section 4.2.4. There it is shown that at most four masses are needed to represent a rigid body. The point masses in MechSys2’s rigid bodies are connected within each body by springs and/or dampers which are stiff enough to prevent significant relative displacements 63

from occurring between the masses. Hereafter we shall refer to these connections as internal or stiff springs with the acknowledgement that they may also contain dampers or some other elements to suit the design requirement of small relative displacements between nodes. The choice of the stiffness of a spring is, at this time, left to the discretion of the user. If a spring is not sufficiently stiff, then it will allow the masses in a rigid body or the parts of rigidly constrained systems to move excessively with respect to each other under the application of the forces involved in a simulation. When this occurs, the constraints in the system are violated, as is the integrity of the output. Springs which are too stiff will cause numerical trouble. The system of ordinary differential equations which form the numerical model will become excessively stiff; roundoff and geometric errors will be amplified sufficiently as to corrupt the simulation output; and the simulation may take an excessively long time for a computer to run. Test results have shown that there is usually a fairly wide range of usable stiffnesses (at least two or so orders of magnitude) within which acceptable values may be chosen by the user, and stiffness tuning has not proven to be a serious limitation on simulations so far. Automatic tuning of stiffness, probably based on estimates of the error due to spring deflection, would be a beneficial feature to add to the software, and its use is being investigated. Springs and dampers in MechSys2 may also act as ordinary mechanical springs and dampers if so required. The code for these elements is the same as that for stiff springs and dampers except that substantial deflections of these elements is not considered an error. In order to use a simulated rigid body, one must be able to apply forces to it. When rigid bodies are modeled through the use of the inertia tensor, the application of forces

64

is accomplished through the standard Euler formulation, Equation 4.16 in Section 4.4.1. However, in the MechSys2 scheme of things, there is no inertia tensor – only a set of point masses, each of whose rotational inertia is by definition zero. In order to apply a force and/or torque to the body at an arbitrary location, we have two options: • Transform the force and torque into a set of forces to be applied to the point masses. The net force and torque on the body due to those at the point masses must add up to that being applied. This method is used in the “ghost point” technique discussed in Section 4.2.2. • Create a new point mass entity located at the point of application of the force. This new point mass must have a finite mass, but the mass may be small enough that it does not significantly alter the mass properties of the rigid body, in which case the mass is called a “dummy mass” and used as discussed in Section 4.2.1. Each of these methods will be more appropriate or efficient in some situations. A search of dynamic simulation literature has not revealed any other use of the dummy mass technique for the application of forces. Geometric constraints are modeled in MechSys2 in a manner which is not particularly different from that used in previous software such as Aero. “Generalized surfaces” are defined as areas of space, relative to a body, within which certain contact points are required to remain. For example, consider a robot with feet which are represented by points. The foot points are constrained to move above the surface of the ground. This configuration would be modeled by assigning to each foot an attachment point, and connecting the foot to the attachment point with a stiff spring of zero rest length. The attachment point is 65

a component of the surface entity (this is convenient for reasons of software organization) and the surface entity constrains the contact point to always remain above ground. If the foot moves to a point below ground, the spring which connects the contact point to ground is extended, causing a force to be exerted upon the foot. The stiffness of the contact spring causes this force to become large enough to push the foot back to the surface of the ground before the penetration, which is of course an error term, becomes significant. The force which is needed to keep the foot at ground level is computed by the contact spring and is readily available. A principal advantage of the simulation method used in this software is that the equations of motion which form the numerical simulation are extremely simple. The only components which intrinsically contribute to the set of ordinary differential equations are the point masses, each of which obeys Newton’s laws (this software is not intended to model relativistic effects, and the user is warned to apply it only to problems of relatively low velocity). Concepts such as angular momentum and torque, for example, are not explicitly modeled at all. At first it would seem odd and undesirable to use software which does not intrinsically deal with basic concepts such as these. However, further reflection should reveal that angular momentum, for example, is not a fundamental property of macroscopic dynamic systems at all; instead, it is a useful abstraction which can be used to describe the dynamic behavior of systems which are made up of many point masses, each of which has linear momentum. Concepts such as the precession of a rotating satellite, which are usually described in terms of angular momentum, can just as accurately be modeled in terms of the interactions of the linear momenta of the point masses making up the system. This is seen

66

in Section 4.4.1, in which a precessing satellite is modeled by techniques involving angular velocity and momentum as well as by point masses. Another unusual feature of the MechSys2 software is the choice of state variables for the point masses. Most algorithms which are in common use choose a mass’s position and velocity as its state variables. MechSys2 uses the position and momentum of each point mass instead. It is anticipated that this choice of variables will facilitate easier implementation of models of systems which have variable mass, such as vehicles which use fuel and become lighter. However, this feature has not yet been fully implemented and definitive conclusions about the significance of using momentum cannot be made at this time.

Educational Benefits The basic principles of dynamics are certainly one of the most important fundamentals which students of engineering need to know. However, the traditional mechanics cirriculum has not been as successful as one might hope in helping undergraduates to gain an insightful understanding of principles as basic as Newton’s Laws. In fact, studies have shown that a thorough understanding of very basic mechanics is lacking in a large number of undergraduate students learning physics [11]. It would seem apparent that an improvement in teaching tools is required. Some educators advocate the use of computationally based instruction. Software based on the principles used in MechSys2 may be helpful to such a curriculum. The underlying components – masses, springs and such – operate in a way which can be easily understood. The concept of a body as a collection of attached point masses is demonstrated in a convincing manner. A good deal of work on the user interface of the software would 67

be needed in order to turn it into an effective educational tool, but the infrastructure as currently implemented shows great promise. Most significant is that using simple principles – the inertia of point masses, the behavior of springs, and simple geometric constraints – problems which are very challenging to traditional techniques can be set up and solved. On a higher educational level, the object-oriented organizational structure of the MechSys2 package is designed to be easily extended, modified, and even “hacked” by educators and students. This not only facilitates the addition of new features such as the aforementioned user interface but also allows for substantial modifications by researchers and students at the graduate level.

Numerical Considerations A number of unusual numerical problems need to be addressed in order to construct a useful dynamic simulation package based on the spring-mass model. The most important issues relate to error quantification and control and ordinary differential equation solver performance. Some of the problems caused by numerical issues are discussed in Section 3.5. A rigid body, by definition, does not experience any internal displacements; the distance between any two points inside the body always remains constant. In the spring-mass modeling structure, however, springs of finite stiffness are used to maintain the distance between the points which comprise the body. In the presence of forces, these springs must deflect to some degree, and this deflection is an error in the representation of a rigid body. In order to keep the errors due to spring deflection low, the springs must be made very stiff. This stiffness can cause numerical problems, in particular with ODE solvers as discussed in the following paragraph. Therefore, the spring stiffness must be chosen such that it does 68

not cause excessive errors nor numerical problems. This is a process with which most users of the software would rather not be concerned; a method of automatically setting a workable spring stiffness would be preferred. A number of schemes for automatically choosing spring stiffness, or perhaps even adjusting it during a simulation, have been examined on a preliminary basis. These schemes involve measuring the relative displacement in a body (represented as a strain) or using some other indication of error based on the energy stored in the springs. Once the error has been quantified, it can be used to adjust the spring stiffness such that the springs are only as stiff as necessary to provide the required accuracy. Time has not permitted an automatic stiffness system to be implemented; instead, the measurement and control of errors, and the adjustment of spring stiffness, must be performed manually by the user of the software. The simulation of a dynamic system involves the solution of a set of ordinary differential equations by an ODE solver. It is the ODE solver which drives the progress of the simulation, and the rate at which simulations run is determined in great part by the efficiency with which the ODE solver does its job. The system of equations generated by a mass-spring model is difficult for ODE sovlers to handle because it is inherently quite “stiff.” This means that the ratio of the fastest time constant in the system to the slowest is very large. The reason for this is easy to visualize: the vibration of the point masses, which are connected by very stiff springs, will take place at a very high frequency (or short time constant) with respect to the motion of the body within the world. For example, consider the spring-mass body shown in Figure 3.1. Let it be a simplified representation of a running robot, with force F being the force applied

69

by the feet. When F is suddenly changed (for example, when the feet contact the ground),

Figure 3.1: Generic Spring-Mass Body

vibrations will occur in the springs which connect the masses together. These vibrations will have a frequency of approximately

p

k/m = 1000 Hz. If the running robot’s hops reach

0.1m high, the frequency of the strides is approximately 7 Hz. The trouble with stiff problems occurs when we attempt to solve them efficiently using a regular “nonstiff” ODE solver. In order to accurately solve the high frequency vibrations, the solver must solve the equation several times per cycle of those vibration. This leads to about 5000 solutions per second for the problem mentioned above. This takes the ODE solver a great deal of time. On the other hand, we don’t care about the vibrations of the masses with respect to each other, and we would prefer to find about five to ten solution points per stride, or 35 to 70 per second. However, if we set a nonstiff solver to solve the system at 70 solutions per second, it will not only find incorrect answers but almost

70

undoubtedly become unstable because of the influence of the vibrations of the springs. Fortunately, a class of ODE solvers exists which uses an algorithm which is always numerically stable, even in the presence of the high frequency vibrations present in this problem. A “stiff solver” uses a different type of algorithm to solve ODE systems, and it can safely be used at time steps which are much greater than the high frequency of the vibrations – if the amplitude of the vibrations is small enough to ignore. Stiff solvers are used for nearly all work with mass-spring dynamic simulations. A very good discussion of the behavior and algorithms used in simpler stiff solvers is given in [33], and a discussion of how specialized versions of ODE solvers are implemented in the MechSys2 software is given in Section 4.1.3. The use of stiff ODE solvers incurs a cost. Although stiff solvers do not need to take nearly as many solution steps as nonstiff solvers, each step takes a great deal more time. One reason is the need for stiff solvers to find the Jacobian matrix (a matrix of derivatives of the state variables with respect to changes in themselves and other state variables) of the system of equations at each step. In general, finding the Jacobian matrix involves estimating derivatives by perturbing state variables and recomputing the entire ODE equation many times. This is a computationally intensive procedure. It would be better – and a great deal faster – if the Jacobian could be computed analytically given a knowledge of the form of the state equations. This analytical computation may be possible, at least for some problems which are solved by the MechSys2 system. Developing software which does so is a promising idea for future work.

71

3.2

Model Components In this section, the components which make the MechSys2 software work are

listed and briefly described. A much more detailed description is given in Chapter 4, and a complete listing of the software documentation for the classes which comprise the software is given in Appendix B. A MechSys2 model needs to have the same functionality as any other dynamic system model, but the basic elements from which the simulation entities are constructed are comparatively simple. Pseudo-rigid bodies are constructed as sets of masses connected by internal connections. The masses are simply point masses; each point mass contributes six states to the dynamic system’s equation. It is possible to change a mass’s mass during a simulation run. The internal connections can consist of springs, dampers, or a combination of both; in any case, an internal connection has a high stiffness, holding the body’s masses close to their desired relative positions. External forces are implemented; these forces can be applied to bodies through the use of several mechanisms which transfer the force to a body’s masses from the point of application. Constraints are implemented by attaching surfaces to bodies. In this context, a surface is a generalized geometric entity which controls the motion of points with respect to a body. As a body moves, its surfaces move with it; attachment points are constrained to move within the bounds of each surface. Surfaces can also be grounded, attached to the reference frame rather than to a body moving within that frame. A dynamic simulation takes place within a global coordinate frame. For some entities, local coordinate frames attached to bodies can be used to specify positions; however, all computation takes place within the global frame. The orientations of bodies are stored as

72

rotation matrices. The rotation matrix which holds the attitude of a body is a matrix of rotation from the body’s original position to its position at the position being measured. The center of rotation is the body’s center of mass. This system is convenient in that it is easy to move items which are attached to the body from their original positions to the body’s current position. In the following sections, the types of components from which a MechSys2 model is built are described. These components are both the basis of the physical model of a system and the basis of an object-oriented software design.

3.3

Language Considerations The class structure could be implemented in any programming language which

supports object-oriented programming, such as Java or Matlab. C++ was chosen because it contains features which allow a substantial improvement in computational speed over many other languages. Some of these features are as follows: • C++ is a fully compiled language; executable code is machine code. The overhead of the source-code interpreter used to execute some Matlab code and the bytecode interpreter used in most Java implementations is avoided. • C++ gives the programmer some control over how memory is allocated. In particular, objects can be designed which will be stored in stack space instead of in heap space. When many objects are created and destroyed in a short time – such as when matrices are manipulated in standard mathematical forms such as “A = (B + C) * D” – storing these objects on the stack causes a serious problem with heap fragmentation 73

and slows computation down severely. • Templates in C++ provide a mechanism to write flexible code while handling the varying details (such as array sizes) at compile time. This can lead to a significant increase in processing time: many object-oriented mechanisms, such as inheritence, require extra processing at run time; and when details are fixed at compile time, an optimizing compiler can be used to improve execution speed considerably. The use of object oriented software design is important in developing a package which is well organized so that it can be easily maintained and modified to serve the needs of the MechSys2 project and others. As this software is being prepared for use in a variety of projects in an educational environment, it is expected to be worked with and on by a number of people. A clean, simple, consistent, and well documented programming interface is of critical importance. This interface is designed around a set of classes which represent the component parts of a dynamic simulation.

74

3.4

Representing and Displaying Attitudes The state of a rigid body is represented by the position of its center of mass in

space, the attitude (orientation) of the body, and quantities related to the derivatives of the position and attitude. The position of the body’s center of mass is easily visualized and represented in a typical Euclidean space as a point (xp , yp , zp ). The linear velocity of the body’s center of mass is also easy to visualize.

1

The representation of a body’s

angular position velocity is more complex to compute, represent, and visualize. There are many ways in which the rotational attitude of a body in three-dimensional space can be represented. [31, 36] The scheme which is most advantageous for use in storing and using information about body attitudes within the software is not well suited for use in visualizing the results, and vice versa. The primary use for attitude information within the MechSys2 package is to allow “attachments” to a body to move with the body itself. The inertia of the body is modeled as a set of point masses, and these are moved by the ODE solver. Other items such as constraint surfaces and reference points must be moved independently as part of the simulation; in order to move the attachments in such a way as to maintain their relative position with respect to the main masses, the program must know the position and attitude of the body. The method by which the attitude of the body is determined from the positions of the point masses is described in Section 3.4.4. In this section the schemes used to compute and represent angular positions are described. Methods for transferring attitude data from one schemes into another for purposes 1

While the MechSys2 software uses linear momentum rather than linear velocity as its native format in simulations, the relationship of momentum to velocity is fairly simple, and momentum is thought of as a representation of velocity.

75

of input and output and to aid in visualization of simulation results are also discussed.

3.4.1

Rotation Matrices A real vector v of dimension N can be rotated from any starting position v0 to

any final position vf through multiplication by a real rotation matrix R of dimension N by N . The “rotation matrix” will change the orientation of the vector but not its length. If a set of vectors vi are each multiplied by the same rotation matrix, they will be moved such that the relative orientation of the vectors remains the same. If the vectors describe the position of points on a body with respect to the body’s center of mass, multiplication by the rotation matrix implements a rotation of a rigid body. Because the elements of a rotation matrix are the cosines of angles between initial and final positions of reference axes attached to a body, the matrix is properly referred to as a direction cosine matrix. However, the term “rotation matrix” is used in this document for convenience. A three-dimensional rotation matrix contains nine elements, but when it is used to represent an attitude in three-dimensional space, there are only three independent degrees of freedom. Therefore the nine-element matrix has a degree of redundancy. The use of a rotation matrix rather than Euler angles or parameters can be thought of as “wasting” computer memory. However, memory is quite inexpensive and is not expected to be a limit on the scale of problems which can be solved through the use of MechSys2 software, so this issue is not particularly significant. More significant could be numerical errors which creep into rotation matrices as the result of roundoff or issues relating to springs which are not perfectly stiff, as discussed in Section 3.4.4. However, it is expected (though not proven) and has been observed that with the use of double–precision floating–point numbers in C++, 76

these errors are not large enough to cause significant problems. Except for a few cases of rotation about coordinate axes, rotation matrices do not lend themselves well to use in visualizing attitudes. Therefore, it is often necessary to convert attitudes from rotation matrices to some other form before presenting them to the user. This conversion is discussed in detail in Section 3.4.5.

3.4.2

Euler Angles It is possible to represent the attitude of a body in terms of a set of three angles

of rotation. Rotating the body through these three angles, about the appropriate axes of rotation, in the given order will move the body from an assigned reference position (in which the angles of rotation are zero) to any possible final position. Euler angles are the system most commonly used for visualization. The attitude of an aircraft, for example, is usually described in terms of roll, pitch, and yaw angles; a camera is often panned to angles of altitude and azimuth and then tilted through some angle about its optical axis. There are many different sets of Euler angles which can be used to represent any given attitude. For example, the attitude of an aircraft could be given by rotating it about the pitch axis first (say, nose up), the roll axis second (bank the wings) and the yaw axis third (turn the nose to the right); or the aircraft could reach the same orientation by moving in yaw first, roll second, and pitch third. One set of Euler angles is diagrammed in Figure 3.2. This figure represents the rotation of an axisymmetric body through the “3–1–3” or “z–x–z” angle set. In this diagram, the (x, y, z) axes are attached to the body. The original positions of the axes are (x, y, z). After a rotation through angle ψ about the z axis, the axes are at positions (x0 , y 0 , z 0 ). A rotation through angle θ about the x0 axis 77

moves the axes to positions (x00 , y 00 , z 00 ). A last rotation through angle φ about the z 00 axis moves the axes to final positions which aren’t shown for clarity.

Figure 3.2: Rotations Through Euler Angles

Euler angles are advantageous in being relatively easy to visualize. They are able to represent a body’s attitude with three numbers, so there is no redundancy in the representation. Euler angles have significant disadvantages, however. In order to use Euler angles in computations such as finding the relative position of a point on a body with respect to its center of mass, a number of trigonometric calculations must be performed, and these calculations consume a large amount of computing resources. The most serious disadvantage of Euler angles appears when computations must be performed to determine or apply the angles when some reach 90◦ . Singularities in the equations can occur which prevent successful computation. An example is the problem of determining the compass

78

direction of a point which is directly overhead. The problem can be avoided by switching from one set of Euler angles to another when close to singularities. This scheme leads to even greater computational complexity, however, and makes it more difficult to visualize attitudes represented by Euler angles. Euler angles are useful for input and output to and from a program but not usually well suited for use as an internal representation of attitude. The MechSys2 software therefore allows attitudes to be entered and output as Euler angles but does not use them internally. The conversion from Euler angles to and from rotation matrices is discussed in Section 3.4.5.

3.4.3

Euler Parameters and the Quaternion Any movement of a body from a starting attitude to a final attitude can be accom-

plished by performing one rotation through some angle Φ about some axis eˆ, where eˆ is a unit vector pointing along the axis of rotation (in other words, eˆ is unchanged by the given rotation). In general, eˆ will not be parallel to a coordinate axis, nor to an axis of symmetry of a body. This is the Euler axis and angle representation of a body’s attitude. This representation is sometimes helpful in visualizing a body’s attitude, although the Euler angle representation is much more commonly used for this purpose. The Euler axis/angle method does have the advantage that one representation can be used for all rotations, rather than choosing a set of Euler angles to use for each problem. In order to use the Euler axis and angle to compute motions within a model, it is necessary to convert an attitude representation into a rotation matrix. The matrix which represents a rotation through angle Φ about axis eˆ = (e1 , e2 , e3 ) is computed in the following 79

form: R = 



e21 (1

− cos Φ)  cos Φ +     e1 e2 (1 − cos Φ) − e3 sin Φ   

e1 e2 (1 − cos Φ) + e3 sin Φ e1 e3 (1 − cos Φ) − e2 sin Φ  cos Φ + e22 (1 − cos Φ)

e1 e3 (1 − cos Φ) + e2 sin Φ e2 e3 (1 − cos Φ) − e1 sin Φ

   e2 e3 (1 − cos Φ) + e1 sin Φ    

cos Φ + e23 (1 − cos Φ)

(3.1) As with Euler angles, this computation involves evaluating a number of trigonometric functions and hence takes a significant amount of computing power. A commonly used relative of the axis/angle representation is the Euler symmetric parameters. These parameters are defined by the following relations: Φ 2 Φ ≡ e2 sin 2 Φ ≡ e3 sin 2 Φ ≡ cos 2

q1 ≡ e1 sin q2 q3 q4

(3.2)

These parameters are often treated as the components of a quaternion, q = (q1 , q2 , q3 , q4 ). They have the advantage over the Euler axis and angle that they permit the rotation matrix to be computed via a somewhat simpler formula, one which does not contain all the trigonometric functions. The rotation matrix is computed as the following:      R=   



q12

−

q22

−

q32

+

q42

2(q1 q2 + q3 q4 )

2(q1 q3 − q2 q4 )

2(q1 q2 − q3 q4 )

−q12 + q22 − q32 + q42

2(q2 q3 + q1 q4 )

2(q1 q3 + q2 q4 )

2(q2 q3 − q1 q4 )

−q12 − q22 + q32 + q42

       

(3.3)

This formula nevertheless involves significant computation. It has been decided that the 80

advantage of Euler parameters over storing attitudes in a rotation matrix – the advantage of less storage space used – is not worth the extra computation.

3.4.4

Computing the Rotation Matrix Before a rotation matrix can be used to move attachments to a body, it must be

computed from the information which is available about the body’s position. The MechSys2 software uses a novel technique for simulating the motions of a rigid body, that of moving a set of point masses by the simple application of Newtonian mechanics. Therefore, a novel technique must be found to compute the attitude of the rigid body at any given point in the simulation. The technique which has been designed determines a rotation matrix which causes the relative positions of the point masses to move from their original position at the start of the simulation to their position at a given time. In Figure 3.3, a body is rotated from its original position (at left) to a final position (at right). The body being modeled is represented by the irregular shape; the MechSys2 representation consists of the four point masses connected by six connections. One point mass is arbitrarily chosen within the body and labeled as mass zero. Three vectors u, v, and w extend from mass zero to the other three masses. As the body rotates about its center of mass, the relative positions of the masses, as represented by the three vectors u, v, and w, change from (u0 , v0 , w0 ) to (u1 , v1 , w1 ). We seek a rotation matrix R which transforms each vector from its initial to its final position. The rotation matrix then satisfies the equations u1 = Ru0 , v1 = Rv0 , and w1 = Rw0 . A straightforward solution can be found by writing the above equations as a system of nine equations in nine unknowns. The unknowns are the coefficients Rij of the rotation 81

Figure 3.3: Rotating a MechSys2 Body

matrix.





                                  

u11  u12 u13 v11 v12 v13 w11 w12 w13





 u01         0           0         v01       =   0         0           w01         0      

0

u02

u03

0

0

0

0

0

0

0

u01

u02

u03

0

0

0

0

0

0

0

u01

u02

v02

v03

0

0

0

0

0

0

0

v01

v02

v03

0

0

0

0

0

0

0

v01

v02

0

0

0

0

0

0

0

w02 w03 0

0

0

0

w01 w02 w03 0

0

0



0   R11 

    0      u03       0      0       v03      0      0    

w01 w02 w03

R12 R13 R21 R22 R23 R31 R32

                                 

(3.4)

R33

82

This equation can be easily be solved through the use of standard numerical solution techniques such as Gaussian elimination or LU decomposition and back substitution. In the MechSys2 software, the LU decomposition solution is used. There are important limitations to the use of this technique for computing rotation matrices; these limits must be kept in mind when designing software. In order to arrive at a valid solution, the matrix of coefficients in Equation 3.4 must be invertible. This condition requires that none of the vectors u, v, and w be collinear. For example, one might wish to use this technique in the simulation of problems involving a slender cylinder (one whose radius is much less than its diameter). The set of point masses which simulate this body will lie at the vertices of a rather long, slender triangular pyramid, and at least two of the vectors u, v, and w will be nearly collinear. This will be accompanied by a determinant in Equation 3.4 approaching zero. The result will be increased numerical errors as the ratio of the cylinder’s length to radius increases. The problem is analogous to that of ill-conditioned elements in finite element analysis. The problem can be mitigated by modifying the means of representing slender bodies – instead of modeling them with a single set of four point masses, for example, one can use two smaller sets of point masses located some distance apart. In some circumstances it may be desirable to use less than four masses to model a body. For example, the dynamic behavior of a “stick” (very slender rod) can often be reasonably approximated through the use of just two point masses. This approximation may be acceptable if the rotational inertia of the stick about its axis of revolution is negligible in the real system. Although such methods can have significant benefits in terms of the

83

computational time required to perform a simulation, they are not directly supported in MechSys2, and the method of determining attitude just described certainly will not work – a three dimensional attitude isn’t even defined for an infinitely slender rod. Not only are there not enough relative position vectors between masses (there is only u), there is no component which can rotate about the axis of symmetry to define an angle. It is important to examine the reliability of a numerical technique. Due to the complexity of the equations, numerical techniques were used to test the technique with a large number of random inputs.

3.4.5

Rotation Matrices To and From Euler Angles The attitude of a body can be represented by a set of Euler angles, by a rotation

matrix, or by various other means. Because each system is able to uniquely represent an attitude, there must be a way to convert any attitude from each representation to each other one. In this section we discuss conversions between rotation matrices and Euler angles. With rotation matrices being the method of choice for the internal representation of angles within MechSys2 and Euler angles often being chosen for attitude input and output, this type of conversion can be expected to be used often. The conversion from Euler angles to rotation matrices is a relatively simple process [36]. For each rotation about a reference frame coordinate axis, there is a corresponding rotation matrix. These rotation matrices are given as follows, with matrix Ri corresponding

84

to a rotation through an angle θ about axis i: 



Rx =

 1     0   

0

0

    sin θ    

cos θ

0 −sin θ cos θ





Ry =

 cos θ     0   

0 −sin θ  1

sin θ 0

0 cos θ



Rz =

      

(3.5)



 cos θ     −sin θ   

0

sin θ 0  cos θ 0

   0    

1

A single rotation of a vector v is performed by multiplying that vector by the rotation matrix. To rotate v about the y axis, we compute Ry v. A combination of rotations is computed by multiplying the vector by each of the rotation matrices in turn or by multiplying the vector by one matrix which is the product of all the rotation matrices, multiplied in the reverse order of rotations: to rotate by the x axis first, then the z axis, and finally the y axis, we compute a total rotation matrix R = Ry Rz Rx . It is important to note that the order of the rotations affects the final result, and hence the choice of Euler angle system is important. The computation of Euler angles from a rotation matrix is necessary to allow easily readable output from a run of the MechSys2 software. This computation is a bit less straightforward than that in the reverse direction. It also depends on the particular sequence of rotations in the Euler angles chosen. Accepted methods exist for performing

85

this conversion; the derivation of the equations for one such conversion will be shown below. In order to create a complete suite of conversions, twelve sets of equations must be coded. As an example of the conversion of rotation matrices to Euler angles, we can examine the computation of the angles (θ, ψ, φ) for a “z–x–z” sequence of rotations. By multiplying the rotation matrices for rotations a rotation through angles θ about the z axis, ψ about the x axis, and φ about the z axis in sequence, we get the following rotation matrix: R= 



 cos φ cos θ − sin φ cos ψ sin θ     −sin φ cos θ − cos φ cos ψ sin θ   

cos ψ sin θ + sin φ cos ψ cos θ −sin φ sin θ + cos φ cos ψ cos θ

sin ψ sin θ

−sin ψ cos θ

−sin φ sin ψ     cos φ sin ψ    

cos ψ

(3.6)

The angles θ, ψ, and φ can be computed from components of this rotation matrix by substitution. By inspection we can see the following: tan θ =

−R31 R32

cos ψ = R33 tan φ =

−R13 R23

(3.7) (3.8) (3.9)

When ψ is computed as the inverse cosine of R33 , the angle will be between and 0 and π. The other two angles are computed as inverse tangents of fractions. This computation can yield angles from 0 to 2π by examining the signs of the components of the fraction. Fortunately, the C++ language provides a convenient function atan2() which computes the 86

sign of an arctangent of a fraction automatically; this function was used in writing the MechSys2 angle conversion code. The weakness of the Euler angle representation scheme becomes apparent when we attempt to calculate angles in a system where ψ is near zero. In this case, Equations 3.7 and 3.8 will attempt to compute the inverse tangent of zero divided by zero! The physical situation which causes this numerical disaster occurs when the axis of rotation for the first rotation coincides with the axis of rotation for the second. There is no way to tell how much of the rotation is in angle θ and how much is in angle φ. This is one of the famous singularities inherent in the Euler angle representation. In software, the singularity can be worked around by making a convenient assumption that one angle, say θ, will perform all the rotation if ψ is near zero. In this case, the rotation matrix will be in the form of the third equation in Equations 3.5, and the angle θ can be calculated as the inverse cosine of the upper left element of the rotation matrix. When this scheme is used to produce output such as graphs of angles in the system over time, discontinuities in the angles will occur when the system crosses singularities. Similar equations can be derived for each of the other possible Euler angle sets. It remains for the software to implement a set of distinct methods to deal with each of the Euler angle sets which are needed to solve a given problem. In the MechSys2 system, an object oriented class hierarchy is used in which separate classes are written to handle each particular angle set.

87

3.4.6

Angular Velocity The representation of angular velocity is considerably less troublesome than that

of angular position. Angular velocity is described by a vector ω: when a rigid body rotates about its center of mass with an angular velocity given by the vector ω, a particle within that body has instantaneous velocity v given by the formula v =ω×r

(3.10)

where r is the position of the particle with respect to the mass center. Angular velocity vectors are well-behaved linear quantities, so if a body is subject to two rotations ω1 and ω2 , the instantaneous linear velocity of a point can be computed as v = (ω1 + ω2 ) × r. In contrast to the addition of angular positions, the order of addition of angular velocities does not matter. In a MechSys2 simulation, the angular velocity issues of interest are those of input and output. We must be able to set the angular velocity of a body as an initial condition of a simulation and determine the angular velocity of a body during a simulation. The task of setting the initial angular velocity is straightforward; the definition of ω is applied at each point in the body. Since the state of each mass is given by its position and momentum, this requires that the momentum of each mass be set appropriately. The equation used to set the momentum pi of each mass, whose mass is mi , is simply pi = mi (ω × r)

(3.11)

This computation is performed in method setInitialAngularVelocity() of class CBody. The method is called just before the dynamic system is initialized, so that the initial con88

ditions of each mass point can be saved prior to setting up a simulation run. Determining the angular velocity of a body from the linear momentum of each of its four main masses is a bit less straightforward for several reasons: • The equation v = ω × r cannot be applied in reverse to find angular velocity; given a position r, an infinite set of angular velocities can produce a given linear velocity v of one mass. The space of these linear velocities forms a line in three dimensions. • A position vector r parallel to the axis of rotation (and hence parallel to the angular momentum vector ω) cannot be used to accurately determine the magnitude of the angular momentum vector because the mass point isn’t moving. • In theory, any two masses whose position vectors r are not collinear can be used to determine the linear velocity. The solution would require finding the lines which represent all possible angular momenta as determined from the velocity of each mass, then finding the intersection of the lines. In the presence of numerical or modeling errors, this solution would have to rely on finding the point closest to the lines. • The use of two masses would lead to conditioning problems if there was a small angle between the positions r1 and r2 of the masses with respect to the body’s center of mass. For a body such as a long, slender cylinder the numerical problems could easily become quite severe. It would therefore be appropriate to find the vectors r1,2 which had the widest angle between them. • A MechSys2 body contains four main masses, and information about the motion of all four masses ought to be used in determining angular velocity if possible. However, 89

in the presence of roundoff errors and particularly modeling errors such as the finite motion of masses as they vibrate against the internal springs of a body, estimates of the angular velocity determined by using different combinations of masses cannot be expected to agree. With the aforementioned points in mind, a scheme has been designed which uses the motion of all four mass points to determine a least-squares best estimate of the body’s overall angular velocity. The angular velocity which causes each mass point to move with respect to the center of mass must obey Equation 3.11. Written in terms of the components of the vectors, this is vx = ωy rz − ωz ry vy = ωz rx − ωx rz

(3.12)

vz = ωx ry − ωy rx Regrouping Equation 3.12 with unknowns ωx , ωy , and ωz and writing in matrix form, we have





 0     −rz   

ry

rz 0 −rx







−ry   ωx      rx    ωy  

0

ωz

 vx            =  vy          

(3.13)

vz

which, of course, cannot be solved for ω because the matrix of coefficients is singular. Because we have four masses, there are actually twelve available equations involving the three components of angular velocity. These twelve equations can be grouped together to create one overconstrained system of equations. Let the position of each mass 90

point mi with respect to the center of mass be ri = (rix , riy , riz ) and its linear velocity be vi = (vix , viy , viz ):

                                                 



0

r1z

−r1z

0

r1x

r1y

−r1x

0

0

r2z

−r2y

−r2z

0

r2x

r2y

−r2x

0

0

r3z

−r3y

−r3z

0

r3x

r3y

−r3x

0

0

r4z

−r4y

−r4z

0

r4x

r4y

−r4x

0





−r1y 

 v1x             v1y                v1z             v    2x            v2y         ωx            v 2z        =    ωy          v   3x     ωz        v    3y             v3z             v    4x             v4y          

(3.14)

v4z

A solution exists [32] which allows an overconstrained system Ax = b to be solved in a straightforward manner. The solution is computed by the following: x = (AT A)−1 AT b

(3.15)

This technique for determining a body’s angular velocity is applied in the method getAngularVelocity() of class CBody.

91

3.5

Modeling and Numerical Issues A number of significant numerical and modeling issues have plagued the develop-

ment of useful models such as running machine simulations in MechSys2. These issues were introduced in Section 2.8.2 and are discussed in more detail here. These issues have the annoying property of being highly interdependent. For example, when the stiffness of stiff internal springs is increased in order to decrease the errors due to the pseudo-rigid body flexing, computing time increases rapidly.

3.5.1

Systems and Solvers The most obvious symptom of numerical trouble appears when simulations become

unstable. This is not a feature of the system being modeled but instead is believed to be caused by interactions between the ODE solver and the spring-mass model. The root of the problem is the nature of masses held together by stiff springs: they vibrate. If the springs are very stiff, the amplitude of these vibrations is small; however, the frequency of the vibrations is high compared to other dynamics in the system. In order to perform an accurate simulation, an explicit ODE solver must accurately model these high-frequency vibrations, and this requires that the solver’s step size be small with respect to the period of vibration. For a system whose masses are on the order of 1kg and whose stiff spring constants are on the order of 106 N/m, the frequency of vibration is of order 1000Hz, and the solver step size should be on the order of 10−4 s. A ten-second simulation takes quite a long time to run in this case. If the step sizes are increased beyond this limit, an explicit ODE solver (such as the trusty Runge-Kutta) will not only lose accuracy but also become

92

unstable. The results appear as shown in Figure 3.4.

Figure 3.4: Simulation which Becomes Unstable

The onset of numerical instability can be delayed considerably by decreasing the step size of the solver or, equivalently, tightening the tolerance of an adaptive step size solver. However, as the tolerances are increased, the solution time increases. The point is quickly reached at which solutions to modestly complex problems such as the simulation of a running machine require hours of computing time to run on contemporary personal computers. The use of an ODE solver which uses an implicit solving algorithm, such as Gear’s or Rosenbrock’s method solvers, will prevent the solver itself from going unstable, even

93

when the vibrations of masses and stiff springs are not accurately modeled. However, other instabilities set in quickly enough that the problems in Figure 3.4 appear anyway. These instabilities are most likely a result of interactions between the errors in the solver and the dynamics of a system which contains masses and springs. Most problems occur when instabilities are triggered by the action of nonlinear entities such as contact surfaces. Unfortunately, such surfaces are key just the sort of problems which MechSys2 was designed to solve.

3.5.2

Energy Some insight into the modeling and numerical process can be gained by examining

the flow of energy in the simulated dynamic system. Such an analysis has been performed in an attempt to isolate the source of the most serious numerical problems, those which cause instability of the simulation. An example of a graph showing the energy in a simulated system is shown in Figure 3.5.

In this simulation, a running machine body supported by

legs which consist only of springs is dropped and bounces on the ground. The graph at lower right shows the time history of energy in the system. The interchange of energy between gravitational potential energy, kinetic energy, and potential energy stored in springs can be clearly seen. The total energy remains relatively constant, with variations being reasonable given that this simulation was run with an implicit-method ODE solver and fairly loose tolerances. The flow of energy is as expected for such a system. In some other simulations, spurious energy changes have been observed in contact springs, and contact surfaces are suspected of being a source of instability. A complete study of this problem has not been completed at the time of this writing. 94

Figure 3.5: Spring Hopping with Energy

The energy in each part of the system was computed by programming the dynamic system software to poll each entity in a given class, asking each entity how much energy it had. Each mass, for example, returns its kinetic energy, which is equal to E = 1/2mv 2 , v being the velocity of the mass; the mass also has a gravitational potential energy E = mgh, with g being the acceleration due to gravity and h the mass’s height above the arbitrarily chosen origin. Linear springs return E = 1/2kx2 , k being the spring constant and x being the deflection of the spring length away from the rest length. The graph shows the total energy of all elements of each type in the system, as well as the total energy for all elements in the system.

95

At this time, the pseudo-rigid dynamic simulation software shows promise as a method for simulating modestly complex systems such as running machines. However, a great deal of study is still required to solve the substantial numerical issues affecting the system. Some suggestions for further study are given in Section 5.4.

96

Chapter 4

Modeling Software Components This chapter discusses the components of the dynamic simulation software in detail. These components are divided into two main groups: • Generic “utility” software components which have use in many simulation software packages but are necessary for the functioning of this project. These components were adapted and optimized for use within the MechSys2 project but do not implement any particularly novel algorithms by themselves. • A set of components which are specific to the MechSys2 project. These components implement the novel dynamic simulation technique which is the focus of Chapters 3 and 4.

4.1

Utility Software Components In order to implement a dynamic modeling system, a large amount of numerical

manipulation must be performed. This numerical work involves a great deal of use of 97

several classes of components which are common to many numerical problems and have been extensively developed and debugged for many years: containers, linear algebra packages, and Ordinary Differential Equation (ODE) solvers. Container classes are used to maintain lists of objects. In the sense of this discussion, lists are sets of references to objects. A list allows a part of a program to find all of the components in a simulation of a given type, or to locate one component with a particular property. A list must be able to be changed as a program executes – adding and subtracting list members, for example. Linear algebra packages permit arrays of numbers to be stored conveniently in data structures; using object oriented programming, these data structures are given the ability to perform arithmetic operations on the data in the arrays, such as addition and subtraction, transpose and inverse, and solving systems of equations. The structured approach to software design inherent in a well organized linear algebra package is very important in making the program as a whole easy to understand, design, debug, and modify. As dynamic systems are described in terms of ordinary differential equations, ODE solvers are a necessary part of any simulation software. There are hundreds of algorithms for solving ODE’s; these have been developed for use in many different types of problems. Different solvers are designed to optimize the features which are most important for dealing with a given class of problem. There are solvers which optimize simplicity of code, usage of memory, and speed of execution. Other solvers are designed to solve systems of high order and numerically stiff problems. The MechSys2 software makes use primarily of wellestablished stiff solvers, but the implementation requires some unusual features with respect

98

to the software implementation.

4.1.1

Container Classes The components of a MechSys2 simulation are implemented as objects in C++.

It is necessary to keep lists by which these components can be organized and looked up by the simulation software. For example, a list of nodes (masses, reference points, and so on) is kept by the object which implements the dynamic system; the system object uses the list to initialize nodes for a simulation. The ODE solver uses its own node list in performing the simulation. Each body also contains a list of the nodes inside it; these lists are used to determine the body’s position from the positions of masses, to move non-mass nodes attached to the body, and so forth. There are other lists used to keep track of the connections, surfaces, and forces in a system. Because multiple lists of the same components are kept in the system, it is convenient to store lists of pointers to objects rather than lists containing the objects themselves. This method is used throughout the MechSys2 software. De-allocation of objects is accomplished by using the destructor of exactly one list to delete all of the objects stored in that list. Since the maintenance of databases is one of the most common uses of computers, software which can maintain lists is available in an extremely wide variety of forms. The list maintenance tasks in MechSys2 are extremely simple by most programming standards, and there are many packages which can easily provide the required functionality. The Standard Template Library (STL) is an integral part of the current C++ standard; STL code implements a wide variety of linked lists, generalized arrays, queues, maps, and other 99

container classes. [30] Because the STL is part of the C++ standard, code written for it is portable across compilers which implement the STL as specified in the ANSI standard.

1

The MechSys2 package uses the STL for container class functionality. The two STL classes which are used most commonly in the package are class list class vector. Class list implements a linked list. An iterator can be used to scan through the list, performing operations on each item in the list. Class vector implements a superset of the functionality in list, being able to access and operate on items by array index.

4.1.2

Matrix Utility Classes A large part of the mathematical manipulation used in the MechSys2 project

requires the use of array objects – matrices and vectors. It is useful, then, to use prewritten matrix manipulation software to handle much of the mathematics in this project. In order to be useful to this project, a matrix math package should have a number of features: • Data storage should be on the stack rather than the heap, in order to take advantage of the benefits of C++ discussed in Section 3.3. • Templates should be used to allow the compiler to optimize performance as much as possible. Templates are also useful in programming for data storage on the stack. • Basic matrix and vector functionality should be supported, including matrix and vector arithmetic, transpose, inverse, solutions of systems of linear equations, and so on. 1 Most modern compilers implement the STL, but a great number contain sufficient quirks in their implementations to ensure that porting code is still a challenge.

100

• The syntax for using the components should be as simple and intuitive as possible. There are many utility packages available which provide much of the functionality discussed above. Some packages which were examined during the course of this project include LaPack++, a subset of the famous Fortran library LAPACK which has been rewritten in C++; LaPack++’s successor TNT, the Template Numerical Toolkit; the Standard Template Library, or STL, of C++; and Blitz++, a newer mathematical matrix package. None of these packages meets all the requirements listed above, however. All of the packages which have been found store data on the heap, for example, and create serious problems with heap fragmentation unless great care is taken to write code which does not create objects in the course of routine matrix manipulations. It was therefore decided to create a simple matrix manipulation package which handles the operations required by the MechSys2 project. This was not a particularly difficult endeavor, because the code for simple matrix arithmetic is very simple and code for more complex operations such as solving sets of linear equations is readily available from sources such as Numerical Recipes in C [33]. The vector and matrix utility classes use templates for the sizing and typing of their elements. This means that the sizes of vectors and matries must be known at compile time, a significant limitation. However, a great deal of the math done in the MechSys2 can be done with the array sizes fixed at compile time. One exception is the mathematics of the ODE solver, but the code for the ODE solver is written in such a way that array classes are not used anyway. The payoff for setting array sizes via templates is substantial: Data can be stored on the stack or heap as desired, and an optimizing compiler can perform operations to make the code more efficient, in particular loop optimizations such as unrolling.

101

Vectors The vector classes are based on a template class called CVector. This class template appears as follows: template class CVector

The vector’s size is the number of elements therein, and class T is the data type of the elements. T defaults to double because most mathematical vectors used in this project are of type double; it is concievable that one might wish to use vectors of type float for speed or type long double for precision in some cases. The memory space for the data is allocated as class member data: T data[size];

//

Array holds vector’s components

This means that if a CVector object is created on the stack, its data will be created on the stack as well – thus satisfying the first requirement in the list above. The class contains a set of member functions and overloaded operators which allow vectors to be manipulated in a manner somewhat similar to that in which scalar data types such as double’s are manipulated. The following principal features are supported by CVector: • Vectors are created and destroyed in the same manner as basic data types. • Vectors can be initialized from arrays of variables of their native type, from other vectors (and the results of calculations which produce vectors), and from function parameters.

2

• The function call operator () is overloaded to allow access to individual elements of a vector. Indices for use with this operator begin with 1 in the Fortran and natural 2

Initializing a vector from a set of function parameters is a bit dangerous – it requires C-style variable argument list processing and precludes type checking. This style is generally only used for testing and debugging, and carefully.

102

math style, not zero as in the C++ style. The index operator [] is also overloaded and can be used to access individual items with indices beginning at zero, but its use is not encouraged in most cases. Parentheses are preferred because they allow a common syntax to be used between vectors and matrices. • Vectors are arithmetically combined via the usual arithmetic operators + and - as well as += and -=. • Vectors are assigned with = and can participate in compound expressions just as can scalar types. • Vectors can be acted upon by scalars in multiplication, division, and so on. • Scalar (dot) products are supported; the cross product is supported for pairs of vectors of length three. • Operations such as projections, scaling to unity, and length are supported in member functions.

Matrices The matrix classes are a two-dimensional complement to the vector classes. They are based upon a class template CMatrix: template class CMatrix

These matrices operate in much the same way as CVector objects. Data is stored in a two-dimensional array whose size is fixed during compilation: T data[nrows][ncols];

///< Array of data allocated automatically

103

These matrix classes support basic matrix mathematics in much the same way as do the vector classes. As with the vectors, the overloaded function call operator () is used for most element indexing. This is done because a two-element function call can be used for a two-dimensional matrix: The first element of matrix A is A(1,1). In addition to basic matrix creation, deletion, and arithmetic, a number of other features are supported: • Vectors can be inserted as rows or columns of a matrix or extracted from rows or columns of a matrix. • Transpose, inverse, and LU factorization are supported. • A matrix multiplied by a scalar produces a scaled matrix; a matrix multiplied by another matrix produces a product matrix; a matrix multiplied by a vector produces a vector. The rule that all row and column counts must match up correctly at compile time is enforced by the template mechanism. Although the vector and matrix classes used in this project do not in and of themselves represent a significant advancement in the state of the art, they make a significant contribution to the operation of the MechSys2 project as a whole.

4.1.3

ODE Solvers The Ordinary Differential Equation solver is the program module which drives the

dynamic simulation run. ODE solvers are common program modules, and many dozens of ODE algorithms are in common use. For the MechSys2 project, a set of ODE solvers is required which implements basic differential equation solving functionality – with a few

104

twists. Because of the unusual requirements of this project, standard ODE algorithms are used, but the code which implements them is written specifically for this project. Standard ODE solvers are usually designed to integrate systems of equations for fixed periods of simulated time. The solver controls the integration step size, running until the end of the end of its assigned integration interval. The equation to be integrated is programmed by the user, usually through the user writing a function which implements the equations to be integrated. An ODE solver used in the MechSys2 software should have some additional capabilities: • The solution process will need to be started and stopped many times in the course of a simulation. The solver should be able to stop and start without losing information such as its optimized step size. • The process should be restartable if possible; this is to allow multiple simulations to be executed during a single program run. This feature is optional but desirable. • The variables which store the states and their derivatives need to be assigned automatically when masses are created. • The order of the system must be changeable as masses are added; this takes place after the solver is created and before the initialization of a simulation. • It should be possible to extend the solver so that additional states are created in an arbitrary fashion by the user. Such states can be used to implement continuous-time controllers, for example. 105

There are many available ODE solver codes written in C++. However, none of those meets all the above requirements. In particular, features which could be used to interface the solver to the objects created in MechSys2 are of course absent from standard codes! It was therefore necessary to either write a “wrapper” class which encapsulates the functionality of an ODE solver suite along with the MechSys2 interface, or use standard algorithms to create a custom ODE solver package. If the former option was chosen, it would be difficult to ensure that the ODE solver was well behaved during frequent starting and stopping; experiments have shown that many, such as the solvers in Matlab, are not. Therefore, it was decided to create an ODE solver suite for this project in the form of a C++ class hierarchy which implements standard ODE solving algorithms while meeting this project’s special needs. The structure of the class hierarchy is shown in Figure 4.1.

The base solver

Figure 4.1: ODE Solver Classes

class CBaseSolver provides a framework around which an ODE solver can be built. No objects of this class can be instantiated; its descendents must be used. A key feature of CBaseSolver is that it the data storage for state variables and derivatives is not allocated by the solver object. Instead, lists of pointers to the data are kept, and the storage for the data is instantiated elsewhere. This method is designed to interface conveniently with mass objects in MechSys2; if the user adds other variables to the system state, these can easily 106

be used by the solver as well. The class hierarchy shown in Figure 4.1 includes a set of different ODE solvers. These solvers are structured for use with non-MechSys2 problems. When a solver is to be compiled into a MechSys2 based program, a class called CDynaSolver is used. This class is inserted into the hierarchy just below the CBaseSolver class, above the base classes for fixed and adaptive step size solvers. The presence or absence of the CDynaSolver class in the hierarchy is controlled by a single preprocessor variable. To include CDynaSolver, one must #define DYNASIM within the files solvers.h and solvers.cpp; if this variable is not defined, the CDynaSolver class will be left out. This structure is designed to allow the solver to be thoroughly tested before integration into the simulation package and used for other types of simulations as well. The classes CBaseFixedSolver and CBaseAdaptiveSolver serve as base classes for fixed step size and adaptive step size solvers respectively. It is useful to have both types of solvers available in MechSys2 in order to permit the effectiveness of different types of solvers to be tested. In addition, it is helpful to compare the results computed by both types of solvers when debugging simulations, in order to isolate solver-induced numerical problems from modeling problems and other problems in the simulation. Furthermore, new simulation techniques such as the use of nonlinear “sliding-mode” elements are being researched, and these require different solver algorithms than are optimal for the linear spring based simulations currently being used. The current implementation of the C...Solver hierarchy contains several classes of solvers:

107

• A very simple solver using Euler’s method used for “sanity checks” of new simulation algorithms, CEulerSolver. • Fixed step size Runge-Kutta solvers of second and fourth order, CRK2FixedSolver and CRK4FixedSolver. • A fourth-order Runge-Kutta solver with adaptive step sizing, CRK45AdaptiveSolver. • A fourth-order stiff solver using Rosenbrock’s method, CRosenbrockAdaptiveSolver. The class hierarchy is designed so that new solvers can easily be added. These will usually be implemented as descendents of either CBaseFixedSolver or CBaseAdaptiveSolver. All of the code associated with inclusion or exclusion of class CDynaSolver is taken care of in the base solver classes, so the writer of new solver classes need not be concerned with the details of this interface when writing a new solver class.

Stiff and Nonstiff Solvers Most pseudo-rigid body simulations create stiff sets of differential equations. This means that the fastest modes of the system are very much faster than the slower modes. For example, a pendulum constructed with point masses connected by stiff springs has slow modes associated with the swinging of the pendulum and fast modes associated with the vibration of the stiff springs which hold the body together. The difference in natural frequency between the slow and fast modes is very great; a typical simulation might use masses on the order of one kilogram, a pendulum on the order of one meter long, and springs whose spring constant is on the order of 106 . The resulting swinging mode has a natural frequency on the order of 3 Hz, while the vibrations of the masses on their stiff springs are 108

on the order of 1000 Hz. When simulating the motion of a pseudo-rigid body, we are usually not interested in the motions of the faster modes. These fast modes represent errors in the simulation which, if kept small by making bodies’ internal springs sufficiently stiff, can be ignored. When an explicit solver using methods such as Euler’s or Runge-Kutta methods is used to solve stiff problems, the performance is often unsatisfactory. Explicit solvers are typically not stable when used to solve such problems. Metastable operation for a long enough time to simulate a system for a reasonable duration – such as several times the period of the slower modes – requires that the solver’s step size be substantially smaller than the period of the fastest modes in the system. Because the fastest modes are so much faster than the slower mode, this requires a very large number of steps to perform a simulation. In addition to this problem of excessive computing load, the fact that the explicit solvers are not inherently stable means that eventually the solution must “blow up.” Implicit ODE solvers, or “stiff solvers”, such as Gear’s and Rosenbrock’s methods allow the stability problem to be solved because they are inherently stable regardless of the stiffness of the problem. [33] If the step size of a stiff solver is properly tuned (either through setting the step size by hand or adjusting the tolerance of an adaptive step sizing algorithm), the small vibrations of the masses and stiff springs will not appear in the solution, as the solver’s step size will be larger than the natural frequency of these vibrations. This will allow the simulation to be performed with much higher computational efficiency than would be the case using an explicit solver. The use of stiff solvers has a price, however. Although a stiff solver can produce a

109

stable solution with far less steps than a nonstiff solver, each step takes a great deal more computation for a stiff than a nonstiff solver. The Jacobian of the state derivative vector must be computed. For some problems, the Jacobian can be computed through the use of an analytically derived equation; in general, it must be estimated numerically. The numerical estimate involves adding a small preturbation to each of the state variables, re-computing the state derivative, and computing the rate of change of the state derivative with respect to the state variables. For a system of order N , this estimation process requires about N 2 computations of the state derivative. For a fourth-order Rosenbrock solver, a system of linear equations of order N involving the Jacobian must then be solved four times, adding more to the computational burden. If the Jacobian could be computed directly from information about the system being simulated, the process of computation would become significantly faster. There is some indication that this may in some cases be possible, as the behavior of the system is a function of the geometry and interconnections of its masses and connections, and these parameters are known. The implementation of such a scheme is left for future study; the current implementation of the solver used in MechSys2 estimates the Jacobian numerically. While verification tests were being done in order to ensure the correct functionality of the MechSys2 system, the performance of stiff and nonstiff solvers was compared. The stiff Rosenbrock solver was able to perform simple simulations about two orders of magnitude more quickly than was a nonstiff Runge-Kutta solver, with both solvers being adaptive and of fourth order. The stiff solver has been used for all subsequent work.

110

Solver Performance and Tuning The following discussion assumes the use of a stiff solver. Error control is critical in running simulations in MechSys2, and the error control of the ODE solver is key in controlling the errors. Controlling the errors in fixed step size solvers is performed by adjusting the step size and examining the resulting effect on the errors in the simulation. This method can effective but requires a good deal of expertise and careful decision making by the program operator. Furthermore, fixed step size algorithms cannot adjust their step size to the changing requirements during a simulation. The step size required to accurately simulate a system whose variables of interest are changing quickly (such as a running robot with its feet contacting the ground) is smaller than that required to simulate a system whose variables are changing more slowly (such as a running robot while in the flight phase). Most commonly used ODE solvers use adaptive step sizing to automate the process of setting the step size and to optimize the step size during a simulation. An estimate of the integration error is computed during the course of the integration process and compared to a tolerance which is specified by the user (or perhaps automatically by an automatic error control algorithm elsewhere in the program). This method permits the step size to be varied as the simulated system evolves. When using an adaptive ODE solver, the user’s responsibility in error control becomes that of choosing an appropriate error tolerance. A tolerance value which is too large (a “loose” tolerance) permits errors to accumulate which cause the simulated states to drift away from their correct values. Figure 4.9 shows the effect of using a somewhat loose tolerance for the ODE solver, as the simulation drifts away from the theoretically computed value. When the tolerance was tightened, the solution

111

converged to the theoretical value. In the problem shown in Figure 4.9, this drift is known to be a numerical error rather than a modeling error, as it causes the energy of the system to decrease over time, while the pseudo-rigid model used in that problem contains only masses and springs – no energy dissipative elements. A tolerance which is too small causes the computing time to rise very quickly as the solver attempts to compute the motions associated with the faster modes in the system. The best error control procedure which has been developed so far is as follows: • Choose a spring stiffness which keeps the motions of the masses to a value small enough to satisfy the error requirements for the simulation. • Set the ODE solver’s error tolerance to a value just slightly higher than the amplitude of the vibrations of stiff internal springs and masses. This value can be experimentally determined, as a slightly smaller tolerance causes the computing time to increase very quickly. An analytical estimate of the amplitude of vibration requires a knowledge of the forces on the bodies as well as spring constants and masses. • Make more program runs with larger and, if possible, slightly smaller tolerances to verify that the solution is converging. This procedure assumes the use of a stiff solver. If a nonstiff solver is used, the tolerance must be set just small enough that the simulation remains stable (that is, the states do not blow up outside of acceptable values) for the duration of the intended simulation. Nonstiff solvers are currently used for testing and verification of the functionality of other MechSys2 software.

112

The error control techniques just described are effective but obviously quite crude. Improved error control algorithms are an important area for further research. Several ideas have been considered for investigation: • Computing the amplitudes of vibrations in springs and using this number as a measurement of error in the system. • Using a method of error estimation based on energy. Pseudo-rigid bodies are used to simulate truly rigid bodies, and any energy present in the springs represents an error. An unanswered question is, to what can this internal energy be compared? • Performing the previously described calculations continuously and adjusting spring constants and ODE solver tolerances as a simulation runs.

4.2

Model Components The structure of MechSys2 includes a set of entities which are implemented as C++

classes. The class structure is designed to reflect the organization of the dynamic modeling system as a whole. A set of utility classes comprise the foundation of the numerical modeling system; these include vector and matrix classes and an ODE solver suite. Classes are also created which handle angular various schemes of angular measurement. The components of a dynamic model are built up in a class hierarchy which includes classes to implement masses, springs, dampers, and the other basic elements of the simulation; other classes group these basic elements into structures such as pseudo-rigid bodies and surfaces.

113

4.2.1

Nodes Rigid bodies are constructed from nodes and connections. Several types of nodes

are used in the construction of bodies; these nodes include various types of masses as well as massless nodes which are used to transfer forces between bodies.

Masses Masses are of course the basis of dynamic behavior. A mass in MechSys2 is a point mass which obeys Newton’s equations of motion. It has six degrees of freedom, three positions and three momentums. Any properties which involve the finite size of a real massive object – for example rotational inertia – are modeled by using sets of point masses connected by springs. In terms of the software, a mass object holds its state variables, one for each degree of freedom, as well as information about the connections (see Section 4.2.3) to which the mass is attached. In the course of a simulation, forces are applied to the mass; the mass adds up the forces acting on it at any given time, computing a net force which is used by the ODE solver to compute the change in the mass’s momentum.

Regular Masses A “regular” mass is designed to be used as a component in a body. It is distinguished by its mass being a substantial part of the body’s mass, usually one fourth. This is in contrast to dummy masses, discussed in Section 4.2.1, whose mass is very small when compared to the mass of a body. Regular masses can also be used in the place of bodies in some problems. This is 114

useful for testing and verification of MechSys2 modeling components, as the equations for the dynamic behavior of a system made of only point masses are much simpler than those which represent a system made of rigid bodies. An example of this use is the simulation of a pendulum. A very simple system can be constructed from a point mass connected to a stiff spring whose other end is connected to a grounded node. This forms the idealized point-mass pendulum which is familiar from many elementary dynamics texts. The results of MechSys2 simulations can be easily and quickly computed and compared to analytical solutions of the idealized problem.

Dummy Masses A dummy mass is similar to a regular mass, except that it is much lighter. Dummy masses can be used as the points of application of the external forces which are applied to a body and as points of attachment between bodies. As masses, dummy masses participate contribute state variables to the dynamic system and have the same six degrees of freedom as regular masses. Section 4.3.2 discusses the way in which dummy masses are used to transfer external forces to a body.

4.2.2

Reference Points Reference points are used to keep track of features attached to a body. These

features include the center of mass and the locations at which constraining surfaces are attached to the body. A reference point has no mass and is not attached to a body by connections such as springs. Instead, the reference point is moved with the body after each step of the ODE solver. The motion is accomplished by first translating the point by a 115

vector equal to the motion of the body’s center of mass, and second rotating the point about the body’s mass center with the body’s rotation. The rotational attitude of the body is present in the body’s rotation matrix, which is determined by the methods described in Section 3.4.4. A reference point cannot be used to transfer forces to a body. Reference points do not have mass properties and are not contributors to the simulated dynamic system, so if a force is applied to a reference point, it does not respond in any way. For this same reason, it would not be correct to use reference points connected by a spring to attach two bodies together: the bodies would move as if the connection were not even present because the forces in the connection would not be felt by the reference points.

Ghost Points Ghost points are massless nodes which can apply forces to a body. As massless nodes, ghost points are moved in the same manner as are reference points: when a body moves, each of its ghost points is translated by an appropriate amount to preserve the orientation of the ghost point with respect to the body’s main masses. In this manner, ghost points are similar to reference points. The difference appears when a force is applied to a ghost point. Whereas applying a force to a reference point has no effect (and makes no sense in terms of a simulation), applying a force to a ghost point causes the force to be applied to a body. The technique by which ghost points are used to apply forces to bodies is discussed in detail in Section 4.3.3.

116

4.2.3

Connections In the MechSys2 project, connections are entities which can transfer forces be-

tween nodes. There are two distinct uses for connections: Internal connections: These connect the masses of a body, maintaining the masses in the correct relative position. They may take the form of stiff springs, stiff dampers, or some other combination of elements. The goal is to simulate the internal structure of a physical rigid body to the extent that there is some physical entity inside which tends to preserve the body’s shape. External connections: These connections affect the interactions between bodies. As connections, they are still required to act between pairs of nodes. External connections may be made stiff to represent kinematic constraints or nonstiff to represent items such as physical springs and dampers. It is possible to create a compound connection by attaching two or more connections between the same pair of nodes. In addition to modeling a McPherson Strut, this technique allows the use of, for example, a stiff spring-damper combination to stabilize the numerics of a simulated kinematic connection. It is also possible to create custom connections. These can produce any behavior which involves the transfer of forces to nodes based on the states of the nodes. For example, a connection could be created which implements the nonlinear and sometimes complex behavior of friction between two surfaces in contact.

117

Zero Length Connections There is no particular reason why abstract connections in MechSys2 cannot have zero length. This is in contrast to many physical connecting devices, of course, which must have some finite length. Zero rest length springs are used to attach a point on one body to a point on another, creating a pivot. They are also used in contact problems to represent the repulsive forces between contacting surfaces. A problem exists, however, with connections of zero length in that when the length is actually zero, there is no way to compute the direction in which the connection is oriented and thus no way to compute the force vector applied to each node. For a spring of zero rest length, this is not a serious problem because when the spring’s actual length is equal to its rest length of zero, the force in the spring is zero; so there is no need to compute the force vector anyway. However, for some other connections such as a linear damper (i.e. a dashpot element), there may be a nonzero force at zero length. The software is configured to always return a zero force when a connection’s length is within a small tolerance of zero; because a connection which is transferring a significant force should spend very little time with its length close to zero as the connected nodes move with some reasonably finite speed, this should cause only a small glitch in the computed output. However, it can be considered good modeling technique for the user to prevent such an occurrence entirely by designing the structure of the model so that conditions such as this do not occur.

118

Springs The basic spring class models ordinary linear springs which obey Hooke’s Law. Physical characteristics such as closed coils, inertia, and failure are, of course, not modeled. Springs can have zero length. The basic spring is a simple linear spring which obeys Hooke’s Law: F = −K(x − x0 )

(4.1)

producing the force F when the spring whose rest length is x0 is stretched to length x. Although useful on its own, class CSpring is intended also to be used as a base class. Various types of nonlinear springs, for example, can be easily implemented as descendents of CSpring.

Dampers Dampers represent the traditional dashpots or shock absorbers in machines. The base damper class is CLinearDamper, an element which behaves rather like the shock absorber in a vehicle. Forces are transmitted in a direction parallel to the orientation of the damping element. The magnitude of the force is computed by the simple linear damping relation: F = −bv

(4.2)

where v is the rate of change of the length of the damper. If the damper has zero length, its direction cannot be computed and forces are not applied; this behavior is discussed in the preceding section on zero length connections. The computation of the rate of change of a damper’s length involves projecting

119

the relative velocity of one end with respect to the other onto the position vector from one end to the other: // Find the velocity one node with respect to the other node; project that // velocity onto the nodes’ relative position CVelocity relVel = pnode1->getVelocity () - pnode2->getVelocity (); CPosition relPos = pnode1->getWorldPosition () - pnode2->getWorldPosition (); double dist = relPos.length ();

For a linear damper, the computation of the force is simple: // Force is -(damping coefficient) * (relative velocity) force = (dotProduct (relPos, relVel) / dist) * dampCoeff;

Friction Elements Dashpots such as those modeled by CLinearDamper are not as common in mechanical systems as are elements which cause friction. Several types of friction are often modeled as a phenomenon which depends on the relative velocity between two points, irrespective of the points’ relative position. With this idea in mind, the class CLinearFriction is created as base class for elements which implement frictional damping. The equation which relates the magnitude of the force to velocity in CLinearFriction is the same as for linear dampers: F = −bv. However, the velocity being used is the relative velocity between the nodes to which the damper is connected, and the direction of the force is parallel to and opposing the direction of relative motion rather than parallel to the relative positions of the end nodes. Friction elements of this type provide a convenient mechanism for modeling “windage” as well. If one end of a friction element is attached to a grounded node, the element provides a force opposing motion in the world reference frame. One could model the effect of wind on a moving or static structure by attaching one end of a friction element to a node which is forced to move at the wind speed. Distributed wind loads could be approximately 120

modeled by using sets of friction elements. Of course, aerodynamic friction is not linear, instead obeying nonlinear laws such as the familiar F = 21 Cd ρv 2 , Cd being a constant drag coefficient, ρ being the density of the fluid, and v being the velocity of the body through the fluid. Therefore, an appropriate quadratic friction element would have to be used.

4.2.4

Bodies The primary organizational entity in a MechSys2 simulation is the pseudo-rigid

body. This entity consists of a set of masses, connected by springs and dampers, attached to other entities by generalized constraint surfaces. It can simulate any rigid body whose moments and products of inertia about given axes are known. The body is not a fundamental part of the underlying simulation in that the equations of motion involve masses, connections, and constraints but not bodies. Bodies are instead a mechanism used to group masses, connections, and constraints together; bodies provide a means to relate the motions of groups of masses to positions and angles which are used by the user to visualize the motions of a system. The principles by which MechSys2 bodies are constructed are as follows: • Any physically realizeable rigid body can be modeled in terms of its inertia tensor I. • The inertial properties represented by any valid intertia tensor can be duplicated by the appropriate spatial distribution of a set of four point masses of equal mass. The first principle is well known from traditional rigid body dynamics. Discussions of the second have not been found in the literature. In order to establish the second principle, a system was designed to distribute a set of equal point masses in a way which duplicates 121

an arbitrary set of principal moments of inertia (Ixx , Iyy , Izz ). The method was developed with the assistance of L¨ uftner, Kr¨opl, and Laus [18]. The fact that this method can create bodies with any valid set of moments of inertia is taken as proof that the second principle is correct. The moments of inertia about the center of mass of a body consisting of a set of n point masses, each with mass mi , attached rigidly together are given by the following: Ixx = Iyy = Izz =

n X i=1 n X i=1 n X

(yi2 + zi2 )mi (zi2 + x2i )mi

(4.3)

(x2i + yi2 )mi

i=1

If the body’s principal axes of inertia are not aligned with the coordinate axes, the body will also have nonzero products of inertia found as follows: Ixy = Iyz = Izx =

n X i=1 n X i=1 n X

(x y)mi (y z)mi

(4.4)

(z x)mi

i=1

A body will be constructed from point masses in such a way that these moments of inertia are as specified by the user.

Constructing Bodies A scheme is designed so that the masses can be arranged in a predesigned pattern so that the moments of inertia come out correctly and the center of mass of the body is in

122

a desired location. The pattern is arranged so that three variables u, v, and w can be used to control the body’s principal moments of inertia; four equal masses are always used. This pattern is shown in Figure 4.2. With this arrangement, the moments of inertia due to the

Figure 4.2: Arrangement of Masses in Body

given masses can be computed by Equation 4.3. The resulting equations are as follows: µ

¶

4 Ixx = mi 2v 2 + w2 3 µ ¶ 4 2 3 2 Iyy = mi w + u 3 2 µ ¶ 3 Izz = mi 2v 2 + u2 2

(4.5)

These equations can be solved analytically. The result is as follows: q

u =

2 −3m(Ixx − Iyy − Izz 3m m(Ixx − Iyy + Izz

q

v =

m

(4.6) 123

q

w =

6m(Ixx + Iyy − Izz ) 2m

These equations place some restrictions on the moments of inertia which can be simulated by point masses. The following equations must be satisfied: Ixx ≤ Iyy + Izz Iyy ≤ Ixx + Izz

(4.7)

Izz ≤ Ixx + Iyy Bodies which do not satisfy these equations cannot be simulated by arranging four point masses; however, such bodies also cannot be physically realized. If one inertia were greater than the sum of the other two, there would have to be a portion of mass in the body distributed in such a way as to create a moment about one axis only; however, this is not possible. If a moment is created about one axis, a corresponding moment must be distributed among the other two axes as well.

“General” Bodies Equations 4.5 are implemented in the constructor of class CGeneralBody. The general body class is designed to be able to implement any type of body whose moments of inertia can be determined. There are two constructors used to create general body objects. The first creates a body given a set of principal moments of inertia Ixx , Iyy , and Izz . The body’s princpal axes of inertia will then be aligned with the world coordinate axes. The prototype for this form of constructor is CGeneralBody (CDynaSystem& pSys, CPosition pos, double mass, CVector prMoms, char* anm = "");

124

The first two parameters are a pointer to the dynamic system in which the body will be used and the position in global coordinates at which the body’s center of mass will be placed. The third parameter is the mass of the body. The fourth parameter is a three-element vector which contains the princpal moments of inertia about the x, y, and z axes in order. If the principal moments of inertia do not satisfy the restrictions in Equations 4.7, code in the CGeneralBody constructor will detect the error and halt the program. The last parameter is an optional name for the body; this name appears on configuration printouts but has no other function. The second general body constructor is used when an inertia tensor is known, but the principal axes of inertia are not necessarily aligned with the global coordinate axes. In this case, the construction of the body is performed in several stages. First, the principal moments of inertia are computed. Then the body is constructed with its principal axes of inertia aligned with the world coordinate axes – in just the same way as in the previously discussed version of the constructor. Next, the orientation of the principal axes of inertia with respect to the world coordinate system is found; then the body is rotated into the correct orientation so that the moments and products of inertia are correct. The procedure for finding the principal moments and axes of inertia is straightforward, with the eigenvalues of the inertia tensor being the principal moments of inertia and the eigenvectors being used to form a rotation matrix by which the body is rotated from a position aligned with the coordinate axes to its correct initial position for the simulation. The general inertia tensor constructor for class CGeneralBody has the following form: CGeneralBody (CDynaSystem& pSys, CPosition pos, double mass, CMatrix iTens, char* anm = "");

The arguments are the same as for the principal moment constructor with the exception of the fourth parameter. This parameter is now a three-by-three matrix which holds the

125

inertia tensor. The inertia tensor is of the form 

  Ixx    I =  Iyx   

Izx

Ixy Ixz  Iyy Izy

   Iyz    

(4.8)

Izz

where the opposing products of inertia should be equal; that is, Ixy = Iyx , Iyz = Izy , and Ixz = Izx . Two means are therefore available by which the user can create a body whose principal axes of inertia are not aligned with the world coordinate system. The body can be created by the principal-moments constructor and rotated manually to the desired orientation using the method rotateInitialPosition(); or it can be created by the general-inertiatensor constructor, the user specifying a tensor of inertia which represents the body in its correct starting position. The first method has the advantage that various methods can be used to specify the body’s initial orientation; for example, the orientation can be specified in terms of Euler angles which are converted by the methods described in Section 3.4.5 to a rotation matrix which is then used to rotate the body. The correct operation of the body construction algorithms implemented in class CGeneralBody was verified by extensive numerical testing using Maple as well as the theoretical examination. Approximately 104 random tensors of inertia were created from within the space of valid values. The CGeneralBody algorithm was used to create bodies which duplicated the desired tensors of inertia. Then the inertia tensor of the constructed body, computed by Equations 4.3 and 4.4, was compared to that originally desired. The results agreed to within one part in 106 of the largest component of the inertia tensor.

126

Specific Body Classes The C...Body class hierarchy is designed so that any number of “custom” body classes can be created for the convenience of the programmer. For example, if spherical bodies are commonly used in a project, a class CSphericalBody could be easily derived from the CGeneralBody class. The mass properties of a spherical body could be specified in terms of one total mass and one moment of inertia; for a sphere, the moment of inertia is the same regardless of the axis about which the body rotates. If desired, the spherical body class could be further specialized to represent particular types of spherical objects, such as round balls of uniform density or spherical shells. The constructor of such a sphere would then be given the geometric properties of the sphere – diameter and if applicable, wall thickness – and the density of the material from which the sphere was made. The constructor would then compute the spherical body’s mass and rotational inertia from the geometry and density.

Degenerate Bodies There are many problems for which it may be convenient to use body representations which contain less than four masses. For example, a long and slender rod can be approximated in many problems with just two masses; this will produce a body whose moment of inertia about the axis of symmetry is zero. For some problems, such as the simulation of a pendulum made from a rod supported by a pivot, this representation is adequate because rotation about the axis is not a concern. Similarly, a disk whose thickness is much less than its radius might be modeled using three masses instead of four. Two-mass

127

and three-mass “degenerate” bodies have an advantage over their standard four-mass counterparts in that there are less nodes and connections in the body, so order of the dynamic system being solved by the ODE solver is smaller. The C...Body class hierarchy does not directly support such degenerate bodies. The reason is that there is a large set of functionality supported for bodies which requires all four masses in order to work. For example, computing the attitude of a body (in terms of a rotation matrix, a set of Euler angles, or any equivalent representation) requires that the body’s position in space be uniquely defined. If the body is a two-mass representation of a slender rod, the angle at which the body is rotated about its axis of symmetry cannot be determined – and in fact has no physical significance. There is a philosophical issue present as well. The two or three mass body is an implementation of an idealized approximation of a body with certain properties. For example, real rods are never infinitely slender; the two-mass body is an approximation of what such a body would be like in a limiting ideal case. The real body can only be very slender, and a four-mass approximation of the very slender rod is a better representation of physical reality than the two-mass one. The reasons for creating a two-mass body are purely practical reasons related to optimizing computing time at the expense of a more precise representation. Although the body class hierarchy does not support degenerate bodies, there is no reason why they cannot be constructed ad hoc by the user, nor why a separate class hierarchy could not be created to support such bodies. Furthermore, body-like components can be readily used in simulations; for example, a simple pendulum can easily be constructed

128

using a single mass point as a bob, a dummy or ghost point as a pivot, and a stiff spring to connect them. A means to determine the angle at which the pendulum swings must be written separately because the algorithm used in the Euler angle class cannot be applied to a two-node body in which rotation about the axis of symmetry is undefined. For the purposes of conducting the research in this project, the computational advantages of using degenerate bodies is slight, and their use is considered not to be a valuable use of time. Some degenerate parts are created and used for testing the MechSys2 software and for simulation of certain dynamic components; these are implemented in an ad hoc fashion.

The “Ground” Body In the real world, there is no actual grounded frame of reference. When an experiment is conducted in the laboratory, the surface of Planet Earth is used as the reference frame (although here in California, this is not always a dependable inertial frame). This stable reference is guaranteed by the large mass of the Earth with respect to everything yet built upon it. Such an arrangement suggests that in order to create grounded objects, we chould create a grounded body, one whose mass greatly exceeds that of all other masses in the system. Points attached to the ground body can then be relied upon to stay put. The use of a grounded heavy body imposes a significant computational burden. First, it increases the order of the dynamic system being modeled. Second, it increases the stiffness of the ODE system – and the system is already quite stiff due to the presence of stiff springs and dampers. This seems a large load to ensure that points attached to the grounded body don’t move. 129

Therefore, functionality has been included in MechSys2 to support attaching points and surfaces to no body. When a surface is created, for example, it can be given a NULL parameter for the pointer to the body to which it will be attached. This creates a surface which will never move: surfaces which are attached to bodies only move when the bodies move, and a surface not attached to a body will not be caused to move by any entity. Because surfaces themselves do not add states to the dynamic system, the grounded surface is an efficent way to implement a fixed constraint. Similarly, a class called CGroundConstraint is provided which implements a node that never moves. This type of node can be conveniently used as a fixed pivot point, for example. It should be noted that the use of CGroundConstraint is considered obsolete for most problems, as it has been replaced by the use of a point surface which is attached to no body. The use of a point surface is more consistent with the body/surface paradigm used in MechSys2.

4.2.5

Constraint Surfaces A key element of the MechSys2 project is the creation of a dynamic modeling

scheme which can solve problems which are very difficult to represent using conventional dynamic modeling techniques – and do so through the use of conceptually simple components. Mechanical constraints are a very important part of most mechanical systems, and they are often very difficult to analyze using conventional techniques. Mechanical constraints in MechSys2 are modeled through the use of surface objects. Surface objects represent a generalized class of surfaces in that they can take the form of zero-dimensional points or one-dimensional lines as well as two-dimensional curves in space. Each surface is attached to one body; a body may have many surfaces attached 130

to it. Each surface is attached to one other surface to produce one constraint. In keeping with the node-connection structure of MechSys2, a surface is implemented by defining a region within which an attachment node is allowed to move. The node may move freely to any point within the region. An external connection such as a stiff spring connects the attachment node to the attachment node of some other surface which is attached to some other body. As the body which owns the surface moves, the connection point is forced to remain outside the region defined by the surface. Surfaces are implemented in a class hierarchy rooted at class CBaseSurface. As with other sets of classes, this hierarchy is designed to be easily extensible by the user, so custom surfaces can be readily created for use in particular problems. Each type of surface includes a definition of “inside” and “outside.” In the case of points and lines being used as surfaces, the volume of the inside is zero; for a plane which bisects the universe, the volume is infinite; a spherical surface has finite volume. In Figure 4.3, stiff springs which maintain rigidity are shown as solid lines, while the relationship by which the ghost point that attaches the body to the surface transfers forces to main body points is shown as a set of dashed lines. The surface’s connection point always remains within the plane surface.

4.3

Applying Forces to a Body In general, forces which are applied to a body will not be conveniently directed

to act upon one of the main mass points in that body. The software must be able to accomodate forces which act upon the body and whose point of application is anywhere in

131

Figure 4.3: Body Connected to Plane Constraint

the space in which the body resides. As the body moves, the point of application of the force moves within the global reference frame. Therefore, each force must have associated with it a method to transfer that force to the point masses which implement the inertia of the body as well as a method to keep track of the position of the point of application. Several methods can be used to transfer a force to the body’s inertial masses. The methods which have been considered in this work are listed below: • Regular Masses: In some cases, a regular mass can be created at the point of application of the force and connected to the main masses of the body with internal connections. The force then acts directly upon the newly created mass. • Dummy Masses: Instead of a regular mass, a dummy mass can be created at the point of application of the force and connected to the body’s main masses. A dummy 132

mass has a much smaller mass than each of the body’s main masses and therefore has a negligible effect on the body’s inertial properties. The force is then applied to the dummy mass. • Ghost Points: A massless node is created at the point of application of the force. This node is not attached to the body’s main masses by connections; instead, it transfers forces applied upon it by calculating a set of forces which, when applied to the main masses of the body, add up to the force and torque applied to the ghost point.

4.3.1

Regular Masses for Force Transfer The most conceptually simple and direct method of applying forces to a body is to

rearrange the main masses of the body in such a way that there is a point mass conveniently located at the position of application of the force. Because a body may have any number of forces applied to it, this method requires that a new point be created at each force’s point of application. Unfortunately, this method has several distinct disadvantages. First, it increases the order of the system which must be solved by the ODE solver, as masses are added to the system. Because time taken by many ODE solvers increases with the system order to the third power, this can lead to excessively slow computation times, particularly for larger systems. The algorithm required to compute the mass of the point mass to be created becomes quite complex: In order to keep the stiffness of the system of differential equations as low as possible, a mass which is close to that of the body’s main masses is desired.

133

However, if the force’s point of application is located far from the body’s center of mass, the mass at the point of application will have to be fairly small, lest it contribute too much rotational inertia to the body, making the problem of duplicating the correct rotational inertia for the body intractable. Worse, every time a new force is added to the body, all of the points in the body will have to be rearranged to preserve the body’s inertial properties in the presence of the new mass. Although this problem is almost certainly solvable, it consumes an unreasonable amount of resources in the presence of even a modest number of forces upon the body.

4.3.2

Dummy Masses for Force Transfer The idea of a “dummy” mass was conceived to allow the transfer of forces to a

body without having to change the properties of the body’s main masses. Of the methods available to apply forces to a body, the dummy mass is the easiest to implement. Not surprisingly, it is also the least computationally efficient method. A dummy mass is a regular mass in that it has finite mass, is attached to the body’s main masses by the same connections (springs and/or dampers) used to connect the main masses to each other, and contributes states to the dynamic system. The configuration of a dummy mass attached to a body is shown pictorially in Figure 4.4. The mass of a dummy mass is much less than that of a main mass. In the same way that stiff internal springs are “pseudo-rigid”, dummy masses are “pseudo-massless.” Dummy masses are implemented in class CDummyMass in files nodes.h and nodes.cpp. They are functionally equivalent to regular masses, but with less mass. The value which should be chosen for a dummy mass’s mass is the largest value which will not cause unacceptable errors in the computed solution. 134

Figure 4.4: Force Transfer by Dummy Mass

Errors occur primarily because the main masses of a body are not adjusted to account for the presence of dummy masses, and therefore the inertial properties of a body are changed slightly by the dummy masses. If the dummy masses are small enough, however, the change in the body’s inertial properties will be negligible. Because dummy masses contribute to the order of the dynamic system, the use of dummy masses shares the disadvantage of numerical inefficiency with the use of regular masses. In fact, dummy masses can be even worse: because their mass is small and they are connected to the body’s main masses by stiff springs and/or dampers, dummy masses make the set of differential equations in the dynamic system stiffer than it would be with only regular masses. Furthermore, when one attempts to measure the velocity of a dummy mass, the result can have large errors because of the vibrations of the dummy mass and its

135

connecting springs: Although the momentum of the dummy mass isn’t necessarily higher than that of normal masses, its velocity can be much higher because its mass is much lower. There are many problems for which the use of dummy masses or full-sized masses is not a reasonable option. When the point of application of a force moves with respect to the body (rather than moving with the body), the mass at the point of application must be moved with respect to the body’s main masses. This requires a complex procedure which moves this mass in a manner which is separate from the motion of the dynamic system – so as not to affect the momentum of the body. The mass must first be moved, and then the springs which attach the mass to the body’s main masses must be adjusted. The rest lengths of the springs, spring constants, and stiffnesses of dampers must be changed so that the state derivatives of the mass at the point of application are unchanged by the movement. In general, this is a very difficult problem to solve; the geometry is nonlinear, and connections may have nonlinear behavior; an iterative solution would almost certainly be required. This solution would consume a great deal of computing time. To make matters worse, many ODE solvers are not designed to accomodate an external routine changing the values of their state variables. Therefore, it is necessary to either modify the ODE solver as well as implement a complex iterative connection adjustment algorithm, or find another way to apply forces to bodies.

4.3.3

Ghost Points for Force Transfer Instead of transferring forces by creating new mass objects, we can transfer forces

by applying them to mass objects which already exist. In particular, we can compute a set of forces which has the same effect on the body’s main masses as would the original force 136

being applied. This computation can be performed in a number of ways, and its solution is not unique. When masses – real or dummy – are used to transfer forces to a body, the ODE solver moves these masses so as to maintain their orientation with respect to the body’s main masses. But when force transfer is performed without using a point mass at the point of application, some other means must be used to keep track of the position of the point of application as the body moves. A reference node (Section 4.2.2) is a logical choice, as reference nodes are designed to keep track of a position on a body and are automatically moved as a body’s position changes. An object which combines the characteristics of a reference node with the ability to redistribute forces has been created and is referred to as a “ghost point.” The use of ghost points to apply forces to bodies has several advantages over the use of real or dummy masses. Because ghost points have no mass, they contribute no states to the dynamic system. This gives ghost points the potential to improve the computational efficiency for larger problems. The time required to progress through one step of the dynamic system is governed at least in part by the time required to invert the Jacobian matrix (or perform a similar operation) during a step of a stiff ODE solver.

3

The computing time

required to invert the Jacobian matrix increases with the third power of system order. A similar prediction cannot be made for the time required to evaluate the system of ordinary differential equations, as the nature of all the equations is not known until the system has been constructed, but the time required for this evaluation usually grows with system order 3 This discussion assumes the use of a stiff ODE solver because the use of stiff springs naturally produces a system with stiff numerics and because repeated testing, as discussed in section 4.1.3, has shown that the performance of stiff solvers is far superior to that of nonstiff solvers.

137

at a power higher than one. For example, a non-sparse linear system grows approximately quadratically. Because each ghost point operates in the same way and does not interact with the rest of the dynamic system through the system equation, the time required to perform all the ghost points’ force redistributions in the dynamic system increases linearly with the number of ghost points in the system. For larger systems, this is a marked improvement over the quadratic or cubic increase in computation time experienced when real or dummy masses are used to transfer forces. Ghost points also have the potential of contributing less error to the system than real or dummy masses. This is primarily due to the fact that the equations which distribute forces from ghost points to a body’s main masses are, in theory at least, exact – instead of carrying the implicit approximation of a spring and damper system. Of course, numerical inaccuracies can degrade the accuracy of any solution, especially when the system is illconditioned. Poor conditioning of the ghost point’s equations will result from the geometry of the ghost point’s orientation with respect to the main masses; this effect will be discussed below.

Algorithms The forces acting on each mass are computed before each evaluation of the ODE system. Forces which are applied directly to masses – for example, gravity forces – are simply added to the sum of forces on these masses, along with forces transmitted through connections to constraints and other masses. When a force is applied to a ghost point, it must be immediately transferred to the main masses of the body with which the ghost point is associated. The “transfer” is accomplished by computing a set of forces on the 138

body’s main masses which produce the same net force on the body and net torque about the body’s center of mass as the force applied to the ghost point. In order to transfer a given force on the ghost point to the body’s four main masses, we must compute twelve force components – three on each of the masses. These components must satisfy six equations describing the net force and torque on the body. The equations are as follows: n X

fi = F

(4.9)

i=1 n X

(ri × fi ) = M

(4.10)

i=1

Here, F is the force applied to the ghost point; M is the moment about the body’s center of mass due to force F ; and each fi is the force acting on each main mass in the body. Because there isn’t one unique solution to this problem, a method must be designed to find a usable solution. Two methods have been studied.

Least Squares This is a “brute force” method for applying forces to the main body masses. The force Fg applied to the ghost point is equivalent to a force Fg and moment Mg = r × Fg applied at the center of mass of the body. A set of forces f1 through f4 is applied to the four main masses of the body so that the net force and moment on the body due to the fi ’s are equal to Fg and Mg . The derivation of an algorithm is straightforward. The force fi applied to each main body mass is represented by its components fxi , fyi , and fzi . We then have a set of twelve force components. There are six equations constraining these components, as the net force and moment acting on the body about its center of mass must be equal to Fg and 139

Mg . The resulting system of equations can be expressed in matrix form as Ax = b with coefficient and constant matrices as follows: 

  1     0      0  A=    0      r13   

−r12

0

0

1

0

0

1

0

0

1

0

1

0

0

1

0

0

1

0

0

1

0

1

0

0

1

0

0

1

0

0

−r13

r12

0

−r23

r22

0

−r33

r32

0

−r43

0

−r11

r23

0

−r21

r33

0

−r31

r43

0

r11

0

−r22

r21

0

−r32

r31

0

−r42

r41

#T

"

b=

Fgx Fgy Fgz Mgx Mgy Mgz

0

    0      1     r42      −r41    

0

(4.11) (4.12)

A commonly used algorithm exists for the solution of underconstrained sets of equations Ax = b. This algorithm takes the form of a simple linear algebraic equation: x = AT (AAT )−1 b

(4.13)

This equation is simple to apply and easy to code into a program. However, as it involves the inversion of a matrix, its computational requirement is fairly large. Fortunately, the size of the equation is constant for all ghost point problems in MechSys2; when a ghost point transfers forces to four masses, there are six equations in twelve unknowns, resulting in the construction and inversion of a six-by-six matrix for each ghost point solution. As the number of ghost points in the system grows, the computational time required to handle force reallocation by ghost points increases linearly. Some improvement in the speed of solution can be gained by improving the efficiency of the numerical process which solves equation 4.13. One method would be to 140

solve the equation AAT analytically, allowing its nonzero components to be computed and plugged into the array instead of computing AAT using matrix multiplication. This would save some multiplications by zero and some additions, but the computation of the nonzero elements of AAT still requires a large number of operations because the elements are complicated functions of the elements of A. Also, the transpose of matrix A must still be created and multiplied by (AAT )−1 b. Another method for improving the computational efficiency could be to use Gaussian elimination to compute (AAT )−1 b in one operation, given AAT and b, instead of first inverting AAT and then multiplying by b. This method, suggested in [33], would reduce the computational effort by the number of operations needed for the multiplication by b. It was decided that pursuing these avenues of speeding up the computation should be left for future work because they would create only minor improvements in the overall performance of the software while requiring substantial effort to implement.

Point Pairs In order to create a faster method to transfer forces from a ghost point to a body, a method was devised which attempts to find a computationally efficient solution to the problem by the addition of cleverly designed constraints. First, the force on the body due to that on the ghost point is separated into a linear force on the center of mass and a moment about the center of mass. The linear force is transferred by dividing it by the number of masses and applying the appropriate fraction to each mass. The moment is transferred by applying pairs of equal and opposing forces to pairs of two masses. The pairs of masses are chosen so that the minimum force will provide maximum moment; this means that pairs of forces are applied to pairs of masses which are most widely separated with respect to a given axis. The force pairs are further constrained by restricting them to act

141

in the directions of the world coordinate axes; this constraint is applied for computational convenience. In acting on a pair of masses, each force pair will in general create a torque about the two axes other than the axis parallel to the directions of the forces in the pair. The configuration is shown in Figure 4.5.

Figure 4.5: Force Transfer by Force Pairs

Let rx , ry , and rz designate the three vectors from the first mass to the second in each pair of masses to which a pair of forces will be applied. The forces on the masses at the heads of the position vectors are fx , fy , and fz , with opposing forces being applied to the masses at the tails of the position vectors. The torque contributed by each force pair is r × f . The torques contributed by the three force pairs are then broken down into their components, which must add up to the components of the desired torque τ = (τx , τy , τz ). τx = (rz )y fz − (ry )z fy 142

τy = (rx )z fx − (rz )x fz

(4.14)

τz = (ry )x fy − (rx )y fx As a three-by-three system of equations, this system can be solved much more quickly than the six-by-twelve system used in the least-squares method. It can even be solved analytically and the result coded into C++ with the simple code below. CPosition xSep = wideX[0]->getWorldPosition () - wideX[1]->getWorldPosition (); CPosition ySep = wideY[0]->getWorldPosition () - wideY[1]->getWorldPosition (); CPosition zSep = wideZ[0]->getWorldPosition () - wideZ[1]->getWorldPosition (); // Compute the forces in the X, Y, and Z directions now double denom = zSep(2) * ySep(1) * xSep(3) - ySep(3) * xSep(2) * zSep(1); CForce Fx ((Tx * ySep(1) * zSep(1) + zSep(2) * ySep(1) * Ty + ySep(3) * Tz * zSep(1)) / denom, 0.0, 0.0); CForce Fy (0.0, (Tz * zSep(2) * xSep(3) + xSep(2) * Tx * zSep(1) + xSep(2) * zSep(2) * Ty) / denom, 0.0); CForce Fz (0.0, 0.0, (Ty * ySep(3) * xSep(2) + xSep(3) * Tx * ySep(1) + xSep(3) * ySep(3) * Tz) / denom);

However, this computational efficiency comes at a price. There are some numerical situations which appear to cause the denominator term in the code to approach zero; a zero denominator indicates that the system cannot be solved, and a denominator close to zero is the result of an ill-conditioned system which may lead to large computational errors. This appears to occur even for some well-formed geometric systems which have reasonable geometry and for which some solution should be feasible; apparently, the choice of point pairs made by the algorithm described above leads to a poorly conditioned third-order system. This is thought of as an error in either the algorithm or its implementation. Because of time constraints, the problem has not been tracked down in the course of this work.

143

4.4

Example Problems In this section, the MechSys2 package is used to solve a fairly simple dynamic

problem. In this example, the solutions computed through the use of MechSys2 are compared to solutions obtained by traditional means in order to verify the proper functioning of the software package.

4.4.1

Free Motion of a Satellite In this section we present an example to illustrate different methods of solving

a simple problem in three-dimensional dynamics. The problem concerns the motion of a cylindrical body in space. In order to create a simple and easily understandable problem, an axisymmetric body is used. This shape might approximate that of a typical communications satellite. A picture of such a body is shown in Figure 4.6. The body’s moments of inertia are specified as: Ixx = Iyy = 1.0kg · m2 Izz = 0.25kg · m2 In order to verify the functioning of the MechSys2 software as well as illustrate the relationship between traditional methods of solving dynamic problems and more modern techniques, this motion is simulated through the use of closed-form analytical solutions; through writing differential equations of motion and then solving these numerically; and through the use of the MechSys2 software.

It is chosen to present a comparison of the results of the three

solutions by plotting the components of the instantaneous rotational velocity ω of the body. This method allows the basic agreement of the methods to be easily seen and permits drift 144

Figure 4.6: Example Body

due to numerical or modeling inaccuracies to be easily seen graphically. The MechSys2 model’s output is also verified by examining the rate of change of “3–1–3” Euler angles to see that they remain constant and equal to the analytically computed values.

Analytical Solution The spinning axisymmetric body is a fairly simple problem in three-dimensional dynamics for which an analytical solution can readily be found. It is a classic problem which is demonstrated in most dynamics textbooks. A traditional way to analyze this problem begins with the choice of a convenient set of Euler angles. The angle system used in this proglem is the “3–1–3” angle system, in which the position of the body is described by rotations through angles ψ about the z axis, θ about the x axis (after the x axis has been rotated by the first rotation), and φ about the z axis (which may have been moved to 145

any possible orientation by the first two rotations) respectively. This set of Euler angles is discussed further in Section 3.4.2. The global coordinate reference frame is aligned so that the global z axis points in the direction of the total angular momentum of the body. This direction must remain constant because there is no externally appiled moment on the body. As an axisymmetric body, the body has moments of inertia Izz about the axis of symmetry and Ixx about any axis perpendicular to the axis of symmetry. The motion of the body can then be described as a combination of two rotations: ˙ and a steady precession in a steady spin of the body about its axis of symmetry, at a rate φ; ˙ The angle θ between which the axis of symmetry rotates about the global z axis at a rate ψ. the body’s axis of symmetry and the global z axis remains constant. The relationship between the rates of spin and precession is determined by the following equation: ψ˙ =

Izz φ˙ (Ixx − Izz ) cos θ

(4.15)

At any given point in time, the total angular velocity ω of the body is found as the sum of ˙ The relationship between the angular velocities is shown in Figure 4.7. From this ψ˙ and φ. relationship, we can compute the angles and the angular velocity of the body as a function of time. It is seen that the component of angular velocity parallel to the z axis remains ˙ constant, while the components along the x and y axes are equal to −ωy0 sin(ψt/2π) and ˙ ωy0 cos(ψt/2π) respectively. This particular analytical solution works only for one type of body and one type of motion, the free motion in space of an axisymmetric body. It enables a complete description of this particular motion to be achieved at very little computational cost. Many other classes of problems can of course be solved by such traditional techniques, but at the considerable 146

Figure 4.7: Components of Angular Velocity

cost of deriving other equations.

Numerical Solution The problem can be solved numerically by applying Euler’s equations directly: ΣMx = Ixx ω˙ x − (Iyy − Izz )ωy ωz ΣMy = Iyy ω˙ y − (Izz − Ixx )ωz ωx

(4.16)

ΣMz = Izz ω˙ z − (Ixx − Iyy )ωx ωy For a body spinning freely in space, the moment sums ΣMx , ΣMy , and ΣMz are zero. Given initial conditions of angle and angular velocity and assuming the object’s center of mass remains at the origin, we have a set of nonlinear ordinary differential equations for the components of angular velocity. These can be solved by a standard ODE solver. The output from the ODE solver is the time history of the components of the body’s angular velocity – 147

the components with respect to a set of coordinate axes attached to the body. In order to compare these results to those obtained through the analytical and MechSys2 techniques, we require a rotation matrix which can be used to perform a coordinate transformation from axes attached to the body to the world reference frame. Since the computation of the angular velocity ω takes place in small increments, the rotation matrix which represents the current position of the body can be built up from small angular changes. For these small changes, the order in which the rotations take place is unimportant, and small-angle approximations (sin θ = θ and cos θ = 1) can be used. The rotation matrix for each small change is 



 1    A =  −θz   

θy

θz 1 −θx





−θy 

1        = θx    −ωz dt     1

ωy dt

ωz dt 1 −ωx dt

−ωy dt     ωx dt    

(4.17)

1

As the simulation progresses, the rotation matrix R from the body’s initial position to its current position is incremented at each time step by multiplying it by the incremental rotation matrix A, so that after n steps the rotation matrix is given by Rn = An Rn−1 = An An−1 An−2 . . . A1 R0

(4.18)

where R0 is the rotation matrix from world coordinates to the body’s initial position. The numerical solution is more general than the analytical in that a wider class of problems can be solved through the use of these equations. For example, equations 4.16 can be applied to any rigid body. One need only be able to specify the principal axes of inertia of the body and the principal moments of inertia Ixx , Iyy , and Izz .

148

MechSys2 Solution A model of a symmetrical body can be constructed within the MechSys2 framework. As shown in Figure 4.8, the model consists of a set of point masses connected by spring connections. The masses are chosen and arranged so that the inertia tensor of the

Figure 4.8: MechSys2 Model

system consisting of the masses is equal to that of the axisymmetric body being modeled. The motion of each mass is determined by the application of Newton’s laws, p˙ = Σfi , where p is the momentum of the mass and the fi ’s are forces acting on the mass. The springs provide forces of f = k(x − x0 ), where x is the length of a spring at a given instant and x0 the spring’s free length; each spring’s force acts in the direction of the spring. The stiffnesses of the springs are high to approximate the rigidity of a rigid body. The initial conditions are specified in terms of the initial position and momentum of each mass. With no externally applied forces or moments, the masses will move in the same 149

way as would a set of four reference points attached to the body being modeled. Similarly, the position and attitude of the body can be deduced from the positions of the masses. The technique used to determine the body’s angular velocity is described in Section 3.4.6, that used to determine its attitude from point mass positions in Section 3.4.4, and that used to convert the attitude into Euler angles in Section 3.4.5.

Results The satellite simulation is run using each of the three methods described in this section. The initial conditions must be specified in different terms for each simulation; so the analytical model is used to transform the initial conditions into specifications which are identical for the numerical and MechSys2 models. The initial angles are ψ = 180o , θ = 30o , and φ = 0o . Figure 3.2 shows the positions of the angles. The body’s spin rate φ˙ about the z 00 axis is chosen to be one radian per second, and Equation 4.15 is used to compute the magnitude of the rate of precession as ψ˙ = 0.385 rad/sec. The analytical solution specifies that the angle θ will remain constant as the body moves in space, while the rates of angular change φ˙ and ψ˙ will remain constant. The initial angular velocity in the global reference frame is ω0 = (ωx , ωy , ωz ) = φ˙ + ψ˙ = (0, 0.5, 1.251) rad/sec, with the angles being added as shown in Figure 4.7. For the numerical simulation, the initial condition must be specified as an angular velocity referenced to a set of coordinate axes attached to the body. This simply requires that ω0 be rotated by 30o before being used as the initial condition for the ODE solver. For the MechSys2 solution, the body is created with its axis of symmetry parallel to the global z axis, then rotated into its initial condition. The initial angular velocity, specified 150

in global coordinates, is then applied to the body. The results of the simulation are shown in Figure 4.9. The basic agreement between the analytical, numerical, and MechSys2 simulations is visible, as is a bit of numerical inaccuracy in the latter two solutions. The inaccuracy is the result of comparatively large tolerances given to the ODE solvers which drive the numerical and MechSys2 simulations; tolerances which give some visible “drift” were chosen to show the error behavior of the non-analytical solvers. Tests were also run in which the tolerances were made an order of magnitude smaller; when the plots of angle components from each solution were superimposed, no drift was visible, as the plots were coincident.

151

Figure 4.9: Results of Satellite Simulation

152

Chapter 5

Summary In this research, several steps were taken toward the goal of making a novel approach toward the design and control of running machines practical. The goal of producing a robot which employs ballistic running techniques to race about the world at record speeds has not yet been reached, but we are somewhat closer.

5.1

Running There are a number of modes of running which are employed by machines that

exist in the world as well as on paper. The running robots built by Raibert et al. have exhibited high performance in the laboratory but at the cost of a great deal of complexity. Simpler designs are desired for practical implementations. Some machines which are under current development achieve great simplicity through the technique of using the mechanical design of the system, rather than feedback control, to achieve stable running operation. This technique results in simpler designs while incurring a cost in performance and flexibility.

153

In Sections 2.1-2.2, this paper proposes a model of running and a design and control strategy based thereon. The type of design which results from using these techniques attempts to form a compromise between the performance and simplicity of the techniques discussed above. This work is primarily to be considered an extension of previous work by Raibert and others. The ballistic running methodology allows a simpler design than those of the Raibert robots while maintaining a comparatively high degree of flexibility of control. As shown in Section 2.3, it also allows the control system to be structured as a hierarchical system. This has significant benefits in that the process of creating, analyzing, and testing a design is significantly simplified because the pieces on various levels can be tested independently to a significant degree prior to system integration. The ballistic running concept applies equally well to bodies which run with a “pronking” gait and those which run with alternating legs. Pronking refers to using all the legs to apply forces at approximately the same time; the alternating gait uses half the legs to apply an impulse during one stance and the other half of the legs to apply an impulse during a following stance. The only difference between these two types of strides (as seen by the ballistic running controller) is that the alternating gait requires that two different sets of legs supply the required impulses. However, the operation of the body controller is independent of the choice of legs to apply an impulse – as long as the leg controllers succeed somehow in applying the impulse required.

154

5.2

Simulation Studies In order to verify the usefulness of the ballistic model of running and the control

system design based thereon, simulation studies were performed. First, a simple dynamic model of a representative running machine was created as discussed in Section 2.4. This model represents the dynamic behavior of the body alone. The legs are assumed to be massless; the legs are also assumed to be able to provide any desired force and moment impulse acting at the body’s center of gravity. The forces applied by the legs are assumed to act over a vanishingly small period of time, in a manner similar to that of a collision. This simple model is used to show that a simple PD controller can stabilize and control the motion of the body as long as the disturbances are small. Second, a technique for allocating the impulses required by the body among the legs during a given stance phase was designed, as shown in Section 2.5. This technique is different from those implemented in earlier works in that it uses the legs to provide linear impulses only, not torques at the hips. Also, the legs do not actively provide impulses in directions to the side of body. These restrictions on the required impulses significantly simplify the design of the legs. Testing of the simulated body reveals that the impulse allocation scheme works as intended. The body can be stabilized by a simple PD controller which is very similar to that used for the body-only case. The only change to the control algorithm is the addition of a technique for “banking” the body to move it to the side. The third step in the simulation study was to examine the effect of force pulse duration on the control algorithm. This was accomplished by modifying the simulation so

155

that the dynamic behavior of the body during the stance phase was fully modeled. The legs were still assumed massless and capable of providing exactly the force desired. The same controller as used in the multi-leg impulse model was used, with the exception of the addition of extra vertical force to compensate for the effect of gravity on the body during the stance phase. As shown in Section 2.6, the impulse based control scheme was successful in stabilizing the motion of the body as long as the duration of the stance was not too long. For an excessively high duration of stance, it was found that a controller which can stabilize a body whose motion is controlled by very short impulses could not stabilize some modes when the force pulses were not of short duration. This behavior was as expected given the effect of the dynamics of the body during the stance phase.

5.3

Dynamic System Modeling In order to more accurately model the motion of a running machine, it is necessary

to significantly increase the complexity of the dynamic model. A model which includes not only the body’s dynamics but also those of the legs is required. The simple Newton-Euler based modeling scheme used in the simulation studies shown in Sections 2.4 through 2.5.4 is very difficult to apply to a complex system such as a body with legs. Therefore, a more refined modeling system is required. Chapter 3 introduces a modeling system which can be used to construct a more complete running machine simulation. The development and refinement of this system constituted a significant portion of the effort described in this work. This modeling scheme is novel in several significant ways. All dynamic behavior is modeled through the use of point

156

masses acted upon by linear forces; there is no representation of torsional force or angular momentum inside the system. Rigid bodies are modeled as “pseudo-rigid” structures by building them from point masses attached together with stiff connections. Forces are applied to bodies by adding small or massless points which are attached to the other masses in a body by more stiff connections and applying linear forces directly to those points. Constraints are modeled by simple geometric constructions which confine the motion of nodes. This modeling scheme is an internally consistent way of simulating the motion of modestly complex systems, such as a running machine body with several legs. It can also be used as instructional software, because it allows the modeling of interesting and meaningful systems while applying an extremely simple underlying scheme of dynamic modeling – that of forces acting on point masses. Although numerical issues and time constraints have limited the application of this technique to complex running machine issues, the problems appear to be solvable given further research and fast enough computers.

5.4

Future Work This research is very much a work in progress, extending previous research in

machine locomotion and suggesting many directions for future work. The results of the simulation work have been encouraging and suggest that the ballistic running strategy may well be a useful method for designing running machines. Some suggestions for future work follow.

157

Newton-Euler Simulations In order to improve the functionality of the Newton-Euler simulations, the code which was used to hold body angles in the satellite example should be introduced into the simulations in Sections 2.4–2.6.1. This would allow these simulations to be used for problems such as that of body navigation, in which the body turns through large angles in the course of its movements.

Impulse Allocation The allocation of impulses among the legs in order to fulfill the body’s commands for impulses has only been studied in its simplest form. Future extensions could begin by implementing two useful additions, gaits and robust leg usage. Alternating gaits could be implemented by allocating impulses among half the legs during any given stance. This would allow the advantages of alternating gaits, such as a reduction in the pitching moment imparted on the body by swinging legs, to be realized. If enough legs are present, a more robust scheme could be designed in which legs which were nonfunctional could be easily removed from the impulse allocation. This would not require any modifications of the high-level body controller nor of the low-level leg controllers. A damaged leg could be tucked into an inactive attitude or perhaps even jettisoned, while the machine would continue to function.

Leg Controllers The leg control algorithms which have been implemented in the course of this research are very simple. As discussed in Section 2.8.1, a more complex leg control system 158

could allow for more precise and robust control of the system. In particular, a closed-loop system in which the leg actively measures and controls the impulse which it applies would be most appropriate as the lowest level of control in the system.

Pseudo-rigid Modeling Issues The pseudo-rigid modeling technique shows significant promise as a method for modeling modestly complex dynamic systems. However, significant issues remain to be solved before this technique will become fully useful. Numerical problems which cause simulations to become unstable are the most serious issues. The mechanism by which the instability occurs is not yet fully understood, and research is needed to identify the problem and determine if and how a solution can be implemented. Once these issues have been resolved, many useful features can be added to the modeling software. Another issue which deserves attention is computational efficiency. The MechSys2 software is written in C++, a relatively efficient, fully compiled language. However, the software was written in a style which emphasizes simplicity and organization of code rather than computational efficiency. After careful profiling to determine which parts of the code are actually slowing simulations down, the software could be modified to improve its efficiency. The ODE solvers which are used in MechSys2 are very simple and basic ones. There may well be more sophisticated solvers which would allow for much faster computation times. For stiff solvers, the analytically based computation of the Jacobian would add greatly to computing speed. In several places within the code, matrices are inverted. The use of more efficient 159

inversion techniques than those used so far could improve computational speed. Techniques which take advantage of the banded nature of some matrices, such as the Jacobian inverted by stiff solvers, could improve speed as well. The improvement of computational speed would not only satisfy a user’s desire for instant gratification but also increase the scope of problems which can reasonably be solved by this system.

Reality Considerably more simulation study would be prudent before experiments with real prototypes – most likely impulse-controlled legs at first – should begin. Hopefully, this work can lead to the development of complete running prototypes. It is the hope of the author that this work can eventually have a positive influence on the design of machines which run.

160

Appendix A

Source Code for Matlab Simulations This Appendix contains the Matlab source code for the simulation programs which were used to study the behavior of the running machine models. Separate directories were maintained for the model which represents only the body, as discussed in Section 2.4; the model which implements an impulse based approximation of a body with idealized legs, discussed in Section 2.5; and the model which uses force pulses of finite length on a body with idealized legs, discussed in Section 2.6.

A.1

Files for Body-Only Simulation

File: body.m % File: body.m % This is the main file for a simulation of a running machine body which receives % an impulse when itsdistance from the ground is "l" (leg length). This is the % body-only problem in which there is one effective "leg" which can exert both % forces and torques on the body at the hip.

161

% % Revisions % 6/10/01 VPS Original file % 7/28/01 JRR Lots of miscellaneous changes and fixes clear all global F_g T_g m_g I_g l %----------------- Set initial values/constants/parameters -------------------% Specify name of function holding ODE’s to be integrated equation = ’BBS_difeq1’; % Set the initial conditions. See ODE file for definitions of state variables x = [0 0 0.4 0 0.27 0 (10 * pi / 180) 0 0 0 0 0]; % Set principal moments of intertia, [Ixx Iyy Izz] are [roll, pitch, yaw] I_g = [0.1 0.4 0.3]; % Specify the acceleration due to gravity a_g = 9.81; % Set the mass of the body m_g = 1.5; % Force applied at flight phase is just gravity F_g = [0, 0, -m_g*a_g]; % Applied moment during flight for most problems is zero T_g = [0, 0, 0]; % Set the "leg length" l = 0.25; % Specify absolute and relative tolerances, if applicable abtol = 1e-9; reltol = 1e-9; % Desired position (for used with controller only -- not used with fixed impulse) top_height = 0.2; xd = [0 0 0 0 0 sqrt(2 * a_g * top_height) 0 0 0 0 0 0]; % Set control gains; Kp = proportional gains and Kd = derivative gains %Kp = 0.5 * [1 1 0 0.02 0.02 0.02]; %Kd = 5 * [0.1 0.1 m_g/5 0.02 0.02 0.02]; % Set control gains. These are functions of system parameters because it’s a % convenient way to roughly tune the controller as we change the system. This % isn’t meant to be any kind of adaptive control though Kp_g = 1; Kp = Kp_g * [m_g; m_g; 0; I_g(1); I_g(2); I_g(3)]’; Kd_g = 1.5; Kd = Kd_g * [m_g;

m_g;

m_g/Kd_g;

I_g(1);

% Kd for everything besides Vz I_g(2); I_g(3)]’;

162

% Impulses to apply during stance (for fixed impulse only -- not used with controller) P = [0 0 10]; H = [0 0 0]; % Set up the RunTime = [0 InitStepSize ODEoptions =

ODE solver to run from RunTime(1) to RunTime(2) 10]; % The time interval over which to run the ODE solver = 0.01; odeset (’Events’, ’on’, ’RelTol’, reltol, ’AbsTol’, abtol, ... ’InitialStep’, InitStepSize, ’MaxStep’, 0.1); t = RunTime(1); tOutput(1) = t; xOutput(1,:) = x; i = 2; % Set up container variables for output data xout = []; tout = []; %-----------------------------------------------------------------------------tic while t < RunTime(2) % Move the setpoint forward so the body will chase it xd = moveSetpoints (t, xd); % Run the ODE solver until its stop condition is met [tout_new, xout_new] = ode45 (equation, [t RunTime(2)], x, ODEoptions); tout = [tout; tout_new]; xout = [xout; xout_new];

% Append new values to the old % same with x

NumSteps = length(tout); % If ODE solver stopped because the feet touched down if tout(NumSteps) < RunTime(2) % If ode solver stopped because of a stance [xOutput, tOutput] = interpolateHit (xout_new, tout_new, ... xOutput, tOutput, i); % These interpolated values will be our new value of x and t x = xOutput(i,:); t = tOutput(i); % Call controller to calculate how much impulse to apply [P, H] = PDImpulse (Kp, Kd, x, xd); % Apply impulses x = BBS_Impulse (x, P, H, m_g, I_g); % Show some progress and advance output counter i = i + 1; fprintf (1, ’Step %d at time %f \r’, i, t); % Otherwise the ODE solver stopped because the simulation is finished else t = tout(NumSteps); % take the last values of t and x x = xout(NumSteps,:);

163

end end fprintf (1, ’\nSimulation took %f seconds.\n’, toc); %------------------------------- Plot the results ----------------------------clf; % Plot linear velocities subplot (2, 2, 1); plot (tout, xout(:,1), ’k--’, tout, xout(:,3), ’k-.’, tout, xout(:,5), ’k-’); grid on; title (’Body Position’); xlabel (’Time [s]’); ylabel (’Position’); legend (’X’, ’Y’, ’Z’); subplot (2, 2, 2); plot (tout, xout(:,2), ’k--’, tout, xout(:,4), ’k-.’); %, tout, xout(:,6), ’k-’); grid on; title (’Body Horizontal Velocity’); xlabel (’Time [s]’); ylabel (’Velocity’); legend (’V_x’, ’V_y’); %, ’V_z’); % Convert angles to degrees; they’re a little more readable this way thetaX = xout(:,7) * 180.0 / pi; thetaY = xout(:,9) * 180.0 / pi; thetaZ = xout(:,11) * 180.0 / pi; subplot (2, 2, 3); plot (tout, thetaX, ’k--’, tout, thetaY, ’k-.’, tout, thetaZ, ’k-’); grid on; title (’Body Angular Position’); xlabel (’Time’); ylabel (’Euler (123) Angles [deg]’); legend (’\theta_x’, ’\theta_y’, ’\theta_z’); % Convert angular velocities to degrees per second omegaX = xout(:,8) * 180.0 / pi; omegaY = xout(:,10) * 180.0 / pi; omegaZ = xout(:,12) * 180.0 / pi; % Plot either velocities, or a 3-D trajectory subplot (2, 2, 4); if (0) plot (tout, omegaX, ’k--’, tout, omegaY, ’k-.’, tout, omegaZ, ’k-’); grid on; title (’Body Angular Velocities’); xlabel (’Time [s]’); ylabel (’Angular Velocity [deg/s]’); legend (’\omega_x’, ’\omega_y’, ’\omega_z’); else plot3 (xout(:,1), xout(:,3), xout(:,5), ’k-’); axis (’equal’);

164

title (’Trajectory of body CG’); xlabel (’X’); ylabel (’Y’); zlabel (’Z’); grid on; end

File: BBS Impulse.m function X = BBS_Impulse (X, P, H, m, I) %============================================================================= % File: BBS_Impulse.m % This function computes the change in linear and rotational speed of % the body due to impulses. Note the absence of cross-coupling terms % because impulses take an infinitely small period of time to act. % % X = state [x Vx y Vy z Vz tx wx ty wy tz wz] % 1 2 3 4 5 6 7 8 9 10 11 12 % t = current simulated time % P = linear impulses [Px, Py, Pz] % H = angular impulses [Hx, Hy, Hz] % m = mass of the body % I = body rotational inertias on principal axes [Ixx, Iyy, Izz] % % Version % 3-17-99 JRR Original file %============================================================================= % Compute the change X(2) = X(2) + (P(1) / X(4) = X(4) + (P(2) / X(6) = X(6) + (P(3) /

in linear velocities due to the linear impulses m); m); m);

% Compute the angular velocity change due to angular impulses X(8) = X(8) + (H(1) / I(1)); X(10) = X(10) + (H(2) / I(2)); X(12) = X(12) + (H(3) / I(3));

File: BBS difeq1.m function varargout = BBS_difeq1 (t, y, flag,varargin) %============================================================================= % File: BBS_difeq1.m % This function contains the differential equation for Bug flight. % % t = current simulated time % X = state [x Vx y Vy z Vz tx wx ty wy tz wz] % 1 2 3 4 5 6 7 8 9 10 11 12 % F = net force applied to the body; has components [Fx, Fy, Fz] % T = net torque [Tx, Ty, Tz] applied to the boxy % m = mass of the body

165

% I = body rotational inertias on principal axes [Ixx, Iyy, Izz] % % Version % 3-17-99 VPS Original file called Dpulse1.m % 3-18-99 JRR Modified slightly to fit into the BBS project %============================================================================= switch flag case ’’ % Return dy/dt = f(t,y). varargout{1} = f(t,y); case ’events’ % Return [value,isterminal,direction]. [varargout{1:3}] = events(t,y); otherwise error([’Unknown flag ’’’ flag ’’’.’]); end function dx = f(t,X) % Global variables ’cause I’m having trouble with parameters global F_g T_g m_g I_g % OK, let’s try this the Pammy way by giving new names to the members of the % array of states. x = X(1); vx = X(2); y = X(3); vy = X(4); z = X(5); vz = X(6); qx = X(7); wx = X(8); qy = X(9); wy = X(10); qz = X(11); wz = X(12); Ixx Iyy Izz

= I_g(1); = I_g(2); = I_g(3);

% Linear acceleration is easy; it’s force over mass A = F_g / m_g; % Angular acceleration uses L(1) = (1 / Ixx) * (T_g(1) + L(2) = (1 / Iyy) * (T_g(2) + L(3) = (1 / Izz) * (T_g(3) +

those pesky (Iyy - Izz) (Izz - Ixx) (Ixx - Iyy)

Euler equations * wy * wz); * wz * wx); * wx * wy);

% Derivatives: dx = [vx; A(1); vy; A(2); vz; A(3); wx; L(1); wy; L(2); wz; L(3)]; %============================================================================== function [value,isterminal,direction] = events(t,X) global l value = X(5) - l; isterminal = 1;

166

direction = -1;

File: BBS eq.m function dx = BBS_eq (X, t) %============================================================================= % File: BBS_difeq1.m % This function contains the differential equation for Bug flight. % % t = current simulated time % X = state [x Vx y Vy z Vz tx wx ty wy tz wz] % 1 2 3 4 5 6 7 8 9 10 11 12 % F = net force applied to the body; has components [Fx, Fy, Fz] % T = net torque [Tx, Ty, Tz] applied to the boxy % m = mass of the body % I = body rotational inertias on principal axes [Ixx, Iyy, Izz] % % Version % 3-17-99 VPS Original file called Dpulse1.m % 3-18-99 JRR Modified slightly to fit into the BBS project %============================================================================= % Global variables ’cause I’m having trouble with parameters global F_g T_g m_g I_g % OK, let’s try this the Pammy way by giving new names to the members of the % array of states. x = X(1); vx = X(2); y = X(3); vy = X(4); z = X(5); vz = X(6); qx = X(7); wx = X(8); qy = X(9); wy = X(10); qz = X(11); wz = X(12); Ixx Iyy Izz

= I_g(1); = I_g(2); = I_g(3);

% Linear acceleration is easy; it’s force over mass A = F_g / m_g; % Angular acceleration uses L(1) = (1 / Ixx) * (T_g(1) + L(2) = (1 / Iyy) * (T_g(2) + L(3) = (1 / Izz) * (T_g(3) +

those pesky (Iyy - Izz) (Izz - Ixx) (Ixx - Iyy)

Euler equations * wy * wz); * wz * wx); * wx * wy);

% Derivatives:

167

dx = [vx; A(1); vy; A(2); vz; A(3); wx; L(1); wy; L(2); wz; L(3)]’;

File: PDImpulse.m function [P,H] = PDImpulse(Kp, Kd, x, xdesired); % This function calculates how much impulse to apply using a PD controller P = + H = +

Kp(1:3) Kd(1:3) Kp(4:6) Kd(4:6)

.* .* .* .*

(xdesired(1:2:5) - x(1:2:5)) ... (xdesired(2:2:6) - x(2:2:6)); (xdesired(7:2:11) - x(7:2:11)) ... (xdesired(8:2:12) - x(8:2:12));

File: StopCondition.m function Flag = StopCondition(x, t) global l if x(5) MinHeight & isempty(TimeStartPulse) % If this is during flight phase NetForce = 0; % no leg force, only gravity which is applied later NetTorque = 0; else % Stance phase. Assume all legs touch the ground at the same time. % The stance continues even after z has risen some so we can complete our pulse nlegs = size (legs, 2); % Find a rotation matrix corresponding to the body’s current attitude rotMatrix = euler2rot ([qx, qy, qz], ’123’); if isempty(TimeStartPulse) % If a pulse has not yet been started, set everything up TimeStartPulse = t; disp([’Bouncing at: ’, num2str(t)]); % Calculate the moving setpoint xd = moveSetPoints(t, xd); % Calculate the banking angle xd = FindBankAngle (Kb, Kp, Kd, X, xd); % Then run the controller to find the desired leg impulses in body coordinate [legs, P, H] = body_control (Kp, Kd, X’, xd, legs, rotMatrix); % Find out what is the largest magnitude of all the legs’ desired impulses for i = 1:nlegs MagImp(i) = norm(legs(i).desImp); end MaxImpulse = max(MagImp); TimeRequired = MaxImpulse / MaxForce; TimeEndPulse = TimeStartPulse+TimeRequired; F_P_ratio = 1/TimeRequired; end if t >= TimeEndPulse % If we’re already done applying the pulses TimeStartPulse = []; % don’t apply any, and reset the starting pulse else % If we’re not done yet. Whether starting or in the middle % Apply any forces generated by the legs to the body for index = 1 : nlegs % Then find the force and the torque in the leg aForce = F_P_ratio * legs(index).desImp; aTorque = cross (legs(index).r_hip, aForce);

191

% but that’s in body coordinate. Convert to global aForce_g = rotMatrix’ * aForce; aTorque_g = rotMatrix’ * aTorque; % Apply that force to the body netForce = netForce + aForce_g; netTorque = netTorque + aTorque_g; end end end %----------------------------- Find the Derivatives --------------------------% Linear acceleration is easy; it’s force over mass A = (F_g + netForce) / m_g; % Angular acceleration uses those L(1) = (1 / Ixx) * (netTorque(1) + L(2) = (1 / Iyy) * (netTorque(2) + L(3) = (1 / Izz) * (netTorque(3) +

pesky Euler (Iyy - Izz) (Izz - Ixx) (Ixx - Iyy)

equations * wy * wz); * wz * wx); * wx * wy);

% Put the derivatives into the output array dx = [vx; A(1); vy; A(2); vz; A(3); wx; L(1); wy; L(2); wz; L(3)]; function [value, isterminal, direction] = events (t, X) global MinHeight value = X(5) - MinHeight; isterminal = [1]; direction = [-1];

192

Appendix B

MechSys2 Software Reference This Appendix contains the documentation for each of the classes which make up the MechSys2 system. The documentation has been written into the source code file as comments; it has been formatted into the text of this chapter by the program Doxygen. Information about this program is available at http://www.doxygen.org.

B.1

CAttitude Class Reference

This class stores and manipulates the attitude of a solid body in a three- dimensional world. #include Inheritance diagram for CAttitude::

Public Methods • CAttitude (void) • CAttitude (CAttitude &)

193

• CAttitude (CVector< 3 > &, CVector< 3 > &, CVector< 3 > &) • ∼CAttitude (void) • CAttitude& operator= (const CAttitude &) • CAttitude& findRotationTo (CVector< 3 > &, CVector< 3 > &, CVector< 3 > &) • CVector rotateForward (const CVector< 3 > &) • CVector rotateBackward (const CVector< 3 > &) • CVector get u0 (void) • CVector get v0 (void) • CVector get w0 (void) • CVector get u1 (void) • CVector get v1 (void) • CVector get w1 (void) • CMatrix getRotationMatrix (void) • void setStartingVectors (CVector< 3 > &, CVector< 3 > &, CVector< 3 > &) • void setActualVectors (CVector< 3 > &, CVector< 3 > &, CVector< 3 > &)

Protected Attributes • CVector u0 • CVector v0 • CVector w0 • CVector u1 • CVector v1 • CVector w1 • CMatrix rot

Friends • ostream& operator &, CVector< 3 > &, CVector< 3 > &) Standard constructor is given vectors u0 , v0 , and w0 .

CAttitude::∼CAttitude (void) Destructor cleans up after the attitude is finished.

Member Function Documentation CAttitude & CAttitude::findRotationTo (CVector< 3 > &, CVector< 3 > &, CVector< 3 > &) Function that finds a rotation matrix given three new vector positions.

CMatrix< 3, 3 > CAttitude::getRotationMatrix (void) [inline] Function that returns a copy of the rotation matrix.

CVector< 3 > CAttitude::get u0 (void) [inline] Function which returns the starting vector u0 .

CVector< 3 > CAttitude::get u1 (void) [inline] Function which returns the current relative position vector u1 .

195

CVector< 3 > CAttitude::get v0 (void) [inline] Function which returns the starting vector v0 .

CVector< 3 > CAttitude::get v1 (void) [inline] Function which returns the current relative position vector v1 .

CVector< 3 > CAttitude::get w0 (void) [inline] Function which returns the starting vector w0 .

CVector< 3 > CAttitude::get w1 (void) [inline] Function which returns the current relative position vector w1 .

CAttitude & CAttitude::operator= (const CAttitude &) This assignment operator copies an attitude’s contents into another.

CVector< 3 > CAttitude::rotateBackward (const CVector< 3 > &) Function rotates vector from current attitude back to original attitude.

CVector< 3 > CAttitude::rotateForward (const CVector< 3 > &) Function to rotate a vector from original attitude to current attitude.

void CAttitude::setActualVectors (CVector< 3 > &, CVector< 3 > &, CVector< 3 > &) Function saves the ending vectors u1 , v1 , and w1 (FK).

void CAttitude::setStartingVectors (CVector< 3 > &, CVector< 3 > &, CVector< 3 > &) Function saves starting vectors u0 , v0 , and w0 (FK).

Friends And Related Function Documentation ostream & operator CAttitude::rot [protected] The rotation matrix R0,1 itself.

CVector< 3 > CAttitude::u0 [protected] Starting position of reference point u.

CVector< 3 > CAttitude::u1 [protected] Position of point u after body has been rotated to new position.

CVector< 3 > CAttitude::v0 [protected] Starting position of reference point v.

CVector< 3 > CAttitude::v1 [protected] Position of point u after body has been rotated to new position.

CVector< 3 > CAttitude::w0 [protected] Starting position of reference point w.

CVector< 3 > CAttitude::w1 [protected] Position of point u after body has been rotated to new position. The documentation for this class was generated from the following file: • attitudes.h

B.2

CBaseAdaptiveSolver Class Reference

This is a base class for solvers with adaptive step sizing. #include Inheritance diagram for CBaseAdaptiveSolver::

197

Public Methods • CBaseAdaptiveSolver (double aStepSize) • virtual ∼CBaseAdaptiveSolver (void) • virtual void initialize (void) • void setStepControls (double aSF, double aGP, double aSP) • void setMinStepSize (double aSize) • void setStepSizeIncrease (double inc) • void setStepSizeDecrease (double dec) • void setRelativeTolerances (double ∗tols) • void setRelativeTolerances (double tol) • void setAbsoluteTolerances (double ∗tols) • void setAbsoluteTolerances (double tol) • virtual void singleStep (void)=NULL • virtual double integrateForInterval (double howLong)

Protected Attributes • double safetyFactor • double growPower • double shrinkPower • double minStepSize • double maxStepSizeIncrease • double maxStepSizeDecrease • double∗ relativeTolerances

198

• double∗ absoluteTolerances • double∗ saveStates • double∗ errors

Detailed Description This is a base class for solvers with adaptive step sizing. It uses an adaptation algorithm taken from the one tested by VP Sirivedhin and modeled after the routine described in Numerical Recipes in C. Note the ‘base’ in the name; this is an abstract base class. Don’t try to instantiate one.

Constructor & Destructor Documentation CBaseAdaptiveSolver::CBaseAdaptiveSolver (double aStepSize) The standard constructor gets step size; other values are set to defaults. It sets the adaptation constants to default values for a Runge-Kutta 45 explicit solver; for other solvers, these constants should be set accordingly. Parameters: aStepSize The initial step size for the solver

CBaseAdaptiveSolver::∼CBaseAdaptiveSolver (void) [virtual] The destructor cleans up memory used by the solver.

Member Function Documentation void CBaseAdaptiveSolver::initialize (void) [virtual] Initializes the ODE solver for a given run. The relative and absolute tolerances are set to default values here, so they must be changed by the user after each initialization if new values are to be used. Reimplemented from CBaseSolver (p. 220). Reimplemented in CRK45AdaptiveSolver (p. 306), and CRosenbrockAdaptiveSolver (p. 309).

double CBaseAdaptiveSolver::integrateForInterval (double howLong) [virtual] Method which integrates from now until we reach the given time.

199

It’s the ‘driver’ routine; the ‘stepper’ routine which it calls is in method singleStep() (p. 201). This method is not to be overridden in descendent classes; singleStep() (p. 201) is. Parameters: howLong The duration of the interval for which the solver should run Reimplemented from CBaseSolver (p. 220).

void CBaseAdaptiveSolver::setAbsoluteTolerances (double tol) A method which sets all the absolute tolerances to one value. One tolerance is set for each state variable. Parameters: tols Pointer to array of doubles which hold the absolute tolerances

void CBaseAdaptiveSolver::setAbsoluteTolerances (double ∗ tols) A method which sets new values for the absolute tolerances. One tolerance is set for each state variable. Parameters: tols Pointer to array of doubles which hold the absolute tolerances

void CBaseAdaptiveSolver::setMinStepSize (double aSize) [inline] Method which sets the minimum legal step size.

void CBaseAdaptiveSolver::setRelativeTolerances (double tol) This method sets all the relative tolerances to the same value. Parameters: tols Double which holds the new relative tolerance for all variables

void CBaseAdaptiveSolver::setRelativeTolerances (double ∗ tols) A method to set the relative tolerances for each variable. There is one tolerance value for each state variable. Parameters: tols Pointer to array of doubles which hold the relative tolerances

200

void CBaseAdaptiveSolver::setStepControls (double aSF, double aGP, double aSP) Method to set the step size control constants. Parameters: aSF A new value for the safety factor aGP A new value for the ‘growth power’ which controls step size increases aSP A new value for ‘shrink power’ which controls step size decreases

void CBaseAdaptiveSolver::setStepSizeDecrease (double dec) [inline] This method sets the maximum step size decrease per step.

void CBaseAdaptiveSolver::setStepSizeIncrease (double inc) This method sets the maximum step size increase in one step. Other parameters which are computed from this one are updated as needed. Parameters: inc The maximum step size increase

void CBaseAdaptiveSolver::singleStep (void) [virtual] Routine which takes one step. This routine must be overloaded in each of the descendent classes (RK45, Stiff, etc.) descended from this class Reimplemented in CRK45AdaptiveSolver (p. 306), and CRosenbrockAdaptiveSolver (p. 310).

Member Data Documentation double ∗ CBaseAdaptiveSolver::absoluteTolerances [protected] Maximum allowable absolute errors.

double ∗ CBaseAdaptiveSolver::errors [protected] Array which holds estimated errors.

double CBaseAdaptiveSolver::growPower [protected] Controls rate of step size increases.

201

double CBaseAdaptiveSolver::maxStepSizeDecrease [protected] Maximum decrease in step size per step.

double CBaseAdaptiveSolver::maxStepSizeIncrease [protected] Maximum increase in step size per step.

double CBaseAdaptiveSolver::minStepSize [protected] Minimum legal step size.

double ∗ CBaseAdaptiveSolver::relativeTolerances [protected] Maximum allowable relative errors.

double CBaseAdaptiveSolver::safetyFactor [protected] Keeps steps from getting big too fast.

double ∗ CBaseAdaptiveSolver::saveStates [protected] Array in which to save system states.

double CBaseAdaptiveSolver::shrinkPower [protected] Controls rate of step size decreases. The documentation for this class was generated from the following files: • solvers.h • solvers.cpp

B.3

CBaseEulerAngles Class Reference

This is the base class for Euler angles. #include Inheritance diagram for CBaseEulerAngles::

202

Public Methods • CBaseEulerAngles (void) • CBaseEulerAngles (const CBaseEulerAngles &oldOne) • CBaseEulerAngles (const CMatrix< 3, 3, double > &rot) • CBaseEulerAngles (double, double, double) • CBaseEulerAngles (CVector< 3, double > &) • virtual ∼CBaseEulerAngles (void) • double operator() (int index) • virtual void setFromRotationMatrix (const CMatrix< 3, 3, double > &rot)=NULL • virtual CMatrix getRotationMatrix (void)=NULL • void writeAnglesTo (ostream &, char ∗=””) • void setAngles (CVector< 3, double > angst) • void setAngles (double ang1, double ang2, double ang3)

Protected Attributes • CVector angles

Friends • ostream& operator & rot) [inline] This constructor constructs an angle set given a rotation matrix. It must be overridden in each descendent class and is here only to serve as an example of the proper definition. Parameters: rot Initial rotation matrix

CBaseEulerAngles::CBaseEulerAngles (double a1, double a2, double a3) This constructor constructs an angle set given a set of three angles. It’s meant to be called by a derived class’s constructor. The meanings of the angles will change depending on the angle system being used. Parameters: a1 The initial value of the first angle a2 The initial value of the second angle a3 The initial value of the third angle

CBaseEulerAngles::CBaseEulerAngles (CVector< 3, double > & angVec) This constructor creates an angle set given a vector of angles. It’s probably useful only within the constructor of a derived class. Parameters: angVec Reference to the vector of angles which are to be copied

CBaseEulerAngles::∼CBaseEulerAngles (void) [virtual] The default destructor frees up memory used by the angle object.

204

Member Function Documentation CMatrix< 3, 3, double > CBaseEulerAngles::getRotationMatrix (void) [virtual] This method returns a rotation matrix corresponding to the angle set which has been saved in this object. Since we can’t actually compute a rotation matrix from the angles unless we know what angle system is being used, this is a pure virtual function. Returns: The rotation matrix computed from the angle object’s angles Reimplemented in CEulerAnglesXwYwZw (p. 264), CEulerAngles Z X Z (p. 261), and CEulerAngles X Y Z (p. 258).

double CBaseEulerAngles::operator() (int index) [inline] This operator returns the current value of one of the angles. Parameters: index Which angle to return (1, 2, or 3)

void CBaseEulerAngles::setAngles (double ang1, double ang2, double ang3) [inline] This method sets the Euler angles to equal the given numbers. Parameters: ang1 A new value for the first angle ang2 A new value for the second angle ang3 A new value for the third angle

void CBaseEulerAngles::setAngles (CVector< 3, double > angst) [inline] This method sets the Euler angles to equal the elements in a vector. Parameters: angst A new set of angles to put into this object’s data field

205

void CBaseEulerAngles::setFromRotationMatrix (const CMatrix< 3, 3, double > & rot) [virtual] This method computes the Euler angles from a rotation matrix. Since we can’t actually compute angles from a rotation matrix unless we know what angle system is being used, this is a pure virtual function. Parameters: rot The rotation matrix from which angles are to be computed Reimplemented in CEulerAnglesXwYwZw (p. 264), CEulerAngles Z X Z (p. 261), and CEulerAngles X Y Z (p. 258).

void CBaseEulerAngles::writeAnglesTo (ostream & str, char ∗ separator = ” ”) This method writes the three angles onto the given stream. It probably doesn’t need to be overridden in a derived class, but one does need to keep track of what each of the angles actually means so the data file makes sense. You can put in a different separator which appears between data items, for example a comma to write the data into a .CSV format spreadsheet file. Parameters: str The stream to which the angle will be printed separator The text which is printed between the angle numbers

Friends And Related Function Documentation ostream & operator CBaseSolver::states [protected] Pointers to system states.

double CBaseSolver::stepSize [protected] Current size of steps being taken.

long CBaseSolver::stepsTaken [protected] Number of integration steps taken.

bool CBaseSolver::stopCommand [protected] Signals integrator to stop ASAP.

double CBaseSolver::stopTime [protected] Time at which we stop the solver.

int CBaseSolver::systemOrder [protected] The number of variables in the system.

double CBaseSolver::theTime [protected] The current simulated time. The documentation for this class was generated from the following files: • solvers.h • solvers.cpp

223

B.7

CBaseSurface Class Reference

This base class is the ancestor of surfaces in the DynaSim project. #include Inheritance diagram for CBaseSurface::

Public Methods • CBaseSurface (void) • CBaseSurface (CDynaSystem &, CBody ∗, const CPosition &) • ∼CBaseSurface (void) • virtual void translateBy (const CPosition &displ)=NULL • virtual void rotateBy (CAttitude &att, const CPosition &cen)=NULL • virtual void resetToInitialConditions (void) • CNode∗ getBasePoint (void) • CNode∗ getConnPoint (void) • CBaseSurface∗ getConnectedSurface (void) • void setBasePointLocation (CPosition loc) • void setConnPointLocation (CPosition loc) • CPosition getBasePointLocation (void) • CPosition getConnPointLocation (void) • bool isGrounded (void) • virtual void connectToSurface (CBaseSurface &) • virtual void connectToNode (CNode ∗) • virtual void moveToClosestPoint (void)=NULL • virtual bool isInside (const CPosition &pos)

224

• virtual const char∗ getConfigString (void)

Protected Attributes • int idNumber • CBody∗ pBody • CDynaSystem∗ pSystem • CBaseSurface∗ connectedSurface • CNode∗ baseNode • CNode∗ connNode • CNode∗ otherNode

Detailed Description This base class is the ancestor of surfaces in the DynaSim project. A surface is an entity which is attached to a body and allows the body to be connected to other bodies. Surfaces can also be grounded, so that they never move; to make a surface grounded, set its body pointer to NULL.

Constructor & Destructor Documentation CBaseSurface::CBaseSurface (void) The default constructor creates an empty surface object.

CBaseSurface::CBaseSurface (CDynaSystem & sys, CBody ∗ bod, const CPosition & pos) Base constructor creates a surface in given system, body, and position. It’s mostly here to make writing the constructors for descendent classes just a little more convenient. Parameters: sys The dynamic system in which this all takes place bod The body to which the surface is attached (NULL for a grounded body) pos Position at which the surface’s base point is placed

225

CBaseSurface::∼CBaseSurface (void) Destructor cleans up after a surface. Note that nodes are deleted by the system object so they don’t have to be deleted here.

Member Function Documentation void CBaseSurface::connectToNode (CNode ∗ ptt) [virtual] Method connects surface to a single point attached to some other entity. It might be used instead of connectToSurface() (p. 226) when one needs to connect to a single grounded point, or maybe to the CG mass of a body. Parameters: ptt Pointer to the point to which this surface will be connected

void CBaseSurface::connectToSurface (CBaseSurface & surf) [virtual] Method which connects this surface to another surface. The connection is made by attaching a spring (etc.) from the connecting node of this surface to the connecting node of that other surface. Parameters: surf The other surface to which this one is connected

CNode ∗ CBaseSurface::getBasePoint (void) [inline] This method returns a pointer to the surface’s base point. Returns: Pointer to the surface’s base point

CPosition CBaseSurface::getBasePointLocation (void) Method which returns the current location of the surface’s base point. Returns: Current world-relative location of the surface’s base point

const char ∗ CBaseSurface::getConfigString (void) [virtual] This method returns a surface’s configuration data in a character string. The string may change, so the function is virtual.

226

Returns: Pointer to a string which contains the force’s configuration data Reimplemented in CPointSurface (p. 300), CLineSurface (p. 282), and CPlaneSurface (p. 297).

CNode ∗ CBaseSurface::getConnPoint (void) [inline] This method returns a pointer to the surface’s base point. Returns: Pointer to the surface’s base point

CPosition CBaseSurface::getConnPointLocation (void) Method which returns the current location of the connection point. Returns: Current world-relative location of the surface’s connection point

CBaseSurface ∗ CBaseSurface::getConnectedSurface (void) [inline] Method which returns the surface to which this surface is connected. Returns: The other surface

bool CBaseSurface::isGrounded (void) [inline] Method which returns a value that tells if the body is grounded or not. Returns: true if the body is grounded, false otherwise

bool CBaseSurface::isInside (const CPosition & pos) [inline, virtual] This method tells if a given point is inside or outside a surface. For point and line surfaces, all points are outside; for surfaces such as planes and spheres, some points are inside. Returns: true if the given point is inside the surface Reimplemented in CPlaneSurface (p. 297).

227

void CBaseSurface::moveToClosestPoint (void) [virtual] Method which moves connection to closest point to the other surface. This method must be overridden in every descendent class. Reimplemented in CPointSurface (p. 300), CLineSurface (p. 282), and CPlaneSurface (p. 297).

void CBaseSurface::resetToInitialConditions (void) [virtual] Method which moves the surface back to its initial condition. Reimplemented in CPointSurface (p. 300), CLineSurface (p. 283), and CPlaneSurface (p. 297).

void CBaseSurface::rotateBy (CAttitude & att, const CPosition & cen) [virtual] Abstract base method which rotates the surface about body’s CG point to the given attitude. Descendents must provide their own implementations. Parameters: att Object holding the new attitude to which surface moves cen Position of center point of rotation Reimplemented in CPointSurface (p. 300), CLineSurface (p. 283), and CPlaneSurface (p. 298).

void CBaseSurface::setBasePointLocation (CPosition loc) [inline] This method forces the base point to the given location. Parameters: loc The new location to which the base point will be moved

void CBaseSurface::setConnPointLocation (CPosition loc) [inline] This method moves the connection point to the given location. Parameters: loc New location to which the connection point will be moved

void CBaseSurface::translateBy (const CPosition & pos) [virtual] Method which moves the surface by the given displacement. This method is abstract; every descendent surface class must provide an implementation.

228

Parameters: displ Displacement by which the surface is moved Reimplemented in CPointSurface (p. 301), CLineSurface (p. 283), and CPlaneSurface (p. 298).

Member Data Documentation CNode ∗ CBaseSurface::baseNode [protected] Reference point for surface geometry.

CNode ∗ CBaseSurface::connNode [protected] Node to which things are connected.

CBaseSurface ∗ CBaseSurface::connectedSurface [protected] Other surface to which we’re connected.

int CBaseSurface::idNumber [protected] Serial number of the surface.

CNode ∗ CBaseSurface::otherNode [protected] Node on other surface to connect to.

CBody ∗ CBaseSurface::pBody [protected] Pointer to body which owns surface.

CDynaSystem ∗ CBaseSurface::pSystem [protected] Pointer to system in which surface is. The documentation for this class was generated from the following files: • surfaces.h • surfaces.cpp

B.8

CBody Class Reference

This is the base class for bodies used in the DynaSim project. #include

229

Inheritance diagram for CBody::

Public Methods • CBody (void) • CBody (CBody &) • CBody (CDynaSystem &, CPosition &, char ∗aname=””) • virtual ∼CBody (void) • CBody& operator= (const CBody &) • void rotateThrough (CMatrix< 3, 3 >) • void rotateInitialPosition (CMatrix< 3, 3 >) • void moveAttachments (void) • CVector rotateWorldToBody (CVector< 3 > &) • CVector rotateBodyToWorld (CVector< 3 > &) • CMass∗ addMainMass (double, const CPosition &, const CPosition &) • CDummyMass∗ addDummyMass (double, const CPosition &) • CNode∗ addCGnode (const CPosition &) • CGhostPoint∗ addGhostPoint (const CPosition &) • CRefPoint∗ addRefPoint (const CPosition &) • CNode∗ addBasePoint (const CPosition &) • void addSurface (CBaseSurface ∗) • CNode∗ getNodePtr (int i) • void connectForce (CBaseForce &) • double getMass (void) • CMatrix getInertia (void)

230

• CPosition getCGposition (void) • CNode∗ getCGnode (void) • CMatrix getRotationMatrix (void) • void setStartingVectors (void) • void setActualVectors (void) • CPosition CalculateCGPosition (void) • void calculateSurfaceForces (void) • void setInitialVelocity (CVelocity &) • void setInitialAngularVelocity (CVector< 3 > &) • CVelocity getLinearVelocity (void) • CVelocity getAngularVelocity (void)

Protected Attributes • int idNumber • CDynaSystem∗ pSystem • CNode∗ CGnode • vector nodeList • list connectionList • list surfaceList • list forceList • CAttitude attitude • double totalMass • CPosition startWorldPosition

Detailed Description This is the base class for bodies used in the DynaSim project. It is not intended that any objects of this class be instantiated; instead, real bodies are to be created from other classes descended from this one.

231

Constructor & Destructor Documentation CBody::CBody (void) Default constructor, not used directly. (Why else would anyone use it??)

CBody::CBody (CBody & oldbod) Copy constructor. Parameters: oldbod Reference to body which is to be duplicated

CBody::CBody (CDynaSystem & pSys, CPosition & aPos, char ∗ aname = ””) Constructor creates a body within a given system with blank name by default. It creates a body in the given position in the given system with the given name. Parameters: pSys Reference to the dynamic system in which the body is created aPos Position at which the new body’s center of mass is to be located aname The name of the new body, for diagnostic printouts and such

CBody::∼CBody (void) [virtual] The default destructor creates a null body object. The masses aren’t deleted in this destructor because the DynaSystem automatically deletes those.

Member Function Documentation CPosition CBody::CalculateCGPosition (void) This function calculate the position of the body’s center of mass (F&K). If there is a ghost or reference point at the center of mass, we’ll need to move that point to this position when the body moves. Returns: The position of the body’s center of mass computed from its main masses

232

CNode ∗ CBody::addBasePoint (const CPosition & pos) Method which adds a base point for a surface to the body (F&K). The ghost point serves as the base point for the surface. The ghost point knows to transfer any forces on it to the body’s four main masses. Parameters: pos Body-relative position at which the new point is placed Returns: Pointer to the new base point

CNode ∗ CBody::addCGnode (const CPosition & CGpos) Method to add a CG (center of mass) node to the body. Depending on the method of external connections being used, it can be a real mass, dummy mass, or ghost node. Parameters: CGpos Position of the body’s center of mass when the node is added Returns: A pointer to the newly created center of gravity node

CDummyMass ∗ CBody::addDummyMass (double amass, const CPosition & apos) Method which adds a dummy mass to the body. Parameters: amass The mass of the new dummy mass apos Body-relative location of the new dummy mass Returns: A pointer to the newly created dummy mass

CGhostPoint ∗ CBody::addGhostPoint (const CPosition & pos) Method that adds a ghost point to the body. The ghost point knows to transfer any forces which act on it to the body’s four main masses. Parameters: pos Body-relative position of the new ghost point

233

Returns: A pointer to the newly created ghost point

CMass ∗ CBody::addMainMass (double amass, const CPosition & apos, const CPosition & CGpos) This method adds a new main mass to the body. Parameters: amass The mass of the new mass apos Body-relative position at which the mass will be created CGpos World-relative position at which the body’s center of mass sits Returns: A pointer to the new mass

CRefPoint ∗ CBody::addRefPoint (const CPosition & pos) This method attaches a new reference point to the body. The reference point can be used to keep track of the position of a surface or other attachment. Parameters: pos Body-relative coordinates of the new reference point Returns: A pointer to the newly created referenece point

void CBody::addSurface (CBaseSurface ∗ newSurface) Method which adds a new surface to the body (F&K). Parameters: newSurf Pointer to the new surface to be added to the body’s list

void CBody::calculateSurfaceForces (void) This function calculates the forces on the surfaces of the body (F&K).

void CBody::connectForce (CBaseForce & newForce) This function connects a force to the body’s force list. The force is also added to the system’s force list for convenience. Parameters: newForce Reference to the force to be applied to the body

234

CVelocity CBody::getAngularVelocity (void) Method to find the angular velocity of the body. The computation is made by constructing a 12x3 matrix of coefficients A from the positions of the body’s main masses with respect to the mass center, constructing a right hand side vector b from the linear velocities of the mass nodes, and solving the overdetermined system Ax = b by the least-squares equation x = (AT A)−1 AT b. Returns: The rotational velocity vector

CNode ∗ CBody::getCGnode (void) [inline] This function returns a pointer to the body’s center-of-mass node.

CPosition CBody::getCGposition (void) [inline] Functions return CG’s current position and a pointer to the CG node. If no center of mass has yet been created, a warning message is printed and a zero position returned. Returns: The position of the center of mass node belonging to the body

CMatrix< 3, 3 > CBody::getInertia (void) Function which computes and returns the body’s inertia tensor. The most basic formula, I = Returns:

P

mr2 is used.

Matrix containing the computed inertia tensor of the body

CVelocity CBody::getLinearVelocity (void) Method which finds the linear velocity of the body. This velocity, equal to the instantaneous velocity of the body’s mass center, is estimated by computing the total momentum of the four main masses in the body, then dividing by the body’s total mass. This averaging should reduce the noise in velocity readings due to vibrations within the body’s connections, and it removes any component due to rotation about the center of mass. Returns: The body’s linear velocity vector

235

double CBody::getMass (void) [inline] This functions returns the body’s total mass.

CNode ∗ CBody::getNodePtr (int i) [inline] This function returns a pointer to the node in the body at index [i].

CMatrix< 3, 3 > CBody::getRotationMatrix (void) This function returns a rotation matrix to the body’s current attitude. Returns: The body’s current attitude, represented by a rotation matrix

void CBody::moveAttachments (void) This method moves ghost and reference points attached to the body. For example, ghost points to which forces are attached are moved by this function, and surfaces are moved by this function.

CBody & CBody::operator= (const CBody & oldbod) This assignment operator makes a copy of a body. Parameters: oldbod Reference to the body whose contents are to be duplicated

CVector< 3 > CBody::rotateBodyToWorld (CVector< 3 > & vec) [inline] Function which rotates vector from body’s attitude to world coordinates. Parameters: vec The vector which is to be rotated Returns: A vector which is the input vector after rotation

void CBody::rotateInitialPosition (CMatrix< 3, 3 > rot) This method rotates the body about its center of mass to a new position. The rotation takes place around the mass center. Parameters: rot The rotation matrix by which the body’s initial position is rotated

236

void CBody::rotateThrough (CMatrix< 3, 3 > rot) This method rotates the body about its center of mass to a new position. The rotation matrix is given as the function’s parameter. Parameters: rot The rotation matrix through which the body will be rotated

CVector< 3 > CBody::rotateWorldToBody (CVector< 3 > & vec) [inline] This function rotates a vector from world coordinates to body’s attitude. The rotation matrix of the ‘attitude’ member object does the work. It’s made inline for speed – this is going to be done pretty often. Parameters: vec The vector which is to be rotated Returns: A vector which is the input vector after rotation

void CBody::setActualVectors (void) Function to set the current-attitude vectors u1, v1, and w1 (F&K).

void CBody::setInitialAngularVelocity (CVector< 3 > & angVel) This method sets an initial condition for the body’s angular velocity. It does so by computing linear velocities for each of the masses and using those to set the initial momenta of the masses in the body. The rotation rate should be given in natural units of radians per second, or radians per whatever is consistent with the linear velocity units, as linear speeds of masses are computed from v = ω × r. Parameters: angVel The angular velocity vector

void CBody::setInitialVelocity (CVelocity & inVel) This method sets the initial condition of a body’s linear velocity. It does so by setting the initial momentum of each of the masses in the body. Reference nodes, ghost points, and other massless nodes don’t need to have an initial velocity and so are ignored.

237

Parameters: inVel Initial linear velocity for the body

void CBody::setStartingVectors (void) This function sets the attitude’s starting vectors u0, v0, and w0 (F&K). These are the relative positions of nodes 1, 2, and 3 with respect to node 0.

Member Data Documentation CNode ∗ CBody::CGnode [protected] Node at body’s center of mass (CG).

CAttitude CBody::attitude [protected] Attitude of this body in space.

list< CConnection ∗> CBody::connectionList [protected] List of connections between masses.

list< CBaseForce ∗> CBody::forceList [protected] List of external forces acting on body.

int CBody::idNumber [protected] Serial number of body.

vector< CNode ∗> CBody::nodeList [protected] List of pointers to nodes in the body.

CDynaSystem ∗ CBody::pSystem [protected] Pointer to system in which body lives.

CPosition CBody::startWorldPosition [protected] Position of CG at start of simulation.

list< CBaseSurface ∗> CBody::surfaceList [protected] List of surfaces attached to the body.

238

double CBody::totalMass [protected] Total mass of all real masses in body. The documentation for this class was generated from the following files: • bodies.h • bodies.cpp

B.9

CBodyRelativeForce Class Reference

This class represents a force that acts on a point which is attached to a body and that always acts in the same direction with respect to the body. #include Inheritance diagram for CBodyRelativeForce::

Public Methods • CBodyRelativeForce (void) • CBodyRelativeForce (const CBodyRelativeForce &oldFrc) • CBodyRelativeForce (CDynaSystem ∗, CBody ∗, const CForce &, const CPosition &) • ∼CBodyRelativeForce (void) • CBodyRelativeForce& operator= (const CBodyRelativeForce &) • virtual void moveWithBody (CPosition CGpos, CAttitude &att) • virtual void resetToInitialConditions (void) • virtual const char∗ getConfigString (void)

239

Detailed Description This class represents a force that acts on a point which is attached to a body and that always acts in the same direction with respect to the body. If the body’s attitude changes, the force’s direction changes with it. The movement takes place when the body’s moveAttachments() method calls the force’s moveWithBody() (p. 241) method.

Constructor & Destructor Documentation CBodyRelativeForce::CBodyRelativeForce (void) This constructor creates an empty body-relative force.

CBodyRelativeForce::CBodyRelativeForce (const CBodyRelativeForce & oldFrc) [inline] The copy constructor creates a duplicate of a body-relative force. Parameters: oldFrc The old force which is to be copied

CBodyRelativeForce::CBodyRelativeForce (CDynaSystem ∗ pSys, CBody ∗ pBod, const CForce & rFrc, const CPosition & rPos) The standard constructor creates a force which acts on a body. Parameters: sys Pointer to the system in which the force is being created bod Pointer to the body on which this force will act frc Vector containing the components of the initial value of the force pos Body-relative position at which the force will be created

CBodyRelativeForce::∼CBodyRelativeForce (void) The destructor cleans up dynamically allocated memory and such.

Member Function Documentation const char ∗ CBodyRelativeForce::getConfigString (void) [virtual] Method to print on a string how this force is configured.

240

Returns: Pointer to the buffer containing the data about the force Reimplemented from CBaseForce (p. 211).

void CBodyRelativeForce::moveWithBody (CPosition CGpos, CAttitude & att) [virtual] This method rotates the force so as to remain aligned with the body. It is called by the body when the body moves. Parameters: newPos The position to which the force must move newAtt New attitude to which the force will be rotated Reimplemented from CBaseForce (p. 212).

CBodyRelativeForce & CBodyRelativeForce::operator= (const CBodyRelativeForce & frc) This assignment operator copies all the data associated with a body relative force from another force object. Parameters: frc The force from which data is to be copied Returns: A reference to the new force object

void CBodyRelativeForce::resetToInitialConditions (void) [virtual] Method to reset force to conditions before dynamic simulation was started. Reimplemented from CBaseForce (p. 213). The documentation for this class was generated from the following files: • forces.h • forces.cpp

B.10

CConnection Class Reference

This class implements a connection between nodes. #include

241

Inheritance diagram for CConnection::

Public Methods • CConnection (void) • CConnection (const CConnection &) • ∼CConnection (void) • virtual CConnection& operator= (const CConnection &) • double getLength (void) const • int getIdNumber (void) • virtual const char∗ getConfigString (void) • virtual void computeForce (void)=NULL • double getForce (void) • virtual CForce getForceOnNode (CNode ∗) • virtual void applyForceToNodes (void) • virtual void resetToInitialConditions (void) • void writeEndPositions (ostream &, const char ∗=””)

Protected Attributes • int idNumber • double force • CNode∗ pnode1 • CNode∗ pnode2

Detailed Description This class implements a connection between nodes. This class is the parent class for springs and dampers.

242

Constructor & Destructor Documentation CConnection::CConnection (void) The default constructor creates connection object that doesn’t do anything.

CConnection::CConnection (const CConnection & oldconnectionthingy) The copy constructor creates a duplicate connection. Parameters: oldconnectionthingy Reference to the connection of which a copy is made

CConnection::∼CConnection (void) [inline] The standard constructor creates.

Member Function Documentation void CConnection::applyForceToNodes (void) [virtual] This method causes the connection force to be applied to both its nodes. It is called right before the nodes update their states. Reimplemented in CLinearFriction (p. 279).

void CConnection::computeForce (void) [virtual] This is a pure virtual base method which computes the force in the connection, storing it in the member data item force. Reimplemented in CSpring (p. 312), CLinearDamper (p. 276), and CLinearFriction (p. 279).

const char ∗ CConnection::getConfigString (void) [virtual] Method which returns a string describing how the node is configured. The string will change depending on the type of connection, so the function is virtual. Returns: A string containing text describing the connection Reimplemented in CSpring (p. 312), CLinearDamper (p. 276), and CLinearFriction (p. 279).

243

double CConnection::getForce (void) [inline] This method returns the last computed value of the force in the connection. The value must have been calculated by computeForce() (p. 243). Returns: The force in the connection

CForce CConnection::getForceOnNode (CNode ∗ onNode) [virtual] Method returns the amount of force from this connection on the given node. The node is asking the connection, “How much force are you exerting on me?” The connection checks which force is doing the asking and returns a force vector. Method calculateForce() is called to calculate the magnitude of the internal force. Note: Since we’ve changed to a multi-pass force summing system for computing the forces on nodes, this function is no longer used. It’s left here in case we need to change back for some reason, and because it looks sorta cool. Parameters: onNode Pointer to the node on which the force is acting Returns: The force acting on the given node

int CConnection::getIdNumber (void) [inline] This method returns an identifying ‘serial number’ of the node. Returns: The node’s identifying number

double CConnection::getLength (void) const Method to find the current distance from one node to the other.

CConnection & CConnection::operator= (const CConnection & oldie) [virtual] This assignment operator makes a copy of a connection. Parameters: oldie Reference to the old connection whose data is to be copied

244

void CConnection::resetToInitialConditions (void) [inline, virtual] Method which resets a connection to its original state. Many connections don’t need to do anything here, as their states are determined completely by the positions and/or velocities of the nodes at their ends

void CConnection::writeEndPositions (ostream & str, const char ∗ separator = ” ”) Function to print the positions of the connection’s ends. That’s nice because as a file, the stream can be read by a program which makes interesting 3-D animations or whatever. The data comes out as six doubles in six columns separated by the given text (space for Matlab is default; commas for CSV files are an option.) File format: x0 y0 z0 x1 y1 z1, where (x0, y0, z0) is the position of one end of the connection and (x1, y1, z1) is the position of the other end. Parameters: str Stream to which the data is to be written separator Text which will be placed between data columns

Member Data Documentation double CConnection::force [protected] Internal force; tensions are positive.

int CConnection::idNumber [protected] Serial number of this connection.

CNode ∗ CConnection::pnode1 [protected] Pointer to the node at one end.

CNode ∗ CConnection::pnode2 [protected] Pointer to the node at the other end. The documentation for this class was generated from the following files: • connections.h • connections.cpp

245

B.11

CDummyMass Class Reference

This class implements a small mass that is used to connect bodies and apply forces to those bodies. #include Inheritance diagram for CDummyMass::

Public Methods • CDummyMass (void) • CDummyMass (const CDummyMass &) • CDummyMass (double, const CPosition &) • ∼CDummyMass (void) • CDummyMass& operator= (const CDummyMass &)

Detailed Description This class implements a small mass that is used to connect bodies and apply forces to those bodies. The dummy’s mass must be large enough so that it can contribute to the state equations and not produce an excessively stiff system, but small enough so that the dummy’s presence doesn’t significantly alter the characteristics of the system.

Constructor & Destructor Documentation CDummyMass::CDummyMass (void) The default constructor creates a massless dummy at the origin. It’s not generally called directly.

246

CDummyMass::CDummyMass (const CDummyMass & odm) The copy constructor creates a copy of a dummy mass. Parameters: odm Reference to the old dummy mass which is being copied

CDummyMass::CDummyMass (double howBig, const CPosition & where) This constructor creates a dummy mass with the given mass and position. It creates a new dummy mass with the given amount of mass at the given location. Parameters: howBig The mass of the new dummy mass where The world-referenced location at which the mass will be created

CDummyMass::∼CDummyMass (void) The destructor cleans up memory after a dummy mass.

Member Function Documentation CDummyMass & CDummyMass::operator= (const CDummyMass & oldmass) This assignment operator makes another copy of the dummy mass. Parameters: oldmass Reference to the mass whose parameters are to be copied The documentation for this class was generated from the following files: • nodes.h • nodes.cpp

B.12

CDynaBase Class Reference

This is a base class for all the other classes used in the DynaSim project. #include Inheritance diagram for CDynaBase::

247

Public Methods • CDynaBase (void) • CDynaBase (const CDynaBase &) • CDynaBase (char ∗) • virtual ∼CDynaBase (void) • CDynaBase& operator= (const CDynaBase &) • const char∗ getName (void) const • void setName (const char ∗an)

Protected Attributes • char∗ name

Detailed Description This is a base class for all the other classes used in the DynaSim project. It contains a few handy things such as a name which are needed by nearly all the other classes in the project.

Member Data Documentation char ∗ CDynaBase::name [protected] The name of the object as a text string. The documentation for this class was generated from the following file: • bases.h

B.13

CDynaSolver Class Reference

Extension of CBaseSolver (p. 215) which adds compatibility with DynaSim objects. #include

248

Inheritance diagram for CDynaSolver::

Public Methods • CDynaSolver (double) • ∼CDynaSolver (void) • void setLists (list< CBody ∗> ∗, list< CNode ∗> ∗, list< CConnection ∗> ∗, list< CBaseForce ∗> ∗, list< CBaseSurface ∗> ∗) • virtual void computeDerivatives (void)

Detailed Description Extension of CBaseSolver (p. 215) which adds compatibility with DynaSim objects. This class extends the CBaseSolver (p. 215) class, implementing an interface to the DynaSim project. Masses, connections, forces, and other DynaSim components are registered with the solver so that their state variables automatically become states in the dynamic system. \ In order to use this class, one must create a descendent class which implements a given type of solver, for example CEulerSolver. Or, a descendent of that class may be used to implement functionality specific to a given problem such as simulating a continuous-time controller by extending the states in the system.

Constructor & Destructor Documentation CDynaSolver::CDynaSolver (double aStepSize) This constructor sets size and saves pointers to node/connection lists. The size of the system (i.e. the system order) is determined at construction. This means that the ODE solver must be created after all the masses and connections and such have been set up. Parameters: aStepSize Default starting step size for the solver bList Pointer to the list of bodies in the system

249

nList Pointer to the list of nodes in the system cList Pointer to the list of connections in the system fList Pointer to the list of force objects in the system

CDynaSolver::∼CDynaSolver (void) This is just a default destructor.

Member Function Documentation void CDynaSolver::computeDerivatives (void) [virtual] This function computes state derivatives

dx dt

= f (x, t).

This method may be overridden to create a custom class, but the overriding method must call CDynaSolver::computeDerivatives() (p. 250). Reimplemented from CBaseSolver (p. 219).

void CDynaSolver::setLists (list< CBody ∗> ∗ bList, list< CNode ∗> ∗ nList, list< CConnection ∗> ∗ cList, list< CBaseForce ∗> ∗ fList, list< CBaseSurface ∗> ∗ sList) Method gives the solver pointers to the dynamic system’s entity lists. Once the solver has pointers to these lists, it uses them to attach its pointers to the system’s states and state derivatives to its own pointer lists. The documentation for this class was generated from the following files: • solvers.h • solvers.cpp

B.14

CDynaSystem Class Reference

This class implements the top level of a dynamic system simulation. #include Inheritance diagram for CDynaSystem::

250

Public Methods • CDynaSystem (char ∗, extConnType ect=DUMMY MASSES) • ∼CDynaSystem (void) • void addBody (CBody ∗) • void addNode (CNode ∗) • void addForce (CBaseForce ∗) • void addConnection (CConnection ∗) • void addSurface (CBaseSurface ∗) • int getNumBodies (void) • int getNumNodes (void) • int getNumConnections (void) • int getNumForces (void) • extConnType getExternalConnectionType (void) • void setInternalSpringConstant (double con) • double getInternalSpringConstant (void) • int getNumStates (void) • void initialize (CDynaSolver ∗pSolver) • void resetToInitialConditions (void) • double runSimulationStep (double) • long getStateFuncRuns (void) • long getODEstepsTaken (void) • void writeMomentums (ostream &, const char ∗=””) • void writeNodePositions (ostream &, const char ∗=””) • void writeConnectionPositions (ostream &, const char ∗=””) • void setStateFile (char ∗) • double getSimTime (void)

251

Friends • ostream& operator &vec) • virtual ∼CEulerAngles X Y Z (void) • virtual void setFromRotationMatrix (const CMatrix< 3, 3, double > &rot) • virtual CMatrix getRotationMatrix (void)

Friends • ostream& operator & rot) This constructor constructs an angle set given a rotation matrix. It calls its method setFromRotationMatrix() (p. 258), which finds Euler angles from the rotation matrix. Parameters: rot The rotation matrix which is to be put into the angle set

CEulerAngles X Y Z::CEulerAngles X Y Z (double a1, double a2, double a3) Construct an angle set given a set of three angles. The angles are just dumped into the angle vector by the parent constructor. Parameters: a1 The initial value of the first angle a2 The initial value of the second angle a3 The initial value of the third angle

257

CEulerAngles X Y Z::CEulerAngles X Y Z (CVector< 3, double > & vec) This constructor creates an angle set given a vector of angles. Parameters: vec A vector containing the three initial angles

CEulerAngles X Y Z::∼CEulerAngles X Y Z (void) [virtual] The default destructor frees up memory used by the angle object.

Member Function Documentation CMatrix< 3, 3, double > CEulerAngles X Y Z::getRotationMatrix (void) [virtual] This method returns a rotation matrix corresponding to this angle set. It uses a formula from Wertz, J.R. et al, Spacecraft Attitude Determination and Control, Reidel, 1978. Returns: A new rotation matrix which corresponds to the given angle set Reimplemented from CBaseEulerAngles (p. 205).

void CEulerAngles X Y Z::setFromRotationMatrix (const CMatrix< 3, 3, double > & rot) [virtual] This method computes the Euler angles from a rotation matrix. The formulae are taken from Wertz, J.R. et al, Spacecraft Attitude Determination and Control, Reidel, 1978. Parameters: rotmat The rotation matrix from which the angles are found Reimplemented from CBaseEulerAngles (p. 206).

Friends And Related Function Documentation ostream & operator &rot) • CEulerAngles Z X Z (double, double, double) • CEulerAngles Z X Z (CVector< 3, double > &vec) • virtual ∼CEulerAngles Z X Z (void) • virtual void setFromRotationMatrix (const CMatrix< 3, 3, double > &rot) • virtual CMatrix getRotationMatrix (void)

Friends • ostream& operator & rot) This constructor constructs an angle set given a rotation matrix. It must call its method setFromRotationMatrix() (p. 261), which performs Guenther’s fancy calculations to find the Euler angles from the rotation matrix. Parameters: rot The rotation matrix which is to be put into the angle set

CEulerAngles Z X Z::CEulerAngles Z X Z (double a1, double a2, double a3) Construct an angle set given a set of three angles. The angles are just dumped into the angle vector by the parent constructor. Parameters: a1 The initial value of the first angle a2 The initial value of the second angle a3 The initial value of the third angle

260

CEulerAngles Z X Z::CEulerAngles Z X Z (CVector< 3, double > & vec) This constructor creates an angle set given a vector of angles. Parameters: vec A vector containing the three initial angles

CEulerAngles Z X Z::∼CEulerAngles Z X Z (void) [virtual] The default destructor frees up memory used by the angle object.

Member Function Documentation CMatrix< 3, 3, double > CEulerAngles Z X Z::getRotationMatrix (void) [virtual] This method returns a rotation matrix corresponding to this angle set. It uses a formula from Wertz, J.R. et al, Spacecraft Attitude Determination and Control, Reidel, 1978. Returns: A new rotation matrix which corresponds to the given angle set Reimplemented from CBaseEulerAngles (p. 205).

void CEulerAngles Z X Z::setFromRotationMatrix (const CMatrix< 3, 3, double > & rot) [virtual] This method computes the Euler angles from a rotation matrix. It uses a formula from Wertz, J.R. et al, Spacecraft Attitude Determination and Control, Reidel, 1978. Parameters: rotmat The rotation matrix from which the angles are found Reimplemented from CBaseEulerAngles (p. 206).

Friends And Related Function Documentation ostream & operator &rot) • CEulerAnglesXwYwZw (double, double, double) • CEulerAnglesXwYwZw (CVector< 3, double > &vec) • virtual ∼CEulerAnglesXwYwZw (void) • virtual void setFromRotationMatrix (const CMatrix< 3, 3, double > &rot) • virtual CMatrix getRotationMatrix (void)

Friends • ostream& operator & rot) This constructor constructs an angle set given a rotation matrix. It must call its method setFromRotationMatrix() (p. 264), which performs Guenther’s fancy calculations to find the Euler angles from the rotation matrix. Parameters: rot The rotation matrix which is to be put into the angle set

CEulerAnglesXwYwZw::CEulerAnglesXwYwZw (double a1, double a2, double a3) Construct an angle set given a set of three angles. The angles are just dumped into the angle vector by the parent constructor. Parameters: a1 The initial value of the first angle a2 The initial value of the second angle a3 The initial value of the third angle

CEulerAnglesXwYwZw::CEulerAnglesXwYwZw (CVector< 3, double > & vec) This constructor creates an angle set given a vector of angles. Parameters: angs A vector containing the three initial angles

263

CEulerAnglesXwYwZw::∼CEulerAnglesXwYwZw (void) [virtual] The default destructor frees up memory used by the angle object.

Member Function Documentation CMatrix< 3, 3, double > CEulerAnglesXwYwZw::getRotationMatrix (void) [virtual] This method returns a rotation matrix corresponding to this angle set. It uses a fairly simple formula found by Guenther to do it. Returns: A new rotation matrix which corresponds to the given angle set Reimplemented from CBaseEulerAngles (p. 205).

void CEulerAnglesXwYwZw::setFromRotationMatrix (const CMatrix< 3, 3, double > & rot) [virtual] This method computes the Euler angles from a rotation matrix. That’s where the real meat and potatoes of this class is anyway. Guenther’s method is used to find the angles. Parameters: rotmat The rotation matrix from which the angles are found Reimplemented from CBaseEulerAngles (p. 206).

Friends And Related Function Documentation ostream & operator, char ∗anm=””)

266

• CGeneralBody (CDynaSystem &, CPosition, double, CVector< 3 >, char ∗anm=””) • ∼CGeneralBody (void) • CGeneralBody& operator= (const CGeneralBody &) • void arrangeMasses (double, CVector< 3 >, const CPosition &)

Protected Attributes • CMatrix inertiaTensor

Detailed Description This class implements any old general-purpose rigid body. The mass properties of the body (mass, inertia tensor) are given to the constructor and a body which has the appropriate properties is constructed from four masses and some virtual duct tape and bailing wire.

Constructor & Destructor Documentation CGeneralBody::CGeneralBody (void) The default constructor creates an empty body object.

CGeneralBody::CGeneralBody (CGeneralBody & abod) The copy constructor creates a body which is a copy of another body. Parameters: abod Reference to the body which is to be duplicated

CGeneralBody::CGeneralBody (CDynaSystem & asys, CPosition aCGpos, double amass, CMatrix< 3, 3 > ainert, char ∗ anm = ””) This constructor creates a body in the given position and attitude. Given a dynamic system, location for a body, mass, and inertia tensor, it creates a set of masses and connections which simulate that body. Parameters: asys Reference to the dynamic system in which the body is created aCGpos Position at which the new body’s center of mass will be amass The total mass of the new body

267

ainert The inertia tensor for the new body anm The new body’s name in a character string

CGeneralBody::CGeneralBody (CDynaSystem & asys, CPosition aCGpos, double amass, CVector< 3 > princin, char ∗ anm = ””) This constructor creates a body in the given position and attitude. It works in the same way as the general-inertia constructor above, except it is given a vector containing the three principal moments of inertia; the body is thus aligned to the coordinate axes. If the body comes out at some funny angle, one can create it and then rotate it. Parameters: asys Reference to the dynamic system in which the body is created aCGpos Position at which the new body’s center of mass will be amass The total mass of the new body princin The principal inertias Ixx , Iyy , and Izz anm The new body’s name in a character string

CGeneralBody::∼CGeneralBody (void) The destructor frees memory used by the body.

Member Function Documentation void CGeneralBody::arrangeMasses (double desiredMass, CVector< 3 > pmoi, const CPosition & CGpos) This function arranges body masses to match the given rotational inertias. Parameters: desiredMass The total mass of the four main mass objects pmoi The principal moments of inertia of the four-mass arrangement CGpos Location of the center of mass of the final arrangement

CGeneralBody & CGeneralBody::operator= (const CGeneralBody & oldie) This assignment operator is used in making a copy of the body.

268

Parameters: oldie Reference to the body whose properties are to be duplicated The documentation for this class was generated from the following files: • bodies.h • bodies.cpp

B.20

CGhostPoint Class Reference

This class contains an abstract sort of point at which forces can be applied. #include Inheritance diagram for CGhostPoint::

Public Methods • CGhostPoint (void) • CGhostPoint (const CGhostPoint &) • CGhostPoint (const CPosition &, CNode ∗, CNode ∗, CNode ∗, CNode ∗, CNode ∗) • ∼CGhostPoint (void) • CGhostPoint& operator= (const CGhostPoint &) • virtual void computeStateDerivatives (void) • virtual int getNumStates (void) • virtual void addTransientForce (const CForce &frc)

Protected Attributes • CNode∗ pNodes [4] • CNode∗ pCGnode

269

Friends • ostream& operator