Inverse Kinematics of a 3-DOF Spider Robot Leg

Given a desired foot position \(\mathbf{P}=(P_x, P_y, P_z)\) in the robot’s body frame and link lengths \(L_1\) (coxa), \(L_2\) (femur), and \(L_3\) (tibia), we solve for the three joint angles coxa yaw \(\theta_1\), femur pitch \(\theta_2\), and tibia pitch \(\theta_3\) using a singularity-free, computationally efficient formulation for real-time microcontroller performance. All angles are measured in radians and follow the right-hand rule, taken positive when rotating counter-clockwise about their respective joint axes.

Definitions & Default Pose

We introduce the following notation for the 2D hip-pitch projection and the leg’s neutral configuration:

• Key points:
  • \(\mathbf{P}=(P_x,P_y,P_z)\): target foot position in the 3D body frame.
  • \(P_0\): hip-yaw pivot at the origin of the hip-pitch plane.
  • \(P_1\): hip-pitch joint, reached by translating \((L_1,0)\) along the coxa.
  • \(P_2\): knee joint at the end of the femur of length \(L_2\).
  • \(P_3\): foot tip at the end of the tibia of length \(L_3\).
• Link lengths:
  • \( L_1 \):coxa length
  • \(L_2 \):femur length
  • \(L_3 \):tibia length
• Angle conventions (positive CCW about each joint axis):
  • \(\alpha\): angle between the vertical (downward) and the line \(\overline{P_1P_3}\) in the hip-pitch plane.
  • \(\beta\): angle between the line \(P_1P\) and the Y-axis.
  • \(\theta_1\): coxa yaw about the global \(z\)-axis, steering the leg toward \((P_x,P_y)\).
  • \(\theta_2\): femur pitch about the local \(y\)-axis, zero when the femur is horizontal forward.
  • \(\theta_3\): knee (tibia) pitch, \(\pi\) when fully extended, positive when flexed.

  By construction, \(\alpha + \beta = 90^\circ\).

1. Computing \(\theta_1\)

Steer the coxa to align with the projection of \(\mathbf P\) onto the ground plane:

\[ \theta_1 = \operatorname{tan}^{-1}\left(\frac{P_x}{P_y}\right). \]

2. Computing the 2D Projection

To decouple the yaw rotation, we rotate the target point \(\mathbf{P}=(P_x,P_y,P_z)\) about the global \(z\)-axis by \(-\theta_1\):

From which we can find the vector \(\mathbf{v} = \mathbf{P}_3 - \mathbf{P}_1\) with its length \(\|\mathbf{v}\|\) and planar offsets \(\Delta y = \sqrt{P_x^2+P_y^2}-L_1\) and \(\Delta z=-P_z\).

3. Computing \(\alpha\)

From the right triangle formed by \(\Delta y\) and \(\Delta y\), three equivalent forms arise from basic definitions of \(\sin,\cos,\tan\):

\[\alpha = \operatorname{sin}^{-1}{\left(\frac{\Delta y}{\|\mathbf{v}\|} \right)} =\operatorname{cos}^{-1}{\left(\frac{\Delta z}{\|\mathbf{v}\|} \right)} =\operatorname{tan}^{-1}{\left(\frac{\Delta y}{\Delta z} \right)}\]

4. Computing \(\beta+\theta_2\)

Apply Law of Cosines in triangle \((\mathbf{P}_3, \mathbf{P}_1,\mathbf{P}_2)\):

\[ L_3^2 = L_2^2 + \|\mathbf{v}\|^2 - 2\,L_2\,\|\mathbf{v}\|\,\cos(\beta+\theta_2) \quad\Longrightarrow\quad \cos(\beta+\theta_2) = \frac{L_2^2- L_3^2 + \|\mathbf{v}\|^2 }{2\,L_2\,\|\mathbf{v}\|} \]

and define the intermediate constant:

\[ C = L_2^2 - L_3^2 + \|\mathbf{v}\|^2. \]

From the identity:

\[ \sin^2(\beta+\theta_2) + \cos^2(\beta+\theta_2) = 1, \]

and substituting the above for \(\cos\) yields

\[ \sin^2(\beta+\theta_2) = 1 - \frac{C^2}{4\,L_2^2\,\|\mathbf{v}\|^2} = \frac{4\,L_2^2\,\|\mathbf{v}\|^2 - C^2}{4\,L_2^2\,\|\mathbf{v}\|^2}. \]

Define the discriminant:

\[ \Delta = 4\,L_2^2\,\|\mathbf{v}\|^2 - C^2, \quad S = \sqrt{\Delta}. \]

Hence

\[ \sin(\beta+\theta_2) = \frac{S}{2\,L_2\,\|\mathbf{v}\|}. \]

Recover the combined angle with one arctan:

\[ \beta+\theta_2 = \operatorname{tan}^{-1}\left(\frac{S}{C}\right). \]

5. Computing \(\theta_2\)

From \(\alpha+\beta=90^\circ\) follows that

\[\begin{array}{rl} \theta_2 &= (\alpha+\beta+\theta_2) - (\alpha+\beta)\\ &= \alpha + \left( \beta+\theta_2\right) - \frac{\pi}{2}\\ &= \operatorname{tan}^{-1}{\left(\frac{\Delta y}{\Delta z} \right)} + \operatorname{tan}^{-1}\left(\frac{S}{C}\right) - \frac{\pi}{2}\\ &= \operatorname{tan}^{-1}\left(\frac{\Delta y\,S - \Delta z\,C}{\Delta y\,C + \Delta z\,S}\right) \end{array} \]

6. Computing \(\theta_3\)

Again Law of Cosines for triangle \((\mathbf{P}_1,\mathbf{P}_2, \mathbf{P}_3)\):

\[ \|\mathbf{v}\|^2 = L_2^2 + L_3^2 - 2\,L_2\,L_3\,\cos\theta_3. \]

Thus

\[\begin{array}{rl} \theta_3&=\operatorname{cos}^{-1}{\left(\frac{L_{2}^{2} + L_{3}^{2} - \|\mathbf{v}\|^{2}}{2 L_{2} L_3} \right).}\\ \end{array} \]

C++ Implementation

The goal of the whole derivation is to have a minimum number of square roots and trig functions for fast execution on Atmegas like Arduino.

#define L1 30.0f  // Coxa length (mm)
#define L2 100.0f // Femur length (mm)
#define L3 150.0f // Tibia length (mm)

// Compute leg joint angles: coxa yaw (theta1), femur pitch (theta2), tibia pitch (theta3)
void calcLegIK(float Px, float Py, float Pz, float *theta1, float *theta2, float *theta3) {

    // 1) Yaw about Z
    *theta1 = -atan2f(Px, Py);

    // 2) Project into hip-pitch plane
    float dy = sqrtf(Px * Px + Py * Py) - L1;
    float dz = Pz;

    float v2 = dy * dy + dz * dz;

    // 3) Inner workspace: too close => knee fully bent
    if (v2 < (L3 - L2) * (L3 - L2)) {
        *theta2 = M_PI + atan2f(dz, dy);
        *theta3 = 0; // folded
        return;
    }

    // 4) Outer workspace: too far => leg fully extended
    if (v2 > (L2 + L3) * (L2 + L3)) {
        *theta2 = atan2f(dz, dy);
        *theta3 = M_PI;
        return;
    }

    // 5) Calculate discriminant
    float C = L2 * L2 - L3 * L3 + v2;
    float disc = 4 * L2 * L2 * v2 - C * C;
    float S = disc > 0 ? sqrtf(disc) : 0;

    // 6) Femur pitch
    *theta2 = atan2f(dy * S + dz * C, dy * C - dz * S);

    // 7) Tibia pitch
    *theta3 = acosf((L2 * L2 + L3 * L3 - v2) / (2 * L2 * L3));
}