I want to program an industrial robot, so that the flange (the „head“ of the robot) is constantly looking at specified points in 3D world space.
- The real world position a(x,y,z) of the robot is set and lies on the basis of the coordinate system.
- The coordinate system of the robot is as follows: x is to the right, y is going "into the screen" and z is up. This is also shown in image Start Position.
- A is rotation about Z, B is rotation about Y and C is rotation about X.
- In its default position with A=B=C=0 the flange is facing towards (0,0,1).
- List The Position b(x,y,z) is also set.
- The robot rotates in the following order: A > B > C
After rotating, the flange should look in the direction of the point b (Goal).
I already found these calculations:
rotx = Math.atan2( y, z )
roty = Math.atan2( x * Math.cos(rotx), z )
rotz = Math.atan2( Math.cos(rotx), Math.sin(rotx) * Math.sin(roty) )
in this topic, which covers the same topic. I tried to fit the assumptions from the coordinate system in that topic to mine, switching y and z axis and changing the signs, but I ended up with the wrong results for some test coordinates.
Therefore, I'm looking for formulas for the rotations A,B and C.
In order to answer this, I'll have to make some assumptions about the robot that you did not make clear in your post. I assume that the $A$ rotation moves the axes about which the $B$ and $C$ rotations occur, and the $B$ rotation moves the axis about which the $C$ rotation occurs. The problem with these assumptions is that the $A$ and $B$ rotations are sufficient to point in any direction. Even if the flange must be aligned in more than one dimension, this is accomplished by a third rotation about the $Z$ axis again, not a rotation about the $X$ axis. So if these assumptions are incorrect, please explain how the rotations actually work.
With the assumptions above, we can easily find the $A$ and $B$ rotations needed to point at $b$. The $A$ rotation will swing the $X$ axis around so that it aligns with $b$. I.e., $b$ will lie in the new $XZ$ plane. The $B$ rotation then lowers the $Z$ axis elevation down to the elevation of $b$. It may be simpler to view it from body coordinates - the coordinate system tied to the flange: then we don't view it as the axes changing direction, but rather, as moving $b$ around in the body coordinates.
First we determine the rotation angle $A$. If $b = (x, y, z)$, then $A$ will be the angle between the projection of $b$ into the $XY$ plane and the $X$ axis. The projection is $(x, y, 0)$, and the angle is given by
A = atan2(x, y). This rotation changes the body coordinates of $b$ to $(\sqrt{x^2 + y^2}, 0, z)$.The $B$ rotation is then the angle between the positive $Z$ axis and this new direction for $b$, which given by
B = atan(z, sqrt(x^2 + y^2)).