For a serial manipulator with \( n \) joints, the joint configuration is the vector \( \mathbf{q} = [q_1,\dots,q_n]^\top \in \mathbb{R}^n \) (revolute joints: angles, prismatic joints: displacements). The forward kinematics map sends joint space to end-effector pose:

\[ f:\mathbb{R}^n \to SE(3), \qquad \mathbf{T}^0_n(\mathbf{q}) = f(\mathbf{q}), \]

where \( \mathbf{T}^0_n(\mathbf{q}) \in SE(3) \) is the homogeneous transformation from base frame 0 to end-effector frame \( n \). In DH form, assuming standard DH parameters \( (a_i, \alpha_i, d_i, \theta_i) \),

\[ \mathbf{T}^0_n(\mathbf{q}) = \prod_{i=1}^n \mathbf{A}_i(q_i), \qquad \mathbf{A}_i(q_i) = \mathbf{R}_z(\theta_i)\mathbf{T}_z(d_i)\mathbf{T}_x(a_i)\mathbf{R}_x(\alpha_i), \]

where for a revolute joint \( i \) we have \( \theta_i = q_i + \theta_i^{\text{off}} \), and for a prismatic joint \( i \) we have \( d_i = q_i + d_i^{\text{off}} \).

In PoE form, given spatial screw axes \( \mathbf{S}_i \in \mathbb{R}^6 \) and the home configuration \( \mathbf{M} \in SE(3) \) at \( \mathbf{q}=\mathbf{0} \),

\[ \mathbf{T}^0_n(\mathbf{q}) = \exp([\mathbf{S}_1] q_1)\exp([\mathbf{S}_2] q_2)\cdots \exp([\mathbf{S}_n] q_n)\,\mathbf{M}, \]

where \( [\mathbf{S}_i] \in \mathbb{R}^{4 \times 4} \) is the matrix representation of the twist \( \mathbf{S}_i \).

For standard 2R, 3R planar, and 6R industrial arms, the geometry is sufficiently regular that compact analytic FK expressions can be derived, which then become templates for more complex manipulators and for software implementations.

flowchart TD
  Q["Joint vector q"] --> P1["Choose parameterization: DH or PoE"]
  P1 --> P2["Assign frames or screw axes"]
  P2 --> P3["Form per joint transforms A_i(q_i) or exp([S_i] q_i)"]
  P3 --> P4["Multiply transforms to obtain T0n(q)"]
  P4 --> P5["Extract position p and orientation R"]
  P5 --> P6["Implement FK in code and test numerically"]
        

Consider a planar 2R arm with link lengths \( l_1, l_2 > 0 \) and revolute joint angles \( q_1, q_2 \). The arm lies in the \( xy \)-plane and rotates about the \( z \)-axis. We choose:

• Frame 0 at the base, with \( x_0 \) along the first link when \( \mathbf{q}=\mathbf{0} \).
• Frame 1 at joint 1; frame 2 at the end-effector.

Let \( c_i = \cos q_i \), \( s_i = \sin q_i \), and \( c_{12} = \cos(q_1 + q_2) \), \( s_{12} = \sin(q_1 + q_2) \). The end-effector position \( (x,y) \) is

\[ \begin{aligned} x(\mathbf{q}) &= l_1 \cos q_1 + l_2 \cos(q_1 + q_2),\\ y(\mathbf{q}) &= l_1 \sin q_1 + l_2 \sin(q_1 + q_2), \end{aligned} \]

and the orientation (about the \( z \)-axis) is simply \( \phi(\mathbf{q}) = q_1 + q_2 \).

These expressions can be obtained rigorously via homogeneous transforms. With \( \mathbf{R}_z(q) \) the rotation around \( z \) by \( q \) and \( \mathbf{T}_x(l) \) the translation along \( x \) by \( l \), define

\[ \mathbf{T}^0_1(q_1) = \mathbf{R}_z(q_1)\mathbf{T}_x(l_1),\qquad \mathbf{T}^1_2(q_2) = \mathbf{R}_z(q_2)\mathbf{T}_x(l_2), \]

so that

\[ \mathbf{T}^0_2(\mathbf{q}) = \mathbf{T}^0_1(q_1)\,\mathbf{T}^1_2(q_2) = \begin{bmatrix} c_{12} & -s_{12} & 0 & l_1 c_1 + l_2 c_{12}\\ s_{12} & c_{12} & 0 & l_1 s_1 + l_2 s_{12}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}. \]

The translation components in the last column reproduce exactly the geometric expressions for \( x(\mathbf{q}) \) and \( y(\mathbf{q}) \), while the rotation submatrix corresponds to a rotation by \( \phi = q_1 + q_2 \).

flowchart TD
  B0["Base frame 0"] --> J1["Joint 1: angle q1"]
  J1 --> L1["Link 1: length l1"]
  L1 --> J2["Joint 2: angle q2"]
  J2 --> L2["Link 2: length l2"]
  L2 --> EE["End-effector frame 2 (x,y,phi)"]
        

3.1 Standard DH representation

Using standard DH parameters with all joint axes along \( z \) and all links in the \( xy \)-plane, a convenient choice is:

• \( a_1 = l_1, a_2 = l_2 \), \( \alpha_1 = \alpha_2 = 0 \), \( d_1 = d_2 = 0 \),
• \( \theta_1 = q_1, \theta_2 = q_2 \).

The per-joint transform is

\[ \mathbf{A}_i(q_i) = \begin{bmatrix} \cos q_i & -\sin q_i & 0 & a_i \cos q_i\\ \sin q_i & \cos q_i & 0 & a_i \sin q_i\\ 0 & 0 & 1 & d_i\\ 0 & 0 & 0 & 1 \end{bmatrix}. \]

With the above parameters and \( d_i = 0 \), the FK is

\[ \mathbf{T}^0_2(\mathbf{q}) = \mathbf{A}_1(q_1)\mathbf{A}_2(q_2), \]

which expands to the same expression derived geometrically in Section 2. This illustrates the equivalence of geometric reasoning and the DH convention.

3.2 PoE representation

We now describe the same 2R arm using PoE. In spatial coordinates, choose joint axes (pure rotations about \( z \)):

• Joint 1 axis: through the origin, \( \boldsymbol{\omega}_1 = [0,0,1]^\top \), point \( \mathbf{q}_1 = [0,0,0]^\top \).
• Joint 2 axis: parallel to \( z \), through point \( \mathbf{q}_2 = [l_1,0,0]^\top \).

The twists are

\[ \mathbf{S}_1 = \begin{bmatrix} 0\\0\\1\\0\\0\\0 \end{bmatrix}, \qquad \mathbf{S}_2 = \begin{bmatrix} 0\\0\\1\\0\\-l_1\\0 \end{bmatrix}, \]

where \( \mathbf{v}_i = -\boldsymbol{\omega}_i \times \mathbf{q}_i \). Let the home configuration (at \( q_1 = q_2 = 0 \)) be

\[ \mathbf{M} = \begin{bmatrix} 1 & 0 & 0 & l_1 + l_2\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}. \]

Then the FK is

\[ \mathbf{T}^0_2(\mathbf{q}) = \exp([\mathbf{S}_1] q_1)\exp([\mathbf{S}_2] q_2)\mathbf{M}. \]

Expanding the exponentials (using Rodrigues' formula for rigid-body twists) and simplifying yields exactly the same homogeneous transform as the DH formulation, confirming that both parameterizations describe the same physical motion.

A planar 3R arm extends the 2R geometry by an additional revolute joint. Let link lengths be \( l_1, l_2, l_3 \) and joint angles \( q_1, q_2, q_3 \). Then the end-effector position can be written compactly as

\[ \begin{aligned} x(\mathbf{q}) &= l_1\cos q_1 + l_2\cos(q_1 + q_2) + l_3\cos(q_1 + q_2 + q_3),\\ y(\mathbf{q}) &= l_1\sin q_1 + l_2\sin(q_1 + q_2) + l_3\sin(q_1 + q_2 + q_3), \end{aligned} \]

and the orientation of the end-effector frame is \( \phi(\mathbf{q}) = q_1 + q_2 + q_3 \). These formulas follow directly by summing the complex-plane representations of each link or by multiplying homogeneous transforms.

Using DH with \( a_i = l_i, \alpha_i = 0, d_i = 0, \theta_i = q_i \), we have

\[ \mathbf{T}^0_3(\mathbf{q}) = \mathbf{A}_1(q_1)\mathbf{A}_2(q_2)\mathbf{A}_3(q_3) = \begin{bmatrix} \cos\phi & -\sin\phi & 0 & x(\mathbf{q})\\ \sin\phi & \cos\phi & 0 & y(\mathbf{q})\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}, \]

where the rotation and translation blocks match the geometric expressions above. Planar 3R arms are an important step toward full 6R manipulators because they already illustrate nontrivial coupling of joint variables in both position and orientation.

Many industrial arms (e.g., PUMA-type, KUKA-type) adopt a 3R positioning arm + 3R spherical wrist architecture:

• Joints 1–3: place the wrist center in 3D space.
• Joints 4–6: orient the end-effector about the wrist center (joint axes intersect).

Let the joint angles be \( \mathbf{q} = [q_1,\dots,q_6]^\top \). Using DH, we define per-joint transforms \( \mathbf{A}_i(q_i) \) and write

\[ \mathbf{T}^0_6(\mathbf{q}) = \mathbf{A}_1(q_1)\mathbf{A}_2(q_2)\mathbf{A}_3(q_3) \mathbf{A}_4(q_4)\mathbf{A}_5(q_5)\mathbf{A}_6(q_6). \]

If frames 3 and 4 are chosen so that their origins coincide at the wrist center, we can separate the FK into positioning and orientation factors:

\[ \mathbf{T}^0_6(\mathbf{q}) = \underbrace{\mathbf{T}^0_3(q_1,q_2,q_3)}_{\text{positioning arm}} \;\underbrace{\mathbf{T}^3_6(q_4,q_5,q_6)}_{\text{spherical wrist}}, \]

with \( \mathbf{T}^0_3 \in SE(3) \) affecting the wrist center position and orientation, and \( \mathbf{T}^3_6 \) a pure rotation about the wrist center (no translation in the wrist-only part).

A typical symbolic DH table (with offsets) is

\[ \begin{array}{c|cccc} i & a_i & \alpha_i & d_i & \theta_i\\ \hline 1 & a_1 & \tfrac{\pi}{2} & d_1 & q_1 \\ 2 & a_2 & 0 & 0 & q_2 \\ 3 & a_3 & -\tfrac{\pi}{2} & d_3 & q_3 \\ 4 & 0 & \tfrac{\pi}{2} & d_4 & q_4 \\ 5 & 0 & -\tfrac{\pi}{2} & 0 & q_5 \\ 6 & 0 & 0 & d_6 & q_6 \end{array} \]

Exact numerical values of \( a_i, d_i \) depend on the particular robot, but the symbolic structure is enough for deriving general FK expressions and implementing reusable software.

6.1 DH-based FK

Using the DH table above, each joint transform is

\[ \mathbf{A}_i(q_i) = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i\\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i\\ 0 & \sin\alpha_i & \cos\alpha_i & d_i\\ 0 & 0 & 0 & 1 \end{bmatrix}. \]

The rotation and position blocks of \( \mathbf{T}^0_6(\mathbf{q}) \) satisfy

\[ \mathbf{T}^0_6(\mathbf{q}) = \begin{bmatrix} \mathbf{R}^0_6(\mathbf{q}) & \mathbf{p}^0_6(\mathbf{q})\\ \mathbf{0}^\top & 1 \end{bmatrix},\qquad \mathbf{p}^0_6(\mathbf{q}) = \mathbf{p}^0_3(q_1,q_2,q_3) + \mathbf{R}^0_3(q_1,q_2,q_3)\,\mathbf{p}^3_6(q_4,q_5,q_6), \]

which reveals explicitly the separation between positioning and orientation: the wrist rotation acts on a fixed offset \( \mathbf{p}^3_6 \) from the wrist center to the end-effector.

6.2 PoE-based FK

In PoE form, we specify screw axes in the base frame. For a standard elbow manipulator with parallel shoulder and wrist axes, one possible choice (symbolic) is:

• \( \mathbf{S}_1 \): shoulder yaw about \( z_0 \) through the base origin,
• \( \mathbf{S}_2 \): shoulder pitch about an axis orthogonal to \( z_0 \),
• \( \mathbf{S}_3 \): elbow pitch, parallel to \( \mathbf{S}_2 \) but shifted,
• \( \mathbf{S}_4, \mathbf{S}_5, \mathbf{S}_6 \): three mutually orthogonal axes intersecting at the wrist center.

Stacking the twists as columns of a matrix \( \mathbf{S}_{\text{list}} \in \mathbb{R}^{6\times 6} \), the FK is

\[ \mathbf{T}^0_6(\mathbf{q}) = \exp([\mathbf{S}_1] q_1)\cdots \exp([\mathbf{S}_6] q_6)\,\mathbf{M}, \]

with \( \mathbf{M} \) the home pose. In practice, DH and PoE can be used interchangeably: DH is convenient when a frame assignment is already available from CAD or datasheets, while PoE is often algebraically cleaner for analytical derivations and for optimization-based algorithms.

Some practical aspects when implementing FK for standard manipulators:

• Numeric stability: use double-precision floating point. For large joint angles, normalize angles into a principal range before calling trigonometric functions.
• Precomputation: cache constant DH parameters \( a_i, \alpha_i, d_i \) (and \( \theta_i^{\text{off}} \)) or screw axes \( \mathbf{S}_i \) and the home pose \( \mathbf{M} \). The FK then only computes sines, cosines, and matrix multiplications depending on \( \mathbf{q} \).
• Frame conventions: maintain a consistent definition (e.g., base at robot pedestal; end-effector frame attached to tool flange). FK must match the mechanical drawings to be useful.
• Verification: test FK against known configurations (e.g., \( \mathbf{q}=\mathbf{0} \), or configurations where the arm is fully stretched) to confirm that the resulting pose matches expectations.

We first implement generic DH-based FK and then a PoE-based 6R FK using the modern_robotics library by Lynch and Park.

8.1 DH-based FK for planar 2R/3R and generic nR

import numpy as np

def dh_transform(theta, d, a, alpha):
    """Single DH transform using standard convention."""
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = np.sin(alpha)
    return np.array([
        [ct, -st * ca,  st * sa, a * ct],
        [st,  ct * ca, -ct * sa, a * st],
        [0.0,    sa,       ca,    d    ],
        [0.0,   0.0,      0.0,   1.0   ]
    ])

def fk_dh(q, a, alpha, d, theta_offset=None):
    """
    Generic DH FK.
    q: array of joint variables (n,)
    a, alpha, d: arrays of constant link parameters (n,)
    theta_offset: constant offsets for revolute joints (n,) or None.
    """
    q = np.asarray(q).flatten()
    n = q.shape[0]
    if theta_offset is None:
        theta_offset = np.zeros_like(q)

    T = np.eye(4)
    for i in range(n):
        theta_i = q[i] + theta_offset[i]
        A_i = dh_transform(theta_i, d[i], a[i], alpha[i])
        T = T @ A_i
    return T

# Example: planar 2R with a1=l1, a2=l2, alpha_i=0, d_i=0
def fk_planar_2r(q, l1, l2):
    q = np.asarray(q).flatten()
    a = np.array([l1, l2])
    alpha = np.array([0.0, 0.0])
    d = np.array([0.0, 0.0])
    return fk_dh(q, a, alpha, d)

# Example: planar 3R
def fk_planar_3r(q, l1, l2, l3):
    q = np.asarray(q).flatten()
    a = np.array([l1, l2, l3])
    alpha = np.array([0.0, 0.0, 0.0])
    d = np.array([0.0, 0.0, 0.0])
    return fk_dh(q, a, alpha, d)
      

8.2 PoE-based FK for a standard 6R arm

Suppose we have already computed the spatial screw axes Slist and the home configuration M. Using modern_robotics:

import numpy as np
import modern_robotics as mr

def fk_6r_poe(q, Slist, M):
    """
    PoE FK for a 6R manipulator.
    Slist: 6x6 matrix whose columns are the screw axes S_i in the space frame.
    M: 4x4 home configuration of the end-effector.
    q: iterable of six joint angles.
    """
    q = np.asarray(q).flatten()
    return mr.FKinSpace(M, Slist, q)

# Example skeleton: the actual Slist depends on robot geometry.
# Here we just show the shape and usage.
Slist_example = np.zeros((6, 6))  # Replace with real screw axes
M_example = np.eye(4)             # Replace with real home pose
q_example = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

T_example = fk_6r_poe(q_example, Slist_example, M_example)
print("T0_6(q_example) =\n", T_example)
      

In practice, Slist and M are derived once from the robot geometry; FK then reduces to evaluating exponentials and matrix products for each new configuration \( \mathbf{q} \).

In C++, it is common to use Eigen for matrix computations and the Orocos KDL library for higher-level kinematics. Below is a minimal Eigen-based implementation of DH FK for an nR chain, plus a planar 2R example.

#include <Eigen/Dense>
#include <vector>

using Mat4 = Eigen::Matrix4d;
using Vec  = Eigen::VectorXd;

Mat4 dhTransform(double theta, double d, double a, double alpha)
{
    double ct = std::cos(theta);
    double st = std::sin(theta);
    double ca = std::cos(alpha);
    double sa = std::sin(alpha);

    Mat4 A;
    A <<
        ct, -st * ca,  st * sa, a * ct,
        st,  ct * ca, -ct * sa, a * st,
        0.0,      sa,      ca,       d,
        0.0,     0.0,     0.0,     1.0;
    return A;
}

Mat4 fkDH(const Vec& q,
          const Vec& a,
          const Vec& alpha,
          const Vec& d,
          const Vec& thetaOffset)
{
    const int n = static_cast<int>(q.size());
    Mat4 T = Mat4::Identity();
    for (int i = 0; i < n; ++i)
    {
        double theta_i = q(i) + thetaOffset(i);
        Mat4 A_i = dhTransform(theta_i, d(i), a(i), alpha(i));
        T = T * A_i;
    }
    return T;
}

// Example: planar 2R FK
Mat4 fkPlanar2R(double q1, double q2, double l1, double l2)
{
    Vec q(2);      q << q1, q2;
    Vec a(2);      a << l1, l2;
    Vec alpha(2);  alpha.setZero();
    Vec d(2);      d.setZero();
    Vec thetaOff(2); thetaOff.setZero();
    return fkDH(q, a, alpha, d, thetaOff);
}
      

With Orocos KDL, one typically constructs a KDL::Chain with KDL::Joint and KDL::Segment objects and then calls KDL::ChainFkSolverPos_recursive to compute FK; the underlying mathematics is equivalent to the DH-based formulation shown here.

10.1 Java implementation (array-based)

In Java, we can implement DH-based FK using plain 2D arrays for matrices. Libraries such as EJML can simplify matrix operations, but here we keep the implementation minimal and dependency-free.

public class DHKinematics {

    public static double[][] dhTransform(double theta, double d,
                                         double a, double alpha) {
        double ct = Math.cos(theta);
        double st = Math.sin(theta);
        double ca = Math.cos(alpha);
        double sa = Math.sin(alpha);

        double[][] A = new double[4][4];
        A[0][0] = ct;          A[0][1] = -st * ca;  A[0][2] =  st * sa;  A[0][3] = a * ct;
        A[1][0] = st;          A[1][1] =  ct * ca;  A[1][2] = -ct * sa;  A[1][3] = a * st;
        A[2][0] = 0.0;         A[2][1] =      sa;  A[2][2] =      ca;  A[2][3] = d;
        A[3][0] = 0.0;         A[3][1] =     0.0;  A[3][2] =     0.0;  A[3][3] = 1.0;
        return A;
    }

    public static double[][] matMul4(double[][] X, double[][] Y) {
        double[][] Z = new double[4][4];
        for (int i = 0; i < 4; ++i) {
            for (int j = 0; j < 4; ++j) {
                double s = 0.0;
                for (int k = 0; k < 4; ++k) {
                    s += X[i][k] * Y[k][j];
                }
                Z[i][j] = s;
            }
        }
        return Z;
    }

    public static double[][] fkDH(double[] q,
                                  double[] a,
                                  double[] alpha,
                                  double[] d,
                                  double[] thetaOffset) {
        int n = q.length;
        double[][] T = new double[4][4];
        // initialize T as identity
        for (int i = 0; i < 4; ++i) {
            for (int j = 0; j < 4; ++j) {
                T[i][j] = (i == j) ? 1.0 : 0.0;
            }
        }
        for (int i = 0; i < n; ++i) {
            double theta_i = q[i] + thetaOffset[i];
            double[][] A_i = dhTransform(theta_i, d[i], a[i], alpha[i]);
            T = matMul4(T, A_i);
        }
        return T;
    }

    // Planar 2R example
    public static double[][] fkPlanar2R(double q1, double q2,
                                        double l1, double l2) {
        double[] q = new double[] { q1, q2 };
        double[] a = new double[] { l1, l2 };
        double[] alpha = new double[] { 0.0, 0.0 };
        double[] d = new double[] { 0.0, 0.0 };
        double[] offs = new double[] { 0.0, 0.0 };
        return fkDH(q, a, alpha, d, offs);
    }
}
      

10.2 MATLAB/Simulink implementation

In MATLAB, we can implement FK using simple functions. In Simulink, these functions can be called inside MATLAB Function blocks to build FK blocks for model-based design.

function T = dh_transform(theta, d, a, alpha)
% Single DH transform (standard convention)
ct = cos(theta);
st = sin(theta);
ca = cos(alpha);
sa = sin(alpha);

T = [ ct, -st * ca,  st * sa, a * ct;
      st,  ct * ca, -ct * sa, a * st;
      0,        sa,       ca,    d;
      0,         0,        0,    1 ];
end

function T = fk_dh(q, a, alpha, d, theta_offset)
% Generic DH FK
if nargin < 5
    theta_offset = zeros(size(q));
end
n = numel(q);
T = eye(4);
for i = 1:n
    theta_i = q(i) + theta_offset(i);
    A_i = dh_transform(theta_i, d(i), a(i), alpha(i));
    T = T * A_i;
end
end

function T02 = fk_planar2R(q1, q2, l1, l2)
% Planar 2R example
q = [q1; q2];
a = [l1; l2];
alpha = [0; 0];
d = [0; 0];
theta_offset = [0; 0];
T02 = fk_dh(q, a, alpha, d, theta_offset);
end
      

In Simulink, a typical pattern is:

1. Create a MATLAB Function block that accepts joint variables and outputs a 4x4 matrix.
2. Paste fk_dh (or a wrapper function) into the block code.
3. Use matrix blocks or Bus objects to distribute position and orientation signals.

10.3 Wolfram Mathematica implementation

Mathematica is well suited for symbolic derivations of FK. A simple DH-based implementation for a 2R planar arm:

Clear[DHTransform]
DHTransform[theta_, d_, a_, alpha_] := { {
   Cos[theta], -Sin[theta] Cos[alpha],  Sin[theta] Sin[alpha], a Cos[theta]},
  {Sin[theta],  Cos[theta] Cos[alpha], -Cos[theta] Sin[alpha], a Sin[theta]},
  {0,           Sin[alpha],            Cos[alpha],            d},
  {0,           0,                     0,                     1}
};

Clear[FKPlanar2R]
FKPlanar2R[q1_, q2_, l1_, l2_] := Module[
  {A1, A2, T02},
  A1 = DHTransform[q1, 0, l1, 0];
  A2 = DHTransform[q2, 0, l2, 0];
  T02 = Simplify[A1 . A2];
  T02
];

(* Example symbolic FK *)
T02sym = FKPlanar2R[q1, q2, l1, l2];
MatrixForm[T02sym]
      

Symbolic FK expressions obtained this way can be exported as optimized C or MATLAB code and used directly in real-time control or simulation.

In this lesson we derived and implemented forward kinematics for three canonical serial manipulators: planar 2R, planar 3R, and standard 6R industrial arms with spherical wrists. We expressed FK in both DH and PoE formulations, highlighting the geometric meaning of each term and the structural decomposition of 6R arms into a 3R positioning subsystem and a 3R spherical wrist. We also showed how to implement these models in Python, C++, Java, MATLAB/Simulink, and Mathematica, forming the computational backbone for subsequent chapters on inverse kinematics, differential kinematics, and dynamics.