1
$\begingroup$

Short Question:

As demonstrated in the image below, how can I calculate the rotation matrix that relates the base coordinate and the rectangle coordinate, given 3 known points P1, P2, P3 on the surface of the rectangle with respect to the base coordinate?

enter image description here

Long Question:

I am currently working on OMRON pick-and-place robots. Basically, the overall layout of the system is similar to what is shown in the following picture:

. https://www.densorobotics.com/user-manuals/002221.html

The system has a rotary encoder that reflects the conveyor belt motion.

What I am trying to do is to come out with a transformation matrix T that represents the belt with respect to the robot coordinates. This transformation matrix is automatically calculated using a calibration wizard, however I am trying to understand how such matrix is calculated based on the 3-point belt calibration carried out as below:

Simple Calibration Steps:

The calibration can simply be completed after teaching 3 points on the belt as shown in the following image:

Belt calibration

The steps are as follows:

  1. A product is placed on the belt at the upstream limit that a robot can start tracking the product from.
  2. The robot tip is then placed on the product. Its location and the encoder value are recorded. Let the point be P1 and encoder value be E1.
  3. The belt is then allowed to move in the product feeding direction until the it reaches to another location which the robot tip can reach.
  4. Robot tip is placed on top of the product. This is the second calibration point P2. The encoder value is also recorded. Let it be E2.
  5. Lastly the robot tip is moved and placed on the calibration product on a side location of the belt called downstream pick limit. This would be the the third point P3 and it is recorded.

After I followed these steps for a test robot in hand, I got the following quantities:

$$P_1 = \begin{bmatrix}-286.8 & -338.0 & 244.6\end{bmatrix} ; E_{1}=-71873$$

$$P_2 = \begin{bmatrix}140.3 & -422.2 & 246.7\end{bmatrix} ; E_{2}=-158710$$ $$P_3 = \begin{bmatrix}-18.004 & -590.452 & 248.746\end{bmatrix} $$

Let b = [bx by bz] be a belt point. Since the belt motion is having a magnitude along the x direction, then the scale factor in units of mm/pulses can be calculated by:

$$s = \frac {|P2-P1|}{E2-E1} = \frac {\sqrt{(140.3-(-286.8))^2 + (-422.2 -(-338.0))^2 + (246.7-244.6)^2}}{−158710 -(-71873)} =-0.005013$$

This is one important scale factor that we need to calculate during the calibration process (the minus sign means the encoder counts opposite to belt rotation)

Along with this scale that we get during calibration, we need to calculate all members of the transformation matrix T which relates belt coordinates with robot coordinates. This has the rotation matrix R and translation matrix D. $$ p=Tb = \begin{bmatrix}& R & & D\\ 0 & 0 & 0 & 1 \end{bmatrix}b $$

$$p=\begin{bmatrix} p_x \\ p_y\\ p_z\\ 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & d_{x}\\ r_{21} & r_{22} & r_{23} & d_{y}\\ r_{31} & r_{32} & r_{33} & d_{z}\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \begin{bmatrix} b_x \\ b_y\\ b_z\\ 1 \end{bmatrix} $$

Since b always has one component along the x axis of the belt, then the coordinate of the first calibration point with respect to belt coordinate is (Belt did not move yet): $$ b_1 = \begin{bmatrix} 0 \\ 0\\ 0\\ 1 \end{bmatrix} $$

and the coordinate of the second calibration point with respect to belt coordinate is (Belt moved from P1 to P2):

$$ b_2 = \begin{bmatrix} s\times(E2-E1) \\ 0\\ 0\\ 1 \end{bmatrix} = \begin{bmatrix} 435.3 \\ 0\\ 0\\ 1 \end{bmatrix} $$

Therefore, from P1 calibration, we have:

$$p_1=\begin{bmatrix} −286.8 \\ −338.0\\ 244.6\\ 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & d_{x}\\ r_{21} & r_{22} & r_{23} & d_{y}\\ r_{31} & r_{32} & r_{33} & d_{z}\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \begin{bmatrix} 0 \\ 0\\ 0\\ 1 \end{bmatrix} $$

Solving for T matrix members, we get dx = −286.8, dy = −338.0 and dz = 244.6

From P2 calibration, we have: $$p_2=\begin{bmatrix} 140.3 \\ −422.2\\ 246.7\\ 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & −286.8\\ r_{21} & r_{22} & r_{23} & −338.0\\ r_{31} & r_{32} & r_{33} & 244.6\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \begin{bmatrix} 435.3 \\ 0\\ 0\\ 1 \end{bmatrix} $$

Solving for the Rotation matrix, we get: $$ R = \begin{bmatrix} 0.981112257 & r_{12} & r_{13}\\ -0.193378015 & r_{22} & r_{23}\\ 0.004866365 & r_{32} & r_{33} \end{bmatrix} $$

Based on OMRON calibration wizard, the following matrix is obtained after finishing teaching the 3 points: $$ T = \begin{bmatrix} 0.981112257 & -0.193427089 & 0.00216786 & −286.8\\ -0.193378015 & -0.981022085 & -0.01416372 & −338.0\\ 0.004866365 & 0.013476983 & -0.999897339 & 244.6\\ 0 & 0 & 0 & 1\\ \end{bmatrix} $$

As seen, the first column and 4th conform with the results that we got. However, I am not really sure what the mathematical way to follow using the third calibration point to calculate the rest of the transformation matrix that I have. How would I be able to calculate r12, r22, r32, r13, r23 and r33?

-EDIT: (Response to the answer provided by @50k4)

I have tried your equations to find P4 for a sample coordinate that I got from the Google Sketchup 3D Designer, however I am getting big illogical values a and b and consequently for its x, y and z. I created a Google sheet which contains the design with assigned values of each point and I also implemented your equations in it to calculate a, b, and P4 as well as $\vec v_1 = P_3 - P_4$ and $\vec v_2 = P_1 - P_4$. Here is the link for this sheet that I shared and can be publicly edited:

https://docs.google.com/spreadsheets/d/1XGTqjk7kfwNi0hfktoVEtkkTNgrD1LUz8ui2Tyu455Y/edit?usp=sharing

Why I Need to Know This in The First Place!

I am an experienced software engineer with robotics and control engineering qualification. I am planning to program 3D models and create a simulation platform and solve a problem in hand for the robotics world. Clear understanding of this field in mathematics is required.

$\endgroup$
  • $\begingroup$ Are there any constraints for P1, P2 and/or P3. e.g. the P1P2 axis must be parallel to the X axis or similar constraints? $\endgroup$ – 50k4 Jun 4 at 9:36
  • $\begingroup$ Yes, the line connecting P1 and P2 is parallel to the belt/rectangle X axis. P3 is not constraint to any other point $\endgroup$ – Moayad Hani Abu Rmilah Jun 4 at 9:43
2
+50
$\begingroup$

A general approach would be to construct a so called axis-angle representation and convert that to a rotation matrix representation.

On order to do so, one could start with a normal vector of the triangle.

Let's take the $P_1P_2$ line and construct the perpendicular of the triangle from $P_3$ to $P_1P_2$. Let's call $P_4$ the point where the perpendicular intersects $P_1P_2$ .Based on this answer the coordinates of this point can be found as:

$\alpha = \frac{(x_3-x_1) \cdot (x_2-x_1) + (y_3-y_1) \cdot (y_2-y_1) + (z_3-z_1) \cdot (z_2-z_1)} {(x_2-x_1) \cdot (x_2-x_1) + (y_2-y_1) \cdot (y_2-y_1) + (z_2-z_1) \cdot (z_2-z_1)}$

$P_4 = (x_1 + \alpha \cdot (x_2-x_1), y_1+\alpha \cdot (y_2-y_1), z_1 + \alpha \cdot (z_2-z_1))$ where:

$P1 = (x_1, y_1, z_1)$

$P2 = (x_2, y_2, z_2)$

$P3 = (x_3, y_3, z_3)$

so $P_1P_2$ and $P_3P_4$ are perpendicular and their intersection point is $P_4$.

Applied to the coordinates described in the attached google sheet file the triangle (in blue) and the perpendicular in red is presented in the following figure:

enter image description here

We can now construct 2 perpendicular vectors, in the $P_1P_2P_3$ plane which share their origin in $P_4$

$\vec v_1 = P_3 - P_4$

$\vec v_2 = P_1 - P_4$ (we could also use $\vec v_2 = P_2 - P_4$, this would cause an 180 deg offset around the z axis)

The cross product of these two vectors will be the normal of the $P_1P_2P_3$ plane.

$\vec v_3 = \vec v_1 \times \vec v_2$

care must be taken to make sure that $\vec v_3$ points "upwards or downwards", depending on the cross product order ($\vec v_1 \times \vec v_2$ or $\vec v_2 \times \vec v_1$). The preferred one has to be chosen.

The three vectors now form an orthogonal coordinate system. in order to get the axes of this coordinate system the vectors should be normalized.

$\vec x_n = normalize(\vec v_2) = [x_{xn}, y_{xn}, z_{xn}]$

$\vec y_n = normalize(\vec v_1) = [x_{yn}, y_{yn}, z_{yn}]$

$\vec z_n = normalize(\vec v_3) = [x_{zn}, y_{zn}, z_{zn}]$

Please note that $\vec v_2$ has been chosen as $\vec x_n$ based on the additional information in the question, that $P_1P_2$ is parallel to the $X$ axis of the belt/rectangle frame.

A rotation matrix can also be interpreted as a direction cosine matrix, based in this, the rotation matrix of the plane can be written as:

$$ R = \begin{bmatrix} x_{xn} & y_{xn} &z_{xn} \\ x_{yn} & y_{yn} & z_{yn} \\ x_{zn} & y_{zn} & z_{zn} \end{bmatrix}. $$

| improve this answer | |
$\endgroup$
  • $\begingroup$ Why are you constructing a $P4$? I read the question and thought generally the same approach that you're providing, but the normal of a plane is normal at any point on the plane. That is, you could get $\overrightarrow{a} = P_2 - P_1$ and $\overrightarrow{b} = P_3 - P_1$, then cross $\overrightarrow{a} \times \overrightarrow{b}$ to the plane normal. $\endgroup$ – Chuck Jun 4 at 13:55
  • 1
    $\begingroup$ but then $norm(\vec a)$, $norm(\vec b) $ and $ norm(\vec {a \times b})$ could not form an orthogonal coordinate system. You could do then an axis-angle approach and convert that to a rotation matrix, I just find this approach easier to debug if implemented. axis angle is one step which is hard to debug and conversion to a rotation matrix is also cumbersome $\endgroup$ – 50k4 Jun 4 at 14:00
  • 1
    $\begingroup$ Ah fair enough. I had thought from OPs post that P1, P2, and P3 were measured with respect to the base coordinate, so they were just looking for the orientation of the plane defined by those three points within a pre-defined coordinate system. $\endgroup$ – Chuck Jun 4 at 14:12
  • 1
    $\begingroup$ The rotation matrix specifically is preferred. I agree that there are many other ways to derive it, this is just my preferred way. $\endgroup$ – 50k4 Jun 4 at 14:33
  • $\begingroup$ Thank you so much for the clear explanation. I am now applying the equations that you gratefully provided, however I get very big values for a and b and, hence for P4. I wonder why this is the case. Please see the updated question for more clarification. $\endgroup$ – Moayad Hani Abu Rmilah Jun 9 at 3:00

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.