Add support for joint position streaming using R553 (modbus/snpx)

[wwerst]

The Fanuc HMI option (R553) is an alternative method for streaming points to the robot to execute. The HMI (Human Machine Interface) option allows variables, position registers, and many other components on the Fanuc robot to be read and edited. An implementation I have been playing around with for streaming joint positions to the robot is to write a program that repeatedly goes to PR[1] and then asynchronously update PR[1] from HMI. Initial experiments show that there is a ~150ms delay from the time HMI sets the joint position until the robot actually moves to that point (tested via varying amplitude and frequency sinusoids and measuring delta in zero crossing time).

The only program that is needed on the robot is a TP program that goes to PR[1]:

1:  LBL[1] ;
2: J PR[1] 100% CNT100    ;
3:  JMP LBL[1] ;

This issue is to track the discussion and implementation of this joint position streaming method.

[wwerst]

Prior discussions related to this are here: ros-industrial/industrial_core#176

[VictorLamoine]

Very nice, would it be possible to stream other things like sending a CNT value, a move type etc... ?

I'm very interested in getting a working ROS driver for 3D printing, at the moment we are generating ASCII .ls programs but we have memory problems (programs are way too big).

Typically our main looks like this:

/MN
    0:!File = Rectangle;
    1:!Number of layers = 1;
    2:!Deposited material width = 2,000000 mm;
    3:!Height between layers = 2,000000 mm;
    4:!Number of layers = 1;
    5:!Execution duration = 0:0:7;
    6:!;
    7:UFRAME_NUM=0;
    8:UTOOL_NUM=1;
    9:DO[107]=OFF;
   10:DO[100]=OFF;
   11:;
   12:!Layer index 0;
   13:!Layer level 0;
   14:L P[1] 400cm/min CNT80 ;
   15:;
   16:!Polygon start;
   18:L P[2] 200cm/min CNT100 ;
   19:DO[107]=ON;
   20:DO[100]=ON;
   21:DO[100]=ON;
   22:L P[3] 200cm/min CNT80 ;
   23:L P[4] 200cm/min CNT80 ;
   24:L P[5] 200cm/min CNT80 ;
   25:L P[6] 200cm/min CNT80 ;
   26:L P[7] 200cm/min CNT80 ;
   27:L P[8] 200cm/min CNT80 ;
   28:L P[9] 200cm/min CNT80 ;
   29:L P[10] 200cm/min CNT80 ;
   30:L P[11] 200cm/min CNT80 ;
   31:L P[12] 200cm/min CNT80 ;
   32:L P[13] 200cm/min CNT80 ;
   33:L P[14] 200cm/min CNT80 ;
   34:L P[15] 200cm/min CNT80 ;
   35:L P[16] 200cm/min CNT80 ;
   36:L P[17] 200cm/min CNT80 ;
   37:L P[18] 200cm/min CNT80 ;
   38:L P[19] 200cm/min CNT80 ;
   39:DO[100]=OFF;
   40:DO[107]=OFF;
   41:!Polygon end;
   44:L P[20] 200cm/min CNT100 ;
   45:DO[100]=OFF;
   46:DO[113]=ON;
/POS

We need to be able to define the move type (linear or joint), the robot speed, the CNT value and send other commands like CALL, WAIT etc...

I have a developed a C++ Fanuc post processor that is able to generate pretty complex Fanuc programs (handle groups, externals axes, a lot of Fanuc commands) but we are very restricted by the size of the program 😞 The post processor is also able to generate multiple programs by splitting them (with a CALL to the next one at the end) and we use the shadow memory option, but this does not solve the problem, it's just a temporary work-around.

[gavanderhoorn]

@wwerst: this is interesting, but afaik R553 is not a standard option, so whether people can use it will depend on whether their controller has it installed. Karel is always supported, so that is why we use(d) that.

I'm also not too much of a fan making all motions CNT100, but I realise that this may be a limitation of the controller we just have to live with.

I have been playing around with for streaming joint positions to the robot is to write a program that repeatedly goes to PR[1] and then asynchronously update PR[1] from HMI.

We're using something similar in the kuka_eki_hw_interface, but then for KUKAs. I've done some experiments with your suggested approach in the past (but using plain Karel) and the simplicity of it is certainly appealing. I quickly lost interest though as I wasn't able to notice any significant performance increase (ie: max velocity, motion smoothness without going full CNT, etc). Another consequence of the approach you propose is that there is little control over velocity, unless your implementation uses 1/x with a fixed timestep assumption to control things (the current iterations also don't really control velocity, so not sure how much of an issue this is).

Having said all that though, I would definitely be interested in seeing what you have. If you're not comfortable 'releasing' anything now (insofar this repository is actually released) I'd be happy to take a look at a beta or alpha (this repository is officially a beta as well).

[gavanderhoorn]

@VictorLamoine: the option allows you to basically read/write a lot of things on the controller (it's essentially Fanuc's SNP over TCP), but the problem is (I believe) synchronisation of motion with for instance IO and other processes. Since the motion is performed async from updates to the register, it's difficult to determine what the state of the controller/robot is.

[gavanderhoorn]

@wwerst: just to clarify, because I realise my earlier comment could come across as dismissive or negative: I've tried to work around the limitations of Fanuc controllers when used in the way we're trying to use them for quite some time. I do not pretend to know everything, but all my experiments so far have not yielded anything that significantly improves over some more naive approaches.

The bottleneck I believe is the trajectory generator of the Fanuc controller: there is no way around it (at least not from TP), and we can only tweak a few knobs. CNT is the worst thing ever, as it's a single value that appears to influence multiple aspects of a motion's execution (I do understand why it's there though).

So every time I thought I'd found something, tests with real hw showed that it didn't really improve things as much as I'd thought (or hoped). This got stale quite quickly, hence the lack of development of fanuc_driver and eventually fanuc_driver_exp.

So summarising: I'd be very much interested in taking a look at what you've found, and see how it performs.

[gavanderhoorn]

@wwerst: I've changed the title of the issue to mention SNPX, as that is what I believe you are using.

[VictorLamoine]

I don't know anything about this option and I never programmed in Karel, but couldn't a buffer of 5 or 10 lines circumvent the problem with asynchronous tasks? I believe @gavanderhoorn already put a lot of work and effort into Fanuc drivers so I'm pretty sure this is far from simple 🤕

I'll keep reading your discussions, at the moment we cannot do large 3D printing because of the Fanuc robot controller limitations!

[gavanderhoorn]

Let's see what @wwerst has done first.

re: buffers: in theory all possible, but it won't necessarily improve performance. Setting any variable/register/position without handshaking with TP will always be asynchronous.

[gavanderhoorn]

@VictorLamoine wrote:

I'll keep reading your discussions, at the moment we cannot do large 3D printing because of the Fanuc robot controller limitation

please contact me off-line, I may have some info for you.

[wwerst]

@VictorLamoine: We have used a TP program builder as well, and FTP'ed the trajectories over to the robot via this as well. We have similar issues to the ones you mentioned, although we have managed to work around that by roughly figuring out how the Fanuc modifies trajectories, and tuning the trajectories to get around this issue. We are looking to move to a motion streaming implementation because of the ridiculously high latency associated with controlling the robot via FTP'ed TP programs...

@gavanderhoorn: R553 is not an option that is normally installed, but as far as I am aware, neither is User Socket Messaging. This HMI approach (I am communicating using Modbus, the option also supports SNPX) replaces both Karel and USM with the R553 option. By setting CNT100, I am able to update the robot's lower level position setpoint quickly because each movement quickly finishes (move to PR[1] -> already close enough by CNT100's metric -> move to PR[1] again). I then asynchronously drag PR[1] around, or if I want the trajectory to slow down at a point, I stop updating PR[1] or start moving it slower. I would set the movement to CNT1000 if such a thing existed because one limitation with this approach is a large update to PR[1] will cause a movement that won't immediately return. In practice, though, PR[1] updates are small updates.

I am working on preparing a repo with demonstration code, I will hopefully have that out later today (9 pm Pacific).

[gavanderhoorn]

Thanks for the additional details.

I've done something similar in the past with a ros_control hardware_interface. But with Karel and USM. I didn't like the asynchronicity of it though, so I implemented hand-shaking. Not doing that can definitely improve performance, as the TP program is completely fixed and the TP VM can use its look-ahead.

I then asynchronously drag PR[1] around, or if I want the trajectory to slow down at a point, I stop updating PR[1] or start moving it slower.

yes, that is what I meant with "1/x with a fixed timestep assumption to control things".

By setting CNT100, I am able to update the robot's lower level position setpoint quickly because each movement quickly finishes (move to PR[1] -> already close enough by CNT100's metric -> move to PR[1] again).

I understand why you set it to 100, but that doesn't mean I have to like it 😄

I've seen robots completely skip points with high CNTs. It's especially fun when radii around multiple points overlap. Not something I feel is conducive to creating a proper real-time driver for a robot.

I am working on preparing a repo with demonstration code, I will hopefully have that out later today (9 pm Pacific).

Great 👍 I'm looking forward to that.