Hello folks,
im hoping you can help me configuring or using the EthernetKRL package.
This is my first time using it and i do not understand how to debug my error.
Im trying to start a relatively simple KRL script which should instanciate an EthernetKRL server to stream robot states and receive positional commands. Im not getting far though, since it throws errors on server instanciation.
The IP "my.address.of.robot" corresponds to the network configuration of virtual5, Windows interface is also chosen. I can ping the corresponding PCs in the same network from KRC to PC and vice versa.
The error occurs within T1 mode on line 45 of the KRL script, the server cannot be opened, thus i also cannot move to the first position. I want the robot to act as a server, to enable streaming from one PC, and also listening from another. The error message reads: "Create server failed", error code 12.
Both files are copied from the kuka experimental ros repo https://github.com/ros-industrial…ki_hw_interface.
EthernetKRL - version 2.2.9
R1Mada - version 8.3.371/KUKA8.3
IPs selected are in 10.100.xxx.xxx range, obviouse not my.address.of.robot, 10.100.10.18 in this case.
My network config:
This is my EthernetXML config file:
<ETHERNETKRL>
<CONFIGURATION>
<EXTERNAL>
<TYPE>Client</TYPE> <!-- Users connect as clients -->
</EXTERNAL>
<INTERNAL>
<ENVIRONMENT>Program</ENVIRONMENT> <!-- Server run via robot interpreter -->
<BUFFERING Limit="512" /> <!-- Allow buffering of up to 512 messages (system max) -->
<ALIVE Set_Flag="1" /> <!-- Use $flag[1] to indicate alive/good connection status -->
<IP>10.100.10.18</IP> <!-- IP address for EKI interface on robot controller (KRC) -->
<PORT>54600</PORT> <!-- Port of EKI interface on robot controller (in [54600, 54615]) -->
<PROTOCOL>UDP</PROTOCOL> <!-- Use UDP protocol -->
</INTERNAL>
</CONFIGURATION>
<!-- Configured XML structure for data reception (external client to robot)
<RobotCommand>
<Pos A1="..." A2="..." A3="..." A4="..." A5="..." A6="...">
</Pos>
</RobotCommand>
-->
<RECEIVE>
<XML>
<!-- Joint position command -->
<ELEMENT Tag="RobotCommand/Pos/@A1" Type="REAL" />
<ELEMENT Tag="RobotCommand/Pos/@A2" Type="REAL" />
<ELEMENT Tag="RobotCommand/Pos/@A3" Type="REAL" />
<ELEMENT Tag="RobotCommand/Pos/@A4" Type="REAL" />
<ELEMENT Tag="RobotCommand/Pos/@A5" Type="REAL" />
<ELEMENT Tag="RobotCommand/Pos/@A6" Type="REAL" />
</XML>
</RECEIVE>
<!-- Configured XML structure for data transmission (robot to external client)
<RobotState>
<Pos A1="..." A2="..." A3="..." A4="..." A5="..." A6="...">
</Pos>
<Vel A1="..." A2="..." A3="..." A4="..." A5="..." A6="...">
</Vel>
<Eff A1="..." A2="..." A3="..." A4="..." A5="..." A6="...">
</Eff>
</RobotState>
-->
<SEND>
<XML>
<!-- Joint state positions -->
<ELEMENT Tag="RobotState/Pos/@A1"/>
<ELEMENT Tag="RobotState/Pos/@A2"/>
<ELEMENT Tag="RobotState/Pos/@A3"/>
<ELEMENT Tag="RobotState/Pos/@A4"/>
<ELEMENT Tag="RobotState/Pos/@A5"/>
<ELEMENT Tag="RobotState/Pos/@A6"/>
<!-- Joint state velocities -->
<ELEMENT Tag="RobotState/Vel/@A1"/>
<ELEMENT Tag="RobotState/Vel/@A2"/>
<ELEMENT Tag="RobotState/Vel/@A3"/>
<ELEMENT Tag="RobotState/Vel/@A4"/>
<ELEMENT Tag="RobotState/Vel/@A5"/>
<ELEMENT Tag="RobotState/Vel/@A6"/>
<!-- Joint state efforts (torques) -->
<ELEMENT Tag="RobotState/Eff/@A1"/>
<ELEMENT Tag="RobotState/Eff/@A2"/>
<ELEMENT Tag="RobotState/Eff/@A3"/>
<ELEMENT Tag="RobotState/Eff/@A4"/>
<ELEMENT Tag="RobotState/Eff/@A5"/>
<ELEMENT Tag="RobotState/Eff/@A6"/>
<!-- Interface state -->
<ELEMENT Tag="RobotState/RobotCommand/@Size"/> <!-- Number of elements currently in command buffer -->
</XML>
</SEND>
</ETHERNETKRL>
Display More
My KRL script:
;
; Utilized system resources:
; Flags:
; $flag[1]: Indicates active client connection
; $timer_flag[1]: Used to trigger periodic send of robot state
; Interrupts:
; 15: Calls eki_hw_iface_reset() on falling edge of $flag[1]
; 16: Calls eki_hw_iface_send() on rising edge of $timer_flag[1]
eki_hw_iface_init()
; BCO (Block COincidence) run to current position
; (requied for below loop continue before first incoming command)
joint_pos_tgt = $axis_act_meas
ptp joint_pos_tgt
; Loop forever
$advance = 5
loop
elements_read = eki_hw_iface_get(joint_pos_tgt) ; Get new command from buffer if present
ptp joint_pos_tgt c_ptp ; PTP to most recent commanded position
endloop
; Note: EKI channels delete on reset or deselect of this program
; See <ENVIRONMENT>Program</ENVIRONMENT> EKI config element
end
def eki_hw_iface_init()
decl eki_status eki_ret
; Setup interrupts
; Interrupt 15 - Connection cleanup on client disconnect
global interrupt decl 15 when $flag[1]==false do eki_hw_iface_reset()
interrupt on 15
; Interrupt 16 - Timer interrupt for periodic state transmission
global interrupt decl 16 when $timer_flag[1]==true do eki_hw_iface_send()
interrupt on 16
wait sec 0.012 ; Wait for next interpolation cycle
$timer[1] = -200 ; Time in [ms] before first interrupt call
$timer_stop[1] = false ; Start timer 1
; Create and open EKI interface
eki_ret = eki_init("EkiHwInterface")
eki_ret = eki_open("EkiHwInterface")
end
def eki_hw_iface_send()
decl eki_status eki_ret
decl real vel_percent
if $flag[1] then ; If connection alive
; Load state values into xml structure
; position
eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A1", $axis_act_meas.a1)
eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A2", $axis_act_meas.a2)
eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A3", $axis_act_meas.a3)
eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A4", $axis_act_meas.a4)
eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A5", $axis_act_meas.a5)
eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A6", $axis_act_meas.a6)
; velocity
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A1", $vel_axis_act[1] * $vel_axis_ma[1] / 100.0)
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A2", $vel_axis_act[2] * $vel_axis_ma[2] / 100.0)
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A3", $vel_axis_act[3] * $vel_axis_ma[3] / 100.0)
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A4", $vel_axis_act[4] * $vel_axis_ma[4] / 100.0)
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A5", $vel_axis_act[5] * $vel_axis_ma[5] / 100.0)
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A6", $vel_axis_act[6] * $vel_axis_ma[6] / 100.0)
; effort
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A1", $torque_axis_act[1])
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A2", $torque_axis_act[2])
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A3", $torque_axis_act[3])
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A4", $torque_axis_act[4])
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A5", $torque_axis_act[5])
;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A6", $torque_axis_act[6])
; interface state
eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1")
eki_ret = eki_setint("EkiHwInterface", "RobotState/RobotCommand/@Size", eki_ret.buff)
; Send xml structure
if $flag[1] then ; Make sure connection hasn't died while updating xml structure
eki_ret = eki_send("EkiHwInterface", "RobotState")
endif
endif
; Set timer for next interrupt [ms]
$timer[1] = -10 ; ~10 ms for above send + 10 ms interrupt timer -> ~50 Hz state transmission
end
deffct int eki_hw_iface_available()
decl eki_status eki_ret
if not $flag[1] then
return 0
endif
eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1")
return eki_ret.buff
endfct
; eki_hw_iface_get
; Tries to read most recent elemnt from buffer. q left unchanged if empty.
; Returns number of elements read.
deffct int eki_hw_iface_get(joint_pos_cmd :out)
decl eki_status eki_ret
decl axis joint_pos_cmd
if not $flag[1] then
return 0
endif
eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1")
if eki_ret.buff <= 0 then
return 0
endif
eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A1", joint_pos_cmd.a1)
eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A2", joint_pos_cmd.a2)
eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A3", joint_pos_cmd.a3)
eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A4", joint_pos_cmd.a4)
eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A5", joint_pos_cmd.a5)
eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A6", joint_pos_cmd.a6)
return 1
endfct
def eki_hw_iface_reset()
decl eki_status eki_ret
eki_ret = eki_clear("EkiHwInterface")
eki_ret = eki_init("EkiHwInterface")
eki_ret = eki_open("EkiHwInterface")
end
Display More