ABB RAPID: Calculating TCP-Relative Z Rotation Delta

Compute the relative pose first, then convert only the resulting orientation to an Euler Z angle. For targets expressed in the same workobject, use PoseMult(PoseInv(StartPose),FoundPose); this makes the start pose the reference and avoids reporting two unrelated absolute Euler decompositions.

Why the Existing Euler Values Are Ambiguous

pStartSearch2.rot and pFound2.rot are orientations expressed relative to the coordinate system used by the motion instructions, here Wobj_Welder. Calling EulerZYX on each quaternion separately therefore returns two absolute orientation decompositions. Those values do not directly describe how the TCP rotated from the start pose to the sensor-triggered pose.

Do not subtract independently calculated Euler angles. Euler angles are a sequence-dependent representation, have branch discontinuities, and become poorly conditioned near singular orientations. Subtracting them can produce misleading component differences even when the physical change is small.

Instead, perform the coordinate transformation with poses or quaternions:

Delta := PoseMult(PoseInv(StartPose), FoundPose);

In transformation form, this is Delta = inverse(T_start) * T_found. The inverse removes the start pose, so an unchanged found pose produces an identity rotation. Delta.rot then contains the orientation change expressed in the start-pose frame.

Coordinate-Frame Meaning of the Result

The search executes both the start and found positions with tPipe\WObj:=Wobj_Welder. When both target poses are represented in that same workobject, the relative-pose calculation cancels the common workobject reference. The resulting Z component is rotation about the Z axis of the start-pose frame, which is the required TCP-relative result when that start frame is aligned with the TCP axis being evaluated.

If the required clocking axis is not aligned with the start TCP Z axis, first establish the intended reference explicitly. One method is to define a temporary workobject at the start location with its axes aligned to the TCP, then record the start and found poses in that workobject. Another is to read both poses through CRobT using the same temporary workobject. In either case, apply the same inverse-start multiplication after both poses share one coordinate basis.

Multiplication order is critical. Use inverse start followed by found:

PoseMult(PoseInv(StartPose), FoundPose)

Reversing the operands, or multiplying the start pose by the inverse found pose, reports a different transform and can reverse the apparent direction or express it in another basis.

Diagnostic Sequence Before Changing the Program

1. Confirm that pStartSearch2 and pFound2 are represented in the same workobject. The shown MoveL and SearchL instructions both specify Wobj_Welder, so retain that common reference.
2. Confirm that tPipe is active for both instructions. A different tool definition changes the physical TCP pose even when the robtarget data appears similar.
3. Capture the complete relative pose, not only its final Z value. Inspect Delta.trans and Delta.rot while commissioning; unexpected X or Y rotation indicates that the search is not a pure Z-axis clocking operation.
4. Run a controlled test with only a RelTool Z rotation. The resulting EulerZYX(\Z,Delta.rot) should follow the commanded rotation, subject to the established sign convention.
5. Repeat the search without changing the part, fixture, speed, tool, workobject, or the existing waittime 0.3. Variation between runs then represents the repeatability of the complete measurement chain.

RAPID Fix Procedure

Build a pose from the translation and rotation fields of each robtarget. This excludes the external-axis and configuration fields, which are not required for the Cartesian relative-pose calculation.

PROC CheckEdge()
    VAR pose Delta;
    VAR num dz;

    MoveL pStartSearch2,vMoveSpeed,fine,tPipe\WObj:=Wobj_Welder;
    waittime 0.3;
    SearchL\Stop,i_R1SearchSensor2,pFound2,pEndSearch2,
            vSearchSpeed,tPipe\WObj:=Wobj_Welder;

    Delta:=PoseMult(
        PoseInv([pStartSearch2.trans,pStartSearch2.rot]),
        [pFound2.trans,pFound2.rot]);

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

Keep the existing array-search loop if it already selects the correct empty record, but store dzpFound2.rot. For the existing one-decimal storage format:

SeamCheck{i,4}:=Trunc(dz\Dec:=1);

Trunc with \Dec:=1 reduces the stored resolution to one decimal place. Preserve the untruncated dz value during sensor evaluation when variation below that resolution matters.

Verify the Rotation Calculation

First test the identity case by using the same pose for the start and found inputs. Delta.rot should be the identity orientation and all three Euler components should indicate no rotation.

Next, generate an end pose from a known start pose with only a RelTool Z rotation. Apply the same inverse-start calculation and verify the sign and magnitude of EulerZYX(\Z,Delta.rot). If the sign is opposite to the plant convention, document that convention and apply one deliberate sign change at the reporting boundary rather than reversing the pose multiplication.

For compound motion, use the tested pattern:

EndPose:=RelTool(StartPose,5,7,10 \Rx:=20 \Ry:=15 \Rz:=90);
Delta:=PoseMult(PoseInv([StartPose.trans,StartPose.rot]),
                [EndPose.trans,EndPose.rot]);

Do not expect the returned Euler X, Y, and Z components to equal the three RelTool rotation arguments in this compound case. RelTool applies rotations in X, then Y, then Z order, while EulerZYX decomposes the final quaternion in Z, then Y, then X order. The final orientation can be correct even though the individual numbers differ.

Recurring Pitfalls and Measurement Limits

The recorded angle evaluates more than i_R1SearchSensor2 alone. It also includes product and fixture movement, workobject accuracy, robot motion, search-speed effects, trigger behavior, and mechanical compliance. Hold those conditions constant and collect repeated raw results before assigning the variation specifically to the sensor.

FAQ