Kinematics User Guide#

This guide covers the SerialManipulator class in ManipulaPy, which provides forward kinematics, inverse kinematics, and Jacobian computations for serial robot manipulators using modern screw theory.

What is Robot Kinematics?#

Robot kinematics studies the geometry of manipulator motion without considering forces. The key problems are:

  • Forward Kinematics (FK): Given joint angles → find end-effector pose

  • Inverse Kinematics (IK): Given desired pose → find joint angles

  • Jacobian Analysis: Relationship between joint velocities and end-effector motion

ManipulaPy uses the Product of Exponentials (PoE) formulation with screw theory for numerically stable computations.

Mathematical Foundation#

Product of Exponentials Formula#

The forward kinematics is computed using:

Space Frame:

\[T(\boldsymbol\theta) = \exp\!\bigl([\mathcal S_{1}]\;\theta_{1}\bigr)\, \exp\!\bigl([\mathcal S_{2}]\;\theta_{2}\bigr)\, \cdots\, \exp\!\bigl([\mathcal S_{n}]\;\theta_{n}\bigr)\,M\]

Body Frame:

\[T(\boldsymbol\theta) = M\, \exp\!\bigl([\mathcal B_{1}]\;\theta_{1}\bigr)\, \exp\!\bigl([\mathcal B_{2}]\;\theta_{2}\bigr)\, \cdots\, \exp\!\bigl([\mathcal B_{n}]\;\theta_{n}\bigr)\]

where

\[\begin{split}[\mathcal S] = \begin{bmatrix} [\boldsymbol\omega]_\times & \mathbf v \\ \mathbf0^T & 0 \end{bmatrix} \quad\text{and}\quad [\mathcal B] = \begin{bmatrix} [\boldsymbol\omega]_\times & \mathbf v \\ \mathbf0^T & 0 \end{bmatrix}\end{split}\]

with \([\boldsymbol\omega]_\times\) the usual 3×3 skew‐symmetric matrix of \(\boldsymbol\omega\).

Screw Axes#

Each joint’s motion is encoded as a 6-vector

\[\begin{split}\mathcal S = \begin{bmatrix}\boldsymbol\omega\\\mathbf v\end{bmatrix}\,\in\mathbb R^6\end{split}\]

with three canonical cases:

  • Revolute joint about axis \(\boldsymbol\omega\) through point \(\mathbf r\)

    \[\begin{split}\mathbf v = \mathbf r \times \boldsymbol\omega,\qquad \mathcal S = \begin{bmatrix} \boldsymbol\omega \\ \mathbf r \times \boldsymbol\omega \end{bmatrix}\end{split}\]

  • Prismatic joint translating along unit vector \(\mathbf v\)

    \[\begin{split}\mathcal S = \begin{bmatrix} \mathbf0 \\ \mathbf v \end{bmatrix},\quad \|\mathbf v\|=1\end{split}\]

  • General (helical) screw with pitch \(h\)

    \[\begin{split}\mathcal S = \begin{bmatrix} \boldsymbol\omega \\ \mathbf r \times \boldsymbol\omega + h\,\boldsymbol\omega \end{bmatrix}\end{split}\]

Jacobian Computation#

Space Jacobian for an n-DOF manipulator:

\[J_{s}(\boldsymbol\theta) = \bigl[\,\mathrm{Ad}_{T_{0}^{-1}(\boldsymbol\theta)}\,\mathcal S_{1}\;,\; \mathrm{Ad}_{T_{1}^{-1}(\boldsymbol\theta)}\,\mathcal S_{2}\;,\; \dots\;,\; \mathrm{Ad}_{T_{n-1}^{-1}(\boldsymbol\theta)}\,\mathcal S_{n}\bigr]\]

where

\[T_{i}(\boldsymbol\theta) = \exp\!\bigl([\mathcal S_{1}]\theta_{1}\bigr)\, \exp\!\bigl([\mathcal S_{2}]\theta_{2}\bigr)\,\cdots\, \exp\!\bigl([\mathcal S_{i}]\theta_{i}\bigr)\]

and

\[\begin{split}\mathrm{Ad}_{T} = \begin{bmatrix} R & \mathbf0 \\ [\mathbf p]_\times R & R \end{bmatrix} \quad\text{for}\quad T = \begin{bmatrix}R & \mathbf p\\\mathbf0^T&1\end{bmatrix}.\end{split}\]

Body Jacobian follows by

\[J_{b}(\boldsymbol\theta) = \mathrm{Ad}_{T(\boldsymbol\theta)^{-1}}\,J_{s}(\boldsymbol\theta)\]

Inverse Kinematics (Newton–Raphson / Damped Least Squares)#

Error twist:

\[\mathbf V_{\mathrm{error}} = \bigl(\log\!\bigl(T_{\mathrm{desired}}\,T(\boldsymbol\theta)^{-1}\bigr)\bigr)^\vee\]

Update step:

\[\boldsymbol\theta_{k+1} = \boldsymbol\theta_{k} + \alpha\,(J^{T}J + \lambda I)^{-1}J^{T}\,\mathbf V_{\mathrm{error}}\]

with damping \(\lambda\) for numerical stability.

SerialManipulator Class#

Constructor#

from ManipulaPy.kinematics import SerialManipulator

robot = SerialManipulator(
    M_list,           # Home configuration (4×4 matrix)
    omega_list,       # Joint axes (3×n matrix)
    r_list=None,      # Points on joint axes (optional)
    b_list=None,      # Body frame points (optional)
    S_list=None,      # Space frame screw axes (6×n matrix)
    B_list=None,      # Body frame screw axes (6×n matrix)
    G_list=None,      # Inertia matrices (for dynamics)
    joint_limits=None # Joint limits [(min, max), ...]
)

Key Parameters:

  • M_list: 4×4 transformation matrix representing the home pose

  • omega_list: 3×n matrix of joint rotation axes

  • S_list: 6×n matrix of space frame screw axes (auto-computed if not provided)

  • joint_limits: List of (min, max) tuples for each joint

Creating a Robot Model#

Manual Definition#

import numpy as np

# Example: 2-DOF planar robot
def create_2dof_planar_robot():
    """Create a simple 2-DOF planar RR robot."""

    # Link lengths
    L1, L2 = 0.5, 0.3

    # Home configuration (fully extended)
    M = np.array([
        [1, 0, 0, L1 + L2],
        [0, 1, 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
    ])

    # Space frame screw axes (both Z-axis rotations)
    S_list = np.array([
        # Joint 1 at origin
        [0, 0, 1, 0, 0, 0],
        # Joint 2 at (L1, 0, 0)
        [0, 0, 1, 0, -L1, 0]
    ]).T  # Shape: (6, 2)

    # Extract omega_list for constructor
    omega_list = S_list[:3, :]

    # Joint limits
    joint_limits = [(-np.pi, np.pi), (-np.pi, np.pi)]

    robot = SerialManipulator(
        M_list=M,
        omega_list=omega_list,
        S_list=S_list,
        joint_limits=joint_limits
    )

    return robot

# Create the robot
planar_robot = create_2dof_planar_robot()

Forward Kinematics#

Basic Forward Kinematics#

import numpy as np

# Define joint angles
theta = np.array([0.5, -0.3])  # radians

# Compute forward kinematics
T = robot.forward_kinematics(theta, frame="space")

# Extract position and orientation
position = T[:3, 3]
rotation_matrix = T[:3, :3]

print(f"End-effector position: {position}")
print(f"End-effector orientation:\n{rotation_matrix}")

Space vs Body Frames#

import numpy as np

theta = np.array([0.2, 0.3])

# Both methods give the same result
T_space = robot.forward_kinematics(theta, frame="space")
T_body = robot.forward_kinematics(theta, frame="body")

# Verify they're identical
error = np.linalg.norm(T_space - T_body)
print(f"Space vs Body frame error: {error:.2e}")  # Should be ~0

End-Effector Pose as Vector#

import numpy as np

# Get pose as [x, y, z, roll, pitch, yaw]
pose_vector = robot.end_effector_pose(theta)

position = pose_vector[:3]
euler_angles = pose_vector[3:]  # in radians

print(f"Position: {position}")
print(f"Orientation (degrees): {np.degrees(euler_angles)}")

Multiple Configurations#

import numpy as np

def test_multiple_configurations():
    """Test FK for multiple joint configurations."""

    # Test configurations
    test_configs = [
        np.array([0.0, 0.0]),      # Home position
        np.array([np.pi/4, -np.pi/4]),  # 45° configuration
        np.array([np.pi/2, 0.0]),      # Elbow up
        np.array([0.0, np.pi/2])       # Forearm up
    ]

    config_names = ["Home", "45° config", "Elbow up", "Forearm up"]

    for config, name in zip(test_configs, config_names):
        T = robot.forward_kinematics(config)
        pos = T[:3, 3]
        print(f"{name:12}: position = [{pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}]")

test_multiple_configurations()

Inverse Kinematics#

Basic Inverse Kinematics#

import numpy as np

# Define target pose
T_target = np.eye(4)
T_target[:3, 3] = [0.6, 0.2, 0.0]  # desired position

# Initial guess
theta_initial = np.array([0.0, 0.0])

# Solve inverse kinematics
solution, success, iterations = robot.iterative_inverse_kinematics(
    T_desired=T_target,
    thetalist0=theta_initial,
    eomg=1e-6,              # rotation error tolerance
    ev=1e-6,                # translation error tolerance
    max_iterations=1000
)

if success:
    print(f"✅ IK converged in {iterations} iterations")
    print(f"Solution: {np.degrees(solution)} degrees")

    # Verify solution
    T_achieved = robot.forward_kinematics(solution)
    error = np.linalg.norm(T_achieved[:3, 3] - T_target[:3, 3])
    print(f"Position error: {error:.6f} m")
else:
    print("❌ IK failed to converge")

Advanced IK Options#

# More robust IK with damping
solution, success, iterations = robot.iterative_inverse_kinematics(
    T_desired=T_target,
    thetalist0=theta_initial,
    eomg=1e-6,
    ev=1e-6,
    max_iterations=1000,
    plot_residuals=True,    # Save convergence plot
    # New solver knobs (optional; off by default on iterative_inverse_kinematics)
    weight_position=1.5,    # emphasis on translational error in the solve step
    weight_orientation=1.0, # emphasis on rotational error
    adaptive_tuning=True,   # adapt damping/step size when error stalls
    backtracking=True,      # simple line search over scaled steps
)

Key IK tuning parameters:

  • damping: Damped least-squares regularization (stability near singularities).

  • step_cap: Maximum norm of \(\Delta\theta\) per iteration.

  • weight_position / weight_orientation: Scale translational vs rotational error in the solve.

  • adaptive_tuning: Adjusts damping/step_cap on the fly when progress is slow.

  • backtracking: Tries scaled steps (1.0, 0.5, 0.25) to pick the best error reduction.

smart_inverse_kinematics enables adaptive/backtracking by default and accepts the same weighting knobs. You can also supply a cache to reuse good solutions:

import numpy as np
from ManipulaPy.ik_helpers import IKInitialGuessCache

cache = IKInitialGuessCache(max_size=200)
T_target = np.eye(4)
T_target[:3, 3] = [0.6, 0.1, 0.0]

theta, success, iters = robot.smart_inverse_kinematics(
    T_target,
    strategy="cached",   # try cache first, then fall back to workspace guess
    cache=cache,
    weight_position=1.5,
    weight_orientation=1.0,
)

if success:
    cache.add(T_target, theta)  # store solution with residuals for future queries

Multiple IK Solutions#

import numpy as np

def find_multiple_solutions(robot, target_pos, n_attempts=5):
    """Find multiple IK solutions for the same target."""

    T_target = np.eye(4)
    T_target[:3, 3] = target_pos

    solutions = []

    for attempt in range(n_attempts):
        # Random initial guess
        joint_limits = np.array(robot.joint_limits)
        theta_init = np.random.uniform(joint_limits[:, 0], joint_limits[:, 1])

        solution, success, _ = robot.iterative_inverse_kinematics(
            T_desired=T_target,
            thetalist0=theta_init,
            max_iterations=500
        )

        if success:
            # Check if this is a new solution
            is_new = True
            for existing_sol in solutions:
                if np.linalg.norm(solution - existing_sol) < 0.1:
                    is_new = False
                    break

            if is_new:
                solutions.append(solution)
                print(f"Solution {len(solutions)}: {np.degrees(solution)}")

    return solutions

# Test multiple solutions
target = [0.5, 0.3, 0.0]
multiple_solutions = find_multiple_solutions(robot, target)
print(f"Found {len(multiple_solutions)} distinct solutions")

Jacobian Matrix#

Computing the Jacobian#

import numpy as np

theta = np.array([0.3, -0.2])

# Compute Jacobian in space frame
J_space = robot.jacobian(theta, frame="space")

# Compute Jacobian in body frame
J_body = robot.jacobian(theta, frame="body")

print(f"Space Jacobian shape: {J_space.shape}")  # (6, n_joints)
print(f"Body Jacobian shape: {J_body.shape}")    # (6, n_joints)

Jacobian Analysis#

import numpy as np

def analyze_jacobian(robot, theta):
    """Analyze Jacobian properties at a configuration."""

    J = robot.jacobian(theta)

    # Basic properties
    rank = np.linalg.matrix_rank(J)
    condition_number = np.linalg.cond(J)

    print(f"Jacobian Analysis:")
    print(f"  Shape: {J.shape}")
    print(f"  Rank: {rank} (full rank: {rank == min(J.shape)})")
    print(f"  Condition number: {condition_number:.2e}")

    # Singularity check
    if condition_number > 1e6:
        print("  ⚠️  Configuration is near singular!")
    else:
        print("  ✅ Configuration is well-conditioned")

    # Manipulability (for square Jacobians)
    if J.shape[0] == J.shape[1]:
        manipulability = abs(np.linalg.det(J))
        print(f"  Manipulability: {manipulability:.6f}")
    else:
        # For non-square Jacobians
        manipulability = np.sqrt(np.linalg.det(J @ J.T))
        print(f"  Manipulability: {manipulability:.6f}")

    return J, condition_number

# Analyze current configuration
J, cond_num = analyze_jacobian(robot, theta)

Velocity Kinematics#

End-Effector Velocity#

import numpy as np

# Current configuration and joint velocities
theta = np.array([0.2, 0.3])
theta_dot = np.array([0.1, -0.2])  # rad/s

# Compute end-effector velocity
V_ee = robot.end_effector_velocity(theta, theta_dot, frame="space")

print(f"Joint velocities: {theta_dot} rad/s")
print(f"End-effector velocity: {V_ee}")

# Decompose spatial velocity
linear_velocity = V_ee[:3]    # [vx, vy, vz]
angular_velocity = V_ee[3:]   # [ωx, ωy, ωz]

print(f"Linear velocity: {linear_velocity} m/s")
print(f"Angular velocity: {angular_velocity} rad/s")

Joint Velocities from Desired Motion#

import numpy as np

# Desired end-effector motion
V_desired = np.array([0.1, 0.0, 0.0, 0.0, 0.0, 0.2])  # Move +X, rotate +Z

# Compute required joint velocities
theta_dot_required = robot.joint_velocity(theta, V_desired, frame="space")

print(f"Desired EE velocity: {V_desired}")
print(f"Required joint velocities: {theta_dot_required} rad/s")

# Verify by forward computation
V_achieved = robot.end_effector_velocity(theta, theta_dot_required, frame="space")
error = np.linalg.norm(V_achieved - V_desired)
print(f"Velocity error: {error:.2e}")

State Management#

Robot State Tracking#

# Update robot state
robot.update_state(
    joint_positions=theta,
    joint_velocities=theta_dot
)

# Access current state
print(f"Current positions: {robot.joint_positions}")
print(f"Current velocities: {robot.joint_velocities}")

# Compute current end-effector state
current_pose = robot.end_effector_pose(robot.joint_positions)
current_velocity = robot.end_effector_velocity(
    robot.joint_positions,
    robot.joint_velocities
)

print(f"Current EE pose: {current_pose}")
print(f"Current EE velocity: {current_velocity}")

Working Examples#

Complete Workflow Example#

import numpy as np

def complete_kinematics_example():
    """Complete example showing all kinematic functions."""

    # Create a simple 3-DOF robot
    def create_3dof_robot():
        L1, L2, L3 = 0.3, 0.25, 0.15

        M = np.array([
            [1, 0, 0, L1 + L2 + L3],
            [0, 1, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
        ])

        S_list = np.array([
            [0, 0, 1, 0, 0, 0],
            [0, 0, 1, 0, -L1, 0],
            [0, 0, 1, 0, -(L1+L2), 0]
        ]).T

        omega_list = S_list[:3, :]
        joint_limits = [(-np.pi, np.pi)] * 3

        return SerialManipulator(
            M_list=M,
            omega_list=omega_list,
            S_list=S_list,
            joint_limits=joint_limits
        )

    robot = create_3dof_robot()
    print("=== Complete Kinematics Example ===")

    # 1. Forward Kinematics
    theta = np.array([0.5, -0.3, 0.8])
    T = robot.forward_kinematics(theta)
    print(f"1. Forward Kinematics:")
    print(f"   Joint angles: {np.degrees(theta)} deg")
    print(f"   End-effector position: {T[:3, 3]}")

    # 2. Inverse Kinematics
    T_target = np.eye(4)
    T_target[:3, 3] = [0.4, 0.2, 0.0]

    solution, success, iterations = robot.iterative_inverse_kinematics(
        T_desired=T_target,
        thetalist0=np.array([0.0, 0.0, 0.0])
    )

    print(f"\n2. Inverse Kinematics:")
    print(f"   Target position: {T_target[:3, 3]}")
    print(f"   Success: {success}")
    if success:
        print(f"   Solution: {np.degrees(solution)} deg")
        print(f"   Iterations: {iterations}")

    # 3. Jacobian Analysis
    J = robot.jacobian(theta)
    cond_num = np.linalg.cond(J)

    print(f"\n3. Jacobian Analysis:")
    print(f"   Shape: {J.shape}")
    print(f"   Condition number: {cond_num:.2e}")

    # 4. Velocity Kinematics
    theta_dot = np.array([0.1, -0.2, 0.3])
    V_ee = robot.end_effector_velocity(theta, theta_dot)

    print(f"\n4. Velocity Kinematics:")
    print(f"   Joint velocities: {theta_dot} rad/s")
    print(f"   EE linear velocity: {V_ee[:3]} m/s")
    print(f"   EE angular velocity: {V_ee[3:]} rad/s")

    return robot

# Run the complete example
example_robot = complete_kinematics_example()

Common Issues and Solutions#

Troubleshooting IK Convergence#

import numpy as np

def troubleshoot_ik(robot, T_target):
    """Helper function to troubleshoot IK issues."""

    print("🔍 IK Troubleshooting:")

    # Check if target is reasonable
    target_pos = T_target[:3, 3]
    target_distance = np.linalg.norm(target_pos)

    print(f"Target position: {target_pos}")
    print(f"Target distance from origin: {target_distance:.3f} m")

    # Try different initial guesses
    joint_limits = np.array(robot.joint_limits)

    attempts = [
        np.zeros(len(joint_limits)),                          # Zero guess
        np.mean(joint_limits, axis=1),                       # Middle of ranges
        np.random.uniform(joint_limits[:, 0], joint_limits[:, 1])  # Random
    ]

    attempt_names = ["Zero", "Middle", "Random"]

    for i, (theta_init, name) in enumerate(zip(attempts, attempt_names)):
        solution, success, iterations = robot.iterative_inverse_kinematics(
            T_desired=T_target,
            thetalist0=theta_init,
            max_iterations=500
        )

        if success:
            T_achieved = robot.forward_kinematics(solution)
            error = np.linalg.norm(T_achieved[:3, 3] - target_pos)
            print(f"  {name} init: ✅ Success (error: {error:.2e}, iter: {iterations})")
            return solution
        else:
            print(f"  {name} init: ❌ Failed after {iterations} iterations")

    print("  All attempts failed. Target may be unreachable.")
    return None

Best Practices#

  1. Robot Definition

    • Use URDF files when possible for real robots

    • Validate screw axes are unit vectors for revolute joints

    • Set realistic joint limits

  2. Forward Kinematics

    • Both space and body frames give identical results

    • Choose the frame that makes your calculations easier

  3. Inverse Kinematics

    • Provide good initial guesses (avoid singularities)

    • Use damping for stability near singularities

    • Try multiple initial guesses for difficult targets

  4. Jacobian Analysis

    • Monitor condition number to detect singularities

    • Use velocity kinematics for real-time control

  5. Performance

    • Cache Jacobian computations when configuration doesn’t change

    • Use appropriate tolerances (don’t over-specify)

Next Steps#

API Reference#

For complete function documentation: Kinematics API Reference