implemented servo actions for 4 algorithms
implemented push_cube() refinement. Saved about 6 seconds in one tested algorithm