Fix for SQP w/ CD
Merge branch 'master' into rll3
Had another bug, small fix
Merge branch 'master' into rll3
Eigen2 on banatao machines.. had to fix my code. Should be good for trajopt w/ CD now...