KUKA机器人frame变量获取及使用

1.计算基于工具坐标系的位置偏移

目的：计算当前同一坐标系下的点相对工具坐标系偏移后的位置；

DEF E6POS Tool_Offset (nPos:IN,nFrame:IN )
;***************************************************
;* Autor : Andrew Wang * *
;* Version : 1.0 *
;* Created : 12.12.2018 *
;* Program Name : Tool_Offset *
;* Fuction : Position offset by Tool CS; *
;***************************************************
DECL E6POS nPos
DECL FRAME nFrame
DECL E6POS xTemp
xTemp=nPos:nFrame ；nFrame写后头
RETURN xTemp
END

2.计算基于基坐标系的位置偏移

目的：计算当前同一坐标系下的点相对基坐标系偏移后的位置；

DEF E6POS Base_Offset (nPos:IN,nFrame:IN )
;***************************************************
;* Autor : Andrew Wang *
;* Version : 1.0 *
;* Created : 12.12.2018 *
;* Program Name : Base_Offset *
;* Fuction : Position offset by Base CS; *
;***************************************************
DECL E6POS nPos,xRet
DECL FRAME nFrame
xRet=nFrame:nPos ；nFrame写前头
RETURN xRet
END

3.对Frame元素设定

目的：通过赋值的方式获取设定一个需要的Frame变量；

DEF FRAME Set_Frame (X:IN,Y:IN,Z:IN,A:IN,B:IN,C:IN )
;***************************************************
;* Autor : Andrew Wang *
;* Version : 1.0 *
;* Created : 12.12.2018 *
;* Program Name : Set_Frame *
;* Function : A frame made by each components ;*
;***************************************************
DECL REAL X,Y,Z,A,B,C
DECL FRAME fTemp
fTemp={X 0,Y 0,Z 0,A 0,B 0,C 0}
fTemp.X=X
fTemp.Y=Y
fTemp.Z=Z
fTemp.A=A
fTemp.B=B
fTemp.C=C
RETURN fTemp
END

4.计算两个点之间的向量Frame

目的：通过两个点计算出两点之间的相对位置向量；

DEF FRAME Get_Frame (nStartPos:IN,nEndPos:IN )
;***************************************************
;* Autor : Andrew Wang *
;* Version : 1.0 *
;* Created : 12.12.2018 *
;* Program Name : Get_Frame *
;* Fuction : Calculate a Frame for 2 position;*
;***************************************************
DECL E6POS nStartPos,nEndPos
DECL FRAME fRet
fRet=INV_POS (nStart):nEndPos
RETURN fRet
END

nStart为起始点，nEndPos为终点，结果fRet为起始点位姿下的工具坐标下的偏差；