Exploiting sparsity in the discrete mechanics and optimal control method with application to human motion planning

The discrete equations of motion derived using a variational principle are particularly attractive to be used in numerical optimal control methods. This is mainly because: i) they exhibit excellent energy behavior, ii) they extend gracefully to systems with holonomic constraints and iii) they admit compact representation of the discrete state space. In this paper we propose the use of sparse finite differencing techniques for the Discrete Mechanics and Optimal Control method. In particular we show how to efficiently construct estimates of the Jacobian and Hessian matrices when the dynamics of the optimal control problem is discretized using a variational integrator. To demonstrate the effectiveness of this scheme we solve a human motion planning problem of an industrial assembly task, modeled as a multibody system consisting of more than one hundred degrees of freedom.