Difference in rotation around two points, relative to a TCP.

Luke1995

Afternoon, 

I have a robot programme that is searching to find the edge of a product. Pretty Simple; 

PROC CheckEdge()

MoveL pStartSearch2,vMoveSpeed,fine,tPipe\WObj:=Wobj_Welder;

waittime 0.3;

SearchL\Stop, i_R1SearchSensor2, pFound2,pEndSearch2,vSearchSpeed,tPipe\WObj:=Wobj_Welder;

ENDPROC 

The two points StartSearch2 and pEndSearch2 are a rotation around Z, relative to "tPipe". To check the accuracy of the sensor (i_R1SearchSensor2) I want to extract, and store the rotation around Z of pFound2 each time I run the search. What is the best way to do this?

I have code, updating and storing it in an array using Euler angles, but i think they're all reference to World or base so what i'm getting is not as clear and definitive as i want. 

  

        FOR i FROM 1 TO Dim(SeamCheck,1) DO 

            IF SeamCheck{i,1}=0 THEN

               SeamCheck{i,1}:=Trunc(EulerZYX(\X,pStartSearch2.rot)\Dec:=1);

               SeamCheck{i,2}:=Trunc(EulerZYX(\Y,pFound2.rot)\Dec:=1);

               SeamCheck{i,3}:=Trunc(EulerZYX(\X,pFound2.rot)\Dec:=1);

               SeamCheck{i,4}:=Trunc(EulerZYX(\Z,pFound2.rot)\Dec:=1);

               bAngleStored2:=TRUE;

               GOTO Enddfor2;

            ENDIF 

        ENDFOR 

        Enddfor2: 

Thanks in advance for any suggestions  

revans

@XNOR by multiplying by the inverse of the start pose, I am putting the output in tool coordinate systems. I did have the wrong variable being inversed, thank you for catching that. I have written a better example with tested math. The only part to be wary of is reltool rotations first X, then Y, then Z. The eulerZYX does it in Z, then Y, then X, so those numbers will not be the same.

PROC Testposemulti()

VAR robtarget StartPose:=[[100,200,300],[0,.707107,.707107,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];

VAR robtarget Endpose;

VAR pose Delta;

VAR num dx;

VAR num dy;

VAR num dz;

Endpose:=RelTool(StartPose,5,7,10 \Rx:=20 \Ry:=15 \Rz:=90);

Delta:= PoseMult(PoseInv([startpose.trans,startpose.rot]),[endpose.trans,endpose.rot]);

dx:= EulerZYX(\X,Delta.rot);

dY:= EulerZYX(\Y,Delta.rot);

dZ:= EulerZYX(\Z,Delta.rot);

        Stop;

        

ENDPROC

revans

I am not sure I follow your code all of the way. If you are just trying to compare the rotations, I would recommend doing your math in quaternions or poses. Then convert to Euler after the math. Something like below may work.

proc SearchComparison
var Pose Delta;

MoveL pStartSearch2,vMoveSpeed,fine,tPipe\WObj:=Wobj_Welder;

waittime 0.3;

SearchL\Stop, i_R1SearchSensor2, pFound2,pEndSearch2,vSearchSpeed,tPipe\WObj:=Wobj_Welder;

delta:= posemult(pStartSearch2, Posinv(pFound2));

tpwrite "Rotation About Z: " \Num:=EulerZYX(\z,delta.rot)

endproc

Assuming pStartSearch2 and pEndSearch2 are just rotated about the tool z like you said, I believe the function above will show just the z rotation. The angle may come out negative, not quite sure.

Good luck and let me know if that makes sense.

XNOR

I think Revans has a good suggestion in doing the math first for the delta and then converting to Euiler angles BUT both those points are defined relative to the wobj frame - not the TCP frame.  I don't think his solution will yield the correct answer. Here are a few thoughts:

• Have your TCP Z aligned with a major axis of your wobj to get your clocking angle using the posemult & inv math. Even if it's hte wobj X or Y you can infer it's the same rotation
• If tool0's Z is aligned with tcp Z you could just record J6 delta if that's the only joint doing work
• Define a temporary wobj that is aligned and in the same place as your TCP. Start point becomes zero'd out. Rotate using reltool in Z. Record your find point in the temp wObj and its rotation is your delta (since start point is zero'd) and you just have to convert orient to euler angle.
• similar to above - keep your existing motion but still create a temp Wobj at start location and record the start point and found point using CRobT using the \wobj option to set your temp wobj and then do the math on it

Luke1995

That is perfect thanks, does the job nicely  

I definitely didn't have a enough understanding of what a pose was and how it could be used. Nor was i aware of the functions "posemult" and "PoseInv".23 these suggestions both forced me to read up and improve my knowledge  

Thanks again for you help