The controller code coresponding to the manuscript submitted to Tmech NOTE: For access to the controller code, please contact Xiangyu Wang at wangxyu@nankai.edu.cn for the academic license. To execute this C++ code, we recommended setting up a multi-thread program with packages OpenCV, OpenMP. The illustration diagram of control scheme in manuscript: