This paper formulates the dynamical equations of mechanics subject to holonomic constraints in terms of the states and controls using a constrained version of the Lagrange-d’Alembert principle. Based on a discrete version of this principle, a structure preserving time-stepping scheme is derived. It is shown that this respect for the mechanical structure (such as a reliable computation of the energy and momentum budget, without numerical dissipation) is retained when the system is reduced to its minimal dimension by the discrete null space method. Together with initial and final conditions on the configuration and conjugate momentum, the reduced time-stepping equations serve as nonlinear equality constraints for the minimisation of a given cost functional. The algorithm yields a sequence of discrete configurations together with a sequence of actuating forces, optimally guiding the system from the initial to the desired final state. The resulting discrete optimal control algorithm is shown to have excellent energy and momentum properties, which are illustrated by two specific examples, namely reorientation and repositioning of a rigid body subject to external forces and the reorientation of a rigid body with internal momentum wheels.
Geometric structure-preserving optimal control of a rigid body