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，当然，还需要更扎实的编程基础和算法知识，路漫漫其修远兮，希望未来能够做出来~
对本文有什么疑问的小伙伴也欢迎和我交流啊，互相进步互相学习，留言可能不及时看到，邮箱[email protected]~~