# Kinematics User Guide

This chapter walks you through everyday kinematics workflows in ManipulaPy: from building a SerialManipulator object, to computing forward & inverse kinematics, Jacobians, workspace envelopes, and velocity mappings. It assumes you already ran the :doc:../installation guide and have a working Python interpreter.

## Prerequisites

  • Python ≥ 3.8 with numpy & matplotlib

  • ManipulaPy ≥ 1.1.0

  • A basic grasp of screw theory / the Product‑of‑Exponentials (PoE) model.

If you need a refresher, see Modern Robotics, Ch 3_ (free online PDF) or our :doc:../theory/screw_theory page.

## What is robot kinematics?

Robot kinematics studies the geometry of a manipulator without worrying about forces or torques. The central problems are:

  • Forward kinematics (FK): joint angles → pose of the tool frame :math:T in mathrm{SE}(3).

  • Inverse kinematics (IK): desired pose → one or more joint angle solutions.

  • Velocity kinematics: joint‑space rates ↔ spatial twist of the end‑effector.

ManipulaPy models every rigid motion as an element of :math:mathrm{SE}(3) cong mathbb{R}^3 times mathrm{SO}(3) using homogeneous transforms and encodes joint axes as screws :math:S_i in mathfrak{se}(3).

Forward kinematics via PoE

```

With screws arranged column‑wise in \(S_{list} \in \mathbb{R}^{6\times n}\) and a home configuration matrix \(M\), the FK map is

\[T(\theta)\;=\;e^{[S_1] \theta_1}\;e^{[S_2] \theta_2}\;\dots e^{[S_n] \theta_n}\,M.\]

ManipulaPy evaluates this with numerically stable Rodrigues formulas.

Quick‑start example

Define a 6‑DoF UR5‑like arm in 10 lines
import numpy as np
from ManipulaPy.kinematics import SerialManipulator

# Home pose of tool frame (millimetres → metres)
M = np.array([[1, 0, 0, 0.817],
              [0, 1, 0, 0.000],
              [0, 0, 1, 0.191],
              [0, 0, 0, 1     ]])

# Screw axes in the *space* frame (ω | v)
S_list = np.array([
    [0,  0,  1,  0.000,  0.000, 0.000],
    [0, -1,  0, -0.089,  0.000, 0.000],
    [0, -1,  0, -0.089,  0.000, 0.425],
    [0, -1,  0, -0.089,  0.000, 0.817],
    [1,  0,  0,  0.000,  0.109, 0.000],
    [0, -1,  0, -0.089,  0.000, 0.817],
]).T

robot = SerialManipulator(M_list=M, S_list=S_list)  # B_list autocomputed

thetalist = np.deg2rad([30, -45, 10, 120, 0, 90])
T = robot.forward_kinematics(thetalist)
print(np.round(T, 3))

Creating a robot from URDF

Most users will load a real robot description:

from ManipulaPy.urdf_processor import URDFToSerialManipulator

urdf_path = "resources/urdfs/panda_arm.urdf"
robot = URDFToSerialManipulator(urdf_path).serial_manipulator

print(robot.n_joints)          # 7
print(robot.joint_limits[:3])  # per‑joint lower/upper bounds

ManipulaPy parses <limit> tags, inertias, and collision geometry so the same object can be reused for dynamics and planning.

Forward kinematics recipes

Single pose

T_B = robot.forward_kinematics(thetalist, frame="body")
xyz = T_B[:3, 3]
R   = T_B[:3, :3]      # orientation matrix

Batch evaluation

If you need thousands of FK calls per frame, wrap them in NumPy arrays:

thetas = np.random.uniform(-np.pi, np.pi, (1024, robot.n_joints))
poses  = robot.batch_forward_kinematics(thetas)  # returns (1024, 4, 4)

(The batch routine is CuPy‑accelerated when installed with pip install ManipulaPy[gpu-cuda11].)

Inverse kinematics

ManipulaPy supplies four flavours:

  1. Newton–Raphson (iterative_inverse_kinematics) – default, robust.

  2. Levenberg–Marquardt (lm_inverse_kinematics) – higher success near singularities.

  3. Position‑only IK (position_inverse_kinematics) – ignores orientation.

  4. Null‑space IK (redundancy resolution) – exposed via NullSpaceIKSolver.

Example (basic Newton):

success, sol, _ = robot.iterative_inverse_kinematics(
    T_desired=T, thetalist0=np.zeros(robot.n_joints))

assert success

Jacobian & singularities

J = robot.jacobian(thetalist, frame="space")  # (6 × n)
manipulability = np.sqrt(np.linalg.det(J @ J.T))
cond_number   = np.linalg.cond(J)

A warning is raised if cond_number > 1e6 (config near singularity).

Velocity kinematics helper

θdot   = np.deg2rad([10, 5, 0, 0, 0, 0])      # joint rates [rad s⁻¹]
V_s    = robot.end_effector_velocity(thetalist, θdot)  # spatial twist

# Solve inverse velocity problem
V_des  = np.array([0.1, 0, 0, 0, 0, 0.2])     # move +X & rotate +Z
θdot_r = robot.joint_velocity(thetalist, V_des)

Workspace visualisation snippet

Scatter plot of reachable positions (CPU‑only)
import matplotlib.pyplot as plt

pts = robot.sample_workspace(n_samples=500)
fig = plt.figure(); ax = fig.add_subplot(111, projection="3d")
ax.scatter(*pts.T, s=3, alpha=.4)
ax.set_xlabel("X [m]"); ax.set_ylabel("Y [m]"); ax.set_zlabel("Z [m]")

Troubleshooting

Symptom

Fix

IK fails to

  • Check if the target pose is outside joint

converge

  limits or workspace. * Provide a better initial guess; use the

smart_initial_guess helper below.

Numerical

  • Call IK with damping > 1e‑4 or switch to

instability

lm_inverse_kinematics.

Joint limit

violation
Smart initial guess helper
def smart_initial_guess(robot, T_desired):
    try:
        return robot.current_configuration   # live robot state
    except AttributeError:
        # fallback: mid‑range of each joint
        return np.mean(robot.joint_limits, axis=1)