Calculating TCP

ambersat · June 26, 2015 at 7:16 PM

Hi all,

How compute TCP from 4 different point:

Measured P1{ X -1212.375 ,Y 3785.411, Z 1180.433, A -0.93, B 47.263, C -92.214}
Measured P2{ X -1150.558 ,Y 3911.037, Z 1180.013, A 48.827, B 47.309, C -92.209}
Measured P3{ X-1464.654 , Y 3785.021, Z 1163.455, A 28.075, B -2.869, C -106.945}
Measured P4{ X -1283.76 , Y 3676.609, Z 1132.282, A -26.651, B 20.973, C -71.511}

I know how compute center of sphere, But I get :

{X-1316,739144363, Y 3913,6685403798,Z 845,4296028114 }, this is reference point of measure, but I need compute relative to the Flange system

KUKA compute:
{X 175.143,Y 301.955, Z 139.15}

How get these result mathematically?

Robot: KUKA KR210

**panic mode** · June 27, 2015 at 1:40 AM

first you need to know what P1-P4 really represent (what tool and base?).

ambersat · June 27, 2015 at 7:32 AM

Hi

P1-P4

$TOOL = $BASE = $NULLFRAME

**panic mode** · June 27, 2015 at 5:37 PM

I assume you already did sanity checks and verified that points are on sphere etc.

what do you get if you use
INV_POS(P1):PC

where PC is frame with your computed sphere center

ambersat · June 29, 2015 at 8:59 AM

Quote from panic mode

I assume you already did sanity checks and verified that points are on sphere etc.

what do you get if you use
INV_POS(P1):PC

where PC is frame with your computed sphere center

Hi,

{POS: X 173.8236, Y 300.3997, Z 138.2551, A -47.26606, B 6.962621E-01, C 91.50240},

This is great, but how to get this answer mathematically?

Puck · June 29, 2015 at 12:20 PM

Take a look at the german robot forum:

http://www.roboterforum.de/roboter-forum/…g55090#msg55090

Calculate the flange orientation matrices and flange positions from the four flange frames:

R_1 * t + r_1 = P with orientation matrix R_1 = R_1(A_1, B_1, C_1) and position r_1 = [X_1, Y_1, Z_1]^(T)
R_2 * t + r_2 = P with orientation matrix R_2 = R_2(A_2, B_2, C_2) and position r_2 = [X_2, Y_2, Z_2]^(T)
R_3 * t + r_3 = P with orientation matrix R_3 = R_3(A_3, B_3, C_3) and position r_3 = [X_3, Y_3, Z_3]^(T)
R_4 * t + r_4 = P with orientation matrix R_4 = R_4(A_4, B_4, C_4) and position r_4 = [X_4, Y_4, Z_4]^(T)

The vector t = [X_TCP, Y_TCP, Z_TCP] is the unknown TCP and the point P is the reference point used by the XYZ 4-point method.
Rearrange the equations and eliminate the reference point P:

R_1 * t + r_1 = R_2 * t + r_2
R_1 * t + r_1 = R_3 * t + r_3
R_1 * t + r_1 = R_4 * t + r_4

Rearrange:

[R_1 - R_2] [r_2 - r_1]
[R_1 - R_3] [ t ]= [r_3 - r_1]
[R_1 - R_4] [r_4 - r_1]

9x3 3x1 9x1

Solve the overdetermined system of the form A x = b. Good luck

Bye Puck

fares f · March 1, 2021 at 8:34 PM

HI ambersat

CAN I KNOW WHAT ARE YOU USING FOR COMPILING .

IM WORKING ON TCP CALIBRATION ALGORITHM AND I FACED SAME PROBLEM .

(IM USING MATLAB FOR MY CALCULATION)

**panic mode** · March 1, 2021 at 9:52 PM

"INV_POS()" and geometric operator ":" are KRL functions

micahstuh · March 1, 2021 at 10:12 PM

Here is some code I wrote for a C# application. It may not be super helpful given the language you're working in but here's hoping.

Code

        /// <summary>
        /// Calculate the XYZ values of a tool
        /// </summary>
        /// <param name="samples">Position samples using $NULLFRAME as $TOOL</param>
        /// <param name="error">Average error between samples from calculated TCP</param>
        /// <returns>Final TCP data</returns>
        private double[] CalculateTCP(List<double[]> samples, out double error)
        {
            // Convert matrices from samples to prepare for linear algebra
            var a = new double[samples.Count, 4];
            var f = new double[samples.Count, 1];
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < samples.Count; j++)
                {
                    a[j, 3] = 1;
                    a[j, i] = samples[j][i] * 2;
                    f[j, 0] += Math.Pow(samples[j][i], 2);
                }
            }

            // Solve for system of linear equations using Guass Elim
            double[,] c = Transpose(GaussianElimination(f, a));

            // Find radius of sphere in which A6 flange falls on
            double radius = Math.Sqrt(c[0, 3] + Math.Pow(c[0, 0], 2) + Math.Pow(c[0, 1], 2) + Math.Pow(c[0, 2], 2));

            // Get the average inverse transformation from center of sphere to A6 flange
            var avgTcp = new double[3];
            var tcpGuess = new List<double[]>();
            for (int i = 0; i < samples.Count; i++)
            {
                tcpGuess.Add(InvTransformPoint(c, samples[i]));

                for (int j = 0; j < 3; j++)
                {
                    avgTcp[j] += tcpGuess[i][j];
                }
            }
            for (int i = 0; i < 3; i++)
            {
                avgTcp[i] /= samples.Count;
            }

            // Calculate error average
            error = 0;
            for (int i = 0; i < samples.Count; i++)
            {
                error += Distance(tcpGuess[i], avgTcp);
            }

            error /= samples.Count;

            return avgTcp;
        }

Display More

**panic mode** · March 2, 2021 at 5:51 AM Best Answer

http://www.convertalot.com/sphere_solver.html

and btw, one can right click on page and view source to see Java code:

Code

<!-- hide
var zero = parseFloat("0");
var Xo, Yo, Zo;
var P = [[zero,zero,zero],[zero,zero,zero],[zero,zero,zero],[zero,zero,zero]];

// evaluate the determinant
function compute()
{
    P[0][0] = parseFloat(calc.x1.value);
    P[0][1] = parseFloat(calc.y1.value);
    P[0][2] = parseFloat(calc.z1.value);
    P[1][0] = parseFloat(calc.x2.value);
    P[1][1] = parseFloat(calc.y2.value);
    P[1][2] = parseFloat(calc.z2.value);
    P[2][0] = parseFloat(calc.x3.value);
    P[2][1] = parseFloat(calc.y3.value);
    P[2][2] = parseFloat(calc.z3.value);
    P[3][0] = parseFloat(calc.x4.value);
    P[3][1] = parseFloat(calc.y4.value);
    P[3][2] = parseFloat(calc.z4.value);

    // check input
    if (isNaN(P[0][0])||isNaN(P[0][1])||isNaN(P[0][2])||
        isNaN(P[1][0])||isNaN(P[1][1])||isNaN(P[1][2])||
        isNaN(P[2][0])||isNaN(P[2][1])||isNaN(P[2][2])||
        isNaN(P[3][0])||isNaN(P[3][1])||isNaN(P[3][2]))
    {
        window.alert("Invalid input!");
        return;
    }
  
    var r = sphere();
    if (r > 0)
    {
        calc.x0.value = Xo;
        calc.y0.value = Yo;
        calc.z0.value = Zo;
        calc.r0.value = r;
    }
    else
        calc.r0.value = "Not a sphere";
}

// Calculate center and radius of sphere given four points
function sphere()
{
    var i;
    var r, m11, m12, m13, m14, m15;
    var a = [[zero,zero,zero,zero],[zero,zero,zero,zero],[zero,zero,zero,zero],[zero,zero,zero,zero]];

    for (i = 0; i < 4; i++)                    // find minor 11
    {
        a[i][0] = P[i][0];
        a[i][1] = P[i][1];
        a[i][2] = P[i][2];
        a[i][3] = 1;
    }
    m11 = determinant( a, 4 );

    for (i = 0; i < 4; i++)                    // find minor 12 
    {
        a[i][0] = P[i][0]*P[i][0] + P[i][1]*P[i][1] + P[i][2]*P[i][2];
        a[i][1] = P[i][1];
        a[i][2] = P[i][2];
        a[i][3] = 1;
    }
    m12 = determinant( a, 4 );

    for (i = 0; i < 4; i++)                    // find minor 13
    {
        a[i][0] = P[i][0]*P[i][0] + P[i][1]*P[i][1] + P[i][2]*P[i][2];
        a[i][1] = P[i][0];
        a[i][2] = P[i][2];
        a[i][3] = 1;
    }
    m13 = determinant( a, 4 );

    for (i = 0; i < 4; i++)                    // find minor 14
    {
        a[i][0] = P[i][0]*P[i][0] + P[i][1]*P[i][1] + P[i][2]*P[i][2];
        a[i][1] = P[i][0];
        a[i][2] = P[i][1];
        a[i][3] = 1;
    }
    m14 = determinant( a, 4 );


    for (i = 0; i < 4; i++)                    // find minor 15
    {
        a[i][0] = P[i][0]*P[i][0] + P[i][1]*P[i][1] + P[i][2]*P[i][2];
        a[i][1] = P[i][0];
        a[i][2] = P[i][1];
        a[i][3] = P[i][2];
    }
    m15 = determinant( a, 4 );

    if (m11 == 0)
    {
        r = 0;
    }
    else
    {
        Xo =  0.5*m12/m11;                     // center of sphere
        Yo = -0.5*m13/m11;
        Zo =  0.5*m14/m11;
        r  = Math.sqrt( Xo*Xo + Yo*Yo + Zo*Zo - m15/m11 );
    }

    return r;                                  // the radius
}


//  Recursive definition of determinate using expansion by minors.
function determinant( a, n )
{
    var i, j, j1, j2;
    var d = parseFloat("0");
    var m = [[zero,zero,zero,zero],[zero,zero,zero,zero],[zero,zero,zero,zero],[zero,zero,zero,zero]];

    if (n == 2)                                // terminate recursion
    {
        d = a[0][0]*a[1][1] - a[1][0]*a[0][1];
    }
    else 
    {
        d = 0;
        for (j1 = 0; j1 < n; j1++ )            // do each column
        {
            for (i = 1; i < n; i++)            // create minor
            {
                j2 = 0;
                for (j = 0; j < n; j++)
                {
                    if (j == j1) continue;
                    m[i-1][j2] = a[i][j];
                    j2++;
                }
            }
            
            // sum (+/-)cofactor * minor  
            d = d + Math.pow(-1.0, j1)*a[0][j1]*determinant( m, n-1 );
        }
    }

    return d;
}

// unhide -->

Display More

fares f · March 2, 2021 at 2:58 PM

thank you so much

fares f · March 2, 2021 at 3:34 PM

I was able to calculate the center of the sphere, but I can't use this point to get the TCP .

{X 446.54, Y 47.42,Z 361.08 }, this is reference point (the center of the sphere), but I need compute relative to the Flange system which is { X 91.818 , Y -1.55 ,Z 215.789 } computed by KUKA

THANK YOU

**panic mode** · March 2, 2021 at 4:03 PM

Quote

relative to the Flange system witch is { X 91.818 , Y -1.55 ,Z 215.789 }

that is a witch...

to do a transform, it is not enough to know flange translation, one also needs flange orientation.

translation is only a vector indicated by X,Y,Z. orientation is defined by angles A,B,C.

orientation angles are needed to form 3x3 rotational matrix which is key part of the general transform.

transform combines rotation matrix and translation into 4x4 matrix. multiplication by this 4x4 matrix is what KUKA calls geometric operator etc.

fares f · March 2, 2021 at 4:24 PM

Quote from panic mode

that is a witch...
to do a transform, it is not enough to know flange translation, one also needs flange orientation.
translation is only a vector indicated by X,Y,Z. orientation is defined by angles A,B,C.
orientation angles are needed to form 3x3 rotational matrix which is key part of the general transform.
transform combines rotation matrix and translation into 4x4 matrix. multiplication by this 4x4 matrix is what KUKA calls geometric operator etc.

Display More

that's what i did .i have my own forward kinematics and it works good (same results as those for KUKA).

and as i told you i got the reference point which is the center of sphere by 4 point method . all what i need now is how to Get the average inverse transformation from center of sphere to A6 flange.

thank you .

micahstuh · March 2, 2021 at 4:28 PM

Herse some more C# but it should be pretty easy to read.

Code

        public static double[] InvTransformPoint(double[] point, double[] origin)
        {
            // If point=TransformPoint(pointOriginal, origin)
            // Then this will give pointOriginal given point and origin
            // If that makes sense...
            double[,] oTI = Transpose(RotToMat(origin[3], origin[4], origin[5]));
            double[] oPosI = MatMult(MatNeg(oTI), GetRange(origin, 0, 2));
            double[] oRotI = MatToRot(oTI);
            double[] pointOut = new double[3];
            pointOut[0] = oTI[0, 0] * point[0] + oTI[0, 1] * point[1] + oTI[0, 2] * point[2] + oPosI[0];
            pointOut[1] = oTI[1, 0] * point[0] + oTI[1, 1] * point[1] + oTI[1, 2] * point[2] + oPosI[1];
            pointOut[2] = oTI[2, 0] * point[0] + oTI[2, 1] * point[1] + oTI[2, 2] * point[2] + oPosI[2];
            return pointOut;
        }

Display More

point has 3 elements XYZ, origin has 6 elements XYZABC, and return has 3 elements XYZ

GetRange is taking a subset of a 1D array.

MatNeg is negating a matrix.

MatToRot is getting A,B,C values from a rotation matrix.

**panic mode** · March 2, 2021 at 8:24 PM

you need angles

Quote

that's what i did ....all what i need now is how to Get the average inverse transformation from center of sphere to A6 flange.

not that i can see, your post only shows translation (X,Y,Z):

Quote

{X 446.54, Y 47.42,Z 361.08 }, this is reference point (the center of the sphere), but I need compute relative to the Flange system which is { X 91.818 , Y -1.55 ,Z 215.789 }

if center of sphere is at {X 446.54, Y 47.42,Z 361.08 }

and flange is at { X 91.818 , Y -1.55 ,Z 215.789 }

you get infinite number of solutions because robot can rotate about center of the flange so that center would still be at { X 91.818 , Y -1.55 ,Z 215.789 } but orientation angles would be different.

if you want to know how to get from flange to center of sphere it is important to know the flange orientation.

Calculating TCP

Advertising from our partners

Anyware Robotics is hiring!

How to see your Job Posting (search or recruit) here in Robot-Forum.com