Continuously satisfying constraints with contact forces in trajectory optimization for humanoid robots

Humanoid robots being underactuated, they need to interact with their environment to move. When optimizing trajectories, the contact forces must therefore be taken into account. In this paper, we first highlight an issue encountered when using parametrized functions in the presence of equality constraints which need to be satisfied continuously over a time interval. We then propose a parametrization of contact forces and a formulation of the constraints which allow to write a tractable optimization program whose solution verifies at any instant all the constraints where those forces appear. We finish by exemplifying the approach with preliminary results obtained on a HRP-2 humanoid robot.