Hi,
I wanted to move Kuka in a conical trajectory, in which only B and C changes in tool point coordinates (X, Y, Z, A, B, C) and rest of the values remain unchanged. Below is the KRL code-
Code
&ACCESS RVP
&REL 28
&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
&PARAM EDITMASK = *
DEF ConeMotion()
REAL kpOri, tilt, desiredB, desiredC, BKorr, CKorr, CNew
FRAME relFrame
INT alpha
;FOLD INI
;FOLD BASISTECH INI
GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
INTERRUPT ON 3
BAS (#INITMOV,0 )
;ENDFOLD (BASISTECH INI)
;FOLD USER INI
;Make your modifications here
;ENDFOLD (USER INI)
;ENDFOLD (INI)
; kp factor for tilt motion
kpOri=0.05
$ADVANCE=1
; 10 mm (I think its too much)
$APO.CPTP=10
; 10 degree tilt
tilt=10.0
$TOOL=TOOL_DATA[2]
$BASE=$NULLFRAME
;FOLD PTP HOME Vel= 100 % DEFAULT;%{PE}%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART = FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS (#PTP_PARAMS,100 )
$H_POS=XHOME
PTP XHOME
;ENDFOLD
; Rotation
FOR alpha = 0 TO 360 STEP 1
relFrame = {X 0,Y 0,Z 0,A 0,B 0,C 0}
desiredC = Asin(SIN(alpha) * SIN(tilt))
desiredB = Asin(COS(alpha) * SIN(tilt))
CNew = Asin(Sin($POS_ACT.C))
BKorr = -(desiredB - CNew) * kpOri
CKorr = -(desiredC - $POS_ACT.B) * kpOri
relFrame.B = BKorr
relFrame.C = CKorr
PTP_REL relFrame C_PTP
ENDFOR
;FOLD PTP HOME Vel= 100 % DEFAULT;%{PE}%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART = FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS (#PTP_PARAMS,100 )
$H_POS=XHOME
PTP XHOME
;ENDFOLD
END
Display More
The relative motion in B and C is commanded by BKorr and CKorr variable. $APO_C shows some different values (Please see the attached screenshot for their run time value)
While running this program in AUT mode, I noticed that although the motion looks continuous to me but there is a vibration in end effector. I tried to change the kpOri factor but it doesn't work out. The motion becomes too slow.