I want to implement in python the inverse kinematic algorithm describied in the following latex paragraph. The python script need as input the robot end-effector pose and rotational matrix and return as output the possibile joint values. Furthermore If I specifify the previous joint values and the joint limits as optional parameters, it return the most close single joint configuration.
The end-effector's position and orientation is given by the homogeneous transformation matrix:
\[\mat{T_e^0(\vet q)}= \begin{bmatrix}
        \vet n_e^0(\vet q) & \vet s_e^0(\vet q) & \vet a_e^0(\vet q)& \vet p_e^0(\vet q)\\
        0 & 0 & 0 & 1
\end{bmatrix}\]
to simplify the notation, the end-effector position is rewritten as \(\vet p_e^0(\vet q)=\vet p_e=(p_x, p_y, p_z)\).

Since the robot considered is a 6-dof robot we can split the inverse kinematic problem in two subproblem.

\paragraph{Wrist Position}
The wrist position can be found as in \autoref{eq:WristPosition}
\begin{equation}
\label{eq:WristPosition}
\begin{aligned}
       \vet p_W^0 (q_1,q_2,q_3) &=(p_{W_x},p_{W_y},p_{W_z})= \vet p_e - d_6 \vet a_e \\
       &= 
       \begin{bmatrix}
       \cos(q_1) (a_1 + a_2 \sin(q_2) + a_3 \sin(q_2 + q_3) + d_4 \cos(q_2 + q_3))\\
       \sin(q_1) (a_1 + a_2 \sin(q_2) + a_3 \sin(q_2 + q_3) + d_4 \cos(q_2 + q_3))\\
       a_2 \cos(q_2) + a_3 \cos(q_2 + q_3) + d_1 - d_4 \sin(q_2 + q_3)
       \end{bmatrix}
\end{aligned}
\end{equation}.

\paragraph{First Joint Angle (\( q_1\))}
The solution for the first joint is only a matter of solving a simple trigonometrical problem. 
%To ensure that all possible solutions are found we need to take into account that arctangent is only defined for angles between [-90\textdegree, 90\textdegree]. To counteract this issue, two different equations (shown in \autoref{eq:IK_first_joint}) are presented for joint-angle one which differs by one period (180\textdegree).
The first joint angle \(q_1\) is determined by:
\begin{equation}
\label{eq:IK_first_joint}
    q_1^{(1)} = \text{atan2}(p_{W_y}, p_{W_x}) 
\end{equation}
Since \(q_1\) has two possible solutions, the other solution is:
\[
q_1^{(2)} = q_1 + \pi
\]

\paragraph{Second Joint Angle (\(q_2\))}

For each \(q_1\), the second joint angle \(q_2\) is computed using geometric analysis of the robot's arm. The parameters \(L_2\) and \(L_3\) are defined as:
\[
L_2 = a_2, \quad L_3 = \sqrt{d_4^2 + a_3^2}
\]
The vector \(\vet p_W^1\) (the wrist position expressed in the reference frame 1) is given by:
\[
\vet p_W^1 =(\mat A_1^0)^{-1}\mat T_3^0 = (\mat A_1^0)^{-1} \begin{bmatrix} \vet p_W \\ 1 \end{bmatrix}
=
\begin{bmatrix} 
-a_1+p_{W_x} \cos (q_1)+p_{W_y} \sin (q_1)\\
d_1-p_{W_z}\\p_{W_y} \cos (q_1)-p_{W_x} \sin (q_1)\\
1
\end{bmatrix}
\]
And \(r\) is the distance:
\[
r = \sqrt{(p_{W_x}^1)^2 +(p_{W_y}^1)^2}
\]
Then, we calculate \(\beta\) and \(\gamma\) as:
\[
\beta = \text{atan2}(-p_{W_y}^1, p_{W_x}^1)
\]
\[
\gamma = \cos^{-1} \left( \frac{L_2^2 + r^2 - L_3^2}{2 L_2 r} \right)
\]
The two possible solutions for \(q_2\) are:
\[
q_2^{(1)} = \frac{\pi}{2} - \beta - \gamma \quad \text{(elbow up)}
\]
\[
q_2^{(2)} = \frac{\pi}{2} - \beta + \gamma \quad \text{(elbow down)}
\]

\paragraph{Third Joint Angle (\(q_3\))}

For the third joint angle \(q_3\), we define the fixed geometry parameters:
\[
L_4 = \sqrt{a_3^2 + d_4^2}, \quad \phi = \cos^{-1} \left( \frac{a_3^2 + L_4^2 - d_4^2}{2 a_3 L_4} \right)
\]
Given the distance \(r\) from the previous step, the angle \(\eta\) is computed as:
\[
\eta = \cos^{-1} \left( \frac{L_2^2 + L_4^2 - r^2}{2 L_2 L_4} \right)
\]
The two solutions for \(q_3\) are:
\[
q_3^{(1)} = \pi - \phi - \eta \quad \text{(elbow up)}
\]
\[
q_3^{(2)} = \pi - \phi + \eta \quad \text{(elbow down)}
\]
So far, we have 4 possible solutions. If $q_1^{(1)}$ is a solution (shoulder-right), $q_1^{(2)}$ is also a solution (shoulder-left). For each ($q_1^{(1)}$,$q_1^{(2)}$) there are two possible solutions for $q_2$ and $q_3$ (namely, elbow up and elbow down solutions). 

Notice finally how it is possible to find the solutions only if at least 
\begin{equation}
    p_{W_x} \neq 0 \quad or \quad p_{W_x} \neq 0 
\end{equation}
In the case where $p_{W_x}= p_{W_y} = 0$, an infinite number of solutions can be obtained, as it is possible to determine the joint variables $q_2$ and $q_3$ independently of the value of $q_1$. 
% The following section will demonstrate that the arm in such a configuration is kinematically singular.

\paragraph{Solution For Spherical Wrist ($q_4$, $q_5$, $q_6$)}
Finally we want to compute the joint variables $q_4$, $q_5$, $q_6$ corresponding to a given end-effector orientation $\mat R_6^3 = (\mat R_3^0)^T \mat R_6^0 = [\vet n_e^3 \ \vet s_e^3 \ \vet a_e^3 ]$. According to the sign of $q_5$ we will have two different solutions namely wrist-up and wrist-down.
\begin{enumerate}
    \item for $q_5\in (0,\pi)$:\\
    \begin{equation}
        \begin{aligned}
            q_4 &= \text{atan2}(a_y^3, a_x^3)\\
            q_5 &= \text{atan2}(\sqrt{(a_x^3)^2 + (a_y^3)^2}, a_z^3)\\
            q_6 &= \text{atan2}(s_z^3, - n_z^3)
        \end{aligned}
    \end{equation}
    \item for $q_5\in (-\pi,0)$:\\
    \begin{equation}
        \begin{aligned}
            q_4 &= \text{atan2}(-a_y^3, -a_x^3)\\
            q_5 &= \text{atan2}(-\sqrt{(a_x^3)^2 + (a_y^3)^2}, a_z^3)\\
            q_6 &= \text{atan2}(-s_z^3, n_z^3)
        \end{aligned}
    \end{equation}
\end{enumerate}
By solving for the joint angles \(q_1\), \(q_2\), and \(q_3\) using geometric analysis, and solving the wrist joints using a spherical wrist solution, we obtain 8 feasible configurations for the ABB IRB 4600 40/255 robot. These solutions can be verified using the forward kinematics to ensure they achieve the desired pose.