Running instructions:
first running image3.py and image4.py(which contain the coordinate calculate by camera1 and camera2);
then running imagepp.py which would subscribe the data from image3 and image4;
For Q3.1 and Q3.2 running close_control.
For Q4.3 stop all the .py file before, running imageblack1.py and imageblack2.py.
To test the joint move, we need to run joint_move.py.