removed kinfu bag
Merge branch 'gkahn'
Merge branch 'gkahn'
split sim and brett versions. now turn sqp into a class
moved sqp to own file
ready to go for live testing