How to calcul 3D point in a coordinate system to another one?

Hello everybody, I'm a newbie here...

I've a problem about coordinate system in Kuka KRC1. I need to code a little program in C# (because there are hundred of trajectories) to transform all the 3D coordinate (x,y,z,a,b,c) in the Base n°1 to the Base n°2, and the Tool never change.

How can I do that?... Maybe have you the transformation matrix (4x4) of Kuka system?

Many thanks in advance for your help!

Quote from SkyeFire

INV_POS() is a system function in KRL that performs a matrix inversion of a FRAME, POS, or E6POS type variable.

The Geometric Operator (a ':' character) performs a matrix cross-product on any two FRAME or POS-type variables.

Between them, these two will do everything you need. They're both discussed at length in the forum archives. The G.O. is documented in the KRL manuals, although for some reason INV_POS isn't.

Thanks, but I don't use KRL, my conversion program will be in (Microsoft) C# to directly manipulate .dat and .src files extracted from Kuka robot backups... That's the reason I search the transformation matrix!

I'll see your links Fubini, thanks

Quote from Spl

Have you tried OrangeEdit?

http://www.orangeapps.de/?lng=en&page=apps/orangeedit

No, I don't know this soft, is it especially for Kuka robot? Can I have the transformation matrix with it?

I work directly in C# with Visual Studio 2015 to manipulate (and modify) robot trajectories, not only for Kuka, sometimes Fanuc too.

I'm back with problems...

Firstable, some precisions:
I have the original base (x 0.0,y 1126.5, z 805.0, a 0.0, b 0.0, c 0.0) with one point (x 1490.458,y -1126.5, z 1459.4, a 180.0, b 81.6634, c 180.0)

and the new one (x -49.907, y 1060.275, z 791.113, a 3.2965, b 1.9967, c 0.115) with the same point but now with (x 1424.618,y -1144.036, z 1526.147, a 176.7055, b 83.6634, c 180)

I know that, because I've two test trajectories, the first one with the original base and five points in that base. And the second traj with the new base and the same five points but in the new coordinate system.

My translation vector is (-49.907,-66.225,-13.887)

Firstable I used a translation matrix:
1 0 0 0
0 1 0 0
0 0 1 0
-49.907 -66.225 -13.887 1

and after I tried to use that (from Fubini) to calculate the 3 rotations in the same time:

public static double[][] matrix(double aDeg, double bDeg, double cDeg) {
      //ABC: Euler angles, A: round z-axis     B: round y-axis        C: round y-axis
      double a = -aDeg * PI / 180;
      double b = -bDeg * PI / 180;
      double c = -cDeg * PI / 180;
      double ca = Math.cos(a);
      double sa = Math.sin(a);
      double cb = Math.cos(b);
      double sb = Math.sin(b);
      double cc = Math.cos(c);
      double sc = Math.sin(c);
      double[][] tt = new double[4][];
      tt[0] = new double[] { ca * cb, sa * cc + ca * sb * sc, sa * sc - ca * sb * cc, 0 };
      tt[1] = new double[] { -sa * cb, ca * cc - sa * sb * sc, ca * sc + sa * sb * cc, 0};
      tt[2] = new double[] { sb, -cb * sc, cb * cc, 0 };
      tt[3] = new double[] { 0, 0, 0, 1 };


      return tt;
   }

But when I use the new base coordinate to calculate the new 3D position of the point, it's totally false!!

I'm not very efficient ( ) in maths, and I don't understand where is my mistake... Please can you help me?

Quote from Fubini

Hi,

did you use the transponed Matrix compared to

1 0 0 0
0 1 0 0
0 0 1 0
-49.907 -66.225 -13.887 1

I know many Microsoft Tools always use transponed matrices (I do not know why), but our algorithms are for non transponed matrices like

1 0 0 -49.907
0 1 0 -66.225
0 0 1 -13.887
0 0 0 1

Fubini

Display More

I use the first one and not the transponed matrix, because when I try my own matrix:
1 0 0 0
0 1 0 0
0 0 1 0
-49.907 -66.225 -13.887 1

I got the same results than when I use the TranslateTransform3D function (in Media3D library in C#)

example just to illustrate:

TranslateTransform3D matTransl = new TranslateTransform3D(-49.907, -66.225, -13.887);
pointRep2 = matTransl.Transform(pointRep1);

and

Matrix3D matriceTranslate = new Matrix3D(1, 0, 0, 0,
                                        0, 1, 0, 0,
                                        0, 0, 1, 0,
                                        -49.907, -66.225, -13.887, 1);
pointRep2 = pointRep1 * MatriceTranslate;

got exactly the same results for pointRep2...

And now, for the rotations I tried with the transponed of your matrix (I hope)
((ca * cb), (-sa * cb), sb, 0,
(sa * cc + ca * sb * sc), (ca * cc - sa * sb * sc), (-cb * sc), 0,
(sa * sc - ca * sb * cc), (ca * sc + sa * sb * cc), (cb * cc), 0,
0, 0, 0, 1)

Results failed...

Quote from Spl

To solve a pose in base 2, you have to solve the equation wHb1*b1Hp = wHb2*b2Hp, from which you get b2Hp = inverse(wHb2)*wHb1*b1Hp.

wHb1 = 4-by-4 matrix of your base 1 in world
wHb2 = 4-by-4 matrix of your base 2 in world
b1Hp = 4-by-4 matrix of your pose in base 1
b2Hp = 4-by-4 matrix of your pose in base 2

I'm not very good in maths (long time ago for me...)
For the 4x4 matrix of the positions and the bases, how can I write them??

For a 3D point, I think it's (x, y, z, w) like (1490.458, -1126.5, 1459.4, 1) for my first point, but how can I include rx, ry and rz?
And I don't know for the base...

Spl I'm sorry but I don't understand how you make the 4x4 matrix for the first point...

I think it's a translation in first like this:
1 0 0 1490.458
0 1 0 -1126.5
0 0 1 1459.4
0 0 0 1

and just after it's a 3D rotation like this:
cos(ry)*cos(rz) -cos(ry)*sin(rz) sin(ry) 0
sin(rx)*sin(ry)*cos(rz)+cos(rx)*sin(rz) -sin(rx)*sin(ry)*sin(rz)+cos(rx)*cos(rz) -sin(rx)*cos(ry) 0
-cos(rx)*sin(ry)*cos(rz)+sin(rx)*sin(rz) cos(rx)*sin(ry)*sin(rz)+sin(rx)*cos(rz) cos(rx)*cos(ry) 0
0 0 0 1

with Kuka A=RZ, B=RY, C=RX, and of course I transform degre in radian with (rx*PI)/180 before calculation

But with this method, I never find your values...

Please can you help me? (Once again...)

OK thanks, I'll try to find the same function in the Media3D library (or maybe Direct3D) for Microsoft C# because I don't code in C++

I (finally) found a solution, but it's a little strange because I've a little gap on my x,y,z ...

To calculate point and bases, I use this 4x4 matrix:
cos(ry)*cos(rz) -cos(ry)*sin(rz) sin(ry) x
sin(rx)*sin(ry)*cos(rz)+cos(rx)*sin(rz) -sin(rx)*sin(ry)*sin(rz)+cos(rx)*cos(rz) -sin(rx)*cos(ry) y
-cos(rx)*sin(ry)*cos(rz)+sin(rx)*sin(rz) cos(rx)*sin(ry)*sin(rz)+sin(rx)*cos(rz) cos(rx)*cos(ry) z
0 0 0 1

and, at the end of your equation, I found x=1424.763 y=-1141.131 z=1528.186 ...

with your method, you find exactly the good values (1424.618,-1144.036,1526.147)!!!

It's strange because I use double type all time, it's 64bits precision in C#, have you any idea to explain this gap??