Overview

This document outlines at a high level how to use FANUC iRPickTool with a Gocator sensor. FANUC iRPickTool (often referred to as iRPick or iRPickPRO) is a specialized software package by FANUC designed to simplify and manage high-speed, intelligent robotic picking and packing from moving conveyors. Out of the box, this tool is specifically made for a FANUC 2D or 3D Vision camera. However, it is also possible to use an external vision camera in place of the FANUC Vision Camera. 

There are three main goals related to this integration:

1. KAREL ASCII Integration between the robot and the Gocator
2. Translating coordinates between the Gocator coordinate system (left-hand) and the FANUC Coordinate system (right-hand)
3. Understanding conveyor tracking math between two systems. There will be an encoder offset between your Gocator scan data and the start of the robot pick area that needs to be compiled.  

External Material to Review Before Getting Started 

Webinars of interest:

1. iRPickTool - How to Successfully Integrate a High-speed Picking System (Important)

2. iRPickTool - Integrating External Vision with a Custom Sensor Task

   FANUC Webinar Link (FANUC account needed for access)

Gocator ASCII Integration Guide

FANUC iRPickTool Manual (FANUC account needed for access)

Needed Materials For Integration

1. Fanuc VO-1800-023 iRVision Grid Calibration Target
2. Alignment Bar with a hole 
3. ROBOGUIDE License (For writing Karel Code)
4. FANUC iRVision Camera 
5. FANUC ASCII Add-on (Gocator ASCII Integration Guide)
6. Encoder tied to conveyor for tracking
7. Multiplexer for encoder signal (Optional, but recommended) - Needed to split one encoder signal to be sent to the robot and the Gocator Sensor
8. External power supply for sending digital input + resistors (Wiring documentation)

It is highly recommended to develop this in a mock cell before deploying to production. A great starting place is to have a cell setup with a FANUC vision camera and the basic iRRPickTool. Once you have a working mock cell, it is easier to integrate an external sensor, knowing that the cell's core functionality is intact. 

Configuring Gocator Sensor Settings for Using iRPickTool

At this point, it is assumed that you have a basic, functioning iRPickTool cell for testing. Having a FANUC vision camera is useful because it will allow you to verify your cell before integrating a Gocator sensor. Some important settings need to be set on the Gocator side to mimic the FANUC 2D camera. 

1. Fixed Length Scans - Fixed-length scans mimic the "window" you would receive from a 2D camera. Part detection does not work when using the iRPickTool when multiple parts can be present side by side. 
   
   To ensure that no parts are missed between two fixed-length scans (part falls in the middle of two scans), it may be necessary to use the surface extend tool. Extend Tool Documentation
    
2. Digital Output for handshake between sensor and robot - A handshake will need to be established to allow the robot to understand when new results have been acquired. One way to accomplish this is by sending a digital output from the sensor to the robot controller. From there, the robot controller will run the custom sensor task and add all found parts to the queue when it receives the digital input.  
   
    
3. Tools to Extract XYZWRP of found parts - If we want to send pick information, we need to have a toolchain setup to extract the proper coordinate information from each part. From there, the feature robot pose tool can be used per part to easily compile this information. Meaning, if you expect to find a max of 5 parts in each scan, you will need five feature robot pose tools. Furthermore, you will have five sets of coordinate information to send over to the robot controller. 
   
    
4. Understanding the Coordinate System of a Gocator - All Gocators operate using the left-hand rule. This can be a major gochya, as all FANUC's operate under the right-hand rule. Meaning, it is important to translate your coordinate information accordingly based on this. There are no settings to change related to this; this behavior cannot be changed. 
    

5. ASCII Output - A custom ASCII output can be used to send over all the coordinate information required for the integration. See the Gocator ASCII Integration guide for more information on sending measurements to the robot controller. 
   
   The following information must be sent to the robot controller to accomplish this integration:

   1. Frame Index - This value will tick upwards for every new scan that is acquired. Allows the controller to track scans and place data in the queue properly.    
   2. XYZWPY (per part)- This is six measurements from the feature robot pose tool that need to be sent over for each part. For example, if you have three parts in one scan, this would represent eighteen measurements from three robot pose tools.
   3. Encoder Value (optional) - For each scan, an encoder value can be sent to the robot, or you can capture the current encoder position from the robot. 
       

   Note: sending an Invalid Value of 0 can help mitigate errors on the FANUC side. This is because when no results are received (INVALID in the toolchain), all 0s will be sent. 
   

How to Align your Gocator sensor for use with iRPickTool

One of the main parts of this integration has to do with how you align your Gocator sensor relative to a FANUC user frame. Ideally, we want to use an alignment bar with a hole in it, so we can easily control the origin location of our Gocators coordinate system. 

Aligning the Gocator Sensor

Bar with a hole in it: 

When performing this kind of alignment, the hole in the alignment target represents the new X, Y, and Z origin location. This can be used in combination with a user frame that is taught with the same origin location, further downstream. Then the only difference between these two created origin locations is an encoder offset. 

A bar with a hole in it can be used in conjunction with the FANUC coordinate grid to sync the origins of the Gocator and robot's coordinate systems. This can be done by first placing the bar on top of the coordinate grid under the Gocator laser line. 
 
FANUC Coordinate Grid Information:

Alignment Bar Origin: 

Overlaying the two origin locations: 

First, align the sensor with the targets placed together upstream from the conveyor pick location, underneath the Gocator laser line. During this step, the conveyor should be stationary to successfully perform a stationary bar alignment. As shown in the image above, the two origin locations should be synced between the two alignment/calibration artifacts, and the laser line should run through the middle of the alignment bar and both origin locations. Run the alignment in the Gocator and note the encoder position before jogging the conveyor.

Next, remove the alignment bar from the FANUC calibration grid, but ensure the grid does not move at all on the conveyor. Leave the FANUC calibration grid in place and jog the conveyor down to the start of where you want the robot to be able to pick parts from. 

Next, train a user frame on the untouched calibration grid within the pick region of the robot. You should have only jogged the conveyor downwards, leaving the origin locations synced, with an encoder offset between them. From there, note the encoder position of where you train the new user frame. This will allow the encoder offset to be calculated between the Gocator and the new user frame location. This value will be needed for a successful implementation. 

Generation of an offset between the vision camera and the user frame location. Encoder Position One is where you would align the Gocator system. Encoder position two is where the user frame would be generated. 

The FANUC external vision webinar offers additional information on how to perform this conveyor tracking. 

Custom Sensor Task Template

One of the last steps is to configure a custom sensor task specific to your integration. The purpose of a custom sensor task is to digest information sent over from the LMI Gocator and feed this information into the iRPickTool queue as a FANUC vision register. For more in-depth information on custom sensor tasks, please refer to the FANUC external vision integration webinar. 

The code below is from a custom sensor task that was developed with a Gocator sensor. The starting code for this example was taken from the FANUC template provided in the external vision interfacing webinar. This code will need to be modified based on your specific integration and toolchain. 

------------------------------------------------------------------------
PROGRAM pksnlibcst
-- You cannot change this KAREL program name
------------------------------------------------------------------------
%COMMENT='CustomSenLib'
%SYSTEM
%NOBUSYLAMP
%NOLOCKGROUP
%NOPAUSESHFT
%NOPAUSE=ERROR+COMMAND+TPENABLE
%NOABORT=ERROR+COMMAND+TPENABLE
%ENVIRONMENT cvis           -- Builtins for vision
%ENVIRONMENT ltrk           -- Builtins for line tracking
%ENVIRONMENT pksndef        -- Variable for sensor
%ENVIRONMENT regope         -- karel standard functions for registers
%ENVIRONMENT sysdef
%ENVIRONMENT lntkdef
%INCLUDE pksnglbl
%INCLUDE vierrdef           -- Vision error definition. VEXE_E_NOMOR


CONST
  eip_xfer_dly = 200
  max_str_len = 128
  version_id = 'v1.4' 
  maxprod = 1
  divide_fctor = 1000       --divide by factor for x, y, z, w, p, r
   
VAR
  v3p_gi_x, v3p_gi_y, v3p_gi_r IN CMOS : INTEGER -- index number for X, Y and R values to read from GIN
  v3p_gi_trgid IN CMOS : INTEGER --index number for trigger ID read from GIN
  v3p_gi_nf IN CMOS : INTEGER --index number for number found to read from GIN)
  v3p_gi_max IN CMOS : INTEGER --index number for max found product
  console : FILE
  v3p_gridDist IN CMOS: REAL
  v3p_uf_num IN CMOS: INTEGER
  v3p_lnschid IN CMOS : INTEGER
  v3p_timeout IN CMOS : INTEGER
  v3p_version : STRING[5]
  v3p_posreg IN CMOS : INTEGER
  v3p_lastTrig IN CMOS : INTEGER
  v3p_currTrig IN CMOS: INTEGER
  v3p_maxprod : INTEGER
  timeout IN CMOS : INTEGER
  debug : BOOLEAN
  prog_index : INTEGER
  nfound      : INTEGER
  v3p_xyz3Pos      : XYZWPR
  
------------------------------------------------------------------------  
ROUTINE WriteConsole (p_message : STRING; p_real_str: STRING; p_real: REAL;
                     p_int_str: STRING; p_int: INTEGER; p_bool_str: STRING;
                     p_bool: BOOLEAN)                   FROM PKUTILS  
------------------------------------------------------------------------  
------------------------------------------------------------------------
ROUTINE GetTimeDiff(piLstFastClk : INTEGER) : INTEGER
------------------------------------------------------------------------
BEGIN
     RETURN( ($FAST_CLOCK - piLstFastClk) * $TICK_RATE )
END GetTimeDiff
------------------------------------------------------------------------

------------------------------------------------------------------------
ROUTINE get_ecnt_idx(indexer_id : INTEGER) : INTEGER FROM pksnlib
------------------------------------------------------------------------
------------------------------------------------------------------------
ROUTINE update_nfnd(cond_id : INTEGER; num_found : INTEGER) FROM pksnlib
------------------------------------------------------------------------
------------------------------------------------------------------------
ROUTINE start_srv(cond_id : INTEGER) FROM pksnlib
------------------------------------------------------------------------
------------------------------------------------------------------------
ROUTINE incre_wid(work_id : INTEGER) FROM pksnlib
------------------------------------------------------------------------
------------------------------------------------------------------------
ROUTINE put_queue(cnv_id : INTEGER; stn_id : INTEGER; work_id : INTEGER; enc_count : INTEGER; model_id : INTEGER; offset : XYZWPR; found_pos : ARRAY OF XYZWPR; meas : ARRAY OF REAL; duplicated : INTEGER; stat : INTEGER) FROM pksnlib
------------------------------------------------------------------------
------------------------------------------------------------------------
ROUTINE CnvGI_ToReal(piToConvert, piDivideBy : INTEGER) : REAL
------------------------------------------------------------------------
BEGIN
     RETURN(piToConvert / piDivideBy)
END CnvGI_ToReal

------------------------------------------------------------------------
ROUTINE put_part_cst(cond_id : INTEGER; stat : INTEGER)
-- Action: "Put part in queue"
-- You cannot change this function name
------------------------------------------------------------------------
VAR
  model_id    : INTEGER
  enc_count   : INTEGER
  offset      : XYZWPR
  i           : INTEGER
  dmy_ofs     : XYZWPR
  dmy_meas    : ARRAY[MAX_MEAS] OF REAL
  dmy_int     : INTEGER
  dmy_real : REAL	
  dmy_regBool :BOOLEAN
  duplicated  : INTEGER
  nframeid      : INTEGER
   xCord: REAL
  yCord: REAL
  zCord: REAL
  Yaw: REAL
  status: INTEGER
  xyzTrkGrdFrm, --lmi tracking frame
  xyzTrkFrmPos, --tracking schedule tracking position
  xyzTpRefPos, --pr from tp tracking program
  xyzOffset : XYZWPR --offset for tp tracking program
  found_pos   : ARRAY[MAX_NUM_VIEW] OF XYZWPR
  real_flag : BOOLEAN
  int_val : INTEGER
  real_val : REAL
  j : INTEGER
  gi_offset : INTEGER
  f_clock : INTEGER
  
BEGIN
  --Initialize 
  v3p_version = version_id  
  IF UNINIT(v3p_maxprod) THEN v3p_maxprod = maxprod; ENDIF
  IF UNINIT(v3p_posreg) THEN v3p_posreg = 57; ENDIF
  IF UNINIT(v3p_lastTrig) THEN v3p_lastTrig = 0; ENDIF
  IF UNINIT(timeout) THEN timeout = 0; ENDIF
  
  --initialize debug
  IF UNINIT(debug) THEN debug = TRUE; ENDIF
  IF debug THEN
  	  OPEN FILE console ('RW', 'CONS:')
  ENDIF 
  -- Initialize number of found part to zero
  nfound = 0
  
  -- Read the triggered encoder count
  IF sens_cfg[cond_id].trig_cnd = PKSN_CND_FLG THEN
    -- For servo conveyor (indexer)
    enc_count = get_ecnt_idx(sens_cfg[cond_id].indexer_id)
  ELSE
    IF sens_cfg[cond_id].trig_cnd = PKSN_CND_DST THEN
      -- For belt conveyor where part is put every the specified distance
      enc_count = $pksnact[cond_id].$enc_count
    ELSE
      -- For belt conveyor where part is put when DI or HDI is input
      enc_count = LINE_COUNT(sens_cfg[cond_id].enc_id)
    ENDIF
  ENDIF

  -- Start the service to read accurate encoder for the next action
  start_srv(cond_id)

  --Get new data from external souce. For GOCATOR, we are assuming if a DI Trigger is sent to trigger the customer sensor task
  --GOCATOR uses User Socket Messaging to connect. So GOCATOR is connected in separate program. We just need to open the Log and get new data.
  
   enc_count = LINE_COUNT(sens_cfg[cond_id].enc_id) --use above at line 180 if using a different kind of conveyor.
   CALL_PROG('SCANGOCATORK', prog_index) --Program created by GOCATOR (Ryan)
	
   GET_REG(50, dmy_regBool, dmy_int, dmy_real, status) -- frame ID from GOCATOR

   IF dmy_regBool THEN
   	nframeid = TRUNC(dmy_real)
   else
   	 nframeid = dmy_int
   ENDIF      
   IF debug THEN; WRITE console(' nframeid = ', nframeid, CR); ENDIF
  
  REPEAT
  	DELAY(8)  	
  	v3p_currTrig = nframeid
  	--Timeout check
           IF (timeout > 0) AND (GetTimeDiff(f_clock) > timeout) THEN
	        GOTO DONE
           ENDIF  	
  	IF debug THEN
  		WRITE console('trig id=', v3p_currTrig, ' last trig id= ', v3p_lastTrig, CR)
  	ENDIF
  UNTIL v3p_currTrig <> v3p_lastTrig
  
  --get number of parts found from gocator sensor 
   GET_REG(51, dmy_regBool, dmy_int, dmy_real, status)
   
   IF dmy_regBool THEN
   	nfound = TRUNC(dmy_real/divide_fctor)
   else
   	 nfound = TRUNC(dmy_int/divide_fctor)
   ENDIF      
   IF debug THEN; WRITE console(' nfound = ', nfound, CR); ENDIF
  
  IF (v3p_maxprod = 0) THEN v3p_maxprod = maxprod; ENDIF
  IF nfound > v3p_maxprod THEN
  		nfound = v3p_maxprod
  		--debug
  		IF debug then
  			WRITE console(' nfound (max) = ', nfound, CR)
  		ENDIF
  ENDIF
  
  --set the GI index for x, y, z and r (this will incremement for each found part)
  gi_offset = 0
  
  --loop to get data based on number found
  WHILE nfound > 0 DO
  	
   --collect data from registers set by GOCATOR
   
   --X Position
   GET_REG((53+gi_offset), dmy_regBool, dmy_int, dmy_real, status)
    IF dmy_regBool THEN
   	XCord = TRUNC(dmy_real/divide_fctor)
   else
   	 XCord = TRUNC(dmy_int/divide_fctor)
   ENDIF      
   v3p_xyz3Pos.x = XCord
   
   --Y Position
   GET_REG((52+gi_offset), dmy_regBool, dmy_int, dmy_real, status)
       IF dmy_regBool THEN
   	YCord = TRUNC(dmy_real/divide_fctor)
   else
   	 YCord = TRUNC(dmy_int/divide_fctor)
   ENDIF      
   v3p_xyz3Pos.y = YCord
   
   --Z Position
   GET_REG((54+gi_offset), dmy_regBool, dmy_int, dmy_real, status)
       IF dmy_regBool THEN
   	ZCord = TRUNC(dmy_real/divide_fctor)
   else
   	 ZCord = TRUNC(dmy_int/divide_fctor)
   ENDIF      
   v3p_xyz3Pos.z = ZCord
   
   --W & P Position
   v3p_xyz3Pos.w = 0
   v3p_xyz3Pos.p = 0
   
   --R Position
   GET_REG((55+gi_offset), dmy_regBool, dmy_int, dmy_real, status)
       IF dmy_regBool THEN
   	Yaw = TRUNC(dmy_real/divide_fctor)
   else
   	 Yaw = TRUNC(dmy_int/divide_fctor)
   ENDIF      
   v3p_xyz3Pos.r = Yaw
     
   --debug
   IF debug THEN
   		WRITE console('x=', v3p_xyz3Pos.x, ' y=',v3p_xyz3Pos.y, ' z=', v3p_xyz3Pos.z, ' r=', v3p_xyz3Pos.r, CR)
   ENDIF
   
    --get tracking grid frame
    --tracking grid frame = INV(line tracking frame) : LMI CalGrid Uframe
    xyzTrkGrdFrm = INV($LNSCH[v3p_lnschid].$TRK_FRAME) : $MNUFRAME[1,v3p_uf_num]
    --subtract the distance the grid traveled from under the cognex to infront of the robot
    xyzTrkGrdFrm.x = xyzTrkGrdFrm.x - v3p_gridDist
    
    --get tracking frame position (like vr found pos)
    --tracking frame position = tracking grid frame : found 3rd party camera position
    xyzTrkFrmPos = xyzTrkGrdFrm : v3p_xyz3Pos
    
      found_pos[1] = xyzTrkFrmPos
    
    --get tp pr[ ]
    xyzTpRefPos = GET_POS_REG(v3p_posreg, stat)
    IF stat = 0 THEN 
    
     --get vr.offset using found position and pr refPos
     --vr.offset = tracking frame position: inv(tp pr[ ] reference position)
     xyzOffset = xyzTrkFrmPos : INV(xyzTpRefPos)        
     --clear out w,p for 4 axis robot app                  
      xyzOffset.w = 0; xyzOffset.p = 0
      
      -- Set dummy results. You can customize them before they are put
      FOR i = 1 TO MAX_MEAS DO
        dmy_meas[i] = 0.0
      ENDFOR
      model_id = 0 

   --debug
   IF debug THEN
   	WRITE console('x found=', xyzTrkFrmPos.x, ' y found=',xyzTrkFrmPos.y, ' z found=',xyzTrkFrmPos.z,' r found=', xyzTrkFrmPos.r, CR)
   	WRITE console('x offset=', xyzOffset.x, ' y offset=',xyzOffset.y, ' z offset=',xyzOffset.z,' r offset=', xyzOffset.r, CR)
   ENDIF
   
      -- Put part information to queue
      put_queue(sens_cfg[cond_id].cnv_id,
                sens_cfg[cond_id].stn_id,
                sens_cfg[cond_id].work_id,
                enc_count,
                model_id,
                xyzOffset,
                found_pos,
                dmy_meas,
                duplicated,
                stat)
      IF debug THEN
      	WRITE console('Part added to Queue - ','stat=',stat,' duplicated= ', duplicated, ' nfound=',nfound, CR)
      ENDIF
      IF (stat = 0) THEN
        -- nfound = 1
        -- Update work id. Work id is updated as below. Work id = 1,2,3...
        incre_wid(sens_cfg[cond_id].work_id)
       IF debug THEN
      		WRITE console(' work ID Incremented', CR)
	ENDIF
      ENDIF
    ENDIF
      
    -- minus 1 from number of found parts
    nfound = nfound - 1

    --offset by 4 for next itteration of parts
    gi_offset = gi_offset + 4
  ENDWHILE
  
  DONE:: --go here if timeout occurs
  
  -- Notify of the number of found part. You must call this function in the end of this routine.
  update_nfnd(cond_id, nfound)
  
   --update last trigger id to compare for next input
   v3p_lastTrig =  v3p_currTrig  
    
  if debug THEN
  	CLOSE FILE console
  endif
END put_part_cst

------------------------------------------------------------------------
ROUTINE run_vis_cst(cond_id : INTEGER; stat : INTEGER)
-- Action: "Find part by vision"
-- You cannot change this function name
------------------------------------------------------------------------
VAR
  model_id    : INTEGER
  enc_count   : INTEGER
  offset      : XYZWPR
  tray_pos    : XYZWPR
  found_pos   : ARRAY[MAX_NUM_VIEW] OF XYZWPR
  meas        : ARRAY[MAX_MEAS] OF REAL
  nfound      : INTEGER
  duplicated  : INTEGER

BEGIN
  -- Initialize number of found parts
  nfound = 0

  -- Find parts
  V_RUN_FIND(sens_cfg[cond_id].vp_name1, -1, stat)
  WHILE (stat = ER_SUCCESS) DO

    -- Get information of found parts
    VT_GET_FOUND(sens_cfg[cond_id].vp_name1, model_id, enc_count, offset, found_pos, meas, stat)
    IF (stat = ER_SUCCESS) THEN

      -- If tray, compute tray position on track frame
      IF (sens_cfg[cond_id].tray_id <> 0) THEN
        found_pos[1] = found_pos[1] : sens_cfg[cond_id].tray_pos
      ENDIF

      -- Put part information to queue
      put_queue(sens_cfg[cond_id].cnv_id,
                sens_cfg[cond_id].stn_id,
                sens_cfg[cond_id].work_id,
                enc_count,
                model_id,
                offset,
                found_pos,
                meas,
                duplicated,
                stat)
      IF (stat = ER_SUCCESS) THEN
        nfound = nfound + 1
        -- Update work id. Work id is updated as below. Work id = 1,2,3...
        incre_wid(sens_cfg[cond_id].work_id)
      ENDIF
    ENDIF
  ENDWHILE
  IF stat = VEXE_E_NOMOR THEN
    stat = ER_SUCCESS
  ENDIF

  -- Notify of the number of found part. You must call this function in the end of this routine.
  update_nfnd(cond_id, nfound)
END run_vis_cst

------------------------------------------------------------------------
BEGIN -- Main Program
------------------------------------------------------------------------
END pksnlibcst

Conclusion

This article provides a high-level overview of all the required building blocks to integrate an LMI Gocator with FANUC's iRPickTool. This integration is complicated and is highly recommended to first be developed in a mock cell before deploying into production.