added support for csqp, and added correct constraints. it's ass. almost certainly needs to be tuned somehow, but fddp seems to be just fine for our purposes so i won't be exploring this further. the next task is to create an mpc controller
