Create virtual robtargets and moves inside RAPID program

Laro88

I have a software issue that I probably need to solve in rapid.
I need to split one moveL (say 10mm) into a set of moveL instructions separated by say 1mm.
The reason is that I need to "wiggle" the orient a bit for every 1mm. of motion. To do this I probably meed some some of vector solution so that I can create a subroutine that allows me to programmatically perform this slicing. If I perform this operation on the pc (planning / generation phase) then I end up with some extremely long chains of MoveL - and then it becomes more or less impossible to upload the generated RAPID code.

pseudocode:
robtargetBegin = robtargetNow //where am I - trivial
vector3D v3d = robtargetEnd-robtargetBegin
int steps = v3d.Length / 1mm

for(int ptr=0;ptr<increment;ptr++)
  robtarget tmp = robtargetBegin+(v3d/steps*ptr); //calculate target as vectors
  tmp.orient *= ComputeWiggleOrient(); //multiply the wiggle to the orient to get desired behavior
  MoveL tmp, speedFree, speedFine, toolProgram \WObj:=woWorkTool_65483223;
endfor

end pseudo

Is this possible in some clever way?

bob865

You can do it with offsets or reltools.  You would only have to teach one position(the start) to do it that way.  You would need to determine a way to calculate x,y,z offsets along the path.  If it's linear, that's easy.

Laro88

Here is my code that splits a MoveL into substeps to make the tool wobble while performing a moveL. Not perfect yet, but it actually works nice on ABB140's

MODULE Wobble
    !Performs a wobbly motion (makes tool pivot rotational) while moving linearly towards the final target
    
    PERS num wobble_opcount; !global counter to get continuous pivotal behaviour
    PERS string wobblelog;
    
    PROC MoveL_Wobble(robtarget tgt, speeddata spd, zonedata zone, pers tooldata tool, pers wobjdata wobj)
    VAR robtarget rtNow; !start position
    VAR num dist_to_tgt; !distance to final target
    VAR num iStepsNeeded; !how manu steps to split motion into
    VAR num distance_pr_increment; !how long is the real stepsize (avoid last step rounding error)
    
    VAR pos unitvector;
    VAR robtarget temptarget;
    VAR robtarget targets{1000};
    
    VAR num angleX;
    VAR num angleY;
    
    wobblelog:="";
    
    rtNow := CRobT(\Tool:=tool, \WObj:=wobj);!Determine present location
    
    WobbleWriteLog 101 , "Wobble from:" + WobbleRobtargetToString(rtNow);
    WobbleWriteLog 101 , "Wobble to  :" + WobbleRobtargetToString(tgt);
    
    dist_to_tgt := Distance(rtNow.trans, tgt.trans);!How far is is the target away
    iStepsNeeded := Round(dist_to_tgt / 1); !assume 1mm steps for now
    IF(iStepsNeeded<1) THEN
        iStepsNeeded := 1;
    ENDIF
    distance_pr_increment:=dist_to_tgt / iStepsNeeded; !2.5mm / 2teps => 1.25mm steps
    WobbleWriteLog 102, "Dist:" + NumToStr(dist_to_tgt,2) + "steps:" + NumToStr(iStepsNeeded, 0) + "dist pr step:" + NumToStr(distance_pr_increment, 5);
    !compute unit vector to create intermediate targets
    unitvector.x := (tgt.trans.x - rtNow.trans.x) / dist_to_tgt;
    unitvector.y := (tgt.trans.y - rtNow.trans.y) / dist_to_tgt;
    unitvector.z := (tgt.trans.z - rtNow.trans.z) / dist_to_tgt;
    
    !loop to create targets
    FOR i FROM 1 TO iStepsNeeded DO
        !compute target using the initial pos and the unit vector * step
        targets{i}:=tgt;
        targets{i}.trans.x := rtNow.trans.x + unitvector.x*i;
        targets{i}.trans.y := rtNow.trans.y + unitvector.y*i;
        targets{i}.trans.z := rtNow.trans.z + unitvector.z*i;
        
        !apply rotational instruction
        angleX := 1 * Cos(wobble_opcount*60);
        angleY := 1 * Sin(wobble_opcount*60);
        wobble_opcount := wobble_opcount + 1;
        
        targets{i}.rot:= targets{i}.rot * OrientZYX(0,angleY, angleX); !20141117jr  yaw pitch correct but does not work
        targets{i}.rot:= targets{i}.rot * OrientZYX(angleY,angleY, angleX); !20141117jr  yaw pitch correct but does not work
    ENDFOR
    
    WobbleWriteLog 103, "wobble opcount:" + NumToStr(wobble_opcount, 0);
    
    !finally perform the desired motion
    FOR i FROM 1 TO iStepsNeeded DO
        temptarget := targets{i};
        MoveL temptarget, spd, zone, tool \WObj:=wobj;
        WobbleWriteLog 200 + i, "target:" + WobbleRobtargetToString(temptarget);
    ENDFOR
    
    ErrWrite \I, "WobbleLog", "Debug" \RL2:=wobblelog;
    ENDPROC
    
    PROC WobbleWriteLog(num ID, string Msg)
        
        ErrWrite \I, "ID:" + numtostr(ID, 0), NumToStr(ID,0) + Msg;
        !wobblelog := wobblelog + " " + NumToStr(ID, 0) + ":" + Msg;
    ENDPROC
    
    FUNC string WobbleRobtargetToString(robtarget RT)
        var string position;
        VAR string ori;
        VAR num ax;
        var num ay;
        var num az;
        position := NumToStr(RT.trans.x, 2) + " " + NumToStr(RT.trans.y, 2) + " " + NumToStr(RT.trans.z, 2);
        
        ax := EulerZYX(\X, RT.rot);
        ay := EulerZYX(\Y, RT.rot);
        az := EulerZYX(\Z, RT.rot);
        
        ori := "Angles xyz:" + NumToStr(ax,2) + " " + NumToStr(ay,2) + " " + NumToStr(az,2);
        RETURN position +" "+ori;
    ENDFUNC

ENDMODULE