MATLAB與Robotstudio建立socket通訊
MATLAB與Robotstudio建立socket通訊
內容概括
哈哈，本人人生中的第一篇部落格，也算是向部落格大佬們看齊的第一個步伐啦，話不多說，直奔主題，這篇寫的是這兩天做的MATLAB與RobotStudio建立socket通訊的一個範例，機器人可以發位置資料給MATLAB，MATLAB也可以傳送想要到達的目標位置點給機器人讓機器人動作，當然，這只是這兩天做的一個初步的成果，有待改善和新增的功能還很多。。。
方法和思路
首先，我是看了這個部落格大佬的寫的MATLAB與Robotstudio建立socket通訊這篇文章，根據這個小case寫的，當然，該博主後期也寫了很多很厲害的範例出來，奈何本人能力有限，先從基礎的練起來（認真臉）！！！
有了小case之後，就理了下思路，查詢那個博主的思路來源，是西班牙的一篇論文，名字和PDF版可以去那位博主的部落格上去找（論文是西班牙語，是時候掏出翻譯軟體了！）。該論文講述了MATLAB三種方法來實現該功能，分別是GUI、CLASS和Simulink，由於是探索階段，本文準備用最簡單最直觀的MATLAB GUI的方法來實現。另外，論文中所說MATLAB需要兩個工具包，一個是外國的Peter Corke大佬寫的工具箱Robotics Toolbox,他的個人主頁上可以免費下載（這個人寫的書也是很厲害的，推薦學機器人的去康康），但是好像需要科學上網,另外一個工具箱是MATLAB裡的Instrument Control Toolbox（一般都會有）。
準備工作做好之後，我的大致步驟是：
1.按照case，MATLAB和Robot Studio之間先建立基本的通訊，即能夠互相簡單的發一串字串給對方。
2.簡單粗略的學習一下MATLAB GUI的知識，推薦視訊一把王者榮耀的時間，讓你學會MATLAB GUI,哈哈，真的就學了一會，能用起來，但是介面做的並不美觀，後期再深造一下吧哈哈。
3.實驗在RobotStudio中獲取當前機器人的位置，並行送過去。
4.實驗在MATLAB中轉換四元數，把位置資料和四元數統一成一個檔案包傳送給RobotStudio。
5.改程序式，使得通訊能夠互相協調。
實踐成果
介面展示
話不多說，先放上基本的介面，由於初次寫部落格，動圖還不知怎麼加上去，反正基本的要求都能實現了,根據這張圖可以自行想象一下動態過程~~
如圖所示，在RobotStudio（作為Sever）中先執行寫好的程式，再執行MATLAB（作為Client），輸入IP地址和Port號點選Connect，會彈出執行成功的視窗，此時，機器人已經把它的位置發過來了，並展現在Position欄的文字方塊中，使用者可以輸入目標點資訊（X、Y、Z位置資訊和RX、RY、RZ工具姿態），點選SendTCP之後，傳送給Robot，Robot就會MoveJ到相應的位置，當然，使用者並不能隨意輸入，因為機器人很多的位置和姿態都是到達不了的，還有奇異值等問題，使用者需要根據實際情況合理的輸入目標點位置和姿態資料。在Robot到達你所指定的位置之後，會傳送給介面它目前所在的位置，即Position一欄會更新位置資訊，便於實時的監控機器人的位置資訊。
程式碼展示
還是直接上程式碼來的利索，先說一下RS那邊的程式碼吧，RS中一定要選擇616-1 PC Interface（進行Socket通訊）和623-1 Multitasking（多工執行）這兩個選項，因為RS中通訊和機器人運動是同時進行的，所以要選擇這個選項。先展示下機器人通訊的主要程式碼：
PROC Routine()
SocketCreate temp_socket;
SocketBind temp_socket,"127.0.0.1",1025;
SocketListen temp_socket;
! Waiting for a connection request
SocketAccept temp_socket,client_socket;
! Communication
WHILE keep_listening DO
!Calculate current position
Current_Pose:=CRobT(\TaskRef:=robot_motionId\Tool:=tool0\WObj:=wobj0);
send_string:=ValToStr(Current_Pose.trans);
!Send current position to matlab
SocketSend client_socket\Str:=send_string+"\0D\0A";
!Receive the TCP data from matlab
receive_status:=FALSE;
SocketReceive client_socket\Str:=received_string;
received:=received_string;
receive_status:=TRUE;
WaitTime 1;
ENDWHILE
SocketClose client_socket;
SocketClose temp_socket;
ENDPROC
RAPID程式好像不太支援Markdown，湊合看吧。。。
這裡程式的這一行值得說一下 ：
SocketSend client_socket\Str:=send_string+"\0D\0A";
一開始我是沒有加這一部分的+"\0D\0A"，這樣就導致了MATLAB收一條資訊要很久，而且會報出如下的警告：
經過排查原因，是終止符的原因，查取ABB的隨機光碟得知，RS裡的終止符合一般的終止符不一樣，並不是回車換行符/s/n，屬於不可列印字元，即ASSCII碼的特殊字元，如回車換行符為「\0D\0A」,因為MATLAB不認識這個終止符，導致很久都找不到結尾在哪裡，並且出現警告，反正加上就好啦，颼颼的~~
接著是解析轉換傳送字串的程式碼（沒來得及寫註釋,先放著吧）。。
PROC encode_string()
Strread:=received;
LenString:=StrLen(Strread);
StartBit1:=1;
EndBit1:=StrFind(Strread,StartBit1,",");
LenBit1:=EndBit1-StartBit1;
StartBit2:=EndBit1+1;
EndBit2:=StrFind(Strread,StartBit2,",");
LenBit2:=EndBit2-StartBit2;
StartBit3:=EndBit2+1;
EndBit3:=StrFind(Strread,StartBit3,",");
LenBit3:=EndBit3-StartBit3;
StartBit4:=EndBit3+1;
EndBit4:=StrFind(Strread,StartBit4,",");
LenBit4:=EndBit4-StartBit4;
StartBit5:=EndBit4+1;
EndBit5:=StrFind(Strread,StartBit5,",");
LenBit5:=EndBit5-StartBit5;
StartBit6:=EndBit5+1;
EndBit6:=StrFind(Strread,StartBit6,",");
LenBit6:=EndBit6-StartBit6;
StartBit7:=EndBit6+1;
EndBit7:=StrFind(Strread,StartBit7,",");
LenBit7:=EndBit7-StartBit7;
StartBit8:=EndBit7+1;
EndBit8:=StrFind(Strread,StartBit8,",");
LenBit8:=EndBit8-StartBit8;
data_type:=StrPart(Strread,StartBit1,LenBit1);
XData:=StrPart(Strread,StartBit2,LenBit2);
YData:=StrPart(Strread,StartBit3,LenBit3);
ZData:=StrPart(Strread,StartBit4,LenBit4);
q1Data:=StrPart(Strread,StartBit5,LenBit5);
q2Data:=StrPart(Strread,StartBit6,LenBit6);
q3Data:=StrPart(Strread,StartBit7,LenBit7);
q4Data:=StrPart(Strread,StartBit8,LenBit8);
DataTRUE:=StrToVal(XData,x);
DataTRUE:=StrToVal(YData,y);
DataTRUE:=StrToVal(ZData,z);
DataTRUE:=StrToVal(q1Data,q1);
DataTRUE:=StrToVal(q2Data,q2);
DataTRUE:=StrToVal(q3Data,q3);
DataTRUE:=StrToVal(q4Data,q4);
ENDPROC
接著是機器人移動的程式碼：
PROC main()
! Wait the receive string
WaitUntil receive_status;
! Received string convert into data
encode_string;
! Give the data to the Target
IF DataTRUE THEN
Target_20.trans.x:=x;
Target_20.trans.y:=y;
Target_20.trans.z:=z;
Target_20.rot.q1:=q1;
Target_20.rot.q2:=q2;
Target_20.rot.q3:=q3;
Target_20.rot.q4:=q4;
Data_Assignment:=TRUE;
ELSE
TPWrite "Problems with data conversion!";
ENDIF
WaitUntil Data_Assignment;
! Make the robot move to the desire position
MoveJ Target_20,v1000,z20,tool0;
ENDPROC
然後是MATLAB這邊的程式碼，由於控制元件程式碼太長了但很簡單，就只展示重要控制元件CallBack函數的程式碼：
%Connect按鈕的Callback
function Connect_to_Robot(~,~)
global GUI
global IP
global Port
global client_socket
IP=get(GUI.IP_Address,'String');
Port=get(GUI.Port_Address,'String');
Port=str2double(Port);
%connect to the robot
client_socket = tcpip(IP,Port);
fopen(client_socket);
msgbox('Successful Connected!')
message= fgetl(client_socket);
message=splitlines(message);
position=message{1,1};
set(GUI.Position_All,'String',position);
guidata(GUI.IP_Address,IP);
guidata(GUI.Port_Address,Port);
end
%TCP Send按鈕的Callback
function TCP_button(~,~)
global GUI
global X
global Y
global Z
global RX
global RY
global RZ
global client_socket
X=get(GUI.X,'string');
Y=get(GUI.Y,'string');
Z=get(GUI.Z,'string');
RX=get(GUI.RX,'string');
RX_num=str2double(RX);
RY=get(GUI.RY,'string');
RY_num=str2double(RY);
RZ=get(GUI.RZ,'string');
RZ_num=str2double(RZ);
%計算四元數
Q=Quaternion(rotx(RX_num*pi/180)*roty(RY_num*pi/180)*rotz(RZ_num*pi/180));
%計算的數值轉換成字串
q1= num2str(Q.s);
q2= num2str(Q.v(1));
q3= num2str(Q.v(2));
q4= num2str(Q.v(3));
%字串整合在一起
Send1=['1',',',X,',',Y,',',Z,',',q1,',',q2,',',q3,',',q4,','];
%傳送給robot
fwrite(client_socket,Send1);
pause(1.5);
%讀取返回的位置資料
message= fgetl(client_socket);
message=splitlines(message);
position=message{1,1};
set(GUI.Position_All,'String',position);
end
總結
核心程式碼都已奉上了，當然不足之處也有很多，該文也只實現了很簡單的一些功能，後期還需要改進和完善，像那位博主一樣，在介面上加上關節值的卷軸來使機器人動作也是能實現的，但是實時性對我來說還是個難題。而且，再大膽想象一下，改進軌跡規劃的演演算法如果能利用這個通訊，在MATLAB中改進演演算法後就可以在RS中看到軌跡規劃的真實場景了，再往後展望，和機器視覺結合起來，自動避障也是一個好的idea，當然，還需要更紮實的程式設計基礎和演演算法知識，路漫漫其修遠兮，希望未來能夠做出來~
對本文有什麼疑問的小夥伴也歡迎和我交流啊，互相進步互相學習，留言可能不及時看到，郵箱lishuo18855443656@163.com~~