Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

KRC2 port for kuka_eki_hw_interface #205

Open
wants to merge 1 commit into
base: melodic-devel
Choose a base branch
from
Open

KRC2 port for kuka_eki_hw_interface #205

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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