Incorrect base shift in the adaptive program

DenVer · January 28, 2022 at 6:23 PM

Hi there!

First of all, I would like to say a huge thank you to all the active forum members - you share invaluable information!

Special thanks to SkyeFire and PanicMode - you guys are my heroes.

So, the problem is this.

I am making a small adaptive program to weld intermittent seams with given characteristics (e.g. 30/100 = 30 mm welding, 70 mm no welding, and so on throughout the seam).

The program automatically builds a discontinuous weld between any two points with the specified characteristics, with approaches and waste, with the specified tool orientation.

The basic idea is to find the direction cosines of the trajectory vector (and the rotation angles, respectively) move the base to the origin of this vector, and align the X-axis of the base with this vector.

But, it doesn't work on the robot the way I expect it to.

If I build a path from the first point to the second point from left to right, the robot moves from right to left after the program runs.

Any advice would be appreciated.

KR8 R2010 KSS 8.6.8

Code

BASE_DATA[32] = $NULLFRAME 
$BASE = BASE_DATA[32]


;  Here we need to change the value of the variable "w" to the actual length of the seam.

w = 50
;----------

;  Here we need to change the value of the variable "t" to the total length of the step.

t = 100
;----------

;  Here you can disable welding of the last incomplete segment.
;   1 - ON , 0 - OFF
weld_last_segment = 1
;----------


NACHALO_ROTATE = $NULLFRAME
KONEC_ROTATE = $NULLFRAME
ROTATE_POINT = $NULLFRAME

PTP XHOME

counter = 1

T1_x = XNACHALO.X
T1_y = XNACHALO.Y
T1_z = XNACHALO.Z

T2_x = XKONEC.X
T2_y = XKONEC.Y
T2_z = XKONEC.Z

T1T2_x = T2_x - T1_x
T1T2_y = T2_y - T1_y
T1T2_z = T2_z - T1_z

l = SQRT(SQUARE(T1T2_x) + SQUARE(T1T2_y)+ SQUARE(T1T2_z))

n = l / t
n_int = n

IF (n - n_int) < 0 THEN
   n_int = n_int - 1
ENDIF

final_w = t*(n - n_int)
IF final_w >= w THEN
   final_w = w
ENDIF

cos_a = T1T2_x /SQRT(SQUARE(T1T2_x) + SQUARE(T1T2_y))
angle_a = ACOS(cos_a)

cos_z = T1T2_z /l
angle_z = ACOS(cos_z) - 90

BASE32_SHIFT = $NULLFRAME
BASE32_SHIFT.X = XNACHALO.X
BASE32_SHIFT.Y = XNACHALO.Y
BASE32_SHIFT.Z = XNACHALO.Z
BASE32_SHIFT.A = angle_a
BASE32_SHIFT.B = angle_z
BASE32_SHIFT.C = 0.0
BASE_DATA[32] = BASE32_SHIFT ; move the Base32 to the beginning of the weld vector


NACHALO_ROTATE = ReturnPosRelToBase(XNACHALO, $NULLFRAME, BASE_DATA[32]) ; recalculate the tool orientation in 
KONEC_ROTATE = ReturnPosRelToBase(XKONEC, $NULLFRAME, BASE_DATA[32])     ; the angles of the final offset Base32

XNACHALO.X = 0.0
XNACHALO.Y = 0.0
XNACHALO.Z = 0.0
XNACHALO.A = NACHALO_ROTATE.A
XNACHALO.B = NACHALO_ROTATE.B
XNACHALO.C = NACHALO_ROTATE.C


XKONEC.Y = 0.0
XKONEC.Z = 0.0
XKONEC.A = KONEC_ROTATE.A
XKONEC.B = KONEC_ROTATE.B
XKONEC.C = KONEC_ROTATE.C

WHILE counter < n
   
   XKONEC.X = ((counter - 1)*t + w)
      
   FAPPROACH = FNACHALO
   XAPPROACH = XNACHALO:SHIFT
   
LIN XAPPROACH

   
;FOLD ARCON WDAT1 LIN NACHALO Vel=1 m/s CPDAT2 Tool[1] Base[32]  ;%{PE}
;FOLD Parameters ;%{h}
;Params IlfProvider=kukaroboter.arctech.arconlin; Kuka.IsGlobalPoint=False; Kuka.PointName=NACHALO; Kuka.BlendingEnabled=False; Kuka.MoveDataName=CPDAT1; Kuka.VelocityPath=1; Kuka.CurrentCDSetIndex=0; Kuka.MovementParameterFieldEnabled=True; ArcTech.WdatVarName=WDAT1; ArcTech.Basic=3.3.4.387
;ENDFOLD
$BWDSTART = FALSE
LDAT_ACT = LCPDAT1
FDAT_ACT = FNACHALO
BAS(#CP_PARAMS, 1.0)
SET_CD_PARAMS (0)
TRIGGER WHEN DISTANCE = 1 DELAY = ArcGetDelay(#PreDefinition, WDAT1) DO ArcMainNG(#PreDefinition, WDAT1, WP5) PRIO = -1
TRIGGER WHEN PATH = ArcGetPath(#OnTheFlyArcOn, WDAT1) DELAY = ArcGetDelay(#GasPreflow, WDAT1) DO ArcMainNG(#GasPreflow, WDAT1, WP5) PRIO = -1
TRIGGER WHEN PATH = ArcGetPath(#OnTheFlyArcOn, WDAT1) DELAY = 0 DO ArcMainNG(#ArcOnMoveStd, WDAT1, WP5) PRIO = -1
ArcMainNG(#ArcOnBeforeMoveStd, WDAT1, WP5)
LIN XNACHALO
ArcMainNG(#ArcOnAfterMoveStd, WDAT1, WP5)
;ENDFOLD


;FOLD ARCOFF WDAT1 LIN KONEC Vel=1 m/s CPDAT2 Tool[1] Base[32] ;%{PE}
;FOLD Parameters ;%{h}
;Params IlfProvider=kukaroboter.arctech.arcofflin; Kuka.IsGlobalPoint=False; Kuka.PointName=KONEC; Kuka.BlendingEnabled=False; Kuka.MoveDataName=CPDAT2; Kuka.VelocityPath=0.033; Kuka.CurrentCDSetIndex=0; Kuka.MovementParameterFieldEnabled=True; ArcTech.WdatVarName=WDAT1
;ENDFOLD
$BWDSTART = FALSE
LDAT_ACT = LCPDAT2
FDAT_ACT = FKONEC
BAS(#CP_PARAMS, gArcBasVelDefinition)
SET_CD_PARAMS (0)
TRIGGER WHEN PATH = ArcGetPath(#ArcOffBefore, WDAT1) DELAY = 0 DO ArcMainNG(#ArcOffBeforeOffStd, WDAT1) PRIO = -1
TRIGGER WHEN PATH = ArcGetPath(#OnTheFlyArcOff, WDAT1) DELAY = 0 DO ArcMainNG(#ArcOffMoveStd, WDAT1) PRIO = -1
ArcMainNG(#ArcOffBeforeMoveStd, WDAT1)
LIN XKONEC
ArcMainNG(#ArcOffAfterMoveStd, WDAT1)
;ENDFOLD


   XAPPROACH = XKONEC:SHIFT      
   

LIN XAPPROACH


   XNACHALO.X = counter * t
   
   counter = counter + 1

ENDWHILE

DEFFCT FRAME ReturnPosRelToBase(p:IN, baseSrc:IN, baseDst:IN)
FRAME res, p, baseSrc, baseDst
  ;Equation:
  ;baseSrc : p = baseDst : res
   
  res = INV_POS(baseDst) : baseSrc : p
  
  return res
ENDFCT

Display More

MOM · January 28, 2022 at 7:05 PM

As a first guess:

your problem starts with line 61

Quote from DenVer

SE32_SHIFT = $NULLFRAME BASE32_SHIFT.X = XNACHALO.X BASE32_SHIFT.Y = XNACHALO.Y BASE32_SHIFT.Z = XNACHALO.Z BASE32_SHIFT.A = angle_a BASE32_SHIFT.B = angle_z BASE32_SHIFT.C = 0.0 BASE_DATA[32] = BASE32_SHIFT ; move the Base32 to the beginning of the weld vector

$Nullframe is write protected (actually you should get error messages)

If you want to change values of this frame you probably asigned as $Nullframe change them to myNullFrame (this time no write protection)

myNullFrame = {X 0.0, Y 0.0, Z 0.0 ,A 0.0, B 0.0, C 0.0}

(hint: take $ROBROOT as an example - if floor mounted!)

**SkyeFire** · January 29, 2022 at 4:41 PM

How is this intended to work? Are you placing the Origin of the Base at the start of the weld, and simply calculating from there to the endpoint? It looks like you're trying to set the X axis of Base 32 to be the vector of the weld, correct?

Does this error occur in all quadrants? If this is a trigonemetry issue, running it in all 4 quadrants (45deg, 135deg, 225deg, 315deg) might reveal something.

hermann · January 30, 2022 at 8:50 AM Best Answer

Think the problem is using ACOS().

This function leads to loosing the sign of the angle.

Use atan2 with the difference of x and y coordinates instead of x and length of hypothenuse.

DenVer · January 30, 2022 at 5:20 PM

Quote from MOM

As a first guess:
your problem starts with line 61

$Nullframe is write protected (actually you should get error messages)

No, of course, I am not trying to overwrite the system variable $Nullframe

Line 61 simply initiates my FRAME variable with null values. Although to tell you the truth, this is unnecessary in this code.

Quote from SkyeFire

How is this intended to work? Are you placing the Origin of the Base at the start of the weld, and simply calculating from there to the endpoint? It looks like you're trying to set the X axis of Base 32 to be the vector of the weld, correct?
Does this error occur in all quadrants? If this is a trigonemetry issue, running it in all 4 quadrants (45deg, 135deg, 225deg, 315deg) might reveal something.

Yes, that's the logic.

I place the origin of the coordinates at the starting point of the weld, align the X-axis of base 32 with the vector of the weld and simply increase the X value to weld the required number of weld segments.

In this case, the wrong direction does not occur in all quarters of the coordinate system.

But I don't think it's trigonometry or linear algebra problem - in my excel the calculations are correct. And if you transfer the resulting coordinates to KUKA SimPro, they are exactly the same as I thought they would be.

Quote from hermann

Think the problem is using ACOS().
This function leads to loosing the sign of the angle.
Use atan2 with the difference of x and y coordinates instead of x and length of hypothenuse.

Yeah! It's very much like the truth!

I thought about the possibility of losing a sign somewhere but couldn't see how that was possible.

I will try to implement alternative logic.

Thanks to all of you guys!

Incorrect base shift in the adaptive program

Advertising from our partners

How to see your Job Posting (search or recruit) here in Robot-Forum.com