eki_hw_interface: An EKI-based hardware interface. #110

BrettHemes · 2018-02-08T02:08:15Z

An EKI-based Kuka driver targeted at use with ros_control. Based off of existing kuka_rsi_hw_interface but with a "soft" real-time Ethernet UDP/IP interface. Streaming happens at ~50 Hz (vs the RSI interface's 250Hz) with all commanded setpoints executed as fast as possible on the KRC. In this configuration, any significant latency from the ROS side simply results in the robot safely decelerating while keeping the server running (i.e., no faults or program stops). 50 Hz streaming from the ROS PC (or really enough to prevent the advanced run from triggering a stop) results in smooth motion and no stutters.

Depending on the displacement of the commanded waypoints, I imagine that full joint speeds will not be reachable due to the 5-step look-ahead limit of the advance run on the controller (further testing and performance graphs planned to confirm this). Despite this, however, I believe that this is totally usable when less-than-maximum performance is required (use RSI in that case...) and will fill the use-case gap identified in #77. In fact the observed performance exceeded my original expectations of the approach. In my limited testing thus far the motion was significantly smooth and correct using MoveIt! RViz generated trajectories (using RRTkConfigDefault) on a real Kuka Agilus robot. Was using ros_control in Ubuntu 16.04 with the default (i.e., non-low-latency) kernel.

gavanderhoorn · 2018-02-08T13:32:03Z

Unfortunately I have no way of testing this, as I don't have any hw that has this interface.

My first question would be: any guestimate as to what sort of performance we can expect from this? You mention that you believe it will/might not be full performance, but can you put any nrs on it?

I can imagine that it would be quite important to get a feeling for any performance differences when trying to use this for continuous processes that require specifiek (EEF) velocities fi.

gavanderhoorn · 2018-02-08T13:32:59Z

@durovsky @simonschmeisser and @ivareri, you might also be interested in this.

simonschmeisser · 2018-02-08T13:46:19Z

thanks for the hint @gavanderhoorn Unfortunately we currently don't have access to any kuka and they were not cooperative wrt licencing a simulation

BrettHemes · 2018-02-09T01:21:22Z

My first question would be: any guestimate as to what sort of performance we can expect from this? You mention that you believe it will/might not be full performance, but can you put any nrs on it?

I plan to measure this. The robot I want to use is getting moved, so probably next week or the week after. A single sent point via the interface (i.e., not using the ros_control-based node wrapper provided) will execute at full speed, no problem. My concern is that the higher density streaming (a la ros_control) might reduce the effective horizon of the lookahead via the advance run and limit max velocity but I am not sure. There is a chance I am over thinking it; we shall see :)

That said, the performance should be better than any of the KUKAVARPROXY implementations I am aware of. They use a single variable as the target position that gets updated in time via the proxy interface. This limits the advance run to a single point (i.e. $advance=1). The EKI approach in question leverages the socket receive buffer and can see up to five points ahead (i.e. $advance=5, the maximum) as they become available thus resulting in higher-speed, smoother motion overall.

I can imagine that it would be quite important to get a feeling for any performance differences when trying to use this for continuous processes that require specifiek (EEF) velocities fi.

Indeed. The data will help understand what people are getting. A trajectory downloading version/mode would also be a relatively easy extension. I am also entertaining the idea of a hybrid extension that can take streamed trajectories but also invoke internal KRL commands (similar to sending ur_script commands via socket) when necessary/desirable. This would get you nice reactionary MoveIt/ros_control style functionality but also rock-solid native velocity controlled motion when required (i.e., processing moves). I need to hash through the details but I envision these all being on different ports and all available at any given time to the user.

gavanderhoorn

Thanks for the PR 👍. This is a welcome addition to this repository.

I added a lot of comments, but I was just commenting on everything I saw, so don't take it as there being many things that need changes.

The RSI hw iface could use some TLC, so I also looked for things that we could avoid with the EKI iface in this PR.

gavanderhoorn · 2018-02-13T14:37:21Z

kuka_eki_hw_interface/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(kuka_eki_hw_interface)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")


We should probably use add_compile_options(-std=c++11) here instead.

Agreed; copypasta from the rsi_hw_interface...

gavanderhoorn · 2018-02-13T14:39:25Z

kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h

+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Brett Hemes (3M)


You probably already checked, but is the copyright yours, or 3M's?

You're always the author of course.

Interesting 😄 3M didn't catch this but yes, this should be changed

gavanderhoorn · 2018-02-13T14:50:58Z

kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h

+{
+
+private:
+


empty line seems redundant (same on line 56)

gavanderhoorn · 2018-02-13T14:57:16Z

kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h

+  ~KukaEkiHardwareInterface();
+
+  void start();
+  void configure();


There is a RobotHW::init(..) in kinetic-devel. Wondering if we can somehow already start using that, to facilitate porting later on (although introducing our own init() here will shadow the one in RobotHW under Kinetic).

See: https://github.com/ros-controls/ros_control/blob/81ed9edf2a399c1aae33fc5b603c2215dde5ffbe/hardware_interface/include/hardware_interface/robot_hw.h#L70-L80

Can you elaborate here?

The kinetic-devel version of RobotHW in ros_control defines an init(..) method that should be used to perform any initialisation actions the hw iface may need. As there was nothing in Indigo, we have a custom configure().

I was wondering whether it would be an idea to instead of keeping our configure(), start using init(..) already, so when we migrate to kinetic-devel of ros_control with this repository we are already up-to-date.

gavanderhoorn · 2018-02-13T15:00:13Z

kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h

+  void start();
+  void configure();
+  bool read(const ros::Time time, const ros::Duration period);
+  bool write(const ros::Time time, const ros::Duration period);


Can we make these references?

I was going to originally but the rsi_hw_interface and Adolfo's examples are by value so I did the same.

Do you mean Dave's boilerplate? His code does use references.

Upstream kinetic-devel also has read(..) and write(..) and those also use references (here). I cannot really come up with a reason not to use them here tbh.

I guess I was looking at some old wiki examples? I don't remember. Maybe here: https://github.com/ros-controls/ros_control/wiki/joint_limits_interface

Aside, but I also have a bad habit of not always realizing I am on an older distributions API documentation page 🙄

By reference is the right answer here. Thanks for the links.

gavanderhoorn · 2018-02-13T15:35:22Z

kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp

+  TiXmlDocument xml_out;
+  TiXmlElement* robot_command = new TiXmlElement("RobotCommand");
+  TiXmlText* empty_text = new TiXmlText("");
+  robot_command->LinkEndChild(empty_text);   // force <RobotCommand></RobotCommand> format (vs <RobotCommand />)


The construction of this XML document seems to be always the same. Would it make sense to move that to either the ctor or some other initialisation method and store xml_out (maybe member variable)?

Same for TiXmlPrinter below.

gavanderhoorn · 2018-02-13T15:36:38Z

kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp

+{
+  ROS_INFO_NAMED("kuka_eki_hw_interface", "Starting Kuka EKI hardware interface...");
+  // TODO(BrettHemes): Error handling?
+  // TODO(BrettHemes): Do these block?  Look into implementing non-blocking receives


See earlier comment about TODOs.

gavanderhoorn · 2018-02-13T15:38:19Z

kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp

+                                             command_server_port_});
+
+  // Initialize joint_position_command_ from initial robot state (avoid bad (null) commands before controllers come up)
+  while (!socket_read_state(joint_position_command_));


I don't know whether it can happen, but perhaps we should add some timeout or maximum retry mechanism here to avoid infinite waits.

Yeah, I will look into this.

gavanderhoorn · 2018-02-13T15:39:08Z

kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp

+  else
+  {
+    ROS_ERROR("Failed to get EKI addresses/ports from parameter server!");
+    throw std::runtime_error("Failed to get EKI addresses/ports parameter server.");


Suggestion: mention which parameters we're looking for here (ref #106).

gavanderhoorn · 2018-02-13T15:43:06Z

kuka_eki_hw_interface/src/kuka_eki_hw_interface_node.cpp

+{
+  ros::init(argc, argv, "kuka_eki_hw_interface");
+
+  ros::AsyncSpinner spinner(1);


Should this be 2? I remember several implementations using 2 threads to avoid (ROS event) operations from blocking the control loop (Dave's ros_control_boilerplate fi (here).

gavanderhoorn · 2018-02-25T14:19:27Z

🛎️ ?

BrettHemes · 2018-02-25T16:03:43Z

Yes yes! I am still here. I got sick and then was on travel. I will followup this week 😃

BrettHemes · 2018-02-28T16:51:38Z

@gavanderhoorn wonderful comments; thank you very much! There are some that I need a bit more help with please. I have begun addressing the rest and will update the PR as I knock them off.

gavanderhoorn · 2018-03-02T11:57:07Z

I've replied to some of your questions @BrettHemes. Thanks for taking care of my comments.

BrettHemes · 2018-03-03T23:41:51Z

Still working on addressing all the comments but had some time Friday to generate some quick plots with rqt_plot and MoveIt!/RViz using a physical Kuka KR6 R700 sixx robot. I have to say, the results were surprisingly good and help relieve my previous performance limit concerns 😃.

To generate the trajectories I used the RViz MotionPlanning plugin with RRTkConfigDefault and alternated between two poses:

With the defaults, the interface keeps up no problem with a constant lag of ~0.25 seconds:

Slowing things down by setting the velocity and acceleration scaling factors to 0.01 was even better, producing the following:

I believe the default behavior without acceleration limits specified is to use the (unrealistic) value of 1.0 rad/s/s. To test something a bit more representative I set the acceleration limits to 10x of the max KR6 r700 sixx velocities and then ran a set of trajectories tweaking the velocity and acceleration scaling limits.

Full tilt (acc_scale = 1.0; vel_scale = 1.0):

acc_scale = 0.6; vel_scale = 1.0

acc_scale = 0.6; vel_scale = 0.6

The 10x, full speed run exhibits some error accumulation (in addition to the previously observed constant lag). This is probably due to BOTH hardware interface performance limits AND the fact that the 10x acceleration limits are a bit much. By scaling down the max velocity and acceleration the accumulating error goes away again leaving only the bounded error due to the time shift.

I ran these in some spare time and they are no means comprehensive but are encouraging nonetheless. Qualitatively, it seems that commanded velocities will be adhered to with realistic acceleration settings and velocities on the order of 60-80% of max. Not bad for a "soft" real-time streaming interface...

gavanderhoorn · 2018-03-04T07:07:03Z

kuka_eki_hw_interface/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
  hardware_interface
  joint_limits_interface
  roscpp
+  angles


Could you move angles to the top? Let's keep these COMPONENTS sorted alphabetically.

gavanderhoorn · 2018-03-04T07:07:11Z

kuka_eki_hw_interface/CMakeLists.txt

@@ -25,6 +26,7 @@ catkin_package(
    hardware_interface
    joint_limits_interface
    roscpp
+    angles


gavanderhoorn · 2018-03-04T07:07:44Z

kuka_eki_hw_interface/package.xml

@@ -20,4 +20,5 @@
  <depend>hardware_interface</depend>
  <depend>joint_limits_interface</depend>
  <depend>roscpp</depend>
+  <depend>angles</depend>


Sorting here again.

And angles is a header only library, so no need to make it a depend.

Note: velocty and effort value population in the krl interrupt currently disabled due to it inducing stuttering. Interrupt service needs to be shorter... could re-add possibly after some optimization and further testing...

…e server

…bility

BrettHemes · 2018-03-08T15:44:29Z

OK I think I got most of @gavanderhoorn's suggestions addressed and the code is all the better for it.

Some things I did not fix / remaining nuances:

Tiny XML document construction in send/recieve.
I tried moving these objects to be member variables but had trouble getting them to update. The first construction worked but then subsequent read/writes were not modifying the xml values. I don't have enough experience with the library to know what the issue was and bailed in the interest of other fixes. How much is really saved by moving this?
I added broadcasting of the full state (position, velocity, and effort) but then had issues with the controller struggling to populate the xml structure for transmission resulting in unacceptable stuttering motion (due most likely to the EKI buffer emptying mid-move and the subsequent advance run stop). Part of this is due to the fact that the joint velocities are reported as a % of max (which is stored in another system variable) and thus requires a multiplication and division for each joint in addition to the various variable reads. Even sending raw values introduced some stutter... Long story short, reading and loading ALL of the state info results in an interrupt service routine that is not sufficiently short. I left the code in to send the whole state but currently have the velocities and efforts commented out. I think there is room for some optimizations that will make this work (and have a couple of approaches in mind) but would like to save this for a later PR.
I implemented a timeout on the socket read using Boost's somewhat involved tutorial on the matter (see here). It works well. I have set the timeout to a constant value of 5 seconds. Thoughts?
Regarding the potential buffer overrun in the socket read. The Boost buffer abstraction prevents this and won't read more than the provided buffer's length. Also, there is no real way to query the size of received data via the udp interface so I don't really know how to do this otherwise.

@gavanderhoorn Would you mind giving the code another once over at this point?

gavanderhoorn · 2018-03-13T21:04:00Z

@BrettHemes wrote:

1 . Tiny XML document construction in send/recieve.
I tried moving these objects to be member variables but had trouble getting them to update. The first construction worked but then subsequent read/writes were not modifying the xml values. I don't have enough experience with the library to know what the issue was and bailed in the interest of other fixes. How much is really saved by moving this?

Repeated construction of these TinyXML objects probably does not incur a very large overhead. Let's leave this as it is for now.

2 . I added broadcasting of the full state (position, velocity, and effort) but then had issues with the controller struggling to populate the xml structure for transmission resulting in unacceptable stuttering motion (due most likely to the EKI buffer emptying mid-move and the subsequent advance run stop). Part of this is due to the fact that the joint velocities are reported as a % of max (which is stored in another system variable) and thus requires a multiplication and division for each joint in addition to the various variable reads. Even sending raw values introduced some stutter... Long story short, reading and loading ALL of the state info results in an interrupt service routine that is not sufficiently short. I left the code in to send the whole state but currently have the velocities and efforts commented out. I think there is room for some optimizations that will make this work (and have a couple of approaches in mind) but would like to save this for a later PR.

hm, unfortunate. But as you say: let's save this for a later PR.

3 . I implemented a timeout on the socket read using Boost's somewhat involved tutorial on the matter (see here). It works well. I have set the timeout to a constant value of 5 seconds. Thoughts?

Other than "holy ****"? :)

Minor suggestion: we could make this time-out perhaps configurable using a ROS parameter.

4 . Regarding the potential buffer overrun in the socket read. The Boost buffer abstraction prevents this and won't read more than the provided buffer's length. Also, there is no real way to query the size of received data via the udp interface so I don't really know how to do this otherwise.

Ok. It sounds like this is sufficiently covered then.

… default)

BrettHemes · 2018-03-29T14:02:06Z

@gavanderhoorn OK, what are your thoughts? I added the ability to set the socket read timeout via parameter and I think we are getting close to being mergable.

Also, extrapolating from previous discussions, I assume all of the tweaks since the original PR should get squashed into the "initial commit" commit when it comes time to merge? Am I correct here?

gavanderhoorn · 2018-03-29T14:05:52Z

@BrettHemes wrote:

@gavanderhoorn OK, what are your thoughts? I added the ability to set the socket read timeout via parameter and I think we are getting close to being mergable.

yes, I think this is quite ok like this.

re: merge: I was hoping to get some time on an Agilus with EKI (which we apparently have) to test this out on real hw. Not that I don't trust you, but I just like to do that. That appears more difficult than I anticipated, so it doesn't make sense to hold off merging.

Also, extrapolating from previous discussions, I assume all of the tweaks since the original PR should get squashed into the "initial commit" commit when it comes time to merge? Am I correct here?

Yes, I typically like to do that, unless you absolutely want to keep the history of those changes. As they are all fixups though, I never really see the value. In a way, the end result is what the PR "should ideally have been".

BrettHemes · 2018-03-29T15:00:05Z

I was hoping to get some time on an Agilus with EKI (which we apparently have) to test this out on real hw. ... That appears more difficult than I anticipated, so it doesn't make sense to hold off merging.

What time frame are you looking at? A second hw test would be nice...

In a way, the end result is what the PR "should ideally have been".

Agreed

gavanderhoorn · 2018-04-14T11:09:26Z

@BrettHemes wrote:

What time frame are you looking at? A second hw test would be nice...

I've just sent the lab with the agilus with EKI an email.

If I don't get a response early next week, I'll merge without testing myself.

gavanderhoorn

Robot is too busy, and I don't want to hold this up too long. It's been quite some time already.

Thanks for the contribution @BrettHemes, this is a very nice addition to this repository.

👍 🍔 🍻

eki_hw_interface: Initial commit. An EKI-based hardware interface.

2626b08

BrettHemes requested a review from gavanderhoorn February 8, 2018 02:08

BrettHemes mentioned this pull request Feb 8, 2018

EKI instead of RSI #77

Closed

BrettHemes changed the title ~~eki_hw_interface: Initial commit. An EKI-based hardware interface.~~ eki_hw_interface: An EKI-based hardware interface. Feb 8, 2018

gavanderhoorn requested changes Feb 13, 2018

View reviewed changes

BrettHemes added 4 commits March 1, 2018 11:00

PR fix: copyright

94da463

PR fix: add_compile_options

d1f4e8f

PR fix: header whitespace and readme backticks / syntax highlighting

5012e5f

PR fix: (invalid) descript ip address placeholders for EKI xml docs

b064062

gavanderhoorn reviewed Mar 4, 2018

View reviewed changes

BrettHemes force-pushed the eki_hw_interface branch from 9fea295 to 2eb02d9 Compare March 4, 2018 16:30

gavanderhoorn mentioned this pull request Mar 5, 2018

kuka mxautomation_hw_interface support #80

Closed

BrettHemes force-pushed the eki_hw_interface branch from 603ef7d to 0e21dc8 Compare March 5, 2018 17:43

BrettHemes added 6 commits March 6, 2018 16:32

PR fix: removing depend on std_msgs

c9d4313

PR fix: use angles package for deg/rad conversions

5d43801

PR fix: use 2 threads for spinning

9ffb522

PR fix: improve err msg for missing ip or port parameters

eba948e

PR fix: ros_control pass read/write params by reference

94258ba

PR fix: socket_read_state return/fail early

9b7117e

BrettHemes added 4 commits March 6, 2018 16:32

PR fix: TODO's -> Github issues...

418c9bd

PR fix: make socket_read_state in_buffer static

f756ecb

PR fix: removing unused velocity/effort command vectors

c52f0aa

PR fix: krl/kuka_eki_hw_interface.src cleanup

93f6eb2

BrettHemes force-pushed the eki_hw_interface branch from 08e6f79 to 93f6eb2 Compare March 6, 2018 22:44

BrettHemes added 8 commits March 7, 2018 08:41

PR fix: Adding velocity and effort to transmitted state

3743152

Note: velocty and effort value population in the krl interrupt currently disabled due to it inducing stuttering. Interrupt service needs to be shorter... could re-add possibly after some optimization and further testing...

PR fix: Adding velocity and effort to transmitted state

cc7ebdf

PR fix: combining state and command servers into single EkiHwInterfac…

de8b9fb

…e server

PR fix: return/fail early in KRL

1f5936d

PR fix: use init() in place of configure() for future Kinetic compati…

c55f307

…bility

PR fix: Check for null FirstChildElement results

349c8de

PR fix: adding comments to EKI xml configuration file

836743c

PR fix: adding timeout (5 seconds) to eki_read_state() socket call

7bff4fd

BrettHemes and others added 2 commits March 8, 2018 12:17

Merge branch 'indigo-devel' into eki_hw_interface

28ceef3

Merge branch 'indigo-devel' into eki_hw_interface

6f9167b

PR fix: making socket timeout optionally settable via parameter (with…

028c017

… default)

gavanderhoorn mentioned this pull request Mar 29, 2018

Switching 'modes' with RSI #116

Closed

Merge branch 'indigo-devel' into eki_hw_interface

1e81e27

Merge branch 'indigo-devel' into eki_hw_interface

0e99fb4

gavanderhoorn approved these changes Apr 20, 2018

View reviewed changes

gavanderhoorn merged commit e8ddefd into ros-industrial:indigo-devel Apr 20, 2018

BrettHemes deleted the eki_hw_interface branch July 12, 2018 14:41

FSund mentioned this pull request Sep 30, 2022

[guidance wanted] Tuning EKI interface for KR 10 R1100-2 #228

Closed

		{

		private:

eki_hw_interface: An EKI-based hardware interface. #110

eki_hw_interface: An EKI-based hardware interface. #110

Conversation

BrettHemes commented Feb 8, 2018 • edited Loading

gavanderhoorn commented Feb 8, 2018

gavanderhoorn commented Feb 8, 2018

simonschmeisser commented Feb 8, 2018

BrettHemes commented Feb 9, 2018

gavanderhoorn left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gavanderhoorn Feb 13, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gavanderhoorn commented Feb 25, 2018

BrettHemes commented Feb 25, 2018

BrettHemes commented Feb 28, 2018

gavanderhoorn commented Mar 2, 2018

BrettHemes commented Mar 3, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

BrettHemes commented Mar 8, 2018 • edited Loading

gavanderhoorn commented Mar 13, 2018 • edited Loading

BrettHemes commented Mar 29, 2018

gavanderhoorn commented Mar 29, 2018

BrettHemes commented Mar 29, 2018 • edited Loading

gavanderhoorn commented Apr 14, 2018

gavanderhoorn left a comment

Choose a reason for hiding this comment

BrettHemes commented Feb 8, 2018 •

edited

Loading

gavanderhoorn Feb 13, 2018 •

edited

Loading

BrettHemes commented Mar 8, 2018 •

edited

Loading

gavanderhoorn commented Mar 13, 2018 •

edited

Loading

BrettHemes commented Mar 29, 2018 •

edited

Loading