smooth motion of kuka in visual servoing

[nabeel801]

I am working on a basic visual servoing demo. The camera sends the coordinates of the
object on conveyor belt to the KUKA at 15 Hz and the kuka is to follow the object.

The problem that I am having is that the motion is not smooth. I am using LIN_REL motion command to move a fixed distance in the direction of the box every time I receive the coordinates of the object from the camera. I am using approximate positioning but its not working and I get warnings 'approximation not possible.' Any ideas how i can have smooth motion of the kuka.

Thanks.

[SkyeFire]

"Approximation not possible" happens when a point is programmed as a continuous move but something else breaks the advance pointer, preventing the continuous motion from executing properly.

Unfortunately, lots of things break the advance pointer: setting outputs, waiting for inputs, assigning values to variables... almost anything.  What you're trying to do is not something that normal KRL was intended for.  There are ways to do this well, but they require buying add-on modules.

Adding to this issue is that the advance pointer ($ADVANCE) is set to 3 by default, meaning that the path planner is looking three motion commands ahead.  If you have one motion command inside a loop, this means that the code will execute up to three loops ahead.

Fortunately, there are solutions.  First, I would try setting $ADVANCE to 1.  If you set it to 0, approximation becomes impossible.

Go to  http://www.robot-forum.com/robotforum/manuals_software_and_tools_for_kuka_robots/krc2_manuals_for_kss_version_52-t229.0.html and download the manuals.  The Expert manual will be especially important.

Judiciously use CONTINUE and/or TRIGGER commands.  A CONTINUE command placed immediately before a line of code that breaks the advance pointer will cause that one line, and that line only, to run without stopping the advance pointer.  However, this means that the command preceded by the CONTINUE line will execute $ADVANCE number of motions ahead of where it is programmed to. 
Hence the TRIGGER command.  Basically, the TRIGGER command is like an interrupt that can be connected to a motion, rather than an input or variable state.  Triggers fire relative to the next motion command, and can be tuned for motion start, motion end, and tuned in time postive and negative.

Be aware of your timing.  If you have this:

Code:

PTP P0 C_DIS
PTP P1 C_DIS
CONTINUE ; prevents the Wait command from breaking the advance pointer
WAIT FOR $IN[100]
PTP P2 C_DIS

the motion from P1 to P2 will only be smooth if $IN[100] is True before the TCP reaches the approximation radius of P1.  If the approximation radius of P1 is, say, 100mm, and $IN[100] becomes True when the TCP is 99mm away from P1, it's too late -- the TCP will move to P1 and come to a very brief but full stop before executing the next move.  One side effect of this is that, in programs that have lots of WAIT/move interactions, your motion can be smoother if your approximation radii are *smaller,* allowing more time for tardy signals to arrive and letting the robot pass through the WAIT command without stopping.

Now, I don't know enough details of what you're trying to do, but if you're trying to perform "on the fly" path corrections, you're fighting KRL's natural tendencies, which are to move-stop-shift-move-stop-shift.  However, one built-in feature is the "Function Generator," which makes it possible to perform Cartesian path corrections on the fly by associating sensor inputs (usually analog input channels, but probably any binary integer value will do) with certain axes of motion.
Or, if you're trying to use plain KRL, you could try breaking up the single move into lots of little ones, and executing them in sequence while using TRIGGER commands to receive new data and update forthcoming motion points before the advance pointer gets to them.  Using the TRIGGER commands to call subroutines which use lots of CONTINUE commands but contain no motion commands is one technique for getting around the issue created by the interaction of $ADVANCE and CONTINUE.

[nabeel801]

Thanks SkyeFire for your help.
I will try what you wrote, may be tomorrow.

I am using UDP to send data to KUKA. and kuka codes look something like this,

while(goodConnection)

retCode = readEthernet()

 switch retCode
  case 0 ; recieved data
  Msg  = True

  case 7 ; no data
    if(Msg)
       lin_rel POS c_vel
    endif

  endswitch
endwhile


I know this code should not work, since only it has finished lin_rel it will read new data from UDP.
I tried putting the ethRead command in an interrupt pragramm that gets called every 40 ms, but still its not working. I think I need to use trigger as you said. If you don't mind and you have time, will you please write a brief psudo code of how to use the trigger command.

Thanks,

[SkyeFire]

Well, here's a quick and rough example:

Code:

PTP P1 C_DIS
PTP P2 C_DIS
TRIGGER WHEN DISTANCE=0 DELAY=-100 DO SubRoutine1() PRIO = -1 ; calls SubRoutine1 100ms before the start of the move to P3, with Priority assigned by the kernel
TRIGGER WHEN DISTANCE=1 DELAY=100 DO SubRoutine2() PRIO = 5 ; calls SubRoutine2 100ms after the end of the P3 motion, with a Priority of 5.
PTP P3 C_DIS
PTP P4 C_DIS

The C_DIS tag is what makes the motion continuous.  A Trigger command always connects to the next motion command, and you can stack multiple Triggers on the same motion.  However, only one Trigger or Interrupt can own a particular Priority number at one time, and there's a limit to how many Triggers and/or Interrupt can be firing at one time (I think it's six).  You could have 100 Triggers all on one motion point, but only that many of them can be firing at the same time.  If you somehow somehow made sure that the first Trigger finished before the seventh Trigger fired, and so on, you could have theoretically unlimited Triggers on the same motion point. 

When dealing with Continuous moves, the Trigger will fire relative to the point of closes approach.  In the example above, the second Trigger would fire 100ms after the TCP's closest approach to P3, and the first Trigger would fire 100ms before the TCP's closest approach to P2. 
With a Distance=0, the Trigger is basing off the moment of transition between P2 and P3.  With Distance=1, it bases off the transition between P3 and P4.
However, DELAY values greater than one-half the time of the move between P2 and P3 or between P3 and P4 will be truncated to the halfway point.

I've been thinking about this a bit, and have a rough idea of something that might work, but it'll probably take some real banging to get smoothed out.

Code:

DEF MainRoutine()
  DECL POS NextPos[3]
  $ADVANCE = 1 ; only look 1 move ahead
  LOOP
    FOR i = 1 TO 3
      TRIGGER WHEN DISTANCE=1 DELAY=100 DO SubRoutine(i, NextPos[]) PRIO = -1
      LIN_REL NextPos[i] C_DIS
     ENDFOR
  ENDLOOP
END
;
DEF SubRoutine (Index : IN, Position :IN) ; receives value of i into Index, NextPos into Position
  DECL INT Index
  DECL POS Position[]
  CONTINUE
  ;put call to get-data routine here.  That routine will probably need CONTINUE every other line
  CONTINUE
  Position[Index] = ; put the XYZABC data just received from the computer here
END

It's late and I'm tired, so there's probably some mistakes in that.  But the concept is to keep the computer two motions (or more) ahead of the robot.  The minimum move the computer sends to the robot will need to cover enough time for a complete vision/communications cycle to take place.  As the robot leaves Point A to head for point C via Point B, the subroutine is called and fills a new value into Point A so that, once the robot reaches Point B, the advance pointer can look 1 move past Point C to the new Point A without waiting for any I/O cycles, and as the robot passes Point B, the subroutine gets called to fill Point B with fresh data. 

Actually, it would probably be better to put the communications stuff into the SPS, and just do the array filling in the subroutine.  That would make things less complex and reduce the number of CONTINUE commands you have to pile into your comm subroutine.

Expanding the array to 4 (or more) instead of 3 might help, I don't know.  But I think 3 will be the minimum.

This should[ avoid any issues starting and stopping motion, since the robot will happily keep moving to its current location endlessly all day long.  (However, do not ever put PTP $POS_ACT into a loop -- the small errors in $POS_ACT may well drive the robot off in a random direction slowly until the robot runs out of reach).

There will be an issue with time lag.  Obviously, this approach will always be lagging at least one full measurement/communication cycle  behind the visual system.  But the faster your update rate and smaller your individual motion size, the less significant that affect will be.

You'll probably need to do some tweaking of your $ACC and $APO_DIST variables to adjust your Acceleration and Approximation Distance to find the "sweet spot" the minimizes timing issues of updates from the PC, but still performs effective path smoothing.

[nabeel801]

thanks for your help.

I will code as you said.

[SkyeFire]

Let us know how it turns out.  And don't be afraid to experiment with it -- like I said, it was just an idea I haven't had a chance to try myself.

To try to make things more clear:

To have smooth motion, you have to have your $ADVANCE set to 1, minimum.  And to have smooth motion using live data incoming from the outside, your point update must stay ahead of the advance pointer.  Using a larger array size would make that easier, but the larger the array gets, the further behind the external data the robot will be -- if the target reverses direction, the robot will keep going in the old direction for as many motion steps as it takes to "catch up" with the new data in the array.  OTOH, with a smaller array, it becomes harder to make sure that the incoming data stays ahead of the robot -- KRL runs based on motion time, not a fixed time index.   By the same token, smaller step sizes reduce the "lag" distance between robot and target, but require faster data update rates and make it more likely that the advance pointer will catch up with the incoming data.

Honestly, if you really need to do this for an actual application, my strong suggestion is to invest in the KUKA RSI-XML tech package, which allows you to, in effect, connect your sensor directly to the robot's servo control loop, without the issue of KRL code getting in the way.  RSI-XML acts in essentially realtime (12ms update rate) -- it's like going below the KRL layer and directly controlling the TCP's command destination.  With RSI, there's no current motion that has to complete before the next motion can be started -- the robot reacts to any input change within 12ms.  The one drawback to RSI-XML is that the incoming data must be packaged inside an XML format, but that's generally not an insurmountable obstacle.

If you're working purely in 3D Cartesian space, you might be able to do something similar using the built-in KSS Function Generator, but I'm afraid that's an item I don't have any real experience with, so I can't be of much help there.

[nabeel801]

As a start, we changed the motion command from LIN_REL to LIN and the motion is much smoother.
We haven't really tested on your suggestions yet and will let you know how it works. BTW whats the cost of KUKA RSI-XML tech package.
Thanks,

[SkyeFire]

Curious.  I wonder why a LIN would be smoother than a LIN_REL?  It shouldn't be anything inherent to the commands, more likely something related to the surrounding code. 

As for RSI-XML (more properly EthernetRSIXML), I don't know the pricing, but based on recent purchases of my own, I would estimate it as something not over $5000.00 US, but that's only an estimate. 

You have some options and some limitations in using this package.  Apparently, ERX has recently been obsoleted as a separate tech package by the introduction of the latest version of "regular" RSI, which has Ethernet communication capability built it.  OTOH, some controllers and KSS versions may not be able to run RSI or ERX.  You'll want to contact KUKA with your complete robot information -- robot type, controller model, KSS version, etc -- in order to find out from them if you can use RSI, and which versions might be (in)compatible.

[thehandoftheking]

I have a bit of information about this project described by nabeel801 that is of interest (I am working with nabeel801).  The reason the lin is smooth where lin_rel is not is that we are computing the relative motion based on $pos_act.  When we issue a lin command it is not necessary to call $pos_act.  That call is what's stopping the advance run pointer.

We tried this:

continue
$pos_act

but the position returned was useless.  I can not guess whether that's because the call to $pos_act is being issued three lines ahead of where the robot is in space, or if it's just an inaccurate value without the robot coming to a stop.

Do you have any thoughts on obtaining a valid $pos_act while approximating motions?

Thanks for all your help.

[SkyeFire]

$POS_ACT is updated constantly with the robot's actual Cartesian position in (near) realtime.  Unfortunately, that means getting $POS_ACT for the exact moment that you want it requires that you structure your code very carefully, since KRL is motion-determinant, not time-determinant.  At minimum, you'll need to use TRIGGER commands to "snapshot" $POS_ACT at the correct moment.  But you'll need to have that snapshot taken at the same moment that the vision systems takes its own snapshot, or the the two pieces of data will be asynchronous to each other and you could end never settling in proper relationship to the target.