Controller is fully functional, everything is debuged. TODO: but only if needed: Check the state of gripper and return successfull after gripper states is reached (possible problem with grasping).