|
Fast Research Interface Library
Manual and Documentation
|
Note line 116 (friOpen(2)), which defines the control sample time (here: 2 milliseconds).
&ACCESS RVP
&REL 67
DEF fricontrol( )
;FOLD Program information
;=====================================
; Stanford University
; Department of Computer Science
; Artificial Intelligence Laboratory
; Gates Computer Science Building 1A
; 353 Serra Mall
; Stanford, CA 94305-9010
; USA
;
; KRL application, which contains the KRL commands
; for appying the Fast Research Interface (FRI) of
; the KUKA Light-Weight Robot (LWR)
;
; July 20, 2010
;
; Author: Torsten Kroeger, tkr@stanford.edu
;
; Latest update: November 19, 2011
;=====================================
;ENDFOLD (Program information)
;FOLD Variable information (open fold to view)
;=====================================
; ------------------------------------
; $FRI_FRM_INT[15]
; This value contains the desired control mode
; (i.e., joint position control (10), joint
; impedance control (30), or Cartesian impedance
; control (20). The new mode will be activated by
; calling the function friStart().
; ------------------------------------
; $FRI_FRM_INT[16]
; This value is used to call the KRL
; functions of the Fast Research Interface.
; A value of 10 calls friStart(), a value of
; 20 calls friStop(), and a value of 30/40
; depending on $FRI_TO_INT[15]) calls
; friClose() to terminate the UDP connection
; and to end the KRL program.
; ------------------------------------
; $FRI_TO_INT[15]
; This value determines the abort condition
; to exit the endless loop of this KRL
; program. The value of $FRI_FRM_INT[16]
; has to be set corresponding to this.
; ------------------------------------
; $FRI_TO_INT[16]
; If this value indicated the current state of
; this KRL program to the remote host. The
; following values are used:
;
; - 10: friOpen() was called (i.e., the FRI is in
; Monitor Mode)
;
; - 20: friStart() was called (i.e., the FRI is in
; Command Mode if the quality of the signals by
; remote host are sufficient).
;=====================================
;ENDFOLD (Variable information)
;FOLD Initialization
;FOLD BCO run to current position
; This part is required to perform a "BCO run".
; A BCO run is required to enable the automatic mode.
$H_POS=XHOME
PDAT_ACT=PDEFAULT
BAS (#PTP_DAT )
FDAT_ACT=FHOME
BAS (#FRAMES )
BAS (#VEL_PTP,100 )
PTP $AXIS_ACT_MES
;ENDFOLD
;FOLD Initialization of the variables for the FRI
$stiffness.base = {X 0, Y 0,Z 0, A 0, B 0, C 0}
$stiffness.tool = {X 0, Y 0,Z 50, A 0, B 0, C 0}
$ACT_TOOL = 2
$TOOL = TOOL_DATA[2]
$ACT_BASE = 0
$BASE = $NULLFRAME
FOR I = 1 TO 16
$FRI_TO_INT[i] = 0
$FRI_TO_REA[i] = 0.0
ENDFOR
$FRI_TO_INT[15] = 30
IF ($FRI_FRM_INT[16] == 30) THEN
$FRI_TO_INT[15] = 40
ELSE
IF ($FRI_FRM_INT[16] == 40) THEN
$FRI_TO_INT[15] = 30
ENDIF
ENDIF
RETURN_VALUE = #OFF
I = 0
STRING_OFFSET = 0
$GRAVITATION[1] = 0.0
$GRAVITATION[2] = 0.0
$GRAVITATION[3] = 9.80665
;ENDFOLD (Initialization of the variables for the FRI)
;ENDFOLD (Initialization)
; Open the FRI with a datarate of 2 ms
RETURN_VALUE = friOpen(2)
;FOLD Screen output
$MSG_T={MSG_T: VALID FALSE, RELEASE FALSE, TYP #NOTIFY, MODUL[] " ", KEY[] " ", PARAM_TYP #VALUE, PARAM[] " ", DLG_FORMAT[] " ", ANSWER 0}
$MSG_T.MODUL[]="FRI-Ctrl"
IF (RETURN_VALUE == #MON) THEN
$MSG_T.KEY[] = "FRI successfully opened."
ELSE
STRING_OFFSET = 0
SWRITE($MSG_T.KEY[], STATE, STRING_OFFSET, "ERROR during friOpen (result: %d)", RETURN_VALUE)
ENDIF
$MSG_T.VALID=TRUE
WAIT FOR ($MSG_T.VALID == FALSE)
WAIT SEC 0.3
;ENDFOLD (Screen output)
; Signalize to the remote host that the FRI is open
$FRI_TO_INT[16] = 10
REPEAT
IF ($FRI_FRM_INT[16] == 10) AND ($FRI_TO_INT[16] <> 20) THEN
;FOLD Setting up $STIFFNESS
$stiffness.tool = tool_data[2]
$stiffness.maxforce = {X 300.0, Y 300.0, Z 300.0, A 30.0, B 30.0, C 30.0}
$stiffness.cpmaxdelta = {X 100.0, Y 100.0, Z 100.0, A 100.0, B 100.0, C 100.0}
;$stiffness.axismaxdeltatorque = {A1 1000.0, A2 1000.0, A3 1000.0, A4 1000.0, A5 1000.0, A6 1000.0, E1 1000.0}
$stiffness.axismaxdelta = {A1 100.0, A2 100.0, A3 100.0, A4 100.0, A5 100.0, A6 100.0, E1 100.0}
$stiffness.axisdamping = {A1 0.7, A2 0.7, A3 0.7, A4 0.7, A5 0.7, A6 0.7, E1 0.7}
$stiffness.axisstiffness = {A1 1000.0, A2 1000.0, A3 1000.0, A4 1000.0, A5 1000.0, A6 1000.0, E1 1000.0}
$stiffness.cpstiffness = {x 200, y 200, z 200, a 20, b 20, c 20}
$stiffness.cpdamping = { x 0.7, y 0.7, z 0.7, a 0.7, b 0.7, c 0.7}
;$stiffness.axisstiffness = {A1 0.0, A2 0.0, A3 0.0, A4 0.0, A5 0.0, A6 0.0, E1 0.0}
;ENDFOLD (Setting up $STIFFNESS)
; Set up the current control scheme
$stiffness.strategy = $FRI_FRM_INT[15]
$stiffness.commit = true
;FOLD Screen output
$MSG_T={MSG_T: VALID FALSE, RELEASE FALSE, TYP #NOTIFY, MODUL[] " ", KEY[] " ", PARAM_TYP #VALUE, PARAM[] " ", DLG_FORMAT[] " ", ANSWER 0}
$MSG_T.MODUL[]="FRI-Ctrl"
IF ($stiffness.strategy == 10) THEN
$MSG_T.KEY[] = "Control scheme: joint position control."
ELSE
IF($stiffness.strategy == 20) THEN
$MSG_T.KEY[] = "Control scheme: Cart. impedance control."
ELSE
IF ($stiffness.strategy == 30) THEN
$MSG_T.KEY[] = "Control scheme: switching to joint impedance control."
ELSE
$MSG_T.KEY[] = "ERROR: Unknown control scheme."
ENDIF
ENDIF
ENDIF
$MSG_T.VALID=TRUE
WAIT FOR ($MSG_T.VALID == FALSE)
WAIT SEC 0.3
;ENDFOLD (Screen output)
; Apply a safety value of 1.0 and start the FRI
RETURN_VALUE = friStart(1.0)
;FOLD Screen output
$MSG_T={MSG_T: VALID FALSE, RELEASE FALSE, TYP #NOTIFY, MODUL[] " ", KEY[] " ", PARAM_TYP #VALUE, PARAM[] " ", DLG_FORMAT[] " ", ANSWER 0}
$MSG_T.MODUL[]="FRI-Ctrl"
IF (RETURN_VALUE == #CMD) THEN
$MSG_T.KEY[] = "FRI successfully started."
ELSE
STRING_OFFSET = 0
SWRITE($MSG_T.KEY[], STATE, STRING_OFFSET, "ERROR during friStart (result: %d)", RETURN_VALUE)
ENDIF
$MSG_T.VALID=TRUE
WAIT FOR ($MSG_T.VALID == FALSE)
WAIT SEC 0.3
;ENDFOLD (Screen output)
; Signalize to the remote host that friStart was called
$FRI_TO_INT[16] = 20
ENDIF
IF ($FRI_FRM_INT[16] == 20) AND ($FRI_TO_INT[16] <> 10) THEN
RETURN_VALUE = friStop()
;FOLD Screen output
$MSG_T={MSG_T: VALID FALSE, RELEASE FALSE, TYP #NOTIFY, MODUL[] " ", KEY[] " ", PARAM_TYP #VALUE, PARAM[] " ", DLG_FORMAT[] " ", ANSWER 0}
$MSG_T.MODUL[]="FRI-Ctrl"
IF (RETURN_VALUE == #MON) THEN
$MSG_T.KEY[] = "FRI successfully stopped."
ELSE
STRING_OFFSET = 0
SWRITE($MSG_T.KEY[], STATE, STRING_OFFSET, "ERROR during friStop (result: %d)", RETURN_VALUE)
ENDIF
$MSG_T.VALID=TRUE
WAIT FOR ($MSG_T.VALID == FALSE)
WAIT SEC 0.3
;ENDFOLD (Screen output)
; Signalize to the remote host that the FRI was stopped
$FRI_TO_INT[16] = 10
ENDIF
; The Fast Research Interface is activated.
WAIT SEC 0.05
UNTIL $FRI_FRM_INT[16] == $FRI_TO_INT[15]
;FOLD For the case, friStop() has not been called, call it here
IF ($FRI_TO_INT[16] == 20) THEN
RETURN_VALUE = friStop()
;FOLD Screen output
$MSG_T={MSG_T: VALID FALSE, RELEASE FALSE, TYP #NOTIFY, MODUL[] " ", KEY[] " ", PARAM_TYP #VALUE, PARAM[] " ", DLG_FORMAT[] " ", ANSWER 0}
$MSG_T.MODUL[]="FRI-Ctrl"
IF (RETURN_VALUE == #MON) THEN
$MSG_T.KEY[] = "FRI successfully stopped."
ELSE
STRING_OFFSET = 0
SWRITE($MSG_T.KEY[], STATE, STRING_OFFSET, "ERROR during friStop (result: %d)", RETURN_VALUE)
ENDIF
$MSG_T.VALID=TRUE
WAIT FOR ($MSG_T.VALID == FALSE)
WAIT SEC 0.3
;ENDFOLD (Screen output)
; Signalize to the remote host that the FRI was stopped
$FRI_TO_INT[16] = 10
ENDIF
;ENDFOLD (For the case, friStop() has not been called, call it here)
RETURN_VALUE = friClose()
; Signalize to the remote host that the FRI was closed
$FRI_TO_INT[16] = 30
;FOLD Screen output
$MSG_T={MSG_T: VALID FALSE, RELEASE FALSE, TYP #NOTIFY, MODUL[] " ", KEY[] " ", PARAM_TYP #VALUE, PARAM[] " ", DLG_FORMAT[] " ", ANSWER 0}
$MSG_T.MODUL[]="FRI-Ctrl"
IF (RETURN_VALUE == #OFF) THEN
$MSG_T.KEY[] = "FRI successfully closed."
ELSE
STRING_OFFSET = 0
SWRITE($MSG_T.KEY[], STATE, STRING_OFFSET, "ERROR during friClose (result: %d)", RETURN_VALUE)
ENDIF
$MSG_T.VALID=TRUE
WAIT FOR ($MSG_T.VALID == FALSE)
WAIT SEC 0.3
;ENDFOLD (Screen output)
END