in the middle of templetizing control loops and putting in dual arm features....

in the middle of templetizing control loops and putting in dual arm features. i have no idea what works or what broken at the moment. the key next steps are making dualarmwholebodyinterface, probably separating point-to-point motions into different files depending on the robot, and finalizing dual-arm ocps and mpcs