Wrote SQP loop using FORCES QP solver (no collision detection version)