2

Background: I'm a Robotics Engineer and I am trying to develop a more flexible, modular, and robust program for our welding robots, which will minimize teaching time for new robots and also minimize the amount of damage team members can do if the mess up reprogramming a path. The robots I am working on need to weld along a changing curved path in 3D space. I have found several helpful answers on here but I am having a little trouble tying them together.

Problem: For the current portion of the program, I need to take three points in 3D space, and calculate the center of curvature. I plan to use this to parse up a path into regular segments, and then describe those regular segments as either circular arcs or straight lines in the same 3D space. For now, I need to find the center of the arc through the three points. I have found the equations to do this in 2D space, and they are pretty simple. However, I was never very good with matrix algebra and that is a BIG part of moving into the 3D space. Any hep on this project would be appreciated!

raz
  • 103
Gabriel
  • 21
  • 1
    You essentially want the circumcenter of the triangle formed by your 3 points in the plane containing the triangle. The formula is given in this answer. – achille hui Jul 30 '15 at 15:02

3 Answers3

1

In other words the question is to find a center of the circumscribed circle in 3D. Using the side lengths of $\triangle ABC$ \begin{align} a&=|BC|,\quad b=|AC|,\quad c=|AB| , \end{align}

the formula for the center of the circumscribed circle is: \begin{align} O&= \frac{a^2(b^2+c^2-a^2)A+b^2(a^2+c^2-b^2)B+c^2(b^2+a^2-c^2)C} {a^2(b^2+c^2-a^2)+b^2(a^2+c^2-b^2)+c^2(b^2+a^2-c^2)} , \end{align}
which is suitable for both 2D and 3D.

g.kov
  • 13,581
1

I suggest turning this into a 2D problem and then find the circle from three points on the plane.

NOTE: Below the $\times$ is vector cross product and $\cdot$ the vector dot product. Also $^\top$ is a matrix transpose (switch rows with columns). Finally $\| \boldsymbol{x} \|$ returns the magnitude of the vector $\boldsymbol{x}$.

Start by finding the normal vector to the plane defined by the three points

$$ \boldsymbol{n} = \mathrm{unitvector}( \boldsymbol{A} \times \boldsymbol{B} + \boldsymbol{B} \times \boldsymbol{C} + \boldsymbol{C} \times \boldsymbol{A}) \tag{1}$$

The find the arbitrary mutually orthogonal directions

$$ \boldsymbol{u} = (\boldsymbol{C}-\boldsymbol{A})\times \boldsymbol{n} \tag{2}$$ $$ \boldsymbol{v} = \boldsymbol{n} \times \boldsymbol{u} \tag{3}$$

and construct a 3×2 rotation matrix from the three direction vectors as columns.

$$ \mathbf{R} = \left[ \boldsymbol{u} \; \boldsymbol{v} \right] \tag{4}$$

You also need the distance of the plane ABC to the origin

$$ d = \boldsymbol{n} \cdot \boldsymbol{A} \tag{5}$$

Now convert the problem into a 2D problem with

$$ \begin{aligned} \boldsymbol{a} = \pmatrix{a_x\\a_y} & = \mathbf{R}^\top \boldsymbol{A}\\ \boldsymbol{b} = \pmatrix{b_x\\b_y} & = \mathbf{R}^\top \boldsymbol{B}\\ \boldsymbol{c} = \pmatrix{c_x\\c_y} & = \mathbf{R}^\top \boldsymbol{C} \end{aligned} \tag{6}$$

Now solve the 2D problem by finding the center point $\boldsymbol{q}=\pmatrix{x\\y}$ using a 2×2 system of equations

$$ \begin{bmatrix} b_x-a_x & b_y -a_y \\ c_x -a_x & c_y - a_y \end{bmatrix} \pmatrix{x\\y} = \pmatrix{ \frac{ (b_x^2+b_y^2)-(a_x^2+a_y^2)}{2} \\ \frac{ (c_x^2+c_y^2)-(a_x^2+a_y^2)}{2} } \tag{7} $$

Lastly construct the 3D point for the circle center $\boldsymbol{Q}$

$$ \boldsymbol{Q} = \boldsymbol{n}\, d + \mathbf{R} \pmatrix{x\\y} \tag{8}$$

If the radius is needed just take the distance from the center to any point

$$ \mathrm{radius} = \| \boldsymbol{A}-\boldsymbol{Q} \| \tag{9} $$

You might also need the sweep angle which you get from the angle between the vectors $\boldsymbol{C}-\boldsymbol{Q}$ and $\boldsymbol{A}-\boldsymbol{Q}$

$$ \theta = \cos^{-1} \left( \frac{ (\boldsymbol{C}-\boldsymbol{Q}) \cdot ( \boldsymbol{A}-\boldsymbol{Q}) }{ \| \boldsymbol{C}-\boldsymbol{Q}\| \,\|\boldsymbol{A}-\boldsymbol{Q} \| } \right) \tag{10} $$


Example

Points:

$$\begin{aligned} \boldsymbol{A} &= \pmatrix{1\\0\\0} & \boldsymbol{B} & = \pmatrix{3\\1\\0} & \boldsymbol{C} &= \pmatrix{2 \\0 \\ -1} \end{aligned} $$

Normal:

$$ \boldsymbol{n} = \pmatrix{ -\tfrac{\sqrt{6}}{6} \\ \tfrac{\sqrt{6}}{3} \\ -\tfrac{\sqrt{6}}{6} } $$

Distance from Origin

$$ d = -\tfrac{\sqrt 6}{6} $$

Rotation:

$$ \mathbf{R} = \begin{bmatrix} \tfrac{\sqrt 3}{3} & \tfrac{\sqrt 2}{2} \\ \tfrac{\sqrt 3}{3} & 0 \\ \tfrac{\sqrt 3}{3} & -\tfrac{\sqrt 2}{2} \end{bmatrix} $$

2D Points:

$$ \begin{aligned} \boldsymbol{a} & = \pmatrix{ \tfrac{\sqrt 3}{3} \\ \tfrac{\sqrt 2}{2} } & \boldsymbol{b} & = \pmatrix{ \tfrac{4 \sqrt 3}{3} \\ \tfrac{3 \sqrt 2}{2} } & \boldsymbol{c} & = \pmatrix{ \tfrac{\sqrt 3}{3} \\ \tfrac{3 \sqrt 2}{2} } \end{aligned} $$

System of equations:

$$\left. \begin{bmatrix} \sqrt{3} & \sqrt{2} \\ 0 & \sqrt{2} \end{bmatrix} \pmatrix{x \\ y} = \pmatrix{\tfrac{9}{2} \\ 2 } \;\right\} \; \pmatrix{x = \tfrac{5 \sqrt 3}{6}\\y = \sqrt 2} $$

Circle Center:

$$ \boldsymbol{Q} = \left( -\tfrac{\sqrt 6}{6}\right) \pmatrix{ -\tfrac{\sqrt{6}}{6} \\ \tfrac{\sqrt{6}}{3} \\ -\tfrac{\sqrt{6}}{6} } + \begin{bmatrix} \tfrac{\sqrt 3}{3} & \tfrac{\sqrt 2}{2} \\ \tfrac{\sqrt 3}{3} & 0 \\ \tfrac{\sqrt 3}{3} & -\tfrac{\sqrt 2}{2} \end{bmatrix} \pmatrix{ \tfrac{5 \sqrt 3}{6}\\ \sqrt 2} = \pmatrix{2 \\ \tfrac{1}{2} \\ 0} $$

Prove that points A, B, and C are equidistant to Q

$$ \mathrm{radius} = \| \boldsymbol{A}-\boldsymbol{Q} \| = \| \boldsymbol{B}-\boldsymbol{Q} \| = \| \boldsymbol{C}-\boldsymbol{Q} \| = \tfrac{\sqrt 5}{2} \;\;\checkmark $$

PS. I am also a fellow roboticist. You do need a basic understanding of linear algebra operations as well as some vector calculus to understand the how and why of things in robotics.

John Alexiou
  • 13,816
0

Hint:

Let's call your three points $a$, $b$, and $c$.

Let's call the center of the circle $p$.

First, let's assume that $a$, $b$, and $c$ are NOT the same line, 'co-linear'. That needs to be handled separately because $p$ cannot be not defined in that case.

We know that $p$ is equidistant from $a$, $b$, and $c$ - because that's the defining property of a circle.

So, we can use that. We just need to know the formula for distance in 3D:

$$\text{distance} = \sqrt{d_x^2 + d_y^2 + d_z^2}$$

So, let's plug it in for one of the distances, from $a$ to $p$:

$$\text{distance}_{ap} = \sqrt{(p_x-a_x)^2 + (p_y-a_y)^2 + (p_z-a_z)^2}$$

Can you plug it in for $p_b$ and $p_c$, then set the distances equal, and solve it? If you can't solve it by hand, you can plug it into a computer algebra solver like Wolfram Alpha.

William
  • 101
  • This helps, and almost gets it, but this gives me three equations with four unknowns. There is a straight line with infinite points that are all an equal distance from all three points. I need a fourth equation to narrow that down to the single point which lays on the plane with the original three points. – Gabriel Jul 30 '15 at 16:11
  • Resolved. I added the equation of the plane, and it all works. Thanks for your help! – Gabriel Jul 30 '15 at 16:27
  • Cool - good work. If it is not too much trouble, you may want to add your general solution after you restrict the sphere to a plane as an answer to your own question. It might help an someone with a similar problem down the line. – William Jul 30 '15 at 18:07