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 :disappointed: 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 :face_with_head_bandage:

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 :smile:

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 :+1: I'm looking forward to that.

[gavanderhoorn]

@wwerst: any news? I'm curious to see how you approached this.

[wwerst]

Yeah, sorry for the delay. Check out this repo: https://github.com/MisoRobotics/fanuc-hmi-jointstreaming

[wwerst]

I recommend testing the robot in an open area and/or defining DCS zones to prevent the robot from colliding with stuff. The current implementation does not handle e-stops well (can skip around parts in a trajectory).

[gavanderhoorn]

Thanks for making this public.

An interesting approach, and nice work on the Python implementation.

I was unable to get the robot to execute any motion though - but I haven't spent too much time trying.

Some things I noticed:

• enum is used, so if you're on Python 2, enum34 needs to be installed
• similarly, umodbus is not declared as a dependency but is used
• it seems V8.30 is required: $SNPX_PARAM.$NUM_MODBUS did not exist for me on V7.70
• I was not sure whether R553 is the only option needed to get the modbus interface (for SNPX) to work (perhaps R581 and / or R800 are needed as well?)
• I used the following command file to setup all the variables:

  
  SETVAR $SNPX_PARAM.$NUM_MODBUS 2

SETVAR $SNPX_ASG[76].$ADDRESS 16300 SETVAR $SNPX_ASG[76].$SIZE 2 SETVAR $SNPX_ASG[76].$VAR_NAME "$SNPX_ASG[80].$ADDRESS" SETVAR $SNPX_ASG[76].$MULTIPLY 1.0

SETVAR $SNPX_ASG[77].$ADDRESS 16302 SETVAR $SNPX_ASG[77].$SIZE 2 SETVAR $SNPX_ASG[77].$VAR_NAME "$SNPX_ASG[80].$SIZE" SETVAR $SNPX_ASG[77].$MULTIPLY 1.0

SETVAR $SNPX_ASG[78].$ADDRESS 16304 SETVAR $SNPX_ASG[78].$SIZE 40 SETVAR $SNPX_ASG[78].$VAR_NAME "$SNPX_ASG[80].$VAR_NAME" SETVAR $SNPX_ASG[78].$MULTIPLY -1.0

SETVAR $SNPX_ASG[79].$ADDRESS 16344 SETVAR $SNPX_ASG[79].$SIZE 2 SETVAR $SNPX_ASG[79].$VAR_NAME "$SNPX_ASG[80].$MULTIPLY" SETVAR $SNPX_ASG[79].$MULTIPLY 0.0

 - `fanuc_driver` as a package name is already taken ;)  (and also too generic)

I'm uncertain whether my IO setup was correct, the `MAIN` program was not started upon starting the driver. Modbus traffic was flowing though, so probably something wrong in Roboguide.

[wwerst]

I'll look at fixing the dependency issues. R581 and R800 aren't needed (I am running without them). I am running on version 8.30/P33 and also 9.x on some robots. There are two likely sources of the issue: DI119,DI120,DI185 are not mapped to something that is all ON (I have mapped to safety scanner inputs), or the system is not properly configured for respecting UOP inputs. In order to configure for startup over UOP, you need to configure a few things in System -> Config. I'll update those in the Readme of repo, and also create a branch that assumes program has automatically started and doesn't read DI[119,120,185] to simplify testing.

[wwerst]

Check out this branch for auto startup disabled and io reading disabled: https://github.com/MisoRobotics/fanuc-hmi-jointstreaming/tree/demo/no-io-no-auto-startup

[gavanderhoorn]

Thanks. I had the 'safety ios' mapped to the always-on rack, but the rest of the setup was incomplete (prog select, etc). I'll try this tomorrow again.

[gavanderhoorn]

Really nice of you guys to open-source this btw.

[wwerst]

Yeah, no problem. It helps us all to be able to interface with these robots better. Any luck with getting this to work for you?

[gavanderhoorn]

Sorry for the delay -- it's not lack of enthusiasm or interest, I just had to prioritise some other work (unfortunately).

[sulabhkumra]

@wwerst Thank you for sharing the code. I am trying to run your program using the branch with disabled auto-startup and safety zone reading. I have configured the SNPX as described in the readme. But when I try to do a roslaunch, I get the following error from umodbus. Can you please help me to figure out what can I be doing wrong?

File "/home/user/catkin_ws/src/fanuc_driver/src/fanuc_driver/modbus_interface.py", line 113, in __send_modbus_message result = tcp.send_message(adu, self.__modbus_socket) File "build/bdist.linux-x86_64/egg/umodbus/client/tcp.py", line 262, in send_message File "build/bdist.linux-x86_64/egg/umodbus/client/tcp.py", line 247, in raise_for_exception_adu File "build/bdist.linux-x86_64/egg/umodbus/functions.py", line 111, in pdu_to_function_code_or_raise_error umodbus.exceptions.IllegalDataAddressError: The data address received in de request is not an allowable address for the server.

[Msantaji1]

@VictorLamoine did you get any solution on this issue..

I have solution for it.. if you want contact me in personal

[gavanderhoorn]

@Msantaji1: afaik, @VictorLamoine works in an entirely different field now, so I'm not sure whether he'd still be interested.

I'd be interested in what you have figured out though.

[Didier-Surface]

@Msantaji1 I would like to contact you, but can't find your personal info. Could you send me an email? Thanks

[gavanderhoorn]

If at all possible, could we try to keep discussions here on the issue tracker?

That way we all benefit. Seems only fair with the enormous amount of work already put into all of this by previous developers.

Whatever @Msantaji1 discovered / figured out can't be that sensitive.