RAPID: Linear Interpolation

daniel72

Linear interpolation between two points with orientation and configuration

Given is a line S determined by robtarget rStart and robtraget rEnd in wobj0.

I'want to calculate a robtarget rRes also in wobj0 which is located on line S and is defined by a distance nDistfromStart from rStart on the route of S. .

The Head of the function looks like this:

   Function robtarget rRes InterpolateL(robtarget rStart, robtarget rEnd, number nDistfromStart)

    ENDFUNC

(Its should work like Stäubli VAL3-Function "interpolateL")

pRes.trans can be calculated using vectors, but how do I calculate rRes.rot and pRes.conf. The orientation of the robot should be the same at rRes as at pStart relative to S.

Iam using RW 6.07.

Thanks

Daniel

S_Smith

 ! this is an example, Replace .Trans.X with the direction you are wanting to move

PROC InterpolateL ( INOUT robtarget rRes,
                                  robtarget rStart,
                                  num nDistfromStart )

rRes.Trans.X  :=  rStart.Trans.X + nDistfromStart ;
rRes.Rot  :=  rStart.Rot ;
rRes.RobConf  :=  rStart.RobConf ;

! if you are using a positioner
rRes.extax:=  rStart.extax;

waittime .5;

ENDPROC

lemster68

Go Here: https://github.com/ernell/ABB-RAPID-UTILITY-LIBRARY/blob/master/Contributions/RobertAndersson/lib_rob.sys  Look for function MidPos.  Use the Distance function to determine the overall distance between start and end.  Then do the math nDistfromStart/overall distance to find the multiplier.  Replace .5 with your answer.  @S_Smith are you going to get in on the github?  P.S. I noticed this question was from last year. 

Micky

Hello,

here is another solution based on the following assumptions:

• All points are in the same work object

• All points are approached with the same tool

• The orientation of the two points are identical or do not have to be taken into account.
  

The new position is calculated by shifting along with the direction vector relative to the endpoint.

By using a positive distance the new point lies behind the endpoint and by using a negative distance the new point lies before the endpoint.

 
MoveL RelDirVect(p10,p20,-100),v1000,fine,tool0;

FUNC robtarget RelDirVect(robtarget FromPoint, robtarget ToPoint, num Dist)<br><a rel="nofollow" href="https://www.roboterforum.de/roboter-forum/user-post-list/370-micky/?pageNo=3#codeLine_5_f5c05b"></a>      VAR pos psDirection;<br><a rel="nofollow" href="https://www.roboterforum.de/roboter-forum/user-post-list/370-micky/?pageNo=3#codeLine_6_f5c05b"></a>      VAR pos psDelta;<br><a rel="nofollow" href="https://www.roboterforum.de/roboter-forum/user-post-list/370-micky/?pageNo=3#codeLine_7_f5c05b"></a>      VAR robtarget pNew;<br><a rel="nofollow" href="https://www.roboterforum.de/roboter-forum/user-post-list/370-micky/?pageNo=3#codeLine_8_f5c05b"></a>      VAR num nLamda;     
 <br><a rel="nofollow" href="https://www.roboterforum.de/roboter-forum/user-post-list/370-micky/?pageNo=3#codeLine_9_f5c05b"></a>      !Vector calculation:  p3 = p2 - lamda * (p2-p1)   !with Lamda = Dist/length of the direction vector  

      !calculate direction vector
      psDirection:= ToPoint.trans-FromPoint.trans;      <br>      !Calculate scale factor "Lamda"<br><a rel="nofollow" href="https://www.roboterforum.de/roboter-forum/user-post-list/370-micky/?pageNo=3#codeLine_14_f5c05b"></a>      nLamda:=Dist/VectMagn(psDirection);      <br>      !Calculate displacement coordinates<br><a rel="nofollow" href="https://www.roboterforum.de/roboter-forum/user-post-list/370-micky/?pageNo=3#codeLine_16_f5c05b"></a>      psDelta:=nLamda*psDirection;<br><a rel="nofollow" href="https://www.roboterforum.de/roboter-forum/user-post-list/370-micky/?pageNo=3#codeLine_17_f5c05b"></a>      !Calculate new position<br><a rel="nofollow" href="https://www.roboterforum.de/roboter-forum/user-post-list/370-micky/?pageNo=3#codeLine_18_f5c05b"></a>      pNew:=ToPoint;<br><a rel="nofollow" href="https://www.roboterforum.de/roboter-forum/user-post-list/370-micky/?pageNo=3#codeLine_19_f5c05b"></a>      pNew.trans:=ToPoint.Trans+psDelta;<br>      RETURN pNew;<br><a rel="nofollow" href="https://www.roboterforum.de/roboter-forum/user-post-list/370-micky/?pageNo=3#codeLine_23_f5c05b"></a>    ENDFUNC

Best regards
Micky