Inverse dynamics‐based formulation of finite horizon optimal control problems for rigid‐body systems

Abstract

We propose a formulation of the finite horizon optimal control problem (FHOCP) based on inverse dynamics for general open-chain rigid-body systems, which reduces the computational cost from the conventional formulation based on forward dynamics. We regard the generalized acceleration as a decision variable and inverse dynamics as an equality constraint. To treat under-actuated systems with inverse dynamics that are well defined only to fully actuated systems, that is, to consider passive joints in this FHOCP, we add an equality constraint to zero the corresponding generalized torques. We include the contact forces in the decision variables of this FHOCP and treat the contact constraints using Baumgarte's stabilization method for numerical stability. We derive the optimality conditions and formulate the two-point boundary-value problem that can be efficiently solved using the recursive Newton–Euler algorithm (RNEA) and the partial derivatives of RNEA. We conducted three numerical experiments on model predictive control based on the proposed formulation to demonstrate its effectiveness. The first experiment involved simulating a swing-up control of a four-link arm with a passive joint and showed that the proposed formulation is effective for under-actuated systems. The second one involved comparing the proposed formulation with the conventional forward-dynamics-based formulation with various numbers of joints and showed that the proposed formulation reduces computational cost regardless of the number of joints. The third experiment involved simulating a whole-body control of a quadruped robot, a floating-base system having four contacts with the ground, and showed that the proposed formulation is applicable even for floating-base systems with contacts.