How to solve this tricky nonlinear system of trigonometric equations?

Question

I have the following problem I'd like to solve (coming from the loop closure equations of a parallel kinematic mechanism) to the unknowns q2 q3 q4 q6 (4 eqns to 4 unknowns). I already found numerical ways to solve the system, however, I'm interested in an analytical solution, since this speeds up calculations (and just because I like analytical solutions ;)). The system is:

$$\begin{align} C_1 &= -L_2 \cos(q_5 + q_6) + L_2 \cos(q_1 + q_2) + L_3 \cos(q_1 + q_2 + q_3) \\ C_2 &= -L_2 \sin(q_5 + q_6) + L_2 \sin(q_1 + q_2) + L_3 \sin(q_1 + q_2 + q_3) \\ C_3 &= \phantom{-}L_2 \cos(q_1 + q_2) + a_5 \cos(q_1 + q_2 + q_3 + q_4) + a_7 \sin(q_1 + q_2 + q_3) + a_9\cos(q_1 +q_2 + q_3) \\ C_4 &= \phantom{-}L_2 \sin(q_1 + q_2) + a_5 \sin(q_1 + q_2 + q_3 + q_4) - a_7 \cos(q_1 + q_2 + q_3) + a_9\sin(q_1 +q_2 + q_3) \end{align}$$

The parameters {$q_1$, $q_5$, $C_1$, $C_2$, $C_3$, $C_4$, $L_2$, $L_3$, $a_5$, $a_7$, $a_9$} are known and nonzero.

Any help would be appreciated in solving this system! There clearly is some symmetry that might be exploited?

I've already tried to use Sympy to obtain a symbolic solution but it keeps running for hours.

EDIT: actually, the case is simplified since $a_5 = L_2$.

Answer 1

EDIT: I first wrote a solution path for $C_k=0$. Here is a start for the general case.

Your equations can be written with complex exponential $$C_1+iC_2=-L_2e^{i\theta_5}+L_2e^{i\theta_2}+L_3e^{i\theta_3}$$ $$C_3+iC_4=L_2e^{i\theta_2}+a_5e^{i\theta_4}+(a_9-ia_7)e^{i\theta_3}$$ where $\theta_5=q_5+q_6$, $\theta_2=q_1+q_2$, $\theta_3=q_1+q_2+q_3$ and $\theta_4=q_1+q_2+q_3+q_4$.

First equation

If $C_1=C_2=0$, the first complex equation is equivalent to $$L_3e^{i\theta_3}=2L_2e^{i(\theta_5+\theta_2+\pi)/2}\sin\left(\frac{\theta_5-\theta_2}2\right)$$ This gives 2 cases, splitting into 4 subcases:

• $\theta_5+\theta_2=2\theta_3-\pi$ modulo $4\pi$ and $\frac{\theta_5-\theta_2}2=\arcsin\left(\frac{L_3}{2L_2}\right)$ or $\frac{\theta_5-\theta_2}2=\pi-\arcsin\left(\frac{L_3}{2L_2}\right)$,
• $\theta_5+\theta_2=2\theta_3+\pi$ modulo $4\pi$ and $\frac{\theta_5-\theta_2}2=-\arcsin\left(\frac{L_3}{2L_2}\right)$ or $\frac{\theta_5-\theta_2}2=\pi+\arcsin\left(\frac{L_3}{2L_2}\right)$,

Therefore $\theta_2=\theta_3\pm\left(\frac{\pi}2+\arcsin\left(\frac{L_3}{2L_2}\right)\right)=\theta_3\pm\beta$.

Otherwise, note $C_1+iC_2=\rho e^{i\gamma_1}$, so that the first equation can be written $$\rho_1 e^{i\gamma_1}= 2L_2 e^{i(\theta_5+\theta_2+\pi)/2}\sin\left(\frac{\theta_2-\theta_5}2\right)+L_3e^{i\theta_3}$$ Assuming $L_3\neq 0$ and using the law of cosines, we get $$\pm\cos(\theta_3-\gamma_1)=\frac{L_3^2+\rho_1^2-4L_2^2\sin^2\left(\frac{\theta_2-\theta_5}2\right)}{2L_3\rho_1} =\frac{L_3^2+\rho_1^2-2L_2^2(1-\cos(\theta_2-\theta_5))}{2L_3\rho_1}$$

If $L_3=0$ you get directly $\theta_2$ and $\theta_5$ from first equation.

Analogously, for $L_2\neq 0$, $$4\rho_1L_2\sin\left(\frac{\theta_2-\theta_5}2\right)\cos\left(\frac{\theta_2+\theta_5+\pi}2-\gamma_1\right)=\rho_1^2+4L_2^2\sin^2\left(\frac{\theta_2-\theta_5}2\right)-L_3^2\\ \iff2\rho_1L_2(\cos(\theta_2-\gamma_1)-\cos(\theta_5-\gamma_1)) =\rho_1^2-L_3^2+2L_2^2(1-\cos(\theta_2-\theta_5))$$