finished pr2_eih_system testing. Time to do sqp