Implemented center of mass with Riccati for the pelvis trajectory