KUKA与PC端通过TCP/IP通信（带有第七轴的）

Xml_motion17.xml

​
<ETHERNETKRL>
<CONFIGURATION>
<EXTERNAL>
<IP>192.168.1.200</IP>
<PORT>59152</PORT>
</EXTERNAL>
</CONFIGURATION>
<RECEIVE>
<XML>
<ELEMENT Tag="Sensor/StepMode" Type="INT"/>
<ELEMENT Tag="Sensor/SX" Type="REAL"/>
<ELEMENT Tag="Sensor/SY" Type="REAL"/>
<ELEMENT Tag="Sensor/SZ" Type="REAL"/>
<ELEMENT Tag="Sensor/SA" Type="REAL"/>
<ELEMENT Tag="Sensor/SB" Type="REAL"/>
<ELEMENT Tag="Sensor/SC" Type="REAL"/>
<ELEMENT Tag="Sensor/SE1" Type="REAL"/>
<ELEMENT Tag="Sensor/Velocity" Type="REAL"/>
<ELEMENT Tag="Sensor" Set_Flag="1"/>
</XML>
</RECEIVE>
<SEND>
<XML>
<ELEMENT Tag="Robot/PX"/>
<ELEMENT Tag="Robot/PY"/>
<ELEMENT Tag="Robot/PZ"/>
<ELEMENT Tag="Robot/PA"/>
<ELEMENT Tag="Robot/PB"/>
<ELEMENT Tag="Robot/PC"/>
<ELEMENT Tag="Robot/PE1"/>
</XML>
</SEND>
</ETHERNETKRL>

​

motion17.src

&ACCESS RVP
&REL 6
&PARAM EDITMASK = *
&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
DEF motion17()



 ;FOLD DECLARATIONS
  ;FOLD BASISTECH DECL
  DECL STATE_T STAT
  DECL MODUS_T MODE
  ;ENDFOLD
  ;FOLD USER DECL
  DECL E6POS pose1 
  DECL E6POS pose2
  DECL EKI_STATUS RET
  REAL Mov_X, Mov_Y, Mov_Z, Mov_A, Mov_B, Mov_C
  REAL Velocity
  REAL Mov_E1  ; 新增第七轴目标位置变量 <--- 重要修改！
  INT StepMode
  ;ENDFOLD
  ;ENDFOLD  

;FOLD INI;%{PE}
  ;FOLD BASISTECH INI
  GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
  INTERRUPT ON 3 
  BAS (#INITMOV,0 )
  ;ENDFOLD (BASISTECH INI)
  ;FOLD USER INI
  ;Make your modifications here
  Mov_X=0
  Mov_Y=0
  Mov_Z=0
  Mov_A=0
  Mov_B=0
  Mov_C=0
  Mov_E1=0
  StepMode=0
  Velocity=0.0
  $FLAG[1]=FALSE
  pose1={X 0,Y 0,Z 0,A 0,B 0,C 0,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
  pose2={X 0,Y 0,Z 0,A 0,B 0,C 0,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}     
;ENDFOLD (USER INI)
;ENDFOLD (INI)

RET=EKI_Init("Xml_motion17")
RET=EKI_Open("Xml_motion17")

LOOP
  WAIT FOR $FLAG[1]
  $FLAG[1]=FALSE
  
  ;Receive data from PC
  RET=EKI_GetInt("Xml_motion17","Sensor/StepMode",StepMode)
  RET=EKI_GetReal("Xml_motion17","Sensor/SX",Mov_X)
  RET=EKI_GetReal("Xml_motion17","Sensor/SY",Mov_Y)
  RET=EKI_GetReal("Xml_motion17","Sensor/SZ",Mov_Z)
  RET=EKI_GetReal("Xml_motion17","Sensor/SA",Mov_A)
  RET=EKI_GetReal("Xml_motion17","Sensor/SB",Mov_B)
  RET=EKI_GetReal("Xml_motion17","Sensor/SC",Mov_C)
  RET=EKI_GetReal("Xml_motion17","Sensor/SE1",Mov_E1)
  RET=EKI_GetReal("Xml_motion17","Sensor/VELOCITY",Velocity)
  


  SWITCH  StepMode
    CASE 1
      pose1.X=$POS_ACT.X
      pose1.Y=$POS_ACT.Y
      pose1.Z=$POS_ACT.Z
      pose1.A=$POS_ACT.A
      pose1.B=$POS_ACT.B
      pose1.C=$POS_ACT.C
      pose1.E1 =$POS_ACT.E1 ; 记录第七轴实际位置 <--- 新增！

      WAIT SEC 1
      ;Send data to PC
      RET=EKI_SetReal("Xml_motion17","Robot/PX", pose1.X)
      RET=EKI_SetReal("Xml_motion17","Robot/PY", pose1.Y)
      RET=EKI_SetReal("Xml_motion17","Robot/PZ", pose1.Z)
      RET=EKI_SetReal("Xml_motion17","Robot/PA", pose1.A)
      RET=EKI_SetReal("Xml_motion17","Robot/PB", pose1.B)
      RET=EKI_SetReal("Xml_motion17","Robot/PC", pose1.C)
      RET=EKI_SetReal("Xml_motion17","Robot/PE1",pose1.E1) ; 第七轴反馈 <--- 新增！
      RET = EKI_Send("Xml_motion17","Robot")


    CASE 2 
      
      WAIT SEC 2
      pose2.X=Mov_X
      pose2.Y=Mov_Y
      pose2.Z=Mov_Z
      pose2.A=Mov_A
      pose2.B=Mov_B
      pose2.C=Mov_C
      pose2.E1=Mov_E1

      BAS(#BASE,0)
      BAS(#TOOL,0)

      
      IF Velocity > 0.0 THEN
          $VEL.CP = Velocity/1000     ; 直线速度
          $VEL.ORI1 = Velocity/1000   ; A轴旋转速度
          $VEL.ORI2 = Velocity/1000   ; B轴旋转速度
      ENDIF

      LIN pose2 C_DIS  ; 带连续运动参数

      ; 获取实际位置（包含第七轴）
      pose2.X=$POS_ACT.X
      pose2.Y=$POS_ACT.Y
      pose2.Z=$POS_ACT.Z
      pose2.A=$POS_ACT.A
      pose2.B=$POS_ACT.B
      pose2.C=$POS_ACT.C
      pose2.E1 = $POS_ACT.E1  ; 获取第七轴实际值 <--- 新增！

      ; 发送反馈
      ;Send data to PC
      RET=EKI_SetReal("Xml_motion17","Robot/PX", pose2.X)
      RET=EKI_SetReal("Xml_motion17","Robot/PY", pose2.Y)
      RET=EKI_SetReal("Xml_motion17","Robot/PZ", pose2.Z)
      RET=EKI_SetReal("Xml_motion17","Robot/PA", pose2.A)
      RET=EKI_SetReal("Xml_motion17","Robot/PB", pose2.B)
      RET=EKI_SetReal("Xml_motion17","Robot/PC", pose2.C)
      RET=EKI_SetReal("Xml_motion17","Robot/PE1", pose2.E1) ; 第七轴反馈 <--- 新增！
      RET = EKI_Send("Xml_motion17","Robot")
    
    DEFAULT
      ; 无操作
  ENDSWITCH

; 重置位置变量（保持E1清零）
pose1={X 0,Y 0,Z 0,A 0,B 0,C 0,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
pose2={X 0,Y 0,Z 0,A 0,B 0,C 0,S 6,T 27,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
ENDLOOP

RET=EKI_Close("Xml_motion17")
RET=EKI_Clear("Xml_motion17")

END

server.py

import socket
from utility import *

class Server:
    def __init__(self,host='',port=59152):
        self.host = host
        self.port = port
        self.server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        if (self.server_socket == -1):
            print("create socket failed")
        else:
            print("create socket success\n")

    def bind_and_listen(self):
        self.server_socket.bind((self.host,self.port))
        self.server_socket.listen(1)
        
    def accept_client(self):
        client_socket, client_address = self.server_socket.accept()
        return client_socket,client_address
    
    def rcv_pose(self,client_socket):
        recv_data=client_socket.recv(1024).decode('utf-8')
        return recv_data
    
    def send_pose(self,client_socket,pose):
        client_socket.send(pose.encode('utf-8'))
        
    def close(self):
        self.server_socket.close()
        print("close server")
        
# def slect_mode():
#     mode = float(input("slect mode: 1 or 2。Ctrl + C for quit\n"))
#     return mode

# pose=[804.63,354.36,1299.71,6.87,18.98,-1.16]
pose=[804.63,354.36,1299.71,6.87,18.98,-1.16,0]

def robot_connect():
    server=Server()
    server.bind_and_listen()
    client_socket,client_address=server.accept_client()
    print(f"connetcted from {client_address}")
    return client_socket


if __name__ == '__main__':
    server=Server()
    server.bind_and_listen()
    client_socket,client_address=server.accept_client()
    print(f"connetcted from {client_address}")
    while True:
        try:
            # mode = slect_mode()
            if mode ==1:   #接收机械臂当前位姿数据 
                xml_data=build_xml(mode=mode,pose=pose)
                server.send_pose(client_socket,xml_data)
                current_pose_xml=server.rcv_pose(client_socket)
                if current_pose_xml=='quit':
                    print("client quit")
                    break
                current_pose=xml_to_array(current_pose_xml)
                print(f"arm's current pose is{current_pose}")
            elif mode ==2: #发送机械臂目标位姿数据
                xml_data=build_xml(mode=mode,pose=pose)
                server.send_pose(client_socket,xml_data)
                print(f"success send data{xml_data}")
        except Exception as e:
            print(e)
            break

utility.py

import xml.etree.ElementTree as ET

def build_xml(mode,pose,velocity):
    xml_data=f'''
    <Sensor>
        <StepMode>{mode}</StepMode>
        <SX>{pose[0]}</SX> 
        <SY>{pose[1]}</SY> 
        <SZ>{pose[2]}</SZ> 
        <SA>{pose[3]}</SA> 
        <SB>{pose[4]}</SB> 
        <SC>{pose[5]}</SC>
        <SE1>{pose[6]}</SE1>
        <VELOCITY>{velocity}</VELOCITY>
    </Sensor>
    '''
    return xml_data

def xml_to_array(xml_data):
    root = ET.fromstring(xml_data)
    result = []
    for child in root:
        result.append(child.text)
    return result

kuka_socket.py

from server import Server
from utility import *
import time
# import utilityXYQ as ut3d
# import open3d as o3d
# from camera import MyCamera
import numpy as np
# from force_socket.sriCommManager import SRICommManager
# from thread_code.matPlot import matPlotFun 
# from Transformations import H_to_Rt, H_to_xyzwpr, xyzwpr_to_H

# extrinsic = xyzwpr_to_H(np.array([-184.961,102.114,314.281,-96.807,13.254,2.063])) 



class KukaArmController:
    def __init__(self, pose=[-197.23,1613,686.65,-168.85,85.15,99.23,0]):
        # 初始化KukaArmController类，设置初始位姿
        self.pose = pose
        self.server = Server()
        self.client_socket = None
        self.client_address = None
         # 相关传入对象
        # self.forceSensor=CsriCommManager  
        # self.forceSensor=SRICommManager()
        # self.plotFunc=matPlotFun

    def bind_and_listen(self):
        # 绑定并监听客户端
        self.server.bind_and_listen()
        self.client_socket, self.client_address = self.server.accept_client()
        print(f"Connected from {self.client_address}")

    # def get_pose(self,velocity):
    #     # 获取机械臂当前位姿
    #     xml_data = build_xml(mode=1, pose=self.pose,velocity=velocity)
    #     self.server.send_pose(self.client_socket, xml_data)
    #     current_pose_xml = self.server.rcv_pose(self.client_socket)
    #     current_pose = xml_to_array(current_pose_xml)
    #     print(f"Arm's current pose is {current_pose}")
    #     return current_pose
    
    def get_pose(self, velocity):
        # 获取机械臂当前位姿
        xml_data = build_xml(mode=1, pose=self.pose, velocity=velocity)
        self.server.send_pose(self.client_socket, xml_data)
        current_pose_xml = self.server.rcv_pose(self.client_socket)
        current_pose = xml_to_array(current_pose_xml)
        
        # 转换 current_pose 为数值列表
        current_pose_numeric = [float(x) if '.' in x else int(x) for x in current_pose]
        return current_pose_numeric

    #velocity=0默认为示教点的速度，单位为mm/s;  velocity可以设置在0~40之间
    def move_to_pose(self, new_pose,velocity):
        # 控制机械臂移动到新的位姿
        xml_data = build_xml(mode=2, pose=new_pose,velocity=velocity)
        self.server.send_pose(self.client_socket, xml_data)
        success_pose_xml = self.server.rcv_pose(self.client_socket)  # 阻塞直到接收到反馈
        success_pose = xml_to_array(success_pose_xml)
        # print(f"Arm has moved to {success_pose}")
        return success_pose
    

# 主程序部分
if __name__ == '__main__':
    import time
    # from sriCommManager import SRICommManager

    #连接kuka机械臂
    # 初始化KukaArmController对象
    kuka_controller = KukaArmController()
    # 绑定并监听客户端连接
    kuka_controller.bind_and_listen()

    # #连接梅卡相机
    # mycamera = MyCamera()
    # mycamera.connect(0)
    

    # 获取当前位姿
    current_pose = kuka_controller.get_pose(velocity=0)
    print("当前位姿",np.array(current_pose))

       



    new_pose1 = [1168.14,82.36,980,37.34,51.7,31.77,-300]
    time.sleep(1)
    # 控制机械臂移动到目标位姿
    kuka_controller.move_to_pose(new_pose1,velocity=10)