Using 3d transformation matrices

Question

In an AI class we have a robot that has an arm with 7 joints. Each joint can rotate a different direction. I need to know where the very end is going to be. I've been trying to do 3d matrix multiplication and it works for one joint but once I add in another it doesn't line up with a model I made using the Java3D api. This is how I'm trying to calculate the position of the second joint.

double[][] b = {{0, 0, 0, 1}};

// Joint 1
b = HercMatrix.MatMul(b, HercMatrix.getTranslation(0, 0, -110));
b = HercMatrix.MatMul(b, HercMatrix.getRotation(0, arm.getJointAngle(0), 0));

// Joint 2
b = HercMatrix.MatMul(b, HercMatrix.getTranslation(0, 0, -250));
b = HercMatrix.MatMul(b, HercMatrix.getRotation(arm.getJointAngle(1), 0, 0));

System.out.println("Matrix: " + b[0][0] + ", " + b[0][1] + ", " + b[0][2]);

I imagine it's they way I go about applying the multiplications. I know for a fact it's not my matmul or matrice generation code, I've tested all that individually.

the second translation I have needs to be on the relative x-axis of the first joint's angle. But I have no idea if this is how I do it...

Any help or ideas is greatly appreciated.

Answer 1

It's difficult to answer exactly without seeing the structure of your affine transformation matrices, and the definitions of the angle variables. The normal definition of the translation matrix assumes the position vector is multiplied from the left, for example, which seems to be opposite to your use. Check also that b[0][3]==1 at the end.

You code seems to be equivalent to the following pseudocode:

b = origin.translate(Joint1).rotate(Joint1).translate(Joint2).rotate(Joint2);

This is not equivalent to

b = origin.translate(Joint2).rotate(Joint2).translate(Joint1).rotate(Joint1);

so check which order of transformations you require.

Answer 2

I find this 3D stuff to be a lot of trial and error. I suggest you rotate first.

// Joint 1
b = HercMatrix.MatMul(b, HercMatrix.getRotation(0, arm.getJointAngle(0), 0));
b = HercMatrix.MatMul(b, HercMatrix.getTranslation(0, 0, -110));



// Joint 2

b = HercMatrix.MatMul(b, HercMatrix.getRotation(arm.getJointAngle(1), 0, 0));
b = HercMatrix.MatMul(b, HercMatrix.getTranslation(0, 0, -250));

System.out.println("Matrix: " + b[0][0] + ", " + b[0][1] + ", " + b[0][2]);

That way your translation is applied in the direction of the angle.