Multibody System Dynamics 12: 17–45, 2004. C 2004 Kluwer Academic Publishers. Printed in the Netherlands.
17
Optimal Control of Multibody Systems Using an Energy Preserving Direct Transcription Method CARLO L. BOTTASSO1 and ALESSANDRO CROCE2 1 Daniel
Guggenheim School of Aerospace Engineering, Georgia Institute of Technology, Atlanta, GA, U.S.A.; E-mail:
[email protected]; 2 Dipartimento di Ingegneria Aerospaziale, Politecnico di Milano, Milano, Italy (Received: 24 July 2003; accepted in revised form: 12 April 2004) Abstract. We consider the solution of path planning problems for multibody systems, using (a) a differential-algebraic formulation, (b) a direct transcription process, and (c) an energy-coherent temporal discretization. The differential-algebraic nature of the equations ensures that problems with arbitrary topologies can be solved with the proposed methodology. The use of a direct transcription process allows for an easy implementation of the method in existing standard multibody software with minimal modifications, since the equations of optimal control are not needed. Furthermore, we show that the governing optimal control equations are exactly recovered as the temporal mesh size tends to zero. Finally, the use of an energy preserving scheme ensures nonlinear unconditional stability in the solution of the resulting multipoint boundary value problems, which ensures superior numerical robustness to the numerical procedures. We test the proposed methodology with the help of representative examples, and we verify the second order accuracy of all computed solution fields, including positions, velocities, controls and final time. Keywords: optimal control, trajectory optimization, direct transcription, integration schemes, energy preserving schemes
1. Introduction Energy algorithms have been proposed in the recent past for achieving nonlinear unconditional stability in the temporal integration of structural systems. These schemes have been generalized to deal with differential-algebraic problems, in particular multibody systems [1–3]. For initial value problems, energy algorithms have been shown to be more robust than conventional integrators [4]. Furthermore, the numerical solution that they produce is qualitatively superior, since it inherits properties of the governing equations. A thorough review of the literature on energy methods is beyond the scope of this work, however the main relevant references can be found in the bibliography of the above cited papers. In this work, we use energy schemes for the trajectory optimization of multibody systems. In the classical dynamic analysis of such systems, one integrates forward-in-time the motion produced by given initial conditions, external loads and known control inputs. On the other hand, in the trajectory optimization case one is concerned with the computation of the controls that minimize a given cost function, while satisfying various possible constraints on the states of the system
18
C.L. BOTTASSO AND A. CROCE
and on the controls. Relevant applications can be found in aerospace and automotive engineering, robotics and biomechanics. The trajectory optimization of multibody systems leads to the solution of optimal control problems. These are boundary value problems, rather than initial value problems as in the case of the classical forward-in-time integration of the equations of dynamic equilibrium. Hence, the solution is potentially expensive to compute. Furthermore, these problems are often highly nonlinear and notoriously difficult to solve, so that robust numerical methods become of primary importance. Practical methods for optimal control are currently based on a transcription process that renders the problem finite-dimensional, followed by discrete optimization through a nonlinear programming (NLP) problem [5, 6]. In other words, instead of deriving first the equations of optimal control and then discretizing them with a suitable numerical method, the process is in effect reversed: first one discretizes the governing equations, so that the problem from infinite dimensional becomes finite dimensional; next, one optimizes the discrete problem. This approach is simpler than the one based on the derivation of the optimal control equations, since only the standard equations of dynamic equilibrium are needed. This is an important aspect when working with general nonlinear multibody systems, since the equations in this case are already quite complicated, so that the derivation of the infinite dimensional optimal control equations would represent a difficult task. Furthermore, this approach also presents a number of numerical advantages which generally make it more robust than other strategies. In this work, we propose to transcribe the equations of dynamic equilibrium of a general nonlinear multibody system using an energy preserving scheme, obtaining an energy-consistent finite-dimensional dynamic system that is then used for constrained optimization. This approach in effect marries together two robust procedures: the direct transcription method, with its superior characteristics in the solution of complex optimal control problem, and the energy method, which is probably one of the most appealing ways of discretizing in time the equations of dynamic equilibrium of a general mechanical system. In the context of structural dynamics problems, an energy consistent method is defined as a discretization scheme that ensures that the change in the total mechanical energy of the system within a time step is exactly equal to the work of the algorithmic external forces. This is achieved here through a two-pronged approach that caters for the two classes of elements present in a multibody system, namely body models and joint models. Each body model, e.g. a rigid body, a beam, a shell, etc., is advanced in time by means of a marching algorithm that is energy consistent, according to the previous definition. Furthermore, each joint model, e.g. a revolute joint, a prismatic joint, etc., is advanced in time in an energy consistent manner. This means, for example, that a perfect scleronomic joint exactly performs null algorithmic work within each time step. This way, since each body model is energy consistent and each joint model is also energy consistent, the global multibody
ENERGY PRESERVING OPTIMAL CONTROL
19
model obtained by assembling an arbitrary number of such models, will also be energy consistent. The numerical procedure that is here proposed has the following characteristics: a) It supports arbitrarily complex multibody systems with general topologies. We remark that a formulation for the path planning of multibody systems written solely in terms of ordinary differential equations (ODEs) would be limited to a specific choice of coordinates and, more importantly, to the case of simple tree topologies. b) It can be easily incorporated in an existing code, since it requires only minor modifications to a standard forward-dynamics software tool. Although the equations of optimal control are not needed for the implementation, a crucial advantage over other formulations when very complex systems are considered, we show in this work that the governing optimal control equations are recovered in the limit as the temporal mesh size vanishes. c) It is robust, being based on an energy method that implies unconditional numerical stability for the resulting boundary value problems. The paper is organized according to the following plan. At first, we describe the problem and introduce the relevant notation by discussing the formulation of optimal control problems. For clarity of presentation, the simpler case of systems governed by ODEs is considered in this part of the work. Next, we discuss the preservation of invariants in the numerical solution of optimal control problems. The total mechanical energy of the system is identified as the invariant that should be conserved in this case, since it leads to nonlinear unconditional stability. We then describe practical methods for the solution of general optimal control problems using nonlinear programming through a transcription process that renders the problem finite dimensional. Having described the overall approach, we then specialize the treatment to the case of nonlinear multibody systems. After introducing the governing equations, we formulate the problem of optimal control for systems governed by differential algebraic equations (DAEs). We show the properties of energy preservation and of null constraint work for multibody systems, which are then to be mimicked at the discrete level to ensure unconditional nonlinear stability. A general way of introducing control variables in a multibody system is then explained, which leads to the formulation of the governing equations. Next, the design of energy preserving schemes is briefly reviewed, together with the resulting transcription process which leads to the final solving nonlinear programming problem. At this point, we show how the necessary conditions for the NLP problem converge to the optimal control equations in the limiting case of vanishing mesh size. The paper is concluded by a few applications of the proposed formulation to simple but representative example problems.
20
C.L. BOTTASSO AND A. CROCE
2. The Optimal Control Problem 2.1. FORMULATION We start by reviewing the formulation of optimal control problems and their numerical solution, in order to introduce the relevant concepts and notation. This material is standard and is covered by numerous text books, for example [7] for optimal control theory and [5] for numerical methods based on nonlinear programming concepts. For the sake of simplicity, we consider first the case of a dynamical system governed by a set of ordinary differential equations, expressed as y˙ − f ( y, u, t) = 0,
(1)
where y ∈ Rn y is the set of state variables, u ∈ Rn u are the controls, while f : Rn y × Rn u × R+ → Rn y and t ∈ I , I = [T0 , T ]. Here and in the following, we use ˙ = d(·)/dt and (·),t = ∂(·)/∂t. the notation (·) The problem of optimal control is to determine the states y, the controls u and possibly the final time T that minimize a cost function T L( y, u, t) dt, (2) J = φ( y, u, t)|TT0 + T0
where the first term represents an initial-final cost and the second an integral cost. The minimization problem is subjected to the state Equations (1), and various possible additional constraints as required by the problem at hand, such as for example boundary conditions ψ( y(T0 )) ∈ [ψ 0min , ψ 0max ],
(3a)
ψ( y(T )) ∈ [ψ Tmin , ψ Tmax ],
(3b)
constraints on states and controls g( y, u, t) ∈ [g min , g max ], integral conditions on states and controls T h( y, u, t) dt ∈ [hmin , hmax ],
(4)
(5)
T0
and upper and lower bounds y ∈ [y min , y max ],
(6a)
u ∈ [umin , umax ].
(6b)
ENERGY PRESERVING OPTIMAL CONTROL
21
For generality, all these conditions are expressed as inequality constraints in the form x ∈ [xmin , xmax ], i.e. xmin ≤ x ≤ xmax . Equality constraints are enforced by simply selecting xmin = xmax . More in general, the total range I can be possibly broken into sequential phases p p p+1 I p = [T0 , T p ], p = (0, P − 1) with events T0 and T p , such that T p = T0 , p = (0, P − 2). In each phase, different system governing equations and/or constraints might apply. Furthermore, the final time and the events location may be unknown. For clarity, we consider here a single phase problem and drop the phasedependent notation. However, all the following discussion can be easily extended to the more general multiphase case. Optimality is determined by first defining an augmented performance index, obtained by adjoining the system governing equations (1) and constraints (3a–5) to the performance index (2) through the use of Lagrange multipliers. Next, the stationarity of the augmented index is imposed, resulting in the definition of a set of differential equations and associated boundary conditions [7]. For example, considering for the sake of clarity a simplified version of the problem with only boundary conditions at the free final time T , ψ(T ) = 0, and initial conditions y(T0 ) = y 0 , we have the augmented performance index Jˆ = (φ + ν · ψ)|T +
T
˙ dt, (L + π · ( f − y))
(7)
T0
and we set its first variation equal to zero, δ Jˆ = 0.
(8)
By defining the Hamiltonian associated with this problem as H = L + π · f,
(9)
we obtain the Euler–Lagrange equations y˙ − f = 0
(10a)
π˙ + H,y = 0,
(10b)
H,u = 0.
(10c)
Finally, setting = φ + ν · ψ,
(11)
22
C.L. BOTTASSO AND A. CROCE
we obtain the transversality equations that provide the necessary boundary conditions required to solve the problem π(T ) − ,y (T ) = 0,
(12a)
,t (T ) + H(T ) = 0,
(12b)
π(T0 ) = 0.
(12c)
By taking the scalar product of the Euler–Lagrange equations with the gradients of the Hamiltonian, it is readily proven that optimality of the solution entails the following conservation property: H˙ = H,t .
(13)
2.2. PRESERVATION
OF INVARIANTS IN THE SOLUTION OF OPTIMAL CONTROL PROBLEMS
The solution of the optimal control problem described above can imply the preservation of invariants at two distinct levels: (a) the optimal control level, and (b) the mechanical system level. At the optimal control level, we have already noted that optimality of the solution implies the conservation of the Hamiltonian, according to H˙ = H,t .
(14)
At the mechanical system level, invariants in the solution will be generated by symmetries in the system Lagrangian, according to Noether’s theorem. In particular, time translational symmetry implies that the total mechanical energy E of the system is conserved for null external loads. More in general, we have E˙ = Pe ,
(15)
where Pe is the power of the external forces. Numerical schemes that preserve at the discrete level qualitative features of the solution behave significantly better than schemes that do not enjoy the same properties. The preservation of invariants is obtained from a backward engineering process: first, one decides which discrete conservation properties one would like to include in the numerical scheme; next, the numerical scheme is constructed so that the sought-for properties can be demonstrated at the discrete solution level. This procedure should be contrasted with the more common approach of using blackbox numerical schemes. Since these general purpose algorithms have no specific knowledge of the problem being solved, they can not in general satisfy invariants of the solution, except for the limiting case of vanishing mesh size.
ENERGY PRESERVING OPTIMAL CONTROL
23
In the present optimal control framework, one could think of requiring the satisfaction or either one or both of the conservation properties (14) and (15). In deciding the best approach to pursue, we observe that the preservation of energy implies the rigorous notion of unconditional stability in the nonlinear regime. Note that classical integration schemes are usually defined as unconditionally stable based on the analysis of simple linear model problems, but clearly loose this property when applied to more general nonlinear problems. On the contrary, an energy preserving scheme designed for example for a flexible nonlinear multibody system, guarantees unconditional stability for an arbitrarily complex motion of an arbitrarily complex system. Given the importance of discrete energy preservation on the characteristics of the resulting numerical scheme, we decide to require the exact satisfaction at the discrete level of Equation (15), rather than Equation (14), in the solution of an optimal control problem. It is not presently known if it might be possible to devise schemes that ensure the exact preservation of both quantities. More precisely, we consider a partition of the time interval I into time steps, i.e. T0 = t0 < t1 < . . . < t N = T , I n = [tn , tn+1 ], n = (0, N − 1), where the generic time step I n is of size h = tn+1 − tn . Here and in the following, quantities associated with the generic element vertex j are indicated using the notation (·) j , while quantities associated with the generic element k are labelled (·)k . Our goal in this work is to develop a methodology for the solution of optimal control problems in multibody dynamics such that the following discrete version of Equation (15) holds, E n+1 − E n = Peh , h
(16)
where E n , E n+1 are the values of the total mechanical energy of the system at times tn , tn+1 , respectively, and Peh is the power of the algorithmic external forces within the step. The proposed way of achieving this result is described in the following pages. 2.3. N UMERICAL
SOLUTION OF OPTIMAL CONTROL PROBLEMS
There are basically two alternative strategies [6] for the solution of the optimal control problem discussed in the previous section. In the indirect approach, one first derives the optimal control equations, i.e. the state, adjoint and control equations, together with the transversality conditions. This defines an infinite-dimensional nonlinear multipoint boundary value problem, whose unknowns are represented by the functions y(t), u(t) and π(t). In order to solve the problem, a suitable numerical method is used for rendering the problem finite-dimensional. In the direct approach, one first discretizes and then optimizes. The discretization can be done in a number of ways, e.g. by multiple shooting or by transcription of the equations of dynamic equilibrium. With the latter approach, the system
24
C.L. BOTTASSO AND A. CROCE
dynamic Equations (1) are discretized using a suitable numerical method; this also implies a discretization of the associated states and controls. All the remaining problem ingredients, i.e. cost function, constraints and boundary conditions, are now expressed in terms of the discrete values of the states and controls. This defines a finite-dimensional nonlinear programming problem [8]. The discrete unknowns x ∈ Rn x , that include discretized states and controls of the original infinite-dimensional problem, are determined so as to minimize the scalar objective function J d = J d (x), while satisfying the constraints φ(x) ∈ [φmin , φmax ],
(17)
φ : Rn x → Rn φ . Equation (17) includes the discretized system dynamic equations, the discretized constraints and the boundary conditions. This method offers some advantages over multiple shooting. First, path and control constraints are trivially enforced, since the corresponding degrees of freedom are directly available, being defined by the discretization of the system dynamic equations. This also means that path and control constraints can be imposed with an accuracy that is compatible with the accuracy of the discretization scheme. Second, unstable modes in the solution typically do not lead to catastrophic results as in shooting methods, since the appropriate boundary conditions are directly enforced. In fact, these methods are indeed two-point boundary value solvers and therefore less prone to instabilities. Here again, necessary conditions for a constrained optimum are obtained, similarly to the case of optimal control, by combining the objective J d with the constraints through the use of Lagrange multipliers, and imposing the stationarity of the augmented cost function. The Lagrangian for this discrete problem is defined as Ld = J d − σ · φ,
(18)
where σ ∈ Rn φ . The gradients of Ld with respect to the unknowns x and σ yield the necessary conditions for optimality. Large NLP problems can be solved efficiently by sequential quadratic programming (SQP) methods [9] or Interior Point (IP) methods [10]. The direct approach has at least three important advantages over the indirect method, and for this reason is currently the method of choice, especially for large, complex applications. First, the indirect method requires the derivation of the optimal control equations. This can be a difficult task for complex systems, and in particular for the general multibody problems that are the goal of this research. General purpose multibody codes are typically formulated for the solution of initial value problems and therefore do not implement the necessary equations for the indirect approach. Given the complexity of modern industrial level multibody software, the implementation of the necessary modifications for the indirect approach
ENERGY PRESERVING OPTIMAL CONTROL
25
represents a difficult task and might seem to discourage the use of these techniques in large software packages. A second disadvantage of the indirect method resides in the necessity of providing suitable starting guesses for all variables, including the system states y and adjoint variables π (costates). The latter in general might not have a clear physical meaning as in the case of the states, and can therefore be difficult to initialize, in practice prejudicing the convergence of the nonlinear iterative process. A third major issue with the indirect approach is due to the need to define a priori the constrained and unconstrained subarcs for problems with state inequalities. This severely undermines the generality of the method. The direct approach can avoid all the above mentioned drawbacks of the indirect method. First, it does not require the derivation of the optimal control equations. In fact, it directly discretizes the system governing equations of dynamic equilibrium, which is exactly what multibody codes do for solving initial value problems. Second, guesses of the adjoint variables are usually not needed. Third, constrained and unconstrained subarcs are automatically computed by the NLP solver, through the determination of the active constraint set. For all the above mentioned reasons, the direct transcription process is the method of choice for the present work. 3. Optimal Control of Multibody Systems In this section we review the formulation of the semidiscrete equations of dynamic equilibrium for multibody systems. Next, we derive the infinite dimensional optimal control equations. Finally, we describe the introduction of control variables in the multibody equations of motion, in a way that is compatible with standard forward-dynamics implementations of the software and that is at the same time as general as possible. All these results will be used in the next section to formulate and analyze the proposed energy preserving direct transcription process. 3.1. EQUATIONS
OF DYNAMIC EQUILIBRIUM OF MULTIBODY SYSTEMS
The multibody dynamics analysis is here cast within the framework of nonlinear finite element methods, and the element library includes rigid and deformable bodies as well as joint elements, including unilateral contact conditions. The formulations of beams and shells are geometrically exact, i.e. they account for arbitrarily large displacements and finite rotations, but are limited to small strains, and the resulting equations are discretized in space using the finite element method. The equations of equilibrium are written in a Cartesian inertial frame, and constraints are modeled using the Lagrange multiplier technique. This leads to systems of equations that are highly sparse, although not of minimal size. This approach can treat arbitrarily complex topologies.
26
C.L. BOTTASSO AND A. CROCE
In general, the equations of dynamic equilibrium of such systems, after spatial discretization of the flexible components using the finite element method, can be written as d(Mw) + f + c,q λ − f e = 0, dt N q˙ − w = 0,
(19b)
c = 0,
(19c)
(19a)
where q ∈ Rn s are the generalized coordinates, w ∈ Rn s the velocities, p = M(q)w : Rn s × Rn s → Rn s the system momenta, f (q, w) : Rn s × Rn s → Rn s the discretized internal and inertial forces, f e (q, w, t) : Rn s × Rn s × R+ → Rn s the external forces, c(q, t) : Rn s × R+ → Rn c the holonomic constraints that model the mechanical joints of the system, and finally λ ∈ Rn c the associated Lagrange multipliers. The generalized coordinates are composed of positions d ∈ Rnl and rotation parameters r ∈ Rnr , q = (d; r ), n l + n r = n s . The notation = (♣; ♠) represents the stacking of the column vectors ♣ and ♠ in the column vector . Similarly, the generalized velocities are w = (v; ω), where v ∈ Rnl are the linear velocities and ω ∈ Rnr the angular velocities. Then matrix N is defined as I 0 , (20) N= 0 S where S is a matrix that for each node in the system relates the time rates of the rotation parameters r to the angular velocities ω. The case of nonholomic constraints is not covered here for the sake of a clearer presentation but can be easily addressed. Energy preservation is readily proven for such systems by dot multiplying the first equation by the generalized velocities, to get w·
d(Mw) + f + c,q λ − f e dt
= E˙ − λ · c,t − f e · w,
(21a)
= E˙ − Pc − Pe ,
(21b)
= 0,
(21c)
where Pc = λ · c,t is the power of the rheonomic constraints and Pe = f e · w the power of the external forces, and where we have made use of the fact that T c˙ = c,q w + c,t ,
= 0.
(22a) (22b)
27
ENERGY PRESERVING OPTIMAL CONTROL
3.2. O PTIMAL
CONTROL EULER- LAGRANGE EQUATIONS FOR MULTIBODY SYSTEMS
Equations (19a–c) can be written in a more compact fashion as d (By) − b(y, λ, u) = 0, dt c(y) = 0,
(23a) (23b)
where
M B= 0
0 , N
b = (− f − c,q λ + f e ; w),
(24)
and the state vector is now defined as y = (w; q). The exact form and nature of the controls u will be more precisely specified later on. The Euler–Lagrange equations for the problem of optimal control governed by Equations (23a, b) can now be obtained as in the ODE case, by simply augmenting the performance index as
T
Jˆ = (φ + ν · ψ)|T +
T0
d L + π · b − (By) + ρ · c dt, dt
(25)
where π and ρ are Lagrange multipliers and by setting its first variation equal to zero. By defining the Hamiltonian associated with this problem as H = L + π · b + ρ · c,
(26)
we obtain the Euler–Lagrange equations d (By) − b = 0, dt c = 0,
(27b)
+ H,y = 0,
(27c)
b,λ π = [c,q |0]π = 0,
(27d)
H,u = 0.
(27e)
BδT π˙
(27a)
Equations (27a, b) are the dynamic equilibrium equations for the differential algebraic problem; Equation (27c) is the ordinary differential equation in the costates π that corresponds to Equation (10b) in the ODE case, and where δ(By) = Bδ δ y; Equation (27d) states that the costates π are constrained by the presence of the conditions c = 0, as expected; finally, Equation (27e) defines the stationarity of the Hamiltonian with respect to the controls.
28
C.L. BOTTASSO AND A. CROCE
Similarly, transversality conditions that generalize Equations (12a–c) are obtained to complement the problem. 3.3. CONTROLS
IN MULTIBODY SYSTEMS
In order to be used for the solution of optimal control problems, Equations (19a–c) have to be complemented by suitable definitions of the controls u ∈ Rn u . In general, controls can be included in the definition of the external forces f e , which become f in this case f e (q, w, u f , t) : Rn s × Rn s × Rn u × R+ → Rn s . It is however useful to associate controls also with internal forces in the joints; for example, one might want to assume as unknown control variable the torque applied by a motor at a revolute joint. This can be done by explicitly defining the relative displacement or rotation within the joint. To this end, we consider two bodies, labeled A and B. At point P A on body A, the Cartesian body-attached frame is (P A , {eiA }i=1, 2, 3 ), and at point P B on body B the associated frame is (P B , {eiB }i=1, 2, 3 ). The position and orientation of the two frames are described with respect to a fixed frame (O, {i i }i=1, 2, 3 ), so that the position vectors of P A and P B are given by u A = P A − O and u B = P B − O, respectively. If the two bodies are rigidly connected to each other, their six relative motions, three displacements and three rotations, vanish at the connection point. If a lower pair joint connects the two bodies, one or more relative motions will be allowed. Let di be the relative displacement between the two bodies in the direction aligned with eiA , and θi the relative rotation about eiA . All lower pair constraints can be expressed by one of the following two equations: eiA · (u A − u B ) − di = 0, cos θi e Aj · ekB − sin θi ekA · ekB = 0.
(28a) (28b)
If di is a prescribed known function of time, the first equation can be interpreted as a constraint condition on the relative displacement in the joint. On the other hand, if di is regarded as a free variable, the same equation defines the unknown relative displacement in that direction. Similarly, the second equation either constrains the relative rotation if θi is prescribed, or defines the unknown relative rotation θi if this is considered as a free variable. Equations (28a) and (28b) are part of the global constraint equations c = 0 in the system (19a–c). The Lagrange multiplier used in Equation (19a) to enforce (28a) or (28b) for a joint, generates the internal joint force associated with the respective relative displacement or rotation. The explicit definition of the relative displacements and rotations in a joint as additional unknown variables represents an important detail of the implementation of a multibody code. First of all, generic spring and/or damper elements can be introduced in the joints. Second, the time histories of joint relative motions can be driven according to suitably specified time functions. Third, in the optimal control
29
ENERGY PRESERVING OPTIMAL CONTROL
case of interest here, the associated Lagrange multipliers can be used as control variables. In fact, the equations of dynamic equilibrium of multibody systems with controls can be written as d(Mw) + f + cλ,q λ + cu,q uc − f e (q, w, u f , t) = 0, dt N q˙ − w = 0, λ
(29a) (29b)
c = 0,
(29c)
c = 0,
(29d)
u
where the constraint equations were partitioned according to c = (cλ ; cu ), where cu are constraint equations of the kind (28a) or (28b) for those joints whose internal λ forces are used as controls. More precisely, cλ (q, t) : Rn s × R+ → Rn c , cu (q) : u λ Rn s → Rn c , n λc + n uc = n c , and now λ ∈ Rn c . Finally, the vector of all system f u controls can be defined and partitioned as u = (uc ; u f ), uc ∈ Rn c , u f ∈ Rn u , f u ∈ Rn u , with n uc + n u = n u . In other words, uc represent control forces in a number of given joints, while u f are all other controls that can be seen as generating externally applied loads. The formalism of Equations (29a–d) covers also the case of dynamic models of actuators. For example, one might have a dynamic model of a motor, where the control input might be the aperture of a fuel valve. This control would be part of the vector u f , and the motor states would be part of the global state vector q. Note that, by using this approach, it is never necessary to assume that the constraint equations depend on the system controls, i.e. c = c(q, u, t), since all constraints are always of the simpler form c = c(q, t). Finally, dot multiplying Equation (29a) by the generalized velocities, we get d(Mw) w· + f + cλ,q λ + cu,q uc − f e = E˙ − Pc − Pu − Pe , (30a) dt = 0, (30b) which shows that the time rate of change of the total mechanical energy of the system is equal to the sum of the power of the rheonomic constraints Pc , the power of the actuated constraints Pu = uc · cu,t , and the power of the external forces Pe . 4. Energy Preserving Transcription Process for Multibody Systems In this section we describe procedures for constructing energy preserving schemes for multibody systems in the context of optimal control problems. At first, we sketch a general approach to the derivation of energy preserving schemes. Full details on our versions of the energy scheme are given in a number of papers published in the last few years, for example [1–3, 4]. Next, we discuss the efficient actual
30
C.L. BOTTASSO AND A. CROCE
implementation of the schemes in the forward dynamics case, and we discuss how this affects the implementation of optimal control problems. Finally, we formulate the resulting NLP problem and we show the recovery of the infinite dimensional optimal control governing equations in the limit case of vanishing mesh size. 4.1. ENERGY
PRESERVING METHODS FOR MULTIBODY SYSTEMS
Energy preserving algorithms for multibody systems can in general terms be described by difference equations of the following form M n+1 w n+1 − Mn w n h n λ − f eh = 0, + f h + c,q h N n+1 q n+1 − N n q n − w h = 0, h cn+1 = 0,
(31a) (31b) (31c)
that represent discrete approximations of Equations (19a–c). In the actual implementation, velocities are eliminated in order to obtain more computationally efficient displacement-based schemes, and incremental generalized displacements are typically used in order to handle arbitrarily large motions. Notice that constraints are imposed only at the end of the step, cn+1 = 0, since the initial conditions are consistent, i.e. cn = 0. In other words, we enforce directly the constraints conditions, and not their time derivatives. This means that there is no drift effect in the satisfaction of the constraints using this formulation, an additional important nonlinear conservation property. In Equations (31a–c), λn are internal values of the Lagrange multipliers, while h h f , f eh , w h and c,q are algorithmic values of the internal and external forces, velocities and constraint Jacobians, respectively. These terms in general depend on the step-boundary values of the generalized coordinates and of the generalized velocities, i.e. they can be formally expressed as f h = f h (q n , q n+1 , w n , w n+1 ),
(32a)
f eh = f eh (q n , q n+1 , w n , w n+1 , t),
(32b)
w = w (q n , q n+1 , w n , w n+1 ),
(32c)
h
h c,q
h
=
h c,q (q n , q n+1 , t).
(32d)
h are designed so as to allow for the The algorithmic values f h , f eh , w h and c,q following proof of discrete energy preservation: Mn+1 w n+1 − Mn w n h h h n h + f + c,q λ − f e (33a) w · h E n+1 − E n = − Pch − Peh = 0, (33b) h
31
ENERGY PRESERVING OPTIMAL CONTROL
which is clearly a discrete version of Equations (21a–c). The details of such process are beyond the scope of this paper and can be found in the cited references. It is however interesting to remark that the algorithmic constraint Jacobians are designed so as to guarantee that no work is performed at the discrete solution level by timeindependent constrains. In other words, it must be possible to prove that cn+1 − cn , h = 0,
T
h c,q w h + c,th =
(34a) (34b)
which is clearly a discrete version of Equation (22a), since in this case one can show that Equation (33b) indeed holds. This format can be easily adapted to the case of multibody systems with controls. In fact, the discrete equations of dynamic equilibrium become in this case M n+1 w n+1 − Mn w n n u,h n h + f h + cλ,h ,q λ + c,q u − f e = 0, h N n+1 q n+1 − N n q n − w h = 0, h cλn+1 = 0, cun+1
(35a) (35b) (35c)
= 0,
(35d)
that represent discrete approximations of Equations (29a–d). The discrete energy preservation statement becomes now
Mn+1 w n+1 − Mn w n n u,h n h + f h + cλ,h ,q λ + c,q u − f e h E n+1 − E n − Pch − Puh − Peh = 0, = h
wh ·
(36a) (36b)
which approximates Equations (30a–b). Notice that the controls u are algebraic variables as the Lagrange multipliers λ. Furthermore, boundary conditions can not be associated with controls. Consequently, when controls are regarded as unknowns in trajectory optimization problems, they are chosen as element internal values, rather than nodal values as the states q and w. This is reflected in the notation un , which indicates the unknown control value at the generic time element n. Although it is perfectly appropriate to evaluate the controls at the time nodes, or even at other points, in forward dynamics, using nodal unknown controls for the solution of boundary value problems as the one here considered, gives highly oscillatory solutions and it is therefore not recommended.
32
C.L. BOTTASSO AND A. CROCE
4.2. I MPLEMENTATION
ISSUES
Given the complexity of a general purpose multibody program, it is important to minimize the modifications to an existing initial value code for covering the boundary value case. Furthermore, to avoid code duplication, it is also important to develop one single software tool that is applicable to both the direct and the inverse dynamics scenarios. As previously noted, the actual forward-dynamics implementation of the discrete equations of equilibrium for a multibody system requires some slight but important modifications to Equations (31a–c). These modifications have to be reflected in the implementation of the code, and are therefore briefly described here. First of all, it is usually convenient to use an incremental formulation within each time step, so that the unknown displacements and unknown rotations at time tn+1 are chosen as relative increments from the known initial values at time tn . This way one can represent arbitrarily large rotations without incurring in singularities. The relation between the absolute unknowns q n+1 and the incremental ones z n+1 can be formally expressed as q n+1 = q n+1 (q n , z n+1 ),
(37)
where, by the incremental nature of z n+1 , z n+1 = 0 if q n+1 = q n . The exact specific form of Equation (37) will depend on the choice of the rotation parameters. Furthermore, the velocities at the end of the time step, w n+1 , can be eliminated from Equation (31a) using Equation (31b), in order to have a nonlinear iteration scheme of smaller size in the sole unknown displacement increments and unknown Lagrange multipliers. The velocities w n+1 are then recovered, once reached a converged solution for the remaining variables at the current time step through Equation (31b), which is expressed in explicit form for this purpose as w n+1 = w n+1 (q n , q n+1 , w n ).
(38)
This is now a vector equation, and consequently no further matrix solution is necessary for computing the final velocities. Considering these two aspects of the implementation, the forward-in-time solution is obtained by using Equation (31a) and Equation (31c) expressed in terms of incremental unknowns after the elimination of the final velocities, which can therefore be written in compact form as ξ h (q n , w n , z n+1 , λn , un ) = 0,
(39)
where ξ h : Rn s × Rn s × Rn s × Rn c × Rn u → Rn s +n c . These equations are solved n[k] by Newton method in the unknown increments dz [k] at the k-th iteration, n+1 , dλ
33
ENERGY PRESERVING OPTIMAL CONTROL
which can be written as
h h [k] dz [k] n+1 ξ ,z |ξ ,λ + ξ n[k] = 0, n[k] dλ
(40)
h h , ξ ,λ are the system Jacobians with respect to z n+1 and λn , respectively. where ξ ,z Once at convergence, the absolute generalized displacements are recovered through Equation (37), and the final velocities are computed through Equation (38).
4.3. D IRECT
TRANSCRIPTION
The discrete energy-coherent equations of dynamic equilibrium for multibody systems are now used for deriving a direct transcription process for the solution of optimal control problems. To this end, Equations (37), (38), and (39) can be expressed in compact form as a set of residual equations ζ h (q n , q n+1 , z n+1 , w n , w n+1 , λn , un ) = 0,
n = (0, N − 1),
(41)
where ζ h : Rn s × Rn s × Rn s × Rn s × Rn s × Rn c × Rn u → R3n s +n c . The NLP problem is now defined as follows. First, the NLP variables x are chosen as: x = q n=(0,N ) ; z n=(1,N ) ; w n=(0,N ) ; λn=(0,N −1) ; un=(0,N −1) ; T − T0 ,
(42)
i.e. they are defined as the discrete state, multiplier and control values within each step (and, possibly, time). Next, the discretized energy-coherent DAEs within each step, Equation (41), become a set of nonlinear NLP constraints. All other problem constraints and bounds, Equations (3a–6b), are expressed in terms of the NLP variables and are appended as linear, nonlinear or bound NLP constraints, as appropriate. Finally, the cost function (2) is expressed in terms of the discrete problem unknowns, x. The resulting problem is large but very sparse and can be solved efficiently by SQP or IP methods. The former approach was adopted in this work. The solution of the NLP problem requires the computation of the Jacobians of the cost function and of the constraints with respect to the unknowns. This can be done by sparse finite differencing [11]. Alternatively, a very significant performance increase can be obtained if the coding of the Jacobian is feasible. The most complicated terms of the constraint Jacobians are those involving the equilibrium Equations (39), but these are in part already available, since they are required for the forward-dynamics solution, Equation (40). The initial guess for the NLP problem is here obtained by guessing values of the control variables, and integrating forward-in-time Equations (37–39). This way, all constraint equations corresponding to the transcription of the system equilibrium
34
C.L. BOTTASSO AND A. CROCE
conditions are automatically satisfied by the initial guess. Furthermore, it is usually convenient to start from rather crude temporal discretizations. In fact, on a coarse grid certain details of the solution will not be captured, and this will usually imply a faster convergence of the NLP problem, especially if the initial guess is poor, i.e. the tentative solution is far from the converged one. If a fine grid is used starting from a poor initial guess, the fine details captured by the grid will tend to slow down or even prevent convergence. An effective way of addressing this issue is the use of a refinement procedure. At first, an initial guess is computed on a crude grid, and the corresponding NLP problem is solved. The computed solution is then projected onto a finer grid and used as initial guess for the subsequent NLP problem. The procedure is continued until sufficient grid refinement has been achieved to yield converged results. Local grid refinement based on a posteriori error estimates, rather than uniform grid refinement, can be used to optimize the use of computational resources. This feature however has not yet been implemented in the present code. 4.4. C ONVERGENCE
OF THE DISCRETE OPTIMALITY CONDITIONS TO THE INFINITE DIMENSIONAL CASE
Using this approach, the optimality conditions of the resulting discrete NLP problem converge to the optimality conditions of the optimal control problem as the grid is refined (h → 0) and the number of discrete optimization variables goes to infinity (N → ∞), similarly to what was shown for the ODE case in [5, 12]. To avoid unnecessary complications, we can neglect in the proof of convergence the previously discussed introduction of incremental unknowns and the elimination of final velocities, since these operations simply amount to local changes of variables. Therefore, Equations (31a–c) defined over the complete range I are now written in compact form as Bn+1 y n+1 − Bn y n − bh (y n , y n+1 , λn , un ) = 0, n = (0, N − 1), h cn (y n ) = 0, n = (0, N ),
(43a) (43b)
where here again y n = (w n ; q n ). If J d (x) is the problem cost function expressed in terms of the discrete unknowns x, the Lagrangian of the discrete problem can be written as Ld = J d −
N −1 n=0
σn ·
N Bn+1 y n+1 − Bn y n − bh + τ n · cn , h n=0
(44)
where we are considering only the constraints arising from the discrete equilibrium equations and not all additional constraints on the states and controls, for the sake of simplicity.
35
ENERGY PRESERVING OPTIMAL CONTROL
Stationarity of the Lagrangian with respect to the problem unknowns yields the necessary conditions for optimality, which write Ld,−σn =
Bn+1 y n+1 − Bn y n − bh = 0, h Ld,τ n = cn = 0, σ n − σ n−1 d + H,y = 0, n h h n h n = b,λ n σ = [c ,q | 0]σ = 0,
T Ld,y n = Bδ,n
Ld,λn
Ld,un
=
d H,u n
= 0,
n = (0, N − 1),
(45a)
n = (0, N ),
(45b)
n = (1, N − 1),
(45c)
n = (0, N − 1),
(45d)
n = (0, N − 1).
(45e)
These equations represent discrete approximations of the infinite dimensional equations of optimal control, i.e. Equations (27a–e). Furthermore, we have set Hd = J d +
N −1
σ n · bh +
n=0
N
τ n · cn ,
(46)
n=0
which is a discrete Hamiltonian that approximates Equation (26). In particular, it is clear that the NLP Lagrange multipliers σ n approximate the optimal control costates π (and similarly for the discrete multipliers τ n that approximate the multipliers ρ). Note also that, since the multipliers σ n are associated with time elements rather than element vertices, the difference Equations (45c) are vertex-centered, rather than element-centered as in Equation (43a). 5. Numerical Examples In this section, we test the proposed methodology on a few simple but representative example problems. The first two problems are used for debugging purposes and for demonstrating the correct implementation and the convergence properties of the procedures, offering comparisons with analytical or previously published results. 5.1. FREE
FINAL TIME PARTICLE TRANSFER
We consider the free final time transfer of a particle [7]. The horizontal and vertical positions of the particle are labeled x and y, while the velocity components are u and v. A constant force a acts upon the particle and forms an angle β with the horizontal. The equations of motion write x˙ = u,
(47a)
y˙ = v,
(47b)
u˙ = a cos β,
(47c)
v˙ = a sin β.
(47d)
36
C.L. BOTTASSO AND A. CROCE
Figure 1. Free final time particle transfer. At left, problem formulation; at right, multibody implementation of the problem.
The problem is to find the control β that minimizes the final time T subjected to the boundary conditions x(T0 ) = 0, u(T0 ) = 0,
y(T0 ) = 0, v(T0 ) = 0,
and y(T ) = Y,
v(T ) = 0,
u(T ) = U,
with Y and U fixed. For this example we consider Y = 100, U = 12.21 and aY /U 2 = 0.75, which corresponds to an initial value of the control β(T0 ) = 75 degree. The analytical solution for this problem is easily computed and can be found in [7]. The problem is modeled with the multibody code using a rigid body. This body is connected to a prismatic joint, which in turn is connected to a revolute joint. Finally, the revolute joint is connected to the ground. The resulting system is depicted in Figure 1. The axis of the revolute joint is orthogonal to the axis of the prismatic joint. This leaves only two degrees of freedom to the three-dimensional body, which effectively becomes a particle constrained to move on a plane. Finally, a follower force of constant value a is applied to the body. The relative rotation in the revolute joint becomes the control variable and plays the role of β in the previous equations. Although in its standard formulation, as given by Equations (47a–d), the problem is governed by a set of ODEs and not DAEs, modeling it by means of a constrained rigid body makes it a standard index 3 holonomically constrained multibody problem, albeit a simple one. This offers a first testing problem for the new procedures. Figures 2–4 show the time history of the analytical and numerical solutions. For all quantities, analytical solutions are shown using a solid line. Figure 2 gives the particle positions. The numerically computed values x h of the horizontal position are indicated with the symbol , while the symbol is used for the component y h .
ENERGY PRESERVING OPTIMAL CONTROL
37
Figure 2. Free final time particle transfer. Time history of particle positions; , : numerical solution x h and y h , respectively; solid line: exact solution.
Figure 3. Free final time particle transfer. Time history of particle velocities; , : numerical solution u h and v h , respectively; solid line: exact solution.
38
C.L. BOTTASSO AND A. CROCE
Figure 4. Free final time particle transfer. Time history of control; : numerical solution; solid line: exact solution.
Figure 3 shows the computed velocities u h and v h , represented with the symbols and , respectively. Finally, the control β is shown in Figure 4. Here again, the numerically computed value β h is indicated with the symbol . A temporal discretization of seven equal elements was used for the solution. For the control, the symbol was plot at the midpoint of the corresponding time element, i.e. at time tn + h/2, n = (0, 6), since in this formulation the controls are step-internal unknowns. Although the mesh is very crude, very good agreement is observed for all computed variables, confirming the correct implementation of the procedures. Next, a convergence study was attempted, taking advantage of the fact that the solution is known for this problem. Progressively finer grids were used, employing 2i equal elements, i = (1, 7). The final time computed on each grid i is indicated T i . The corresponding element size is h i = T i 2−i . The results of the convergence study are shown in Figure 5, which reports the behavior of the nondimensional error of the computed quantities with respect to the number of elements T i / h i . Four curves are shown in this plot. The position error at the final time, computed as |(x h (T i ) − x(T ))/x(T )|, is shown using the symbol. The velocity error at the interval midpoint, |(v h (T i /2) − v(T /2))/v(T /2)|, is shown using the symbol. The final time error |(T i − T )/T | is plotted using the ◦ symbol. The control error, defined as |(β h − β)/β|∞,h i /2 , i.e. the error infinity norm at the step midpoints, is shown using the ∗ symbol.
ENERGY PRESERVING OPTIMAL CONTROL
39
Figure 5. Free final time particle transfer. Convergence of the computed solutions with respect to the grid size T i / h i . : position error |(x h (T i )− x(T ))/x(T )|; : velocity error |(v h (T i /2)− v(T /2))/v(T /2)|; ◦: final time error |(T i − T )/T |; ∗: control error |(β h − β)/β|∞,h i /2 ; i = (1, 7). The thick line at the lower left corner gives the slope of second order convergence.
The figure shows that the proposed optimal control scheme yields second order accuracy for all field variables, states, controls and time. These results are coherent with the accuracy of the underlying energy preserving time discretization algorithm, thereby confirming the analysis of Section 4.4. 5.2. TWO
DEGREE OF FREEDOM MANIPULATOR
As a second example problem, we consider the two degree of freedom manipulator of [13, 14]. The system topology is sketched in Figure 6. Here again, this simple system could be expressed in terms of two coupled ODEs in the two unknown
Figure 6. Multibody model of a two degree of freedom robotic manipulator.
40
C.L. BOTTASSO AND A. CROCE
relative rotations φ1 and φ2 depicted in the figure. Using the general Lagrangian formulation adopted here, the robot is modeled using two rigid bodies, each one with six degrees of freedom, and two revolute joints, one of the two being in turn connected to the ground. The relative rotations in the joints, defined by Equation (28b) as previously explained, correspond to the minimal set coordinates φ1 and φ2 . Their associated Lagrange multipliers are identified with the driving torques C1 and C2 exerted by the robot motors at the same joints. This highly redundant description of the system, although clearly not necessary for modeling such a simple system, gives the opportunity of further validating the implemented procedures against previously published results. The problem requires to compute the optimal trajectories for the cost functional J=
T
2
T0 i=1
Ci2 dt,
i.e. one seeks to minimize the control effort exerted by the actuator torques at the revolute joints throughout the whole motion. Point masses m 1 = 1.3 and m 2 = 1.6 are located at the tip of each link. Complete data for this problem can be found in the already cited references [13, 14]. The problem here considered is a fixed time one and seeks to construct a trajectory that connects two points. The robot arm begins and ends the trajectory at rest, so that the initial and final velocities are zero. The boundary conditions for this problem are specified as φ1 (T0 ) = π/6, φ2 (T0 ) = π/7, φ˙ 1 (T0 ) = 0, φ˙ 2 (T0 ) = 0, and φ1 (T ) = π/5, φ2 (T ) = π/6, φ˙ 1 (T ) = 0, φ˙ 2 (T ) = 0, where T0 = 0 and T = 0.5. Figure 7 shows the the joint rotations computed with the proposed procedures. The rotation at the ground point φ1 , is indicated with the symbol , while the symbol is used for the rotation at the arm elbow φ2 . Figure 8 shows the time histories of the controls C1 and C2 , indicated with the symbols and , respectively. Here again, in the case of the controls the plot symbols were drawn at the midpoint of the corresponding time element. The computation was initiated on a coarse five element grid. The solution on the first grid was then projected on a finer 10 element mesh and used as initial guess. The same process was then repeated to obtain the solution on a final 20 element
ENERGY PRESERVING OPTIMAL CONTROL
41
Figure 7. Two degree of freedom manipulator. Time history of rotations; , solution φ1 and φ2 , respectively.
: numerical
Figure 8. Two degree of freedom manipulator. Time history of controls; , solution C1 and C2 , respectively.
: numerical
42
C.L. BOTTASSO AND A. CROCE
Figure 9. Multibody model of a three degree of freedom robotic manipulator.
mesh. These results are in good agreement with those obtained in [14] with an indirect multiple shooting method. 5.3. THREE
DEGREE OF FREEDOM MANIPULATOR
In this example, we consider the three degree of freedom robotic manipulator of Figure 9. As for the previous problem, rigid bodies are used to model the robot links and revolute joints are used to model the robot articulations. The controls are given in this case by the three torques Ci , i = (1, 3), applied by the control motors at the three revolute joints. As previously, the control torques are identified as the Lagrange multipliers associated with the relative rotations in the joints. We seek a minimum energy transfer from an initial condition denoted by the state values φ1 (T0 ) = 0,
φ2 (T0 ) = 0,
φ3 (T0 ) = 0,
to the final condition given by φ1 (T ) = π/2,
φ2 (T ) = 0,
φ3 (T ) = 0,
with T0 = 0 and T = 1. The system starts from a condition at rest to reach another condition at rest, i.e. the initial and final velocities are null. Furthermore we set bounds on the second and third states −π/6 ≤ φ2 (t) ≤ π/6,
−π/6 ≤ φ3 (t) ≤ π/6.
The performance index is chosen here again as J=
T
3
T0 i=1
Ci2 dt.
ENERGY PRESERVING OPTIMAL CONTROL
43
Figure 10. Three degree of freedom manipulator. Time history of rotations; , , ◦: numerical solution φ1 , φ2 and φ3 , respectively.
Figure 11. Three degree of freedom manipulator. Time history of controls; , , ◦: numerical solution C1 , C2 and C3 , respectively.
44
C.L. BOTTASSO AND A. CROCE
The computed time histories of the rotations in the revolute joints and the associated controls are depicted in Figures 10 and 11, respectively. The rotation at the ground point, φ1 , is indicated with the symbol , the symbol is used for the rotation at the first elbow, φ2 , while the rotation at the second elbow, φ3 , is indicated with the symbol ◦. Figure 11 shows the time histories of the controls C1 , C2 and C3 , indicated with the symbols , and ◦, respectively. The control force C1 in the first joint produces the rotation about the robot base. The other two controls are responsible for making the end effector reach its final destination, while opposing the inertial forces produced by the fast rotation about the base joint.
6. Conclusions We have described a computational procedure based on direct transcription and nonlinear programming for solving path planning problems for multibody systems. While the basic algorithmic framework represents a well known methodology for the solution of complex optimal control problems, there are three main highlights of the present work. First, we have used energy preserving integration schemes for carrying out the temporal discretization of the underlying equations of dynamic equilibrium. Energy methods provide superior robustness and enhanced performance with respect to standard black-box integrators. In fact, energy methods are nonlinearly unconditionally stable, a property that is inherited by the overall optimal control solution strategy. These nonlinear conservation properties can be of non-negligible importance when complex multibody systems are analyzed. The use of nonlinearly stable integration schemes for the solution of boundary value problems has not been previously analyzed in the literature, to our knowledge. Second, the proposed procedures are based on the differential algebraic formulation for multibody dynamics based on the use of Lagrange multipliers. Although this has the disadvantage of increasing the number of problem unknowns with respect to minimal set ODE methods, it also has the clear advantage of being able to deal with arbitrary system topologies. Third, the approach here followed can be implemented in a standard multibody solver, originally designed and developed for the solution of classical initial value problems. Only minor modifications to the code are necessary in order to implement the procedures here described. In fact, given the complexity of general nonlinear multibody systems, it seems important to be able to avoid a complete rewriting of the software. The simple initial numerical experiments here reported are encouraging; the observed second order convergence of all field variables seems to be particularly interesting. More complex examples will be the subject of a forthcoming application-oriented work.
ENERGY PRESERVING OPTIMAL CONTROL
45
References 1. Bottasso, C.L., Borri, M. and Trainelli, L., ‘Integration of elastic multibody systems by invariant conserving/dissipating schemes. Part I: formulation’, Computer Methods in Applied Mechanics and Engineering 190, 2001, 3669–3699. 2. Bottasso, C.L., Borri, M. and Trainelli, L., ‘Integration of elastic multibody systems by invariant conserving/dissipating schemes. Part II: numerical schemes and applications’, Computer Methods in Applied Mechanics and Engineering 190, 2001, 3701–3733. 3. Bottasso, C.L. and Bauchau, O.A., ‘On the design of energy preserving and decaying schemes for flexible, nonlinear multibody systems’, Computer Methods in Applied Mechanics and Engineering 169, 1999, 61–79. 4. Bauchau, O.A., Bottasso, C.L. and Trainelli, L., ‘Robust integration schemes for flexible multibody systems’, Computer Methods in Applied Mechanics and Engineering 192, 2003, 395–420. 5. Betts, J.T., Practical Methods for Optimal Control Using nonlinear Programming, SIAM, Philadelphia, 2001. 6. Betts, J.T., ‘Survey of numerical methods for trajectory optimization’, Journal of Guidance, Control and Dynamics 21, 1998, 193–207. 7. Bryson, A.E. and Ho, Y.C., Applied Optimal Control, Wiley, New York, 1975. 8. Gill, P.E., Murray, W. and Wright, M.H., Practical Optimization, Academic Press, London and New York, 1981. 9. Barclay, A., Gill, P.E. and Rosen, J.B., ‘SQP methods and their application to numerical optimal control’, Report NA 97–3, Department of Mathematics, University of California, San Diego, CA, 1997. 10. Renegar, J., A Mathematical View of Interior Point Methods in Convex Optimization, SIAM, Philadelphia, 2001. 11. Betts, J.T. and Huffman, W.P., ‘Application of sparse nonlinear programming to trajectory optimization’, Journal of Guidance, Control and Dynamics 15, 1992, 198–206. 12. Hull, D.G., ‘Conversion of optimal control problems into parameter optimization problems’, Journal of Guidance, Control and Dynamics 20, 1997, 57–60. 13. Agrawal, S.K. and Fabien, B.C., Optimization of Dynamic Systems, Kluwer Academic, Dordrecht, 1999. 14. Agrawal, S.K., Claewplodtook, P. and Fabien, B.C., ‘Optimal trajectories of open-chain robot systems: A new solution procedure without Lagrange multipliers’, ASME Journal of Dynamic Systems, Measurement, and Control 120, 1998, 134–136.
