|
1. 空间表示一个位姿，通常需要XYZABC来表示（其中ABC分别为RZ,RY,RX的欧拉角）
2. 3D相机通过socket发送如下形式坐标：1,2,3,4,5,6 （”,”为分隔符），ABB机器人如何通过socket接受以上数据并存储到对应数组内容，并将对应数据转化为机器人点位Robtarget？
3. 对于接收到的字符串1,2,3,4,5,6，可以使用如下代码解析并存入数组a12中
PERSnum a12{6}:=[1,2,3,4,5,6];PROC test22() datadecode "1,2,3,4,5,6",",",a12; ENDPROC
PROCdatadecode(string inputstr,string sp,inout num data{*}) !inputstr为输入数据字符串 !sp为分割字符 !data为存入数据数组 VAR string s1; VAR string s2; VAR num count; VAR bool flag1; count:=1; FORi FROM 1 TOStrLen(inputstr) DO !遍历字符串内所有字符 s1:=StrPart(inputstr,i,1); !每次取一个字符 IF s1=sp THEN !如果当前字符是分隔符，则将之前的字符串转化为数据并存入data{count}中 flag1:=strtoval(s2,data{count}); s2:=""; Incr count; ELSE !如果当前字符不是分割字符，则字符串拼接 s2:=s2+s1; ENDIF flag1:=strtoval(s2,data{count}); ENDFORENDPROC
4. 对于存入数组a12中的数据，可以通过如下代码赋值到对应robtarget并移动机器人
PROCpath_move() ptmp2:=p10; !p10为标准位置 ptmp2.trans.x:=a12{1}; ptmp2.trans.y:=a12{2}; ptmp2.trans.z:=a12{3}; ptmp2.rot:=OrientZYX(a12{4},a12{5},a12{6}); !orientzyx(rz,ry,rx) !将欧拉角转化为四元数 MoveL ptmp2,v100,fine,tool1WObj:=wobj0; ENDPROC
5. 完整通过socket接受3D相机坐标并移动示例代码如下：
MODULEm3PERSnum a1:=4; pers robtarget p100:=[[637.0908,93.56882,1145.292],[0.9970773,0.07639935,-6.336661E-06,7.70224E-06],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; VAR socketdev socket1; VAR string received_string;PERSnum a12{6}:=[1,2,3,4,5,6];VARrobtarget ptmp:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];VARrobtarget ptmp2:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];persnum curr_position{6}:=[0,0,0,0,0,0];CONSTrobtarget p10:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
PROCmain1()VARstring str_pose; SocketCreate socket1; SocketConnect socket1,"127.0.0.1",8025; WHILE TRUE DOptmp:=CRobT();curr_position{1}:=ptmp.trans.x;curr_position{2}:=ptmp.trans.y;curr_position{3}:=ptmp.trans.z;curr_position{4}:=EulerZYX(\z,ptmp.rot);curr_position{5}:=EulerZYX(\y,ptmp.rot);curr_position{6}:=EulerZYX(\x,ptmp.rot);
SocketSendsocket1\Str:="request cam";SocketSendsocket1\Str:="curr_pos "+ValToStr(curr_position); !send curr position
SocketReceivesocket1\Str:=received_string; !receive postionTPWrite"Server wrote - "+received_string;datadecodereceived_string,",",a12; !put receive string position into a12 arraypath_move;ENDWHILEreceived_string:=""; ERRORSocketClosesocket1; ENDPROC
PROC path_move()ptmp2:=p10;ptmp2.trans.x:=a12{1};ptmp2.trans.y:=a12{2};ptmp2.trans.z:=a12{3};ptmp2.rot:=OrientZYX(a12{4},a12{5},a12{6}); !orientzyx(rz,ry,rx)MoveLptmp2,v100,fine,tool1\WObj:=wobj0; ENDPROC
PROCdatadecode(string inputstr,string sp,inout num data{*})VARstring s1;VARstring s2;VARnum count;VARbool flag1;
count:=1;FORi FROM 1 TO StrLen(inputstr) DOs1:=StrPart(inputstr,i,1);IFs1=sp THENflag1:=strtoval(s2,data{count});s2:="";Incrcount;ELSEs2:=s2+s1;ENDIFflag1:=strtoval(s2,data{count});ENDFOR ENDPROCENDMODULE
|
