# Forward and Revers Kinematics For Modified Hanging Plotter

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.

• I doubt that this has a single solution. Unless it's a special edge case, there are infinitely many ways to have two points at a fixed distance to each other with each one of them on its own circle. "discussing the center of gravity of the pen" which you are not interested in, is the important aspect of the problem, which reduces the infinite number of solutions to the purely geometrical problem stated, to a single one of the mechanical/physical problem. Gravity will let the system come to a resting point. The pure mathematical model appears to be insufficient. – Bending Unit 22 Oct 7 '16 at 13:03
• German Cable Robot including Sourcecode in Forth: VIERTE DIMENSION Das FORTH-Magazin 2 / 2003, page 29 – Manuel Rodriguez Oct 9 '16 at 21:34

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.

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$$

• Is ti sure that the CDF triangle has fixed zero orientation? Your question stated that you have not interest in solutions including the center of gravity... – 50k4 Oct 17 '16 at 7:10
• You would make your life a lot easier if you changed the mechanics so that the tilting wouldn't happen. – Bending Unit 22 Oct 17 '16 at 17:00
• CDF does not have fixed zero orientation, but as @BendingUnit22 pointed out, it simplifies the math significantly so for a first approximation I'm going to pretend that it does. – Bar Smith Oct 17 '16 at 22:20
• @Bar Smith why are you so inclined to keep the system as is and not change it mechanically? There are very rarely bonus points in engineering for doing things the most complicated way. And as you are (understandably) struggling to do the math for the apparently needlessly more complicated mechanics for the static case, I'd like to point out that this plotter is probably supposed to move around and modeling the dynamic system won't be easier. Good luck writing a control algorithm to get the tip of the pen where you want it to be in order to draw something. – Bending Unit 22 Oct 17 '16 at 22:36

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.

• I am sorry to say, but this answer clearly goes against your constraints, stating that you are NOT interested in answers including center of gravity, and forces, you wanted pure kinematics.... – 50k4 Jan 24 '17 at 16:33

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)
step=10
for a1 in range(0,360,step):
for a2 in range(0,360,step):
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.

• 3D map, heuristics, gradient descent and particle swarm optimization are all interesting and powerful tools that certainly solve the problem as you suggest. But they also appear to be overengineered tools for the problem at hand. I mean we're talking about hanging a rigid body with two ropes. This is not rocket science, is it? – Bending Unit 22 Oct 8 '16 at 11:59
• Google Scholar has currently 55300 Results about the problems of finding parameters in kinematic chains. It seems, that the scientific community has a lot to discuss ... – Manuel Rodriguez Oct 8 '16 at 12:08
• Why would you combine gradient descent with particle swarm? An why would they need heuristics? Also the answer has nothing to do with the question... It shows a plot for a 2dof robot arm, which has nothing to do with parallel structures and even less with cable structures. – 50k4 Oct 8 '16 at 14:52
• Thank you for your downvoting. You're right, particle swarm is only needed if more than one solution is possible. – Manuel Rodriguez Oct 8 '16 at 18:26