First full version of the automatic charge algorithm

Completed the main state machine with joint_cartesian and gripper modules to move arm and open or close grippers.
For the moment a hard-coded point for joint cartesian module is given.
Added a search for small qr.