Thanks. One small point for now about your use of CONTINUE. Does a conditional branch (if or switch) stop the advance run? Also, do you think I could just drop CONTINUE in front of eth_read and eth_write?
Posts by thehandoftheking
-
-
Okay, I will have to ponder this. One thing I forgot to mention is that I'm going to assume the sequence of positions is set once we tell the robot to go. Therefore the approximation would be possible (assuming nothing stops the advance-run pointer). While I was waiting for responses I wrote the following "code," where the array indices are determined by our protocol (for example, int_array[16] = Status Response).
Code
Display More; The command "brake" has to be in an interrupt program isDesiredToStopNow = false interrupt decl 5 when isDesiredToStopNow == true do callBrake() interrupt on $advance = 5 ; Move to home $bwdstart = false pdat_act=pdefault bas(#ptp_dat) fdat_act=fhome bas(#frames) bas(#vel_ptp,100) $h_pos=xhome ptp xhome ipaddr[] = "10.80.30.47" port_num = 2002 cnxn_type = 1 ; tcp cnxn ncnxn_handle = 0 ; Connect to the server ; If successful set connected flag to true and start the main loop ret_code = eth_open(ipaddr[], port_num, cnxn_type, cnxn_handle) if(ret_code == 0) then ; Connection successful isConnected = true else isConnected = false endif sequenceIndex = 0 sequenceLength = 0 isReadyToMove = false while isConnected == true ; Check for new message and act accordingly ret_code = eth_read(cnxn_handle, int_array[], float_array[], char_array[], 0) ; Read from port with no wait time switch ret_code case -2 ; EKRL is running short on memory. Please restart the controller. stopAndBreak() case 0 ; New message received switch int_array[1] ; This is the field specifying the command type case 0 ; Received "get status" sendStatus() case 1 ; Received "get pose" sendPose() case 2 ; Received "send stop" receivedStop() case 3 ; Unused case 4 ; Received "clear pose sequence" sequenceLength = 0 sequenceIndex = 0 case 5 ; Received "add to pose sequence" addToPosesSequence() case 6 ; Received "execute pose sequence" isReadyToMove = true default receivedStop() int_array[16] = 2 ; Set error status char_array = char_array + " Invalid approximation command number received " ; Not sure character strings are appended in KRL endswitch case 1 ; Invalid Connection ID stopAndBreak() case 3 ; Operation timed out case 5 ; Connection closed unexpectedly stopAndBreak() case 6 ; Invalid data int_array[16] = 2 ; Set error status char_array = char_array + " Invalid data received via EKRL " ; Not sure character strings are appended in KRL case 7 ; Connection still valid, but no new message case 267 ; Message queue error, EKRL automatically closes connection in this case stopAndBreak() default stopAndBreak() endswitch if (isReadyToMove == true) ldat_act = sequenceLdat[sequenceIndex] fdat_act = myFdat bas(#cp_params, ldat_act.vel) ; The second argument to this call is kinda weird, but I think that's how it works ; Command motion with requested approximation criteria switch int_array[2] ; Approximation criterion mode case 0 ; Translational distance lin sequencePoses[sequenceIndex] c_dis case 1 ; Rotational distance $apo.cori = apo_cori_array[sequenceIndex] lin sequencePoses[sequenceIndex] c_ori case 2 ; Velocity $apo.cvel = apo_cvel_array[sequenceIndex] lin sequencePoses[sequenceIndex] c_vel endswitch sequenceIndex = sequenceIndex + 1 if (sequenceIndex > sequenceLength) isReadyToMove = false endif endif endwhile ; End main loop ; Ensure connection is closed ret_code = eth_close(cnxn_handle) cnxn_handle = 0 ; Move home again $bwdstart = false $h_pos=xhome pdat_act=pdefault bas (#ptp_dat ) fdat_act=fhome bas (#frames ) bas (#vel_ptp,100 ) ptp xhome end def addToPoseSequence() sequenceLength = sequenceLength + 1 sequencePoses[sequenceLength].x = float_array[1] sequencePoses[sequenceLength].y = float_array[2] sequencePoses[sequenceLength].z = float_array[3] sequencePoses[sequenceLength].a = float_array[4] sequencePoses[sequenceLength].b = float_array[5] sequencePoses[sequenceLength].c = float_array[6] sequenceLdat[sequenceLength].vel = float_array[7] ; Velocity ; Default values for remaining ldat fields sequenceLdat[sequenceLength].acc = 100 ; Acceleration sequenceLdat[sequenceLength].apo_dist = 0 ; Translational distance approximation criterion sequenceLdat[sequenceLength].apo_fac = 50 ; I do not know what this is for mqm 121004 sequenceLdat[sequenceLength].ory_typ = #var ; Orientation varies from one pose to the next switch int_array[2] ; Approximation criterion mode case 0 ; Translational distance sequenceLdat[sequenceLength].apo_dist = float_array[8] case 1 ; Rotational distance apo_cori_array[sequenceLength] = float_array[9] $apo.cori = float_array[9] case 2 ; Velocity apo_cvel_array[sequenceLength] = int_array[3] $apo.cvel = int_array[3] default int_array[16] = 2 ; Set error status char_array = char_array + " Invalid approximation criteria received " ; Not sure character strings are appended in KRL endswitch end def effectivelyClearPoseSequence() sequenceIndex = 0 sequenceLength = 0 end def receivedStop() callBrake() isReadyToMove = false end def sendPose() resetArrays() float_array[17] = $pos_act.x float_array[18] = $pos_act.y float_array[19] = $pos_act.z float_array[20] = $pos_act.a float_array[21] = $pos_act.b float_array[22] = $pos_act.c float_array[23] = $vel_act ; Linear velocity for continuous-path motions sendMessage() end def sendStatus() ; ? ; ? Is this the best way to handle an error code? mqm 121004 ; ? if (int_array[16] <> 2) ; An error could have been set elsewhere, do not overwrite it if ($pro_move == true) ; Robot is moving int_array[16] = 1 else int_array[16] = 0 endif endif sendMessage() end def sendMessage() ret_code = eth_write(cnxn_handle, int_array[], float_array[], char_array[]) if (ret_code <> 0) ; Either invalid connection ID or send failed stopAndBreak() endif end def stopAndBreak() isDesiredToStopNow = true isReadyToMove = false isConnected = false end def resetArrays() for i = 1 to 32 if i < 29 then int_array[i] = 0 endif float_array[i] = 0 endfor char_array = " " end def callBrake() brake ; This command can only be placed in an interrupt program isDesiredToStopNow = false end
-
I'm working on a KRL program that will allow the robot to be controlled in a limited sense by a remote computer using Ethernet KRL. By "limited" I mean that the capability will be there to load a series of positions on the robot (along with associated velocities, approximation criteria, etc.) and then have it execute the desired motion. We're setting up a communication protocol and I want to have six different commands available (from the PC to the KR C)
1. Get status
2. Get position
3. Send stop
4. Clear pose sequence
5. Add to pose sequence
6. Execute pose sequenceThis would be straightforward except for the fact that we want these commands to be available while the robot is in motion (that is, while it's moving through a series of positions). Roughly speaking, my idea for effecting this is to run a loop that first checks for EKRL messages (with timeout of zero), then executes the appropriate subroutine, and finally moves to the current position in the sequence (and increments the sequence index). A rough sketch of this follows in pseudo-code.
Code
Display Moreloop ethread() if newMessage switch messageCommandType 1: ethwrite(status) 2: ethwrite(position) 3: somehow execute BRAKE command (I guess in an interrupt program) readyToMove = false 4: sequenceLength = 0 sequencePosesIndex = 0 5: sequenceLength++ sequencePoses[sequenceLength] = messagePoseData lcpdat[sequenceLength] = messageLCPData 6: readyToMove = true endif if readyToMove ldat_act = lcpdat[sequencePosesIndex] fdat_act = myFdat bas(#cp_params, lcpdat[sequencePosesIndex].vel) ; I'm not sure about this. It seems this call to bas() takes in two velocities, one in ldat_act and one in the call. lin sequencePoses[sequencePosesIndex] c_vel sequencePosesIndex++ endif if sequencePosesIndex > sequenceLength ; Done moving through sequence of positions sequencePosesIndex = 0 sequenceLength = 0 readyToMove = false endif endloop
I have concerns about this approach, mainly about the advance run. Do you know if with this ethread and possibly ethwrite between the motion commands (lin) will approximation still be possible? I'm thinking another approach is to (possibly?) have the Ethernet stuff be in sps.sub and let the arrays of positions and ldat's be global. It would be more work to set up but seems like it has the potential to be cleaner and better performing than keeping it all in a single KRL program. I'd appreciate any comments.PS --- It's an older controller, a KR C2 running I don't know which version of the software.
-
For the sake of posterity, I'm posting the code that's running now. It is the non-RSI solution. It's working quite well in early testing. Thanks again.
Code
Display More$advance = 1 bumpDistance = 20 ; Distance (in mm) for each motion command while goodConnection ; main program loop retCode = eth_read(cnxnHandle, ints[], reals[], eMsg.chars[], 0) switch retCode case 0 ; Message found ; Find the magnitude of the vector to the target. magnitude = sqrt(reals[1]*reals[1] + reals[2]*reals[2] + reals[3]*reals[3]) ; Calculate the number of bumpDistance size motions ; needed to move "magnitude" mm floor() iterations = 1 setPos() lin_rel relPos c_vel case 7 ; No message found, "7" is ONLY used when TIMEOUT set to 0 if iterations > 0 then iterations = iterations + 1 setPos() lin_rel relPos c_vel endif default ; Bad connection or bad message data or queue error goodConnection = false endswitch endwhile ; End main program loop retCode = eth_close(cnxnHandle) ; Close ethernet connection end def floor() quotient = magnitude / bumpDistance for i = 1 to 1000 if i > quotient then floorInt = i - 1 decimalPart = quotient - floorInt i = 1001 endif endfor end ; floor() subroutine def setPos() if (iterations == 1) and (floorInt <> 0) then relPos.x = bumpDistance * reals[1] / magnitude relPos.y = bumpDistance * reals[2] / magnitude relPos.z = bumpDistance * reals[3] / magnitude endif if iterations > floorInt then ; Final portion of motion relPos.x = decimalPart * bumpDistance * reals[1] / magnitude relPos.y = decimalPart * bumpDistance * reals[2] / magnitude relPos.z = decimalPart * bumpDistance * reals[3] / magnitude iterations = 0 endif end
-
I am still working to determine whether RSI is necessary for our application. I am hoping you can enlighten me. Please tell me what differences we would see in performance between the following two scenarios; one using RSI and the other not. In both scenarios there is an image-processing system that sends data to the KRC via Ethernet. This comprises cartesian info on where the robot is to move (relative to its current position) in order to reach the target. The target is assumed to be moving. This system sends new information every "P" milliseconds.
Scenario A uses RSI, specifically an ST_PATHCORR object as mentioned in the reply by SkyeFire, to move the robot. The purpose for using RSI and ST_PATHCORR in this manner would be to allow update of the robot motion without having to wait to complete a motion command.
Scenario B does not use RSI. In order to be able to update the path of the robot motion "mid flight" it divides the relative motion command sent via EthernetKRL into smaller increments. These are executed within a loop, and when the image-processing system sends new data it is just applied to the target position of the relative motion command. So the robot is still having to complete one motion command before going to a new one, it's just that in this manner it is proposed that the motion commands will be executed more quickly. Please refer to the pseudo-code below for clarification of this scenario. The details are not perferctly addressed in the pseudo-code, but the main things I am concerned about are smooth motion (i.e. the behavior of the advance run pointer) and the speed of the motion.
Code
Display More$advance = 2 $vel = 2 ; Set programmed velocity to 2 m/s, the maximum amount. $apo.cvel = 100 ; Set the approximation at 100% of the programmed velocity P = 100; approximate number of milliseconds between updates from vision system movePos = {x 0, y 0, z 0, a 0, b 0, c 0} ; the relative position to which the robot wil move moveDist = 10 n = P / (moveDist / $vel) ; maximum number of motion commands robot will complete during P continue = true while (continue) do retCode = eth_read() ; Messages contain distances for the robot to move. Data provided by vision system. Data stored in reals[]. switch retCode case 0 ; no new message lin_rel movePos c_vel case 7 ; new data stored in reals[] movePos.x = reals[1] / n movePos.y = reals[2] / n movePos.z = reals[3] / n movePos.a = reals[4] / n movePos.b = reals[5] / n movePos.c = reals[6] / n lin_rel movePos c_vel default continue = false endswitch endwhile
edit: In reviewing the pseudo code, I noticed that the whole "moveDist" and "n" stuff is not done correctly. Please ignore that error and just consider the idea of dividing the motion into smaller increments so that a new direction of relative motion can be commanded more readily.
Questions:
1. Will the RSI scenario allow the robot to move faster through space?
2. Do you see any red flags about the concept for the non-RSI scenario?
3. I am assuming that we will want the robot to move through space as quickly as possible. If this is the case, would we just set the ST_PATHCORR to try to move the robot the full amount indicated by the image-processing system in the 12 ms period?
Thanks a lot. Any feedback is appreciated as it will probably be helpful in some way.
-
Wow, thanks for the reply. I do not have our version number handy, but I know that our controller is too old to support EthernetRSIXML. Too bad because that sounds quite appropriate for us. We do not yet have RSI, I am just working to figure out if that would best meet our needs.
Your explanation of how we would use PATHCORR to obtain velocity control is quite helpful. I'm also interested in your mention of the KUKA RSI training. Maybe if we buy RSI then our company would register me for the training.
Thanks again. Ciao.
-
I have two questions about using RSI for an application. First of all, can you tell me how I will use our "sensor" with RSI? It is a vision system, and up until now we have transferred the data to the KRC via EthernetKRL. Is this still a feasible way to provide "sensor" data to the KRC? I read the RSI 2.0 manual but did not learn anything about connecting a sensor or how to select which one to use. There are a couple of lines about ST_DLRSENS, and the example says "'ST_DLRSENS' creates a sensor object which is used to access the force--motion sensor", but it's missing from the "RSI Commands" section; it does not have an entry so the comments in the example are the only information about it.
My second question is this: How would we use RSI to run the robot in velocity control? The manual talked about path correction, but I would like to not program a path. I do not want to instruct the robot to go to a certain point and then have RSI adjust this path based on the sensor. Rather I would like to use RSI to get the robot to move in a certain direction at a certain speed. I did not see a new motion command in the RSI manual, just PTP, LIN, and CIRC. Will I have to just provide a dummy target point and keep using path correction to provide the desired direction of motion?
Thanks in advance. I would also appreciate links to or copies of any example code using RSI.