Static multi-contact inverse problem for multiple humanoid robots and manipulated objects

In this paper we solve the static-equilibrium constrained inverse kinematics problem for a system made of multiple humanoid robots and manipulated objects given a set of contacts between any surfaces of the robots, any surfaces of the manipulated objects, and any surfaces of the environment. In particular, inter-robots contacts are possible. The contacts considered here are neither necessarily coplanar, nor necessarily horizontal, frictional, might be unilateral (support) or bilateral (grasp). We solve both the geometric variables (configurations) and the statics variables (contact forces) simultaneously within one optimization query. In the resulting configurations all the robots and the manipulated objects are in static equilibrium under the action of gravity and actuator torques that are constrained to stay within their bounds. The main focus of the paper is on the formulation of the problem rather than the optimization algorithm, as we consider the latter as a black box that only requires a mathematical model providing algorithms to compute the values of the objective function, the constraints functions, and their derivatives. We apply this work to quasi-static multi-contact legged locomotion planning on irregular terrain, multi-fingered dexterous manipulation planning, and collaborative manipulation planning.