Inverse Kinematics of a 3-DOF Spider Robot Leg

Legged locomotion represents each desired foothold as a Cartesian point relative to the robot. The inverse kinematics of one leg converts this target into three actuator angles: the yaw angle of the coxa, the pitch angle of the femur, and the angle at the knee. For the common three-joint geometry considered in this chapter, this mapping has a closed-form solution. Rotational symmetry separates the yaw motion from a planar two-link problem, and the remaining geometry follows from a single triangle.

The resulting solver has fixed execution time through direct closed-form evaluation. It also produces a deterministic boundary pose for targets outside the exact workspace. A distant target extends the leg toward the target, while a target inside the inner workspace boundary folds the femur and tibia to their minimum attainable distance. This behaviour is well suited to real-time gait generation, where interpolation, body motion, and terrain compensation can temporarily move a desired foothold beyond the reachable set.

Geometry and Coordinate Conventions

All target coordinates are expressed in a right-handed frame attached to the base of the leg. The neutral coxa direction coincides with the positive local y-axis. The axes have the following meanings:

\(x\)

Lateral direction, positive toward the robot's right side.

\(y\)

Forward direction and neutral direction of the coxa.

\(z\)

Vertical direction, positive upward.

A robot with several legs usually defines one such frame for each mounting point. If a desired foot position is available in the body frame, it is transformed into the corresponding leg frame before applying the equations below. For a fixed mounting rotation \(R_{B i}\) and mounting position \(\mathbf{t}_{B i}\), the target for leg \(i\) is

\[ \mathbf{p}_i =R_{B i}^{T}\bigl(\mathbf{p}_B-\mathbf{t}_{B i}\bigr). \]

The leg contains three rigid links. The coxa has length \(\ell_1\) and rotates about the local \(z\)-axis. The femur has length \(\ell_2\), and the tibia has length \(\ell_3\). After the coxa rotation, the femur and tibia move in the local \(y\)-\(z\) plane about the local \(x\)-axis.

The joint angles use these conventions:

• \(\theta_1=0\): the coxa points along \(+y\).
• \(\theta_2=0\): the femur is horizontal and points along the yawed \(+y\)-direction.
• \(\theta_3=\pi\): femur and tibia form a straight line.
• \(\theta_3=0\): the mathematical two-link mechanism is folded to its minimum length.

The planar mechanism has two mirror-image solutions. The branch variable \(\sigma\in\{+1,-1\}\) selects the side of the femur on which the tibia bends. A physical leg generally uses one fixed branch because its mounting orientation, joint limits, and mechanical stops determine the permitted knee direction.

Forward Kinematics

Let \(\mathbf{e}_y=(0,1,0)^T\), and define the elementary rotation matrices

\[ R_x(\alpha)= \begin{bmatrix} 1&0&0\\ 0&\cos\alpha&-\sin\alpha\\ 0&\sin\alpha& \cos\alpha \end{bmatrix}, \qquad R_z(\alpha)= \begin{bmatrix} \cos\alpha&-\sin\alpha&0\\ \sin\alpha& \cos\alpha&0\\ 0&0&1 \end{bmatrix}. \]

The absolute pitch direction of the tibia is

\[ \beta=\theta_2+\sigma(\theta_3-\pi). \]

The complete forward kinematics can therefore be written compactly as

\[ \boxed{ \mathbf{p}(\theta_1,\theta_2,\theta_3) =R_z(\theta_1) \left( \ell_1\mathbf{e}_y +\ell_2R_x(\theta_2)\mathbf{e}_y +\ell_3R_x\!\left(\theta_2+\sigma(\theta_3-\pi)\right)\mathbf{e}_y \right) }. \]

The inverse problem starts with a desired target \(\mathbf{p}=(p_x,p_y,p_z)^T\) and determines the three angles that reproduce this point on the selected kinematic branch.

Separating the Coxa Yaw

The coxa rotates the entire planar mechanism about the vertical axis. Its angle can therefore be determined independently of the two pitch angles. Rotating the target by \(-\theta_1\) places it in the local \(y\)-\(z\) plane:

\[ \mathbf{p}'=R_z(-\theta_1)\mathbf{p}. \]

The first component of this vector is

\[ p'_x=p_x\cos\theta_1+p_y\sin\theta_1. \]

Requiring \(p'_x=0\) gives the target-facing yaw solution

\[ \boxed{\theta_1=-\operatorname{atan2}(p_x,p_y)}. \]

Define the horizontal distance from the vertical axis as

\[ h=\sqrt{p_x^2+p_y^2}. \]

Substitution of the yaw angle yields

\[ \mathbf{p}'= \begin{bmatrix} 0\\ h\\ p_z \end{bmatrix}. \]

This convention chooses the yaw orientation for which the target lies in the forward half of the yawed plane. Some highly folded configurations admit an equivalent representation with the yaw shifted by \(\pi\). Walking robots usually restrict each coxa to a nominal angular sector, making the target-facing branch the useful one for gait control.

Reduction to the Planar Hip-to-Foot Vector

In the yawed frame, the coxa endpoint and the hip pitch joint are located at \(\ell_1\mathbf{e}_y\). Subtracting this offset gives the vector from the hip pitch joint to the desired foot position:

\[ \mathbf{d} =\mathbf{p}'-\ell_1\mathbf{e}_y = \begin{bmatrix} 0\\ h-\ell_1\\ p_z \end{bmatrix} = \begin{bmatrix} 0\\ d_y\\ d_z \end{bmatrix}. \]

Thus

\[ d_y=h-\ell_1, \qquad d_z=p_z, \qquad r^2=\mathbf{d}^{T}\mathbf{d}=d_y^2+d_z^2. \]

The scalar \(r=\sqrt{r^2}\) is the desired distance from the hip pitch joint to the foot. The final angle equations use \(r^2\) directly, so a second square root for \(r\) is unnecessary.

Moving the pointer across the diagram changes the planar target and shows the triangle formed by \(\ell_2\), \(\ell_3\), and \(r\).

The Femur-Tibia Triangle

The femur, tibia, and hip-to-foot vector form a triangle with side lengths \(\ell_2\), \(\ell_3\), and \(r\). Let \(\varphi\) denote the direction of \(\mathbf{d}\), measured from the positive \(y\)-axis:

\[ \varphi=\operatorname{atan2}(d_z,d_y). \]

Let \(\delta\in[0,\pi]\) be the angle between the femur and the vector \(\mathbf{d}\). Applying the law of cosines to the side opposite \(\delta\) gives

\[ \ell_3^2=\ell_2^2+r^2-2\ell_2r\cos\delta. \]

Solving for the cosine yields

\[ \cos\delta =\frac{\ell_2^2+r^2-\ell_3^2}{2\ell_2r}. \]

Introduce the auxiliary quantity

\[ C=\ell_2^2-\ell_3^2+r^2. \]

Then

\[ \cos\delta=\frac{C}{2\ell_2r}. \]

Using \(\sin^2\delta=1-\cos^2\delta\),

\[ \begin{aligned} \sin^2\delta &=1-\frac{C^2}{4\ell_2^2r^2}\\ &=\frac{4\ell_2^2r^2-C^2}{4\ell_2^2r^2}. \end{aligned} \]

Define the discriminant and its non-negative square root as

\[ \Delta=4\ell_2^2r^2-C^2, \qquad S=\sqrt{\max(0,\Delta)}. \]

The discriminant can also be factored into the two workspace boundaries:

\[ \begin{aligned} \Delta &=4\ell_2^2r^2-\left(\ell_2^2-\ell_3^2+r^2\right)^2\\ &=\left((\ell_2+\ell_3)^2-r^2\right) \left(r^2-(\ell_2-\ell_3)^2\right). \end{aligned} \]

Consequently, \(\Delta\ge 0\) exactly when

\[ |\ell_2-\ell_3|\le r\le \ell_2+\ell_3. \]

Inside this interval,

\[ \sin\delta=\frac{S}{2\ell_2r}, \qquad \cos\delta=\frac{C}{2\ell_2r}. \]

Femur Angle

The two mirror-image solutions place the femur on opposite sides of \(\mathbf{d}\). Their common expression is

\[ \theta_2=\varphi+\sigma\delta, \qquad \sigma\in\{+1,-1\}. \]

Evaluating \(\varphi\) and \(\delta\) separately would require two inverse trigonometric operations. Their sum can be reconstructed directly. From the definitions above,

\[ \sin\varphi=\frac{d_z}{r}, \qquad \cos\varphi=\frac{d_y}{r}, \qquad \sin\delta=\frac{S}{2\ell_2r}, \qquad \cos\delta=\frac{C}{2\ell_2r}. \]

The angle-addition identities give

\[ \begin{aligned} \sin\theta_2 &=\sin(\varphi+\sigma\delta)\\ &=\sin\varphi\cos\delta +\sigma\cos\varphi\sin\delta\\ &=\frac{d_zC+\sigma d_yS}{2\ell_2r^2},\\[0.7em] \cos\theta_2 &=\cos(\varphi+\sigma\delta)\\ &=\cos\varphi\cos\delta -\sigma\sin\varphi\sin\delta\\ &=\frac{d_yC-\sigma d_zS}{2\ell_2r^2}. \end{aligned} \]

The denominator \(2\ell_2r^2\) is positive for \(r>0\) and appears in both arguments of \(\operatorname{atan2}\). It therefore cancels, giving the direct femur solution

\[ \boxed{ \theta_2= \operatorname{atan2} \left( d_zC+\sigma d_yS, d_yC-\sigma d_zS \right) }. \]

This form avoids the explicit computation of both \(\varphi\) and \(\delta\), preserves the quadrant of the result, and remains usable when the target is projected onto a workspace boundary.

Knee Angle

The angle \(\theta_3\) is the interior angle between femur and tibia. Applying the law of cosines to the side of length \(r\) gives

\[ r^2=\ell_2^2+\ell_3^2-2\ell_2\ell_3\cos\theta_3. \]

Define

\[ K=\ell_2^2+\ell_3^2-r^2. \]

Then

\[ \cos\theta_3=\frac{K}{2\ell_2\ell_3}. \]

The corresponding sine contains the same discriminant used for the femur angle. Indeed,

\[ \begin{aligned} 4\ell_2^2\ell_3^2-K^2 &=4\ell_2^2\ell_3^2- \left(\ell_2^2+\ell_3^2-r^2\right)^2\\ &=4\ell_2^2r^2- \left(\ell_2^2-\ell_3^2+r^2\right)^2\\ &=\Delta. \end{aligned} \]

Since \(\theta_3\in[0,\pi]\), its sine is non-negative:

\[ \sin\theta_3=\frac{S}{2\ell_2\ell_3}. \]

Reconstructing the angle from sine and cosine gives

\[ \boxed{\theta_3=\operatorname{atan2}(S,K)}. \]

The interior knee angle is independent of the branch variable \(\sigma\). The branch variable determines the signed relative tibia rotation in the forward model through \(\sigma(\theta_3-\pi)\).

Complete Closed-Form Solution

The following animation applies the complete inverse solution to a moving three-dimensional target. The angle indicators display the values generated by the same equations used in the firmware implementation.

Reachability and Boundary Behaviour

The exact workspace of the femur-tibia pair is the annulus

\[ |\ell_2-\ell_3|\le r\le\ell_2+\ell_3. \]

The factorized discriminant

\[ \Delta= \left((\ell_2+\ell_3)^2-r^2\right) \left(r^2-(\ell_2-\ell_3)^2\right) \]

is non-negative precisely inside this annulus. The clamped value \(S=\sqrt{\max(0,\Delta)}\) extends the same formulas continuously to both workspace boundaries.

Target Beyond Maximum Reach

For \(r>\ell_2+\ell_3\), the discriminant is negative and therefore \(S=0\). In this region \(K<0\), so

\[ \theta_3=\operatorname{atan2}(0,K)=\pi. \]

The femur and tibia form a straight line. Since \(C>0\), the femur equation becomes

\[ \theta_2=\operatorname{atan2}(d_zC,d_yC) =\operatorname{atan2}(d_z,d_y), \]

which points the fully extended leg toward the target.

Target Inside Minimum Reach

For \(r<|\ell_2-\ell_3|\), the discriminant is again negative and \(S=0\). Here \(K>0\), giving

\[ \theta_3=\operatorname{atan2}(0,K)=0. \]

The two links fold to the minimum attainable distance \(|\ell_2-\ell_3|\). The sign of \(C\) automatically orients the longer link so that the resulting hip-to-foot vector points toward the target. The same closed-form equations therefore produce the nearest radial boundary pose through one common expression for both workspace boundaries.

Singular Configurations

Two configurations require a controller-level convention because the geometry leaves one angle undetermined.

At \(p_x=p_y=0\), the target lies on the coxa rotation axis. Every value of \(\theta_1\) produces the same horizontal position. A stateless implementation may use the library result of \(\operatorname{atan2}(0,0)\), while a motion controller generally preserves the previous coxa angle to avoid an arbitrary jump.

At \(r=0\), the target coincides with the hip pitch joint and the direction of \(\mathbf{d}\) is undefined. The link geometry still determines whether the mechanism tends toward a folded pose, but the absolute femur direction remains free. A continuous controller should retain the previous femur angle or apply a configured neutral angle at this single point.

C++ Implementation

The firmware implementation writes the three angles into reference parameters and uses only scalar local variables. Workspace saturation follows directly from the clamped discriminant. Its interface consists of the three output angles; a gait planner can compute a reachability flag separately from the squared workspace bounds when it needs to distinguish an exact solution from a saturated pose.

#include <math.h>

#define LEG_L1 30.0f   // Coxa length in mm
#define LEG_L2 100.0f  // Femur length in mm
#define LEG_L3 150.0f  // Tibia length in mm
#define LEG_BEND 1.0f  // +1 or -1, depending on the mechanical knee branch

void legIK(float px, float py, float pz,
           float &theta1, float &theta2, float &theta3) {

    const float h = sqrtf(px * px + py * py);

    const float dy = h - LEG_L1;
    const float dz = pz;
    const float r2 = dy * dy + dz * dz;

    const float C = LEG_L2 * LEG_L2 - LEG_L3 * LEG_L3 + r2;
    const float K = LEG_L2 * LEG_L2 + LEG_L3 * LEG_L3 - r2;
    const float disc = 4.0f * LEG_L2 * LEG_L2 * r2 - C * C;
    const float S = sqrtf(disc > 0.0f ? disc : 0.0f);

    theta1 = -atan2f(px, py);

    theta2 = atan2f(dz * C + LEG_BEND * dy * S,
                    dy * C - LEG_BEND * dz * S);

    theta3 = atan2f(S, K);
}

Implementation Notes

Transform every target into its leg frame. The equations assume that \(+y\) is the neutral coxa direction and that the origin lies on the coxa axis. Static mounting rotations and offsets belong in the body-to-leg transform rather than in the inverse kinematics itself.

Keep the geometric and actuator conventions separate. The solver returns the interior knee angle \(\theta_3\), with \(\theta_3=\pi\) representing a straight leg. A servo whose zero position represents a straight knee usually receives a signed relative command such as \(q_3=\sigma(\theta_3-\pi)\), followed by the servo-specific direction sign, zero offset, and gear ratio. The same calibration principle applies to \(\theta_1\) and \(\theta_2\).

Use atan2 for angle reconstruction. The pair atan2(sin, cos) retains quadrant information and behaves naturally at the straight and folded boundaries. An acos-based implementation needs explicit input clamping and a separate branch sign.

Compare squared distances for reachability. A reachability flag can be computed directly from squared distances:

const float inner = (LEG_L2 - LEG_L3) * (LEG_L2 - LEG_L3);
const float outer = (LEG_L2 + LEG_L3) * (LEG_L2 + LEG_L3);
const bool reachable = r2 >= inner && r2 <= outer;

Floating-point rounding near a boundary can be handled with a small tolerance chosen in the same squared units as \(r^2\).

The discriminant has an equivalent factorized form. The implementation above uses \(\Delta=4\ell_2^2r^2-C^2\), which reuses \(C\). An equally valid form is

\[ \Delta= \left((\ell_2+\ell_3)^2-r^2\right) \left(r^2-(\ell_2-\ell_3)^2\right). \]

The factorized form makes the workspace limits explicit and can be convenient when the inner and outer squared radii are already available. Both forms should be clamped to zero before the square root.

Preserve angle continuity at singular points. A stateless solver must choose some value when yaw or femur direction is geometrically free. A gait controller has access to the previous joint state and can keep that angle unchanged, producing continuous motion through the singular configuration.

Saturated poses are useful control outputs. Foot trajectories can briefly leave the workspace during interpolation or body stabilization. The boundary pose produced by the equations remains finite and physically meaningful. The reachability flag can still inform the trajectory generator, velocity limiter, or fault monitor.

Use consistent units. The target coordinates and all three link lengths must use the same length unit. The equations return radians. Conversion to degrees belongs at the actuator interface when required by a particular servo protocol.

Organize multi-leg data for the update pattern. A structure-of-arrays layout stores all coordinates and all output angles in contiguous arrays. This layout keeps the leg update loop compact and supports later vectorization or DMA-oriented processing:

float px[6], py[6], pz[6];
float theta1[6], theta2[6], theta3[6];

for (unsigned i = 0; i < 6; ++i) {
    legIK(px[i], py[i], pz[i],
          theta1[i], theta2[i], theta3[i]);
}

Account for the transcendental operations in the control budget. Each leg evaluation uses two square roots and three atan2 calls. The remaining work consists of additions and multiplications. Modern microcontrollers with a floating-point unit handle this workload comfortably for a small number of legs. On controllers where floating-point transcendental functions are expensive, the same geometry can use a suitable fast atan2 approximation or lookup method after verifying the resulting angular error against the mechanical resolution of the joints.