Create virtual robtargets and moves inside RAPID program

Robot Controller
1

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?

2

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.

3

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.xi;
targets{i}.trans.y := rtNow.trans.y + unitvector.yi;
targets{i}.trans.z := rtNow.trans.z + unitvector.z*i;

!apply rotational instruction
angleX := 1 * Cos(wobble_opcount60);
angleY := 1 * Sin(wobble_opcount60);
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