Skip to content

Commit

Permalink
KRC2 port for kuka_eki_hw_interface
Browse files Browse the repository at this point in the history
  • Loading branch information
raniot_git committed Sep 28, 2021
1 parent b3ddcbb commit 489130b
Show file tree
Hide file tree
Showing 6 changed files with 168 additions and 191 deletions.
6 changes: 4 additions & 2 deletions kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,7 @@ class KukaEkiHardwareInterface : public hardware_interface::RobotHW
// EKI
std::string eki_server_address_;
std::string eki_server_port_;
int eki_cmd_id_count_;
int eki_cmd_buff_len_;
int eki_max_cmd_buff_len_ = 5; // by default, limit command buffer to 5 (size of advance run in KRL)

Expand All @@ -81,8 +82,9 @@ class KukaEkiHardwareInterface : public hardware_interface::RobotHW
int eki_read_state_timeout_ = 5; // [s]; settable by parameter (default = 5)
boost::asio::io_service ios_;
boost::asio::deadline_timer deadline_;
boost::asio::ip::udp::endpoint eki_server_endpoint_;
boost::asio::ip::udp::socket eki_server_socket_;
boost::asio::ip::tcp::endpoint eki_server_endpoint_;
boost::asio::ip::tcp::socket eki_server_socket_;
boost::asio::ip::tcp::acceptor eki_acceptor_;
void eki_check_read_state_deadline();
static void eki_handle_receive(const boost::system::error_code &ec, size_t length,
boost::system::error_code* out_ec, size_t* out_length);
Expand Down
10 changes: 10 additions & 0 deletions kuka_eki_hw_interface/krl/EkiHwInterface+.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
<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>
<RobotCommand ID="" Size="5">
</RobotCommand>
</RobotState>
82 changes: 11 additions & 71 deletions kuka_eki_hw_interface/krl/EkiHwInterface.xml
View file
Original file line number Diff line number Diff line change
@@ -1,71 +1,11 @@
<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>address.of.robot.controller</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>
<Elements>
<Element Tag="RobotCommand" Type="STRUCTTAG" Stacksize="5" />
<Element Tag="RobotCommand.ID" Type="INTEGER" Stacksize="5" />
<Element Tag="RobotCommand.Pos" Type="STRUCTTAG" Stacksize="5" />
<Element Tag="RobotCommand.Pos.A1" Type="REAL" Stacksize="5" />
<Element Tag="RobotCommand.Pos.A2" Type="REAL" Stacksize="5" />
<Element Tag="RobotCommand.Pos.A3" Type="REAL" Stacksize="5" />
<Element Tag="RobotCommand.Pos.A4" Type="REAL" Stacksize="5" />
<Element Tag="RobotCommand.Pos.A5" Type="REAL" Stacksize="5" />
<Element Tag="RobotCommand.Pos.A6" Type="REAL" Stacksize="5" />
</Elements>
8 changes: 5 additions & 3 deletions kuka_eki_hw_interface/krl/kuka_eki_hw_interface.dat
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,6 @@
&ACCESS RVO
defdat kuka_eki_hw_interface
&ACCESS RVP
DEFDAT KUKA_EKI_HW_INTERFACE
EXT bas (BAS_COMMAND :IN,REAL :IN )
ext bas(bas_command :in,real :in )
enddat
INT id_count
ENDDAT
193 changes: 90 additions & 103 deletions kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src
View file
Original file line number Diff line number Diff line change
@@ -1,45 +1,14 @@
&ACCESS RVO
&ACCESS RVP
&REL 5
def kuka_eki_hw_interface()

; Software License Agreement (BSD License)
;
; Copyright (c) 2018, 3M
; All rights reserved.
;
; Redistribution and use in source and binary forms, with or without
; modification, are permitted provided that the following conditions are met:
;
; * Redistributions of source code must retain the above copyright
; notice, this list of conditions and the following disclaimer.
; * Redistributions in binary form must reproduce the above copyright
; notice, this list of conditions and the following disclaimer in the
; documentation and/or other materials provided with the distribution.
; * Neither the name of the copyright holder, nor the names of its
; contributors may be used to endorse or promote products derived
; from this software without specific prior written permission.
;
; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
; POSSIBILITY OF SUCH DAMAGE.

; Author: Brett Hemes (3M) <[email protected]>


; Declarations
decl axis joint_pos_tgt
decl int elements_read

; INI
bas(#initmov, 0) ; Basic initializasion of axes
bas(#initmov, 0) ; Basic initialization of axes

;FOLD description
; Initialize eki_hw_interface server
; Config located in C:/ROBOTER/Config/User/Common/EthernetKRL/EkiHwInterface.xml
; Starts a TCP state sever on xml-specified IP address/port.
Expand All @@ -59,11 +28,12 @@ def kuka_eki_hw_interface()
; 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]
;ENDFOLD
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
; BCO (Block Coincidence) run to current position
; (required for below loop continue before first incoming command)
joint_pos_tgt = $axis_act
ptp joint_pos_tgt

; Loop forever
Expand All @@ -73,15 +43,17 @@ def kuka_eki_hw_interface()
ptp joint_pos_tgt c_ptp ; PTP to most recent commanded position
endloop

;FOLD note
; Note: EKI channels delete on reset or deselect of this program
; See <ENVIRONMENT>Program</ENVIRONMENT> EKI config element
;ENDFOLD
end



def eki_hw_iface_init()
decl eki_status eki_ret

decl int ekx_ret
id_count = 0
; Setup interrupts
; Interrupt 15 - Connection cleanup on client disconnect
global interrupt decl 15 when $flag[1]==false do eki_hw_iface_reset()
Expand All @@ -94,99 +66,114 @@ def eki_hw_iface_init()
$timer_stop[1] = false ; Start timer 1

; Create and open EKI interface
eki_ret = eki_init("EkiHwInterface")
eki_ret = eki_open("EkiHwInterface")
; ekx_ret = eki_init("EkiHwInterface")
ekx_ret = EKX_Open("EkiHwInterface")
EKX_HandleError(ekx_ret)

$flag[1] = true
end



def eki_hw_iface_send()
decl eki_status eki_ret
decl real vel_percent
decl int ekx_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])
ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A1", $axis_act.a1)
EKX_HandleError(ekx_ret)
ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A2", $axis_act.a2)
EKX_HandleError(ekx_ret)
ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A3", $axis_act.a3)
EKX_HandleError(ekx_ret)
ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A4", $axis_act.a4)
EKX_HandleError(ekx_ret)
ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A5", $axis_act.a5)
EKX_HandleError(ekx_ret)
ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A6", $axis_act.a6)
EKX_HandleError(ekx_ret)

;FOLD velocity
;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Vel.A1", $vel_axis_act[1] * $vel_axis_ma[1] / 100.0)
;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Vel.A2", $vel_axis_act[2] * $vel_axis_ma[2] / 100.0)
;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Vel.A3", $vel_axis_act[3] * $vel_axis_ma[3] / 100.0)
;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Vel.A4", $vel_axis_act[4] * $vel_axis_ma[4] / 100.0)
;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Vel.A5", $vel_axis_act[5] * $vel_axis_ma[5] / 100.0)
;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Vel.A6", $vel_axis_act[6] * $vel_axis_ma[6] / 100.0)
;ENDFOLD
;FOLD effort
;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Eff.A1", $torque_axis_act[1])
;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Eff.A2", $torque_axis_act[2])
;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Eff.A3", $torque_axis_act[3])
;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Eff.A4", $torque_axis_act[4])
;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Eff.A5", $torque_axis_act[5])
;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Eff.A6", $torque_axis_act[6])
;ENDFOLD
; interface state
eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1")
eki_ret = eki_setint("EkiHwInterface", "RobotState/RobotCommand/@Size", eki_ret.buff)
; ekx_ret = eki_checkbuffer("EkiHwInterface.RobotCommand.Pos.A1")
; ekx_ret = EKX_WriteInteger("EkiHwInterface.RobotState.RobotCommand.Size", ekx_ret.buff)

ekx_ret = EKX_WriteInteger("EkiHwInterface.RobotState.RobotCommand.ID", id_count)
EKX_HandleError(ekx_ret)

; Send xml structure
if $flag[1] then ; Make sure connection hasn't died while updating xml structure
eki_ret = eki_send("EkiHwInterface", "RobotState")
ekx_ret = EKX_Send("EkiHwInterface.RobotState")
EKX_HandleError(ekx_ret)
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.
; Tries to read most recent element 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 bool ekx_ret
decl axis joint_pos_cmd
decl bool is_new
decl int rec_id

; ekx_ret = eki_checkbuffer("EkiHwInterface.RobotCommand.Pos.A1")
; if ekx_ret.buff <= 0 then
; return 0
; endif

ekx_ret = EKX_WaitForSensorData(1, "EkiHwInterface.RobotCommand.ID", 10000)
ekx_ret = EKX_GetIntegerElement(1, "EkiHwInterface.RobotCommand.ID", rec_id, is_new)
; if id_count == rec_id then
; return 0
; endif

; if not ekx_ret then
; return 0
; endif

ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A1", joint_pos_cmd.a1, is_new)
ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A2", joint_pos_cmd.a2, is_new)
ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A3", joint_pos_cmd.a3, is_new)
ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A4", joint_pos_cmd.a4, is_new)
ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A5", joint_pos_cmd.a5, is_new)
ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A6", joint_pos_cmd.a6, is_new)

id_count = rec_id

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
decl int ekx_ret_int
decl bool ekx_ret_bool

eki_ret = eki_clear("EkiHwInterface")
eki_ret = eki_init("EkiHwInterface")
eki_ret = eki_open("EkiHwInterface")
ekx_ret_bool = EKX_Close("EkiHwInterface")
ekx_ret_int = EKX_Open("EkiHwInterface")
EKX_HandleError(ekx_ret_int)
$flag[1] = true
end
Loading

0 comments on commit 489130b

Please sign in to comment.