## Finding offset and angle of planes

Shape and space, angle and circle properties, ...
monkey68pl
Posts: 4
Joined: Wed Mar 12, 2014 4:36 pm

### Finding offset and angle of planes

Hello,

First of all - I'm sorry for my English – I may have messed up some mathematical terms, so if anyone finds mistake – please tell me, I’ll try to correct that.

Some background, what I’m trying to accomplish:
I work with industrial robots. To make programming them a little easier, we create coordinate system for each stationary tool (Tool1=X,Y,Z,W,P,R) which consist of offset (X,Y,Z) relative to reference coordinate system, and rotation (W,P,R) around respective axes of reference coordinate system. The problem is, if we move this tool, these coordinates are no longer valid – I want to find a way to calculate them, so in case of moving my tool, I won’t have to program my robot from scratch.

So what I want to do:
Given coordinates of tool (Tool1=X,Y,Z,W,P,R), three points (P1=Xp1,Yp1,Zp1, P2=Xp2,Yp2,Zp2, P3=Xp3,Yp3,Zp3), and the same three points (relative to tool coordinate system) after moving and rotating tool (P’1=Xp’1,Yp’1,Zp’1, P’2=Xp’2,Yp’2,Zp’2, P’3=Xp’3,Yp’3,Zp’3) find new coordinates of tool (Tool’1=X’,Y’,Z’,W’,P’,R’). Every coordinate is given relative to reference coordinate system.

If more clarification is needed, feel free to ask. I may even draw a picture if you wish.

I know that this is (or should be) solvable – but I have no idea where I should start, especially with rotation - I don’t have a clue where should I start when calculating angles between planes.

This is probably too complicated to post solution on forum, but if someone has any source of information that might help, please post it, I can look it up and maybe I’ll figure something out.

Best regards,

monkey68pl

//Edit

1. Calculate rotation matrix from plane defined by first 3 points to reference coordinate system - intrinsic rotation
2. Calculate angles of plane defined by second set of points relative to reference coordinate system
3. Apply rotation matrix calculated in step 1 to coordinate system calculated in step 2. Voila.

I think it should be done with x-y-z Tait-Bryan angles, but it's a wild guess. I think that I'll try that with simple cases, and test them in real life.

I'm learning what Eigenvectors and tensor products are, so it may take some time. Fun. Bubbler
Posts: 28
Joined: Sat Feb 23, 2013 6:12 am

### Re: Finding offset and angle of planes

Before talking about the solutions, a few assumptions are needed to have a unique solution:
(1) The two triangles seen from both tools must be identical (have same side lengths).
(2) The triangle must be scalene (different side lengths) and non-degenerate (three points not on a straight line).

Here is my suggestion, though it is still a rough guess and needs some research. For simplicity, I call the system of tool 1,2 as system 1,2 respectively.
(1) Transform 3 points in system 1 into the reference system. Inverse transformation matrix of system 1 may be used.
(2) Pick one of 3 points in reference system and move the system so that the point moves to the origin. Name the system as A.
(3) Find the point in system 2 that matches the point from (2) and move system 2 similarly. Name the system as B.
(4) Find the (pure) rotation from system A to B using two other points from each system. This can be done by 2 steps, first rotating one point to correct direction, then rotating around the matched point to match the last point.
(5) Combine the transformation matrices (2), (4), and inverse of (3) to find the matrix for system 2.
(6) Extract the offset and rotation from the final matrix. 