Forward and Revers Kinematics For Modified Hanging Plotter

Question

I'm building a modified version of the standard hanging plotter (v-plotter). The basic idea is that you have two cables hanging from stepper motors which form a triangle supporting the pen at the tip.

My design the strings anchored at points $C$ and $D$ which causes the behavior to be somewhat different that the normal plotter, especially when operating close to the motors.

I was able to work out the forward kinematics fairly easily, but the inverse kinematics are turning out to be a real headache. You can see my attempt at a standard geometry solution on math.stackexchange here.

Is there anything specific to calculating kinematics for robotics which could help me?

I'm not interested in modifying the hardware to make the math easier. I'm also not interested in discussing the center of gravity of the pen, cable weight...etc. I want to calculate the kinematics for this as an ideal system.

Any and all advice is appreciated.

Thank you.

Answer 1

I assume input is the X and Y coordinates of E. In this case, unless you know the orientation also, there is no straightforward way to calculate the coordinates of C and D.

I would write 2 parameterized circle or distance equations around A and B. The $C_x$, $C_y$ and $D_x$ and $D_y$ are unknown. (4 unknowns in this system and 2 equations) Distance from E to D and distance from E to C can be written and so there is a system with 4 equations and 4 unknowns where besides the unknowns all other parameters shoulds be known.

Solve for C and D coordinates and calculate distances to A and B and it is done. I do not think that the equation system will have an explicit solution but a numerical solver (like Newton-Raphson) can be used to solve it iteratively.

Or search for 5 bar linkage inverse kinematics and maybe that will inspire a solution.

Answer 2

Alright, this a work in progress at this point.

Basically I decided to rethink the problem after using the triangle based approach was clearly solvable, but required a HUGE amount of algebra (at least as far as I could work out).

The new approach is to not think of the system as a system of triangles, but instead as a sort of trapezoid like so:

For our first pass let's ignore the tilting of the drawing head completely to make life simpler.

The geometry of $C,D,E,F$ is known. Only the lengths of the cables $AC$ and $BD$ change. What we want to find is forward and inverse kinematic equations which relate the lengths of $AC$ and $BD$ to $F_x,F_y$. The other equation we need to find relates $F_x,F_y$ to the lengths of $AC$ and $BD$.

Let's begin by finding $F_x,F_y$ when we know the lengths of $AC$ and $BD$.

We will use a system of three equations. The first two equations will be circles with centers $A$ and $B$, the third equation will be that $CD$ is a known length which we will call $w$.

$$C_x = A_x+\sqrt{L_{AC}^2-A_y^2+2A_yC_y-C_y^2} \tag1$$ $$D_x = B_x-\sqrt{L_{BD}^2-B_y^2+2B_yD_y-D_y^2} \tag2$$ $$D_x - C_x = w \tag3$$

Because $A_y, L_{AC}, B_y$, and $L_{BD}$ are known, let's say $\alpha = L_{AC}^2 - A_y^2$, $\beta = L_{BD}^2 - A_y^2$ where $A_y = B_y$.

We also know that $C_y = D_y$ which simplifies the equations to:

$$C_x = A_x+\sqrt{\alpha+2A_yC_y-C_y^2} \tag1$$ $$D_x = B_x-\sqrt{\beta+2B_yD_y-D_y^2} \tag2$$ $$D_x - C_x = w \tag3$$

Plugging $(1)$ and $(2)$ into $(3)$ we find:

$$\left(B_x-\sqrt{\beta+2B_yD_y-D_y^2}\right) - \left(A_x+\sqrt{\alpha+2A_yC_y-C_y^2}\right) = w \tag4$$

Distributing terms and re-organizing we can find: $$\sqrt{\beta+2B_yD_y-D_y^2} + \sqrt{\alpha+2A_yC_y-C_y^2} = B_x-A_x-w \tag4$$

We can also say $\gamma = B_x-A_x-w$ to simplify things further.

$$\sqrt{\beta+2A_yC_y-C_y^2} + \sqrt{\alpha + 2A_yC_y-C_y^2} = \gamma\tag4$$

Solving for $y$ we find: $$C_y = \frac{8 \gamma^2 A_y-\sqrt{64 \gamma^4 A_y^2-16 \gamma^2 (\alpha^2-2 \alpha \beta-2 \alpha \gamma^2+\beta^2-2 \beta \gamma^2+\gamma^4})}{8 \gamma^2}\tag5$$

We can then plug that answer back into $(1)$ to get $C_x,C_y$. $F_x$ is then given by $F_x = C_x + \frac{w}{2}$ and $F_y = C_y - h$

---

Now, working in the other direction when we have $F_x$ and $F_y$ and want to find the lengths of $AC$ and $BD$. This direction of computation is done more often, and should be optimized.

First we find $C$ and $D$ which is fairly trivial because we know the geometry there and can say:

$$C_x = F_x - \frac{w}{2}$$ $$C_y = F_y + h$$ $$D_x = F_x + \frac{w}{2}$$ $$D_y = F_y + h$$

From Pythagorean theorem, we can then directly compute the lengths $L_{AC}$ and $L_{BD}$ as:

$$L_{AC} = \sqrt{{(A_x-C_x)}^2+{(A_y-C_y)}^2}\tag6$$ $$L_{BD} = \sqrt{{(B_x-D_x)}^2+{(B_y-D_y)}^2}\tag7$$

Answer 3

All of the effort and credit for this answer go to Keith Selbo, I am simply reproducing his work here to make the explanation public.

Development of a Method for Translating Hanging Carver (X, Y) Coordinates Into Suspension Lengths That Accounts for Sled Tilt and Finite Radius of the Suspension Anchors

The figure on the next page represents the convex quadrilateral formed by components of a vertical carver; a plotter that suspends a router sled from two points and moves it about the plotting surface by varying the length of the suspension lines. This development encompasses a sled with center of mass not co-located with the plotter bit, causing the sled to adopt a tilt to an angle at which the moment of the sled mass cancels the net moments exerted by the lateral and vertical forces exerted by the sled suspension. Also covered is an offset from the plot origin of the suspension anchor points due to the finite radius of a windlass or sprocket used for pay-out and take-up of the suspension lines as the sled is moved. This offset results in movement of the effective anchor point of the suspension which in turn affects the equilibrium tilt of the sled.

The inputs to the algorithm are the plotter geometry and the x, and y coordinates of the desired bit placement. The output is the length of the left and right suspension lines. It includes the straight portion from the attachment at the sled to the variable tangent on the suspension anchor plus the curved portion of the line from the tangent to the top center of the anchor disk.

The sled tilt angle, and the variable left and right anchor points result in three non-linear equations in three unknowns. The expedient of adding an imaginary extension of the suspension line (see drawing) past the anchor point (tangent) to the y axis allows the use of a pseudo-anchor point that varies only in the y direction which simplifies the equations and the computation. A simple transformation corrects the line length after the solution is obtained. This transformation is included in the final equations for the length of the suspension lines.

There was no obvious closed form solution to the equations, so multi-variate a Newton-Raphson algorithm is used to estimate the three unknowns.

Net Forces:

$$\vec{f}_{y_1} + \vec{f}_{y_2} = -\vec{W}\tag{a}$$ $$\vec{f}_{x_1} + \vec{f}_{x_2} = 0\tag{b}$$

Net Moment:

$$\vec{h_1}\times\vec{f}_{y_1}+\vec{h_2}\times\vec{f}_{y_2} +\vec{h_3}\times\vec{W}=0\tag{c}$$

Definitions:

$$\vert\vec{h_1}\vert = \vert\vec{h_2}\vert = h\tag{d}$$

$$\vert\vec{h_3}\vert = h_3\tag{e}$$

$$\vert\vec{f_{x_1}}\vert = \vert\vec{f_{x_2}}\vert = fx(f)\tag{f}$$

$$\vert\vec{W}\vert = W\tag{g}$$

$$\phi_1 = \theta - \phi, \phi_2 = \theta + \phi \tag{h}$$

Moment Cross Product:

$$\vert\vec{h_3}\vert\vert\vec{W}\vert\sin{\phi}+\vert\vec{f_{x_2}}\vert\vert\vec{h_2}\vert\sin{\phi_2}-\vert\vec{f_{x_1}}\vert\vert h_1\vert\sin{\phi_1}+\vert\vec{f_{y_1}}\vert\vert\vec{h_1}\vert\cos{\phi_1} - \vert\vec{f_{y_2}}\vert\vert\vec{h_2}\vert\cos{\phi_2} = 0\tag{i}$$

Given that:

$$\tan{\gamma} = \frac{y+y_1^+-h\sin{\phi_1}}{x-h\cos{\phi_1}}\tag{j}$$ and:

$$f_{y_1} = fx\tan{\gamma}$$

$$\tan{\gamma} = \frac{y+y_2^+-h\sin{\phi_2}}{D-(x+hcos{\phi_2})}\tag{k}$$

$$fy_2 = fx\tan{\lambda}\tag{l}$$

Combining $(a),(j)$ and $(l)$ yields:

$$W = fx(\tan{\lambda}+\tan{\gamma})\tag{m}$$

$$\frac{W}{tan{\lambda}+tan{\gamma}} = fx\tag{n}$$

Then the net moment is zero when:

$$h_3W\sin{\phi}+ \left\vert\frac{Wh}{\tan{\lambda}+\tan{\gamma}}\right\vert(\sin\phi_2 - \sin\phi_1+\tan\gamma\cos\phi_1 - \tan\lambda\cos\phi_2) = 0 \tag{o}$$

Which simplifies to:

$$f(\phi) = h_3\sin{\phi}+ \left\vert\frac{h}{\tan{\lambda}+\tan{\gamma}}\right\vert(\sin\phi_2 - \sin\phi_1+\tan\gamma\cos\phi_1 - \tan\lambda\cos\phi_2) = 0 \tag{1}$$

From the sprocket detail in the figure we have:

$$y_1^+ = r\sqrt{1+tan^2\gamma}\tag{p}$$

Which yields

$$\tan\gamma = \frac{\sqrt{{(y_1^+)}^2-r^2}}{r} = \frac{y+y_1^+-h\sin\phi_1}{x-h\cos\phi_1}\tag{q}$$

or

$$\frac{\sqrt{{(y_1^+)}^2-r^2}}{r} - \frac{y+y_1^+-h\sin\phi_1}{x-h\cos\phi_1} = 0\tag{2}$$

similarly

$$\frac{\sqrt{(y_2^+)^2-r^2}}{r} - \frac{y+y_2^+-h\sin\phi_2}{D-(x+h\cos\phi_2)} = 0 \tag{3}$$

We now have a nonlinear system F(x), consisting of equations (1), (2), and (3):

$$\begin{bmatrix}f_1(\phi,y_1^+, y_2^+) \\ f_2(\phi, y_1^+, y_2^+) \\ f_3(\phi, y_1^+, y_2^+)\end{bmatrix}$$

Applying the Newton-raphson Method we iteratively solve $J_F(x_n)(\Delta x) = -F(x_n)\tag{r}$ where J is the system Jacobian, x is the vector:

$$\begin{bmatrix}\phi \\ y_1^+ \\ y_2^+\end{bmatrix} and x_{n+1} = x_n+\Delta x \tag{s}$$

Until

$$F(X_n+1) \approx 0 \tag{t}$$

Then:

$$LeftChain = \sqrt{(x-h\cos(\phi_1))^2+(y+y_1^+-h\sin(\phi_1))^2}-r \tan(\gamma)+r\gamma \tag{u}$$ $$RightChain = \sqrt{(D-(x+h\cos(\phi_2)))^2 + (y+y_2^+-h\sin(\phi_2))^2}-r\tan(\lambda)+r\lambda \tag{v}$$

Again, a HUGE thank you to Keith Selbo for this solution.

Answer 4

I decided to build a V-plotter. I am using Arduino based software. I'd like to adapt your V-plotter math solution. Can you solve an example problem with your calculations? Can you help me. I will be waiting for your answer. thank you

Answer 5

The kinematics model which Keith Selbo contributed (and OP Bar Smith posted) is great. I've implemented it in Java for use in the g-code generator for a hanging plotter that I'm building. I haven't totally tuned it, but it already provides better results than treating the system as a simple triangle and using Pythagorean theorem.

Answer 6

Inverse Kinematics are easier to calculate with visualizing the problemspace. Here is a example for a robotarm with 2 DOF:

fig = plt.figure()
ax = Axes3D(fig) 
x,y,z=[],[],[]
p1=(300,300)
goal=(400,300)
radius1,radius2=50,100
step=10
for a1 in range(0,360,step):
  for a2 in range(0,360,step):
    p2 = pa.polarpoint(p1, a1, radius1)
    p3 = pa.polarpoint(p2, a2, radius2)
    diff = pa.distance(goal, p3)
    if diff<10:
      print a1,a2,p3,diff
      ax.scatter(a1, a2, diff,c='r')
    x.append(a1)
    y.append(a2)
    z.append(diff)
ax.scatter(x, y, z)
plt.show()

The program plots the errorvalue in a 3D Map. For other robotypes like a hanging plotter the codelines with the polarpoint has to be adjusted. The 3D map is not the solution directly, but it helps to program a heuristics for finding the lowest point in the map. In most cases Gradient descent in combimation with particle swarm optimization is the way to go.