Transforming Coordinates from one user frame to another

Example demonstrating what you're asking for (from the karel reference manual):

To get a point into a user frame from the world frame, you just times the point by the inverse of the user frame.

ufPoint = uFrame.Inverse() * worldPoint; (Pretty much what TitusLepic shows)

I do this in matrix form. Here is the code for converting a point to matrix rep:

public Matrix<double> ConvertXYZWPRtoMatrix(double[] input)
{
Matrix<double> ResultMatrix = Matrix<double>.Build.DenseIdentity(4, 4);
Matrix<double> rX = Matrix<double>.Build.DenseIdentity(4, 4);
Matrix<double> rY = Matrix<double>.Build.DenseIdentity(4, 4);
Matrix<double> rZ = Matrix<double>.Build.DenseIdentity(4, 4);
double x = input[0];
double y = input[1];
double z = input[2];
double w = input[3];
double p = input[4];
double r = input[5];


//R
rZ[0, 0] = Math.Cos(r);
rZ[0, 1] = -Math.Sin(r);
rZ[1, 0] = Math.Sin(r);
rZ[1, 1] = Math.Cos(r);
//P
rY[0, 0] = Math.Cos(p);
rY[0, 2] = Math.Sin(p);
rY[2, 0] = -Math.Sin(p);
rY[2, 2] = Math.Cos(p);
//W
rX[1, 1] = Math.Cos(w);
rX[1, 2] = -Math.Sin(w);
rX[2, 1] = Math.Sin(w);
rX[2, 2] = Math.Cos(w);


ResultMatrix = rZ * rY * rX;
ResultMatrix[0, 3] = x;
ResultMatrix[1, 3] = y;
ResultMatrix[2, 3] = z;


return ResultMatrix;
}

And the code for turning a matrix back to XYZWPR:

public double[] ConvertMatrixToXYZWPR(Matrix<double> inputMatrix)
{
bool IsClose(double numToCompare, double Comparison) { return Math.Abs(numToCompare - Comparison) < 0.000000001; }


double[] results = { 0, 0, 0, 0, 0, 0 };


//X Y Z
results[0] = inputMatrix[0, 3];
results[1] = inputMatrix[1, 3];
results[2] = inputMatrix[2, 3];


//W P R
double w, p, r, cosP;
r = 0.0;
if (IsClose(inputMatrix[2, 0], -1))
{
p = Math.PI / 2;
w = Math.Atan2(inputMatrix[0, 1], inputMatrix[0, 2]);
}
else if (IsClose(inputMatrix[2, 0], 1))
{
p = -Math.PI / 2;
w = Math.Atan2(-inputMatrix[0, 1], -inputMatrix[0, 2]);
}
else
{
p = -Math.Asin(inputMatrix[2, 0]);
cosP = Math.Cos(p);
w = Math.Atan2(inputMatrix[2, 1] / cosP, inputMatrix[2, 2] / cosP);
r = Math.Atan2(inputMatrix[1, 0] / cosP, inputMatrix[0, 0] / cosP);
}
results[3] = w;
results[4] = p;
results[5] = r;


return results;
}

Quote from nhfMinion

Do you know of a source I can look at to understand the intuition behind the WPR math?

You can look over at the "Geometry" forum for a lot of discussions on this topic.

The key is Euler angles, and understanding that the rotations are done sequentially, not in parallel.

(There's also Tait-Bryant angles, which are really just a particular subset of Euler)

The pain point is, there are many different, equally valid, sequences for Euler rotations -- the only real rule is that you cannot rotate around the same axis twice in a row. And every brand uses a different Euler sequence.

Kawasaki uses ZYZ, Fanuc uses XYZ, KUKA uses ZYX, and there are a few others.

And just to complicate things further, there's intrinsic vs extrinsic methods -- KUKAs use intrinsic, while Fanucs (IIRC, someone check me on this) use extrinsic. Which means that on a KUKA, you rotate around Z, then rotate around the new position of its own Y axis (Y'), then around the new X axis (X''). While if KUKA was an extrinsic system, you would rotate your tool/point/whatever around the Z axis of the reference frame (World or Base), then rotated the rotated point around the Y axis of the reference frame, then around the X of the reference frame.

So, intrinsic rotates around itself, with each rotation affecting the rotations after it, while extrinsic rotates around the unmoving axes of the reference frame.

All of which means that, thanks to Euler angles, adding 90deg to your W value in a Fanuc does not rotate your point 90deg around X... unless the point started at all 0s for WPR. And if your point starts at some combination of 90deg (like 0,90,0, or 0,0,90, or 0,180,0), you can get into a strange situation where rotating the point around the X axis has no real effect on the W value, but might have major effects on the P and R values. This is why you have to use matrix math when doing rotations in robot coordinates, while simple addition will work for the XYZ values. Fortunately, most (all?) robots offer some sort of tool in their programming language to handle this.

The free version of RoboDK is nice for illustrating how the different robot brands do this -- you can create a point, then change its representation between the different major brands to see how the same location&orientation in Cartesian space is represented with completely different values in different Euler schemes.