Finished implementing new controller objects and tests. We now have head control (in robot_controller.h), joint space arm control, cartesian space arm control, joint space hand control and force-position hand control (in control.h). Also added a nicer argument-to-test_function mapping system.