3-DOF Spherical Wrist

Kinematics derivation

The mechanism is a 3-DOF spherical wrist. Now, in order to define the kinematics, we need to define the joint frames. We’ll use the Denavit-Hartenberg convention to define the frames, mainly because that’s just what I learned in school. Typically, a conventional spherical wrist has all frame origins located at the wrist center, with the exception of the end-effector. This removes a lot of complexity, especially as we tackle the Jacobian later on. Something like this:

But you know what? That’s too easy. Since the formulas are already known and sitting in my old course textbook, let’s define a new wrist with some impractical frames. It’ll serve as a good test case for our derivations:

Here we have the base frame at the bottom joint, frames 1 and 2 offset to the ‘center’, and then frame 3 at the end effector. The first thing we need to do is define the DH parameters for this wrist. The table below shows the parameters for each joint:

• $\alpha_i$: angle from $\vect{k}_{i-1}$ to $\vect{k}_i$, about $\vect{i}_i$
• $d_i$: distance between origins $\underset{\sim}{o}_{i-1}$ and $\underset{\sim}{o}_i$, along $\vect{k}_{i-1}$
• $a_i$: distance between $\vect{k}_{i-1}$ and $\vect{k}_i$, along $\vect{i}_i$
• $\theta_i$: angle from $\vect{i}_{i-1}$ to $\vect{i}_i$, about $\vect{k}_{i-1}$

Link 0 is just the base frame.

DH	$\theta_i$	$d_i$	$a_i$	$\alpha_i$
Link 1	$q_1$	$l_1$	$0$	$-90^\circ$
Link 2	$q_2-90^\circ$	$0$	$0$	$-90^\circ$
Link 3	$q_3$	$l_2$	$0$	$0^\circ$

$l_1$ is the distance from the base to the center of the wrist ($o_0$ to $o_1$), and $l_2$ is the distance from the center of the wrist to the end effector ($o_1$ to $o_3$). Now, we can use these parameters to define the transformation matrices for each joint.

For a revolute joint, we have the frame given by the following equations. Here, $k\mathord{\times}$ is the skew-matrix of the $k$ vector:

$$\begin{aligned} C_i &= C_{i-1} e^{\theta_i k\mathord{\times}} R_i, \\ \underset{\sim}{o}_i &= \underset{\sim}{o}_{i-1} + C_{i-1} e^{\theta_i k\mathord{\times}}\vect{\delta}_i \end{aligned}$$

And for a prismatic joint, we have the frame given by:

$$\begin{aligned} C_i &= C_{i-1} R_i, \\ \underset{\sim}{o}_i &= \underset{\sim}{o}_{i-1} + C_{i-1}\left(\vect{\delta}_i+d_i k\right) \end{aligned}$$

Using the parameters above, we can convert the DH parameters into homogeneous transformation matrices. We will then use these matrices to determine the overall transformation from the base frame to the end-effector frame, giving us the forward kinematics of the wrist.

$$\begin{bmatrix} C_i & \underset{\sim}{o}_i \\ \vect{0}^{\,T} & 1 \end{bmatrix} = \begin{bmatrix} C_{i-1} & \underset{\sim}{o}_{i-1} \\ \vect{0}^{\,T} & 1 \end{bmatrix} \underbrace{ \begin{bmatrix} e^{\theta_i k\mathord{\times}}R_i & e^{\theta_i k\mathord{\times}}\vect{\delta}_i \\ \vect{0}^{\,T} & 1 \end{bmatrix}}_{{}^{i-1}T_i}$$

Or, for prismatic joints:

$$\begin{bmatrix} C_i & \underset{\sim}{o}_i \\ \vect{0}^{\,T} & 1 \end{bmatrix} = \begin{bmatrix} C_{i-1} & \underset{\sim}{o}_{i-1} \\ \vect{0}^{\,T} & 1 \end{bmatrix} \underbrace{ \begin{bmatrix} R_i & \vect{\delta}_i+d_i k \\ \vect{0}^{\,T} & 1 \end{bmatrix}}_{{}^{i-1}T_i}$$

And so, to combine these in a general formula for the homogeneous transformations, we can write:

$${}^{i-1}T_i = \begin{bmatrix} e^{\theta_i k\mathord{\times}}R_i & e^{\theta_i k\mathord{\times}}\left(\vect{\delta}_i+d_i k\right) \\ \vect{0}^{\,T} & 1 \end{bmatrix}$$

Now, using our DH parameters, we can write out the transformation matrices for each joint.

$${}^{0}T_1 = \begin{bmatrix} e^{q_1 k\mathord{\times}}e^{-\frac{\pi}{2}i\mathord{\times}} & l_1 k \\ \vect{0}^{\,T} & 1 \end{bmatrix}$$ $${}^{1}T_2 = \begin{bmatrix} e^{\left(q_2-\frac{\pi}{2}\right)k\mathord{\times}} e^{-\frac{\pi}{2}i\mathord{\times}} & 0 \\ \vect{0}^{\,T} & 1 \end{bmatrix}$$ $${}^{2}T_3 = \begin{bmatrix} e^{q_3 k\mathord{\times}} & l_2 k \\ \vect{0}^{\,T} & 1 \end{bmatrix}$$

And there we go. We can now determine the forward kinematics of the wrist by multiplying these transformation matrices with one another.

And as a bit of an unnecessary but cleaner intermediate step, we can calculate all the transformations in base frame 0:

$$\begin{aligned} {}^{0}T_2 &= {}^{0}T_1\,{}^{1}T_2, \\ {}^{0}T_3 &= {}^{0}T_1\,{}^{1}T_2\,{}^{2}T_3 = {}^{0}T_2\,{}^{2}T_3 \end{aligned}$$

And there we have it: the forward kinematics of the wrist. Now we can write the overall transformations as:

$$\begin{aligned} \begin{bmatrix} C_{1} & \underset{\sim}{o}_{1} \\ \vect{0}^{\,T} & 1 \end{bmatrix} &= \begin{bmatrix} C_{0} & \underset{\sim}{o}_{0} \\ \vect{0}^{\,T} & 1 \end{bmatrix} {}^{0}T_1 \end{aligned}$$ $$\begin{aligned} \begin{bmatrix} C_{2} & \underset{\sim}{o}_{2} \\ \vect{0}^{\,T} & 1 \end{bmatrix} &= \begin{bmatrix} C_{0} & \underset{\sim}{o}_{0} \\ \vect{0}^{\,T} & 1 \end{bmatrix} {}^{0}T_2 \\ &= \begin{bmatrix} C_{1} & \underset{\sim}{o}_{1} \\ \vect{0}^{\,T} & 1 \end{bmatrix} {}^{1}T_2 \end{aligned}$$ $$\begin{aligned} \begin{bmatrix} C_{3} & \underset{\sim}{o}_{3} \\ \vect{0}^{\,T} & 1 \end{bmatrix} &= \begin{bmatrix} C_{0} & \underset{\sim}{o}_{0} \\ \vect{0}^{\,T} & 1 \end{bmatrix} {}^{0}T_3 \\ &= \begin{bmatrix} C_{2} & \underset{\sim}{o}_{2} \\ \vect{0}^{\,T} & 1 \end{bmatrix} {}^{2}T_3 \end{aligned}$$

Velocity kinematics

I’ll be honest because this section was tough to revisit. I actually skipped it and jumped straight to inverse kinematics first. But, it makes the most sense to cover it right after forward kinematics, so here we go.

Angular Velocities

Back to the kinematics. Rather than looking at the whole transformation, including the frame and origin, let’s just look at the frame rotation matrices. These represent the pure orientation changes between frames.

$$C_{1}=C_{0}\,{}^0C_1=C_{0}Q, \qquad Q={}^0C_1$$

Since $Q$ is a rotation matrix, it is orthogonal, so we can write:

$$Q^{T}Q = QQ^{T} = I$$

With a little bit of algebra, we can manipulate this to get:

$$\dot{Q}=\dot{Q}I=\dot{Q}Q^TQ$$

Let’s define $\Omega=\dot{Q}Q^T$:

$$\dot{Q}=\Omega Q$$

To verify this property, let’s go back to the orthogonal identity and differentiate it:

$$\begin{aligned} QQ^T&=I, \\ \dot{Q}Q^T+Q\dot{Q}^T&=0 \end{aligned}$$

Substituting $\Omega=\dot{Q}Q^T$ yields:

$$\Omega+\Omega^T=0$$

or

$$\Omega^T=-\Omega$$

Thus, $\Omega$ is skew-symmetric. This matrix allows us to extract the physical angular velocity vector. In robotics, the cross-product operator \mathord{\times} converts a 3-element velocity vector into this skew-symmetric matrix form.

Relating this back to those initial formulas, we can express the angular velocity matrix as

$${}^0w_{1,0}\mathord{\times} =\Omega =\dot{Q}Q^T ={}^0\dot{C}_1\left({}^0C_1\right)^T$$

Which, in this example, is the angular velocity of frame 1 with respect to frame 0, expressed in frame 0.

To retrieve the actual angular velocity vector, $\vect{w}_{1,0}$, we just multiply the angular velocity matrix by the base frame:

$$\vect{w}_{1,0} = \vect{C}_0\,{}^0w_{1,0}$$

Now that we have that, and assuming we’ve calculated the homogeneous transformations, we can move on to calculating the angular velocities of each frame.First, we grab the angular velocities with respect to the previous frame only, expressed in that local frame:

$$\begin{aligned} {}^0w_{1,0}&=\dot{\theta}_1 k, \\ {}^1w_{2,1}&=\dot{\theta}_2 k, \\ {}^2w_{3,2}&=\dot{\theta}_3 k \end{aligned}$$

Remember that each $k$ is the unit z-axis of that respective joint frame about which the rotation occurs.

It is important to remember that angular velocities simply add together, but we need to make sure they are all expressed in the exact same coordinate frame. To make them all relative to and expressed in the base frame 0, we rotate the local vectors forward:

$$\begin{aligned} {}^0w_{2,0}&={}^0w_{1,0}+{}^0C_1\,{}^1w_{2,1} \\ {}^0w_{3,0}&={}^0w_{2,0}+{}^0C_2\,{}^2w_{3,2} \end{aligned}$$

Finally, we determine the global vectors:

$$\begin{aligned} \vect{w}_{2,0} = \vect{w}_{1,0} + \vect{C}_1\,{}^1w_{2,1} \\ \vect{w}_{3,0} = \vect{w}_{2,0} + \vect{C}_2\,{}^2w_{3,2} \end{aligned}$$

And there we have it, the final angular velocity vectors for our spherical wrist.

Translational velocities

Let’s move on to the translational velocities, or $\underset{\sim}{\dot{o}}_n$.

These represent the linear velocities of the origins of each frame with respect to the base frame. We can use the angular velocities we just calculated to find them. While a more direct formula is typically used for the Jacobian, deriving this from first principles helps show exactly what is happening under the hood.

We start by extracting the position vectors of the frame origins from the fourth column of our homogeneous transformation matrices:

$$\begin{aligned} \underset{\sim}{o}_1&=\left({}^0T_1\right)_{0:3,\,3} \\ \underset{\sim}{o}_2&=\left({}^0T_2\right)_{0:3,\,3} \\ \underset{\sim}{o}_3&=\left({}^0T_3\right)_{0:3,\,3} \end{aligned}$$

The base origin is simply defined as $\underset{\sim}{o}_0=\vect{0}$.

Recursive method: moving link by link

Calculating the velocity of each individual frame origin step-by-step down the chain is the first method I learned as it provides a very intuitive approach, so we’ll do the same here. For this, we essentailly track the physical motion outwards, building the velocity profile link by link.

To find how each joint’s rotation we need the radius vectors. Here, we will define each radius as the displacement between the consecutive origins:

$$\begin{aligned} \vect{r}_1&=\underset{\sim}{o}_1-\underset{\sim}{o}_0 \\ \vect{r}_2&=\underset{\sim}{o}_2-\underset{\sim}{o}_1 \\ \vect{r}_3&=\underset{\sim}{o}_3-\underset{\sim}{o}_2 \end{aligned}$$

We then propagate the velocity outwards from the base. Each link inherits the linear velocity of the previous origin and adds its own rotational cross-product contribution:

$$\begin{aligned} \underset{\sim}{\dot{o}}_1&=\underset{\sim}{\dot{o}}_0 +\vect{w}_{1,0}\times\vect{r}_{1} \\ \underset{\sim}{\dot{o}}_2&=\underset{\sim}{\dot{o}}_1 +\vect{w}_{2,0}\times\vect{r}_{2} \\ \underset{\sim}{\dot{o}}_3&=\underset{\sim}{\dot{o}}_2 +\vect{w}_{3,0}\times\vect{r}_{3} \end{aligned}$$

As you can imagine, this link-by-link approach, calculating every intermediate step, can get a bit tedious if you only care about the end-effector.

Direct Jacobian Method: each joint’s contribution to the end-effector velocity

Rather than looking at step-by-step segments, let’s instead look directly at how each joint’s rotation impacts the end-effector origin $\underset{\sim}{o}_{3}$ directly.

To do this, we change our perspective on the radius vectors. The radius vector becomes the total lever arm extending from the joint origin in question all the way to that final endpoint:

$$\begin{aligned} \vect{r}_1&=\underset{\sim}{o}_3-\underset{\sim}{o}_0 \\ \vect{r}_2&=\underset{\sim}{o}_3-\underset{\sim}{o}_1 \\ \vect{r}_3&=\underset{\sim}{o}_3-\underset{\sim}{o}_2 \end{aligned}$$

Using these lever arms, the cross product $\vect{w}_{i,i-1}\times{}^0\vect{r}_i$ gives the linear velocity contribution of each individual joint to the final tip. Summing up the base velocity and the individual joint contributions yields the same total linear velocity of our end-effector origin:

$$\begin{aligned} \underset{\sim}{\dot{o}}_3&=\underset{\sim}{\dot{o}}_0 + \left(\vect{w}_{1,0}\times \vect{r}_1\right) + \left(\vect{w}_{2,1}\times\vect{r}_2\right) + \left(\vect{w}_{3,2}\times \vect{r}_3\right) \end{aligned}$$

This structural relationship is exactly what lets us build the geometric manipulator Jacobian. The Jacobian maps joint velocities directly to end-effector velocities:

$$\begin{aligned} \begin{bmatrix} \underset{\sim}{\dot{o}}_n \\ \vect{w}_{n,0} \end{bmatrix} &= \begin{bmatrix} \vect{J}_1 & \cdots & \vect{J}_n \end{bmatrix} \vect{\dot{q}} \\ &=J(\vect{q})\vect{\dot{q}} \end{aligned}$$

By separating out the individual ($\dot{q}_{i}$) terms from our velocity equations, we reveal the standard column formula for a revolute joint:

$$\vect{J}_i = \begin{bmatrix} \vect{k}_{i-1}\times \left(\underset{\sim}{o}_n-\underset{\sim}{o}_{i-1}\right) \\ \vect{k}_{i-1} \end{bmatrix}$$

I know we didn’t really cover it, but for a prismatic joint, there is no angular velocity, only linear velocity along the direction of the joint. So, the Jacobian column for a prismatic joint is simply:

$$\vect{J}_i = \begin{bmatrix} \vect{k}_{i-1} \\ \vect{0} \end{bmatrix}$$

Inverse kinematics

Since we went with our own custom wrist frames, the inverse kinematics will need to be derived manually. For a more conventional spherical wrist with all the frames meeting perfectly at the center, the inverse kinematics is well known and can be found in many textbooks. Our inverse kinematics formulas are still quite similar, but let’s go ahead and derive them from scratch.

Our inverse kinematics are still similar, but again, let’s go ahead and derive them.

To start, suppose we know $C_{3}$ and $\underset{\sim }{o}{}_{3}$, which represent our target end-effector frame orientation and position. We want to find the corresponding joint angles $\theta _{1}$, $\theta _{2}$, and $\theta _{3}$ to achieve this target. We can begin by looking at our forward-kinematics equations and doing some algebra to isolate the joint angles, keeping in mind the custom frames we defined and tracking which vectors line up.

$$\begin{aligned} C_3 &=e^{\theta_1\vect{k}_0\mathord{\times}} e^{\theta_2\vect{k}_1\mathord{\times}} e^{\theta_3\vect{k}_2\mathord{\times}} \vect{C}_0 \\ &=e^{\theta_1\vect{k}_0\mathord{\times}} e^{\theta_2\vect{j}_0\mathord{\times}} e^{\theta_3\vect{i}_0\mathord{\times}} \vect{C}_0 \\ &= \vect{C}_0 e^{\theta_1 k\mathord{\times}} e^{\theta_2 j\mathord{\times}} e^{\theta_3 i\mathord{\times}} \\ &=e^{\theta_1\vect{k}_0\mathord{\times}} \vect{C}_0 e^{\theta_2 j\mathord{\times}} e^{\theta_3 i\mathord{\times}} \end{aligned}$$

we post-multiply both sides (multiply on the right) by $e^{-\theta_3 i\mathord{\times}}$. This corresponds to a rotation of $-\theta _{3}$ about the $\vect{i}_3$ axis and cleanly cancels out that last rotation term.

$$\begin{aligned} \vect{C}_3e^{-\theta_3 i\mathord{\times}} &=e^{\theta_1\vect{k}_3\mathord{\times}}\vect{C}_3 \\ &=e^{\theta_1\vect{k}_0\mathord{\times}}\vect{C}_0 e^{\theta_2 j\mathord{\times}} \end{aligned}$$

Now we’re on to something, but we still have too many variables. We need to remove at least one of them. For $\theta _{2}$, the rotation happens entirely about the $j$ axis. If we isolate only the $j$ component of our matrix equation, the $\theta _{2}$ term drops out because a frame rotating around its own $j$ axis leaves that axis unchanged.

$$e^{-\theta_3\vect{k}_3\mathord{\times}}C_3j =e^{\theta_1\vect{k}_0\mathord{\times}}C_0j, \qquad e^{\theta_2 j\mathord{\times}}j=I$$

By looking at this shared relationship from both the base side and the end-effector side, we can break the main equation into two separate vector conditions.

$$\begin{aligned} e^{\theta_1\vect{k}_0\mathord{\times}}\vect{j}_0 &=\vect{k}_1, \\ e^{-\theta_3\vect{k}_3\mathord{\times}}\vect{j}_3 &=-\vect{k}_1. \end{aligned}$$

These equations do not bring both vectors onto the exact same target vector. In a conventional wrist they would, but our custom layout changes things. The first equation rotates $\vect{j}_0$ onto $\vect{k}_1$, while the second rotates $\vect{j}_3$ onto $-\vect{k}_1$. This opposite sign in the second equation comes directly from our DH frame definitions where at $\theta_3=0$ we have $\vect{j}_3=\vect{j}_2=-\vect{k}_1$.

Even though the two target vectors end up with opposite signs, both equations still depend on that exact same intermediate joint-axis direction, $\vect{k}_1$. From our frame geometry, $\vect{k}_1$ is perpendicular to both $\vect{k}_0$ and $\vect{k}_3$. Therefore, as long as we are in a nonsingular pose, we can calculate $\vect{k}_1$ directly.

$$\vect{k}_1 = \pm \frac{\vect{k}_0\times\vect{k}_3} {\left\lVert\vect{k}_0\times\vect{k}_3\right\rVert}.$$

The two possible signs give us the two standard Euler-angle branches of the inverse-kinematics solution. We will just carry this $\pm$ choice through the math for now and return to what it physically means after we finish solving for the three joint angles.

$$\vect{k}_1 =\pm\frac{\vect{k}_0\times\vect{k}_3} {\left\lVert\vect{k}_0\times\vect{k}_3\right\rVert}$$

Now let’s look closely at how those three frames relate geometrically.

For clarity, we can draw these direction vectors with a common tail even though their coordinate-frame origins do not actually sit at the exact same point in space. This visual shift does not alter our vector math. Since $i_{n}$, $j_{n}$, and $k_{n}$ are free unit direction vectors expressed in frame 0, their dot products, cross products, sums, and differences remain completely independent of where we choose to sketch them. The physical joint axes themselves still intersect at the wrist center where $O_1=O_2$.

$$e^{\theta_1\vect{k}_0\mathord{\times}}\vect{j}_0=\vect{k}_1$$

This equation shows that rotating the direction vector $\vect{j}_0$ about $\vect{k}_0$ by $\theta_1$ produces $\vect{k}_1$. It does not mean that the physical base frame is being rotated. Rather, the rotation operator is being applied to a copy of the known direction vector $\vect{j}_0$.

Now we have our constructed triangle with our three vectors. The trick is to translate the vector $\vect{k}_1-\vect{j}_0$ to form two right triangles:

$$\begin{aligned} \frac{|\theta_1|}{2} &=\arctan\left( \frac{\left\lVert\frac{1}{2}(\vect{k}_1-\vect{j}_0)\right\rVert} {\left\lVert\frac{1}{2}(\vect{k}_1+\vect{j}_0)\right\rVert} \right) \\ |\theta_1| &= 2\arctan\left( \frac{\left\lVert\vect{k}_1-\vect{j}_0\right\rVert} {\left\lVert\vect{k}_1+\vect{j}_0\right\rVert} \right) \end{aligned}$$

$\vect{j}_0$ is known from the base frame, $\vect{k}_3$ is known from the end-effector frame, and $\vect{k}_1$ \vect{k}_1 comes directly from our cross-product formula.

To determine the sign of $\theta_1$, we need to check whether $\vect{k}_1$ lies on the positive or negative side of our rotation axis. To do this, recall from figure 3 that positive right-handed rotation about $+k_0$ carries $j_0$ toward $-i_0$. Therefore, a positive $\theta_1$ gives $k_1$ a negative $i_0$ component.

$$\begin{aligned} \operatorname{sign}(\theta_1) &=-\operatorname{sign}\left(\vect{k}_1^T\vect{i}_0\right) \end{aligned}$$

We can use this exact same triangle trick to solve for $\theta _{3}$, but we have to keep our custom negative sign in mind. At $\theta_3=0$, our DH definitions state that $j_3=j_2=-k_1$. Since frame 3 is built by rotating frame 2 about their common axis $k_2=k_3$, we can set up the rotation tracking.

$$j_3=e^{\theta_3 k_{3} \times}(-k_1),$$

Or, as we found earlier:

$$e^{-\theta_3\vect{k}_3\mathord{\times}}\vect{j}_3=-\vect{k}_1$$

$\vect{k}_1$, $\vect{j}_3$, and $\vect{k}_3$ are all known.

Since $\vect{k}_1$, $\vect{j}_3$, and $\vect{k}_3$ are all known quantities, we can solve the triangle directly.

$$\begin{aligned} e^{-\theta_3\vect{k}_3\mathord{\times}}\vect{j}_3&=-\vect{k}_1, \\ \left|\theta_3\right| &=2\arctan\left( \frac{\left\lVert\vect{j}_3-(-\vect{k}_1)\right\rVert} {\left\lVert\vect{j}_3+(-\vect{k}_1)\right\rVert} \right) \\ &=2\arctan\left( \frac{\left\lVert\vect{j}_3+\vect{k}_1\right\rVert} {\left\lVert\vect{j}_3-\vect{k}_1\right\rVert} \right), \\ \operatorname{sign}(\theta_3) &=\operatorname{sign}\left((-\vect{k}_1)^T\vect{i}_3\right) \end{aligned}$$

For the sign, again we are essentially checking whether $-\vect{k}_1$ lies on the positive or negative side of the rotation axis $\vect{k}_3$. A positive $\theta_3$ rotates $-\vect{k}_1$ toward $\vect{i}_3$. Therefore, to determine the sign we check the projection of $-\vect{k}_1$ onto $\vect{i}_3$.

Revisiting $\theta_2$, we can use a similar approach and the same tricks. But instead of deriving the full the algebraic formula and equivalent vectors, at this point we can just look at it:

$$\begin{aligned} \vect{j}_1 = -\vect{k}_0 \\ \vect{i}_1 = \vect{j}_1 \times \vect{k}_1 \\ e^{\theta_2\vect{k}_1\mathord{\times}}\vect{i}_1 =\vect{k}_2 =\vect{k}_3 \end{aligned}$$

And again, we have:

$$\begin{aligned} \left|\theta_2\right| &=2\arctan\left( \frac{\left\lVert\vect{k}_3-\vect{i}_1\right\rVert} {\left\lVert\vect{k}_3+\vect{i}_1\right\rVert} \right), \\ \operatorname{sign}(\theta_2) &=-\operatorname{sign}\left(\vect{k}_3^T\vect{k}_0\right) \end{aligned}$$

And there we have it: $\theta_1$, $\theta_2$, and $\theta_3$ solved for one nonsingular IK branch. There is still one important detail hiding in our first equation for $\vect{k}_1$: the $\pm$ means that there are two possible angle solutions for the same end-effector pose.

We can see where those two solutions come from by going back to the cross product:

$$\vect{k}_0\times\vect{k}_3 = \cos(\theta_2)\vect{k}_1$$

If we take the positive normalized cross product, we are choosing the branch where $\cos(\theta_2)>0$, or equivalently

$$-\frac{\pi}{2}<\theta_2<\frac{\pi}{2}.$$

This is the branch returned by our Python implementation. Taking the negative direction for $\vect{k}_1$ gives the other Euler-angle solution, which uses different joint angles but still produces the same end-effector pose. In a more complete solver, we could choose between the two using joint limits, collision checks, or trajectory optimization. That is a bit beyond what we need here though, so we simply use the positive normalized cross product.

Finally, if $\vect{k}_0\times\vect{k}_3=0$, we cannot normalize the cross product at all. This is the wrist’s singular configuration: $\vect{k}_1$ is no longer uniquely determined, which also means that $\theta_1$ and $\theta_3$ cannot be recovered individually.

Wrist singularities

Certain configurations create singularities in this wrist, which we can see directly using the Jacobian matrix. A singularity does not mean that the wrist is completely stuck. It means that two Jacobian columns have become linearly dependent, meaning two separate joints are producing the same instantaneous direction of motion. As a result, the wrist temporarily loses one independent degree of freedom (the Jacobian loses rank).

For the angular part of our Jacobian, each column represents one joint axis expressed in the base frame 0.

$$J_\omega(\vect{q}) = \begin{bmatrix} {}^0\vect{k}_0 & {}^0\vect{k}_1 & {}^0\vect{k}_2 \end{bmatrix}, \qquad {}^0w_{3,0}=J_\omega(\vect{q})\vect{\dot{q}}.$$

To see where the determinant comes from, we can write each of those axes in frame 0 coordinates.

$$\begin{aligned} {}^0\vect{k}_0 &=\begin{bmatrix}0\\0\\1\end{bmatrix}, \\ {}^0\vect{k}_1 &=\begin{bmatrix}-\sin\theta_1\\\cos\theta_1\\0\end{bmatrix}, \\ {}^0\vect{k}_2 &=\begin{bmatrix} \cos\theta_1\cos\theta_2\\ \sin\theta_1\cos\theta_2\\ -\sin\theta_2 \end{bmatrix}. \end{aligned}$$

Placing those three vectors into the columns of our angular Jacobian, $J_\omega$, gives:

$$J_\omega = \begin{bmatrix} 0 & -\sin\theta_1 & \cos\theta_1\cos\theta_2 \\ 0 & \cos\theta_1 & \sin\theta_1\cos\theta_2 \\ 1 & 0 & -\sin\theta_2 \end{bmatrix}.$$

Because the first column contains only one nonzero entry, expanding the determinant along that column keeps the algebra fairly simple.

$$\begin{aligned} \det\left(J_\omega\right) &= \begin{vmatrix} -\sin\theta_1 & \cos\theta_1\cos\theta_2 \\ \cos\theta_1 & \sin\theta_1\cos\theta_2 \end{vmatrix} \\ &=-\sin^2\theta_1\cos\theta_2 -\cos^2\theta_1\cos\theta_2 \\ &=-\left(\sin^2\theta_1+\cos^2\theta_1\right)\cos\theta_2 \\ &=-\cos\theta_2. \end{aligned}$$

For our custom wrist definitions, the final determinant expression works out to:

$$\det\left(J_\omega\right)=-\cos\theta_2.$$

Therefore, the wrist hits a singularity for the condition:

$$\theta_2=\pm90^\circ + n180^\circ, \quad n\in\mathbb{Z}.$$

At these specific configurations, $\vect{k}_2$ aligns parallel or antiparallel to $\vect{k}_0$. In other words, joints 1 and 3 are rotating about the exact same physical line in space, meaning their velocity contributions are no longer independent. The Jacobian drops from rank 3 to rank 2. The wrist can still rotate in two independent directions, but it cannot instantaneously produce an angular velocity in every possible direction.

This also explains the failure case in the inverse kinematics derivation. We calculated $\vect{k}_1$ using $\vect{k}_0\times\vect{k}_3$, but at the singularity, $\vect{k}_0$ and $\vect{k}_3$ become parallel, causing that cross product to collapse to zero. The individual values of $\theta _{1}$ and $\theta _{3}$ are no longer unique because only their combined, cumulative effect is defined.

One last interesting note is that near the singularity, although the Jacobian has not quite lost rank, it becomes poorly conditioned. This means that a small requested change in end-effector velocity might demand very large or conflicting joint speeds. In a real-life implementation, these speed spikes may stress the motors and mechanical joints, causing premature wear or failure. And so, these poorly conditioned zones need to be actively avoided just as much as the singularity itself.