Robot Kinematics

Overview

Robot kinematics deals with the mathematical description of robot motion without considering the forces that cause it. It establishes the relationship between joint variables and the position/orientation of the robot end-effector.

Forward Kinematics

Forward kinematics computes the position and orientation of the end-effector given the joint angles.

Problem Statement

Given: Joint angles \(\theta_1, \theta_2, ..., \theta_n\)

Find: End-effector position \([x, y, z]\) and orientation \([roll, pitch, yaw]\)

Solution Methods

1. DH Parameters - Systematic approach using Denavit-Hartenberg convention
2. Product of Exponentials (PoE) - Screw theory based formulation

Homogeneous Transforms

Scenario: For serial manipulators with multiple joints, we need a systematic way to compose transformations. Homogeneous transforms combine rotation and translation into a single 4x4 matrix, enabling us to chain link transformations and compute the end-effector pose efficiently.

Mathematical Formulation:

A homogeneous transformation matrix combines rotation \(R\) and translation \(d\) into a single matrix:

\[T = \begin{bmatrix} R_{3 \times 3} & d_{3 \times 1} \\ \mathbf{0}_{1 \times 3} & 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & d_x \\ r_{21} & r_{22} & r_{23} & d_y \\ r_{31} & r_{32} & r_{33} & d_z \\ 0 & 0 & 0 & 1 \end{bmatrix}\]

Key properties: - Composition: \(T_{AB} \cdot T_{BC} = T_{AC}\) (chaining transforms) - Inverse: \(T^{-1}\) recovers the relative pose - Application: \(p_{world} = T \cdot p_{local}\) transforms a point between frames

For 2D planar case, the transform simplifies to:

\[T = \begin{bmatrix} \cos\theta & -\sin\theta & x \\ \sin\theta & \cos\theta & y \\ 0 & 0 & 1 \end{bmatrix}\]

Code Implementation:

import numpy as np

class HomogeneousTransform:
    """A 4x4 homogeneous transformation matrix for robot kinematics."""

    def __init__(self, matrix: np.ndarray | None = None):
        if matrix is not None:
            self.matrix = np.array(matrix, dtype=np.float64)
        else:
            self.matrix = np.eye(4)

    @classmethod
    def rotation_z(cls, theta: float) -> 'HomogeneousTransform':
        """Create a rotation about the Z-axis."""
        c, s = np.cos(theta), np.sin(theta)
        T = np.array([
            [c, -s, 0, 0],
            [s,  c, 0, 0],
            [0,  0, 1, 0],
            [0,  0, 0, 1]
        ])
        return cls(T)

    @classmethod
    def rotation_y(cls, theta: float) -> 'HomogeneousTransform':
        """Create a rotation about the Y-axis."""
        c, s = np.cos(theta), np.sin(theta)
        T = np.array([
            [c,  0, s, 0],
            [0,  1, 0, 0],
            [-s, 0, c, 0],
            [0,  0, 0, 1]
        ])
        return cls(T)

    @classmethod
    def rotation_x(cls, theta: float) -> 'HomogeneousTransform':
        """Create a rotation about the X-axis."""
        c, s = np.cos(theta), np.sin(theta)
        T = np.array([
            [1, 0,  0, 0],
            [0, c, -s, 0],
            [0, s,  c, 0],
            [0, 0,  0, 1]
        ])
        return cls(T)

    @classmethod
    def translation(cls, x: float, y: float, z: float = 0.0) -> 'HomogeneousTransform':
        """Create a pure translation transform."""
        T = np.eye(4)
        T[:3, 3] = [x, y, z]
        return cls(T)

    def compose(self, other: 'HomogeneousTransform') -> 'HomogeneousTransform':
        """Compose this transform with another: self * other."""
        return HomogeneousTransform(self.matrix @ other.matrix)

    def inverse(self) -> 'HomogeneousTransform':
        """Compute the inverse transformation."""
        R = self.rotation()
        d = self.translation_vector()
        T_inv = np.eye(4)
        T_inv[:3, :3] = R.T
        T_inv[:3, 3] = -R.T @ d
        return HomogeneousTransform(T_inv)

    def rotation(self) -> np.ndarray:
        """Extract the 3x3 rotation matrix."""
        return self.matrix[:3, :3]

    def translation_vector(self) -> np.ndarray:
        """Extract the 3x1 translation vector."""
        return self.matrix[:3, 3]

    def apply(self, point: np.ndarray) -> np.ndarray:
        """Transform a 3D point from local to world coordinates."""
        p_h = np.append(point, 1.0)
        p_world_h = self.matrix @ p_h
        return p_world_h[:3]

    def __mul__(self, other: 'HomogeneousTransform') -> 'HomogeneousTransform':
        """Support * operator for composition."""
        return self.compose(other)


def fk_2link_homogeneous(l1: float, l2: float, theta1: float, theta2: float) -> tuple:
    """Compute FK for 2-DOF robot using homogeneous transforms."""
    T_joint1 = HomogeneousTransform.translation(0, 0, l1) @ HomogeneousTransform.rotation_z(theta1)
    T_end = HomogeneousTransform.translation(0, 0, l2) @ HomogeneousTransform.rotation_z(theta2)
    T_end_effector = T_joint1 @ T_end
    return T_joint1, T_end_effector


if __name__ == "__main__":
    l1, l2 = 1.0, 0.8
    theta1, theta2 = np.pi/2, np.pi/4
    T_j1, T_ee = fk_2link_homogeneous(l1, l2, theta1, theta2)
    ee_pos = T_ee.translation_vector()
    print(f"End-effector: ({ee_pos[0]:.4f}, {ee_pos[1]:.4f})")

Code Breakdown: - Line 3-11: HomogeneousTransform class definition with __init__ accepting optional 4x4 matrix or defaulting to identity - Line 13-24: rotation_z() creates rotation matrix using standard formula with \(\cos\theta\) and \(\sin\theta\) in the xy-plane - Line 39-47: translation() creates identity matrix with translation vector in the last column - Line 49-51: compose() multiplies matrices: \(T_{AC} = T_{AB} \cdot T_{BC}\) to chain transformations - Line 53-59: inverse() computes \(T^{-1} = \begin{bmatrix} R^T & -R^T d \\ 0 & 1 \end{bmatrix}\) - Line 67-70: apply() converts point to homogeneous coordinates, applies transform, returns 3D result - Line 76-79: fk_2link_homogeneous() composes two transforms for 2-DOF chain

Output Example:

End-effector: (1.3536, 1.3536)

Inverse Kinematics

Inverse kinematics computes the joint angles required to achieve a desired end-effector pose.

Challenges

• Multiple solutions may exist
• Singularities where solutions become undefined
• Workspace boundaries

Solution Methods

1. Analytical Solutions - Closed-form equations (when available)
2. Numerical Solutions - Iterative methods like Newton-Raphson
3. Geometric Solutions - Trigonometric approaches

Geometric IK for 2-DOF Planar Robot

Scenario: Given a target position \((x, y)\) for the end-effector and known link lengths \(l_1\) and \(l_2\), what joint angles \(\theta_1\) and \(\theta_2\) should we command? For a 2-DOF planar robot, there are typically two valid solutions: "elbow-up" and "elbow-down".

Mathematical Formulation:

The geometric solution uses the Law of Cosines. Given the target position, we can compute the distance \(r\) from the base to the end-effector:

\[r = \sqrt{x^2 + y^2}\]

The angle \(\theta_2\) (elbow angle) is found using the law of cosines:

\[\cos(\theta_2) = \frac{r^2 - l_1^2 - l_2^2}{2 \cdot l_1 \cdot l_2}\]

From this, two solutions emerge: - Elbow-Up: \(\theta_2 > 0\) (elbow bent upward) - Elbow-Down: \(\theta_2 < 0\) (elbow bent downward)

The angle \(\theta_1\) is computed as:

\[\theta_1 = \text{atan2}(y, x) - \alpha\]

Where \(\alpha\) accounts for the geometry of link 2 relative to link 1:

\[\alpha = \text{atan2}\left(l_2 \sin(\theta_2), l_1 + l_2 \cos(\theta_2)\right)\]

Code Implementation:

import numpy as np

def compute_fk(l1: float, l2: float, theta1: float, theta2: float) -> tuple:
    """Forward kinematics helper for verification."""
    theta_total = theta1 + theta2
    x = l1 * np.cos(theta1) + l2 * np.cos(theta_total)
    y = l1 * np.sin(theta1) + l2 * np.sin(theta_total)
    return (x, y)


def ik_2dof(l1: float, l2: float, x: float, y: float,
            elbow_up: bool = True) -> tuple | None:
    """
    Compute inverse kinematics for a 2-DOF planar robot arm.

    Args:
        l1, l2: Link lengths (meters)
        x, y: Target end-effector position (meters)
        elbow_up: If True, return elbow-up; else elbow-down

    Returns:
        Tuple of (theta1, theta2) in radians, or None if unreachable
    """
    r = np.sqrt(x**2 + y**2)
    max_reach = l1 + l2
    min_reach = abs(l1 - l2)

    if r > max_reach or r < min_reach:
        print(f"Target unreachable. Range: [{min_reach:.2f}, {max_reach:.2f}]")
        return None

    cos_theta2 = (r**2 - l1**2 - l2**2) / (2 * l1 * l2)
    cos_theta2 = np.clip(cos_theta2, -1.0, 1.0)

    theta2 = np.arccos(cos_theta2)
    if not elbow_up:
        theta2 = -theta2

    phi = np.arctan2(y, x)
    alpha = np.arctan2(l2 * np.sin(theta2), l1 + l2 * np.cos(theta2))
    theta1 = phi - alpha

    return (theta1, theta2)


def ik_2dof_all_solutions(l1: float, l2: float, x: float, y: float) -> list:
    """Compute all valid IK solutions for a 2-DOF planar robot."""
    r = np.sqrt(x**2 + y**2)
    if r > l1 + l2 or r < abs(l1 - l2):
        return []

    solutions = []
    sol_up = ik_2dof(l1, l2, x, y, elbow_up=True)
    sol_down = ik_2dof(l1, l2, x, y, elbow_up=False)
    if sol_up:
        solutions.append(sol_up)
    if sol_down:
        solutions.append(sol_down)
    return solutions


if __name__ == "__main__":
    l1, l2 = 1.0, 0.8
    targets = [(1.5, 0.5), (0.5, 1.0)]

    for x, y in targets:
        print(f"\nTarget: ({x}, {y})")
        for config, (t1, t2) in enumerate(ik_2dof_all_solutions(l1, l2, x, y), 1):
            name = "Elbow-Up" if t2 > 0 else "Elbow-Down"
            fx, fy = compute_fk(l1, l2, t1, t2)
            err = np.sqrt((fx - x)**2 + (fy - y)**2)
            print(f"  {name}: theta1={np.degrees(t1):.1f}, theta2={np.degrees(t2):.1f}, error={err:.6f}")

Code Breakdown: - Line 3-8: compute_fk() helper function implementing FK for verification purposes - Line 15-21: ik_2dof() main IK function with docstring explaining inputs and outputs - Line 23-28: Computes distance \(r = \sqrt{x^2 + y^2}\) and checks reachability: \(|l_1 - l_2| \leq r \leq l_1 + l_2\) - Line 30-34: Applies Law of Cosines formula: \(\cos(\theta_2) = \frac{r^2 - l_1^2 - l_2^2}{2 l_1 l_2}\), clamps and flips sign for elbow-down - Line 36-39: Computes \(\phi = \text{atan2}(y, x)\) and \(\alpha = \text{atan2}(l_2 \sin \theta_2, l_1 + l_2 \cos \theta_2)\) - Line 41: Returns final joint angles: \(\theta_1 = \phi - \alpha\) - Line 44-56: ik_2dof_all_solutions() returns both elbow-up and elbow-down configurations - Line 63-67: Verification loop computing forward kinematics to check IK accuracy

Output Example:

Target: (1.5, 0.5)
  Elbow-Up: theta1=17.9, theta2=75.5, error=0.000000
  Elbow-Down: theta1=60.9, theta2=-75.5, error=0.000000

Choosing Between Solutions:

The choice depends on: 1. Joint limits: Each configuration has different joint angle ranges 2. Obstacle avoidance: Choose the configuration that avoids collisions 3. Manipulability: Elbow-up typically has better dexterity near singularities 4. Physical constraints: Some robots can only achieve one configuration

Differential Kinematics

Jacobian Matrix

Scenario: The Jacobian matrix describes how changes in joint angles translate to changes in end-effector position. Watch the animation below to see how joint velocities map to end-effector velocities.

Animation — Watch how joint movements translate to end-effector velocity:

Mathematical Formulation:

The Jacobian matrix \(J(q)\) relates joint velocities to end-effector velocities:

\[\dot{x} = J(q) \dot{q}\]

Where \(\dot{x} = [\dot{x}, \dot{y}]^T\) is the end-effector velocity vector and \(\dot{q} = [\dot{\theta}_1, \dot{\theta}_2]^T\) is the joint velocity vector.

For a 2-DOF planar robot, the Jacobian elements are the partial derivatives of position with respect to joint angles:

\[J = \begin{bmatrix} \frac{\partial x}{\partial \theta_1} & \frac{\partial x}{\partial \theta_2} \\ \frac{\partial y}{\partial \theta_1} & \frac{\partial y}{\partial \theta_2} \end{bmatrix}\]

Computing the partial derivatives from the forward kinematics equations:

\[\frac{\partial x}{\partial \theta_1} = -l_1 \sin(\theta_1) - l_2 \sin(\theta_1 + \theta_2)$$ $$\frac{\partial x}{\partial \theta_2} = -l_2 \sin(\theta_1 + \theta_2)$$ $$\frac{\partial y}{\partial \theta_1} = l_1 \cos(\theta_1) + l_2 \cos(\theta_1 + \theta_2)$$ $$\frac{\partial y}{\partial \theta_2} = l_2 \cos(\theta_1 + \theta_2)\]

Code Implementation:

import numpy as np


def compute_jacobian(l1: float, l2: float, theta1: float, theta2: float) -> np.ndarray:
    """
    Compute the Jacobian matrix for a 2-DOF planar robot.

    Args:
        l1, l2: Link lengths (meters)
        theta1, theta2: Joint angles from vertical (radians)

    Returns:
        2x2 Jacobian matrix J(q)
    """
    theta_total = theta1 + theta2

    dx_dtheta1 = -l1 * np.sin(theta1) - l2 * np.sin(theta_total)
    dy_dtheta1 = l1 * np.cos(theta1) + l2 * np.cos(theta_total)

    dx_dtheta2 = -l2 * np.sin(theta_total)
    dy_dtheta2 = l2 * np.cos(theta_total)

    J = np.array([
        [dx_dtheta1, dx_dtheta2],
        [dy_dtheta1, dy_dtheta2]
    ])
    return J


def inverse_velocity(J: np.ndarray, end_effector_velocity: np.ndarray) -> np.ndarray:
    """
    Compute joint velocities from desired end-effector velocities.

    Args:
        J: 2x2 Jacobian matrix
        end_effector_velocity: [vx, vy] desired end-effector velocities

    Returns:
        [theta1_dot, theta2_dot] required joint velocities
    """
    if np.linalg.matrix_rank(J) == J.shape[0]:
        return np.linalg.inv(J) @ end_effector_velocity
    else:
        return np.linalg.pinv(J) @ end_effector_velocity


if __name__ == "__main__":
    l1, l2 = 1.0, 0.8
    theta1, theta2 = np.pi/2, 0.0
    J = compute_jacobian(l1, l2, theta1, theta2)

    print("Jacobian at theta1=90, theta2=0:")
    print(J)

    desired_velocity = np.array([0.1, 0.0])
    joint_velocities = inverse_velocity(J, desired_velocity)
    print(f"Joint velocities: {joint_velocities}")
    print(f"Verification: J @ q_dot = {J @ joint_velocities}")

Code Breakdown: - Line 3-23: compute_jacobian() function computing partial derivatives of FK equations - Line 19-22: Computes \(\frac{\partial x}{\partial \theta_1}\) and \(\frac{\partial y}{\partial \theta_1}\) using chain rule - Line 24-27: Computes \(\frac{\partial x}{\partial \theta_2}\) and \(\frac{\partial y}{\partial \theta_2}\) — note these only depend on \(\theta_1 + \theta_2\) - Line 29-33: Assembles 2x2 Jacobian matrix with columns for each joint's effect on end-effector - Line 35-46: inverse_velocity() computes \(\dot{q} = J^{-1}(q)\dot{x}\) using direct inverse or pseudoinverse near singularities - Line 43-45: Checks singularity via matrix rank; uses pseudoinverse when Jacobian is singular

Output Example:

Jacobian at theta1=90, theta2=0:
[[-1.8  -0.8]
 [ 0.    0. ]]
Joint velocities: [ 0.    -0.125]
Verification: J @ q_dot = [0.1 0. ]

Applications of the Jacobian:

The Jacobian is essential for: - Inverse velocity computation: \(\dot{q} = J^{-1}(q)\dot{x}\) — convert desired end-effector motion to joint motions - Singularity detection: When \(\det(J) = 0\), the robot loses a degree of freedom - Force resolution: \(F_{joint} = J^T F_{end}\) — map end-effector forces to joint torques - Kinematic control: Jacobian transpose method for position control

Examples

2-DOF Planar Robot

Scenario: Imagine a 2-link robot arm on a table. Given joint angles \(\theta_1\) and \(\theta_2\), where is the end-effector? The first link of length \(l_1\) rotates by \(\theta_1\) from the vertical, and the second link of length \(l_2\) rotates by \(\theta_2\) relative to the first link.

Mathematical Formulation:

\[x = l_1 \cos(\theta_1) + l_2 \cos(\theta_1 + \theta_2)$$ $$y = l_1 \sin(\theta_1) + l_2 \sin(\theta_1 + \theta_2)\]

Where: - \((x, y)\): end-effector position in the plane - \(l_1\): length of the first link - \(l_2\): length of the second link - \(\theta_1\): angle of the first joint from vertical (radians) - \(\theta_2\): angle of the second joint relative to the first link (radians)

Code Implementation:

import numpy as np


def compute_fk(l1: float, l2: float, theta1: float, theta2: float) -> tuple[float, float]:
    """
    Compute forward kinematics for a 2-DOF planar robot arm.

    Args:
        l1: Length of the first link (meters)
        l2: Length of the second link (meters)
        theta1: Angle of joint 1 from vertical (radians)
        theta2: Angle of joint 2 relative to link 1 (radians)

    Returns:
        Tuple of (x, y) end-effector coordinates
    """
    x1 = l1 * np.cos(theta1)
    y1 = l1 * np.sin(theta1)

    theta_total = theta1 + theta2

    x = x1 + l2 * np.cos(theta_total)
    y = y1 + l2 * np.sin(theta_total)

    return (x, y)


if __name__ == "__main__":
    l1, l2 = 1.0, 0.8
    test_cases = [
        (0.0, 0.0),
        (np.pi/2, 0.0),
        (np.pi/2, np.pi/2),
    ]

    print("Forward Kinematics Results:")
    for theta1, theta2 in test_cases:
        x, y = compute_fk(l1, l2, theta1, theta2)
        print(f"theta1={np.degrees(theta1):6.1f}, theta2={np.degrees(theta2):6.1f} -> x={x:.3f}, y={y:.3f}")

Code Breakdown: - Lines 458-459: Computes x and y coordinates of joint 1 using \(l_1 \cos(\theta_1)\) and \(l_1 \sin(\theta_1)\) - Line 461: Calculates the absolute angle of link 2 by adding joint angles: \(\theta_1 + \theta_2\) - Lines 463-464: Adds the second link's contribution: \(l_2 \cos(\theta_1 + \theta_2)\) and \(l_2 \sin(\theta_1 + \theta_2)\)

Output Example:

Forward Kinematics Results:
theta1=   0.0, theta2=   0.0 -> x= 1.800, y= 0.000
theta1=  90.0, theta2=   0.0 -> x= 0.000, y= 1.800
theta1=  90.0, theta2=  90.0 -> x= 0.000, y= 1.000

Practice Problems

1. Derive forward kinematics for a 3-DOF robot arm
2. Implement inverse kinematics for a SCARA robot
3. Compute Jacobian for a 6-DOF industrial manipulator

---

← Back to Index | Next: Dynamics →