Robot Kinematics & Motion Planning: The Ultimate Guide

There is a moment in every robotics project where someone points at a coffee mug on a table and says "just make the arm pick that up." Between that sentence and a stream of joint torques sits a stack of mathematics that has been refined for fifty years and is still, in 2026, where most of the hard bugs live. The mug has a pose in the world. The arm has six or seven joints. Somewhere in between you need transforms, an inverse-kinematics solve, a collision-free path through a cluttered scene, a time-parameterized trajectory that respects velocity and jerk limits, and a controller fast enough to track it. Each layer has its own failure modes, and each one quietly assumes the layer below it is correct.

This guide is the long version for the people who build that stack — controls engineers, motion-planning developers, and the advanced makers who have outgrown copy-pasting move_group examples and want to know why the IK solver returns garbage near a singularity, or why their beautifully smooth path takes 9 seconds when the hardware could do it in 3. We'll go layer by layer: rigid-body transforms, forward kinematics, the Jacobian, inverse kinematics, singularities, redundancy, configuration space, sampling-based and optimization-based planning, trajectory generation, and finally how it all fits together in MoveIt 2 and the real ROS 2 stack. Math with units, code you can read, and opinions with reasons.

The take: Kinematics is a solved problem and you should treat it as one — use Pinocchio or KDL, not hand-rolled DH matrices, for anything that ships. Motion planning is not solved; it is a collection of tools with sharp trade-offs, and the single biggest mistake teams make is reaching for a sampling-based planner when the problem actually wants trajectory optimization (or vice versa). Get the representation right — frames, quaternions, C-space — and 80% of the "planning bugs" evaporate, because they were never planning bugs. They were transform bugs wearing a costume.

Companion reading: industrial robot arms, real-time control systems, ROS 2, and collaborative robots (cobots).

Table of contents

1. Key takeaways
2. The motion stack: from goal to joint commands
3. Rigid-body pose & transforms
4. Forward kinematics
5. The Jacobian
6. Inverse kinematics
7. Singularities
8. Redundancy & redundancy resolution
9. Configuration space & the planning problem
10. Sampling-based planning
11. Optimization-based planning
12. Trajectory generation
13. Putting it together in practice
14. Frequently asked questions

The motion stack: from goal to joint commands

A useful mental model: motion software is a stack of four layers, each converting a goal in its language into a goal in the language of the layer below.

Layer 1 — Kinematics. The geometry. Given joint angles, where is the tool (forward kinematics)? Given a desired tool pose, what joint angles get you there (inverse kinematics)? Given joint velocities, how fast is the tool moving (the Jacobian)? No time, no dynamics, no collisions — pure geometry of frames and links.

Layer 2 — Planning. Find a path: an ordered sequence of configurations from start to goal that is collision-free and respects joint limits. A path is a curve in configuration space, parameterized by a unitless path coordinate s ∈ [0,1]. It says where to go, not how fast.

Layer 3 — Trajectory generation. Take the geometric path and attach time. Produce position, velocity, and acceleration as functions of t — q(t), q̇(t), q̈(t) — respecting the robot's velocity, acceleration, and jerk limits. This is where a path becomes executable.

Layer 4 — Control. Track the trajectory on real hardware: a feedback loop (PID, computed-torque, impedance) running at 1–10 kHz that turns the reference q(t) and the measured state into motor currents. This is the domain of real-time control and ultimately FOC motor controllers closing the current loop on each joint.

Rule: Each layer assumes the one below it works. If your robot jerks, don't start in the planner — verify the controller tracks a hand-fed trajectory first, then the trajectory respects limits, then the path is sane. Debug bottom-up, design top-down.

Task space vs joint space

Two coordinate systems dominate everything that follows.

Joint space (configuration space, C-space) is the vector of joint values q = [q₁, …, qₙ]. For a 6-axis arm, that's six angles in radians. Motion in joint space is trivially feasible — every point is reachable by definition — but the tool traces a complicated, hard-to-predict curve through Cartesian space.

Task space (operational space, Cartesian space) is the pose of the end-effector: position p ∈ ℝ³ plus orientation, living in SE(3). Motion in task space is intuitive — "move 10 cm in +X, keep the tool level" — but every Cartesian point must be mapped back to joint space through inverse kinematics, and not every Cartesian path is reachable (singularities, joint limits, the boundary of the workspace).

The art of motion software is knowing which space to plan in. Free-space "get from A to B" moves plan in joint space (fast, always feasible). Process moves — welding a seam, dispensing a bead, dragging a tool across a surface — must hold a Cartesian path, so they plan in task space and pay the IK and singularity tax.

Rigid-body pose & transforms

Everything downstream rests on representing where things are. A pose is a position plus an orientation — six numbers of information (3 translation + 3 rotation), living in the group SE(3).

Frames and homogeneous transforms

We attach a coordinate frame to every interesting body: the world, the robot base, each link, the tool flange, the camera, the workpiece. A pose is always relative: "the tool in the base frame," written T_base_tool or {}^{base}T_{tool}. The frame-subscript convention is worth its weight in gold; the single most common transform bug is composing two transforms in the wrong order or the wrong frame, and disciplined subscripts catch it.

A pose is encoded as a homogeneous transformation matrix — a 4×4 that packs a 3×3 rotation R and a 3×1 translation t:

T = | R   t |     R ∈ SO(3) (3x3 rotation), t ∈ R^3 (translation)
    | 0   1 |     bottom row [0 0 0 1]

      | r11 r12 r13 | tx |
T  =  | r21 r22 r23 | ty |
      | r31 r32 r33 | tz |
      |  0   0   0  |  1 |

The magic of the homogeneous form is composition by multiplication. To find the tool in the world when you know the tool in the base and the base in the world:

T_world_tool = T_world_base @ T_base_tool      # chain rule for frames

And to transform a point p = [x, y, z, 1]ᵀ (note the homogeneous 1) from the tool frame into the world frame, you left-multiply: p_world = T_world_base @ T_base_tool @ p_tool. Inverting a transform is cheap and structured — you don't do a general 4×4 inverse:

T^-1 = | R^T   -R^T @ t |     transpose the rotation, negate-and-rotate the translation
       |  0        1    |

Rule: Never invert a homogeneous transform with a generic matrix inverse. Use the closed form above — it's exact, faster, and immune to numerical drift in the rotation block.

Rotation representations, and why quaternions win

Translation is uncontroversial — three numbers, done. Orientation is where engineers bleed, because SO(3) is a curved 3-dimensional manifold and there is no perfect 3-number parameterization of it. The four representations you'll meet:

Representation	Numbers	Pros	Cons	Use it for
Rotation matrix	9 (6 constraints)	No singularities; direct transform; composes by matmul	Redundant; drifts off SO(3) under integration; needs re-orthonormalization	Internal math, FK chains
Euler angles (RPY, ZYX, …)	3	Human-readable; compact	Gimbal lock; order-dependent; ambiguous; bad for interpolation	Display, teach pendants, config files
Axis-angle (θ·ê)	3 (or 4)	Minimal; natural for rotation error; maps to angular velocity	Singular at θ=0 and θ=π; awkward to compose	IK error terms, exponential coordinates
Quaternion (unit)	4 (1 constraint)	No gimbal lock; cheap compose; clean SLERP; numerically stable	Double cover (q and −q same rotation); not human-readable	Orientation state, interpolation, controllers

Gimbal lock is the killer for Euler angles. When the middle rotation hits ±90°, the first and third axes align and you lose a degree of freedom — the orientation is fine, but the rates become singular and small Cartesian changes demand huge angle changes. It's the same pathology as a kinematic singularity, one level down.

A unit quaternion q = (w, x, y, z) with w² + x² + y² + z² = 1 encodes a rotation of angle θ about unit axis ê as q = (cos(θ/2), sin(θ/2)·ê). Composition is the quaternion product (≈16 multiplies, cheaper than 3×3 matmul), rotating a vector is v' = q ⊗ v ⊗ q*, and — crucially — interpolating between two orientations is SLERP (spherical linear interpolation), which traces the shortest geodesic on the sphere at constant angular velocity. There is no equivalent of gimbal lock anywhere.

Rule: Store and integrate orientation as a unit quaternion (renormalize each step). Convert to a matrix only to build a transform, and to Euler only to show a human. If your controller integrates Euler angles, you have a latent gimbal-lock bug waiting for the wrong pose.

The one wrinkle: the double cover. q and −q represent the same rotation, so when you compute an orientation error or interpolate, check the sign and flip if the dot product is negative — otherwise SLERP takes the long way around (the 359° path instead of the 1° one).

Forward kinematics

Forward kinematics (FK) answers: given joint values q, what is the end-effector pose T_base_ee? It is the easy direction — always a unique answer, no iteration, just multiplication.

The transform chain

A serial manipulator is a chain of links connected by joints. Each joint i contributes a transform T_{i-1}_i(qᵢ) that depends on its joint value. Stack them:

T_base_ee = T_0_1(q1) @ T_1_2(q2) @ ... @ T_(n-1)_n(qn)

That's it conceptually. The only question is how you express each T_{i-1}_i.

Denavit–Hartenberg parameters

The classic encoding is Denavit–Hartenberg (DH): four parameters per joint that fully describe the relationship between consecutive link frames. Using the modified DH convention (Craig), the four parameters are aᵢ₋₁ (link length), αᵢ₋₁ (link twist), dᵢ (link offset), and θᵢ (joint angle). For a revolute joint, θᵢ is the variable; for a prismatic joint, dᵢ is.

The per-joint transform expands to:

import numpy as np

def dh_transform(a, alpha, d, theta):
    """Modified DH (Craig) link transform T_{i-1}_i."""
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)
    return np.array([
        [    ct,    -st,   0.0,      a   ],
        [ st*ca,  ct*ca,   -sa,  -sa*d   ],
        [ st*sa,  ct*sa,    ca,   ca*d   ],
        [   0.0,    0.0,   0.0,    1.0   ],
    ])

def forward_kinematics(dh_table, q):
    """dh_table: list of (a, alpha, d, theta_offset). q: joint angles (rad)."""
    T = np.eye(4)
    for (a, alpha, d, theta0), qi in zip(dh_table, q):
        T = T @ dh_transform(a, alpha, d, theta0 + qi)
    return T   # T_base_ee: read R from T[:3,:3], position from T[:3,3]

A worked fragment: a generic 6-axis arm with a spherical wrist (a KUKA/ABB/UR-class geometry) has a DH table whose first three rows position the wrist center and whose last three (with a = d = 0 for the intersecting wrist axes) set orientation. Plug in q = [0,0,0,0,0,0] and you read off the home pose; perturb q₁ and only the columns rotating about the base axis change. This is exactly the chain a teach pendant computes thousands of times a second to display the TCP position.

Opinion with a reason: Don't hand-derive DH tables for production code in 2026. They're error-prone (sign conventions, standard vs modified, offset bookkeeping) and they only describe serial chains. Author your robot once in URDF and let a library walk the chain. DH is worth learning so you can read papers and debug, not so you can type 24 numbers and an off-by-π error into a config file.

What the libraries do instead

Pinocchio (the spatial-algebra library behind a lot of modern whole-body control) and KDL (the Kinematics and Dynamics Library that ships in ROS) both parse URDF into a kinematic tree and compute FK — and the Jacobian, and dynamics — via recursive spatial-vector algorithms. Pinocchio is the faster, more modern choice: it computes FK for a humanoid in single-digit microseconds and gives you analytical derivatives, which matters enormously when you get to optimization-based planning and MPC. KDL is older, slower, and still everywhere because MoveIt 2 ships it as the default kinematics plugin. RBDL and Drake's multibody engine round out the field.

The Jacobian

If FK is the single most-used kinematic operation, the Jacobian is the most important, because it's the bridge between joint rates and Cartesian rates — and, by transpose, between Cartesian forces and joint torques.

From joint velocities to end-effector twist

Differentiate FK with respect to the joint vector and you get the Jacobian J(q), a 6×n matrix that maps joint velocities to the end-effector twist (stacked linear and angular velocity):

| v |   = J(q) @ q_dot        v in R^3 (linear vel, m/s)
| w |                         w in R^3 (angular vel, rad/s)
                              q_dot in R^n (joint rates, rad/s)
J is 6 x n :  top 3 rows -> linear (Jv), bottom 3 rows -> angular (Jw)

For a revolute joint i, the geometric Jacobian column has a beautifully simple form. Let zᵢ be the joint's rotation axis (in the base frame) and pᵢ its origin; let pₑ be the end-effector position:

Ji = | z_i x (p_e - p_i) |     linear part: axis crossed into the lever arm
     |        z_i         |     angular part: just the axis

You can read the whole geometric Jacobian straight off the FK transforms — every zᵢ and pᵢ is a column of an intermediate T_base_i you already computed. That's why libraries hand you FK and the Jacobian together.

Force mapping: the transpose identity

Here is the identity that earns the Jacobian its keep. By the principle of virtual work, the joint torques τ required to exert an end-effector wrench F (force + moment) are:

tau = J^T @ F        tau in R^n (joint torques, N*m)
                     F = [fx fy fz mx my mz]^T  (wrench, N and N*m)

No new computation — you already have J. This single line is the foundation of Cartesian impedance control, gravity compensation done in task space, and the leg force control that makes a quadruped springy and compliant: command a virtual Cartesian force at the foot, push it through Jᵀ, and you have the joint torques. It's also how cobots feel forces — they don't all have wrist force/torque sensors; many estimate the external wrench from joint-torque residuals via (Jᵀ)⁺.

Geometric vs analytic Jacobian

A subtlety that trips people up. The geometric Jacobian maps to a twist where the angular part is the body's angular velocity vector ω. The analytic Jacobian maps to the time-derivative of whatever orientation representation you chose (Euler-angle rates, quaternion rates). They differ by a representation-dependent transform on the angular block:

J_analytic = | I      0    |  @  J_geometric      where E(phi) relates orientation-rate to omega
             | 0   E(phi)^-1|

When your task error is expressed as a quaternion or Euler error, you may need the analytic form — and E(φ) itself becomes singular at the same gimbal-lock configurations, which is one more reason orientation error is best expressed in axis-angle / so(3) terms where the geometric Jacobian applies directly.

Rule: Use the geometric Jacobian with axis-angle (rotation-vector) orientation error. It's the cleanest, has no representation singularity, and matches what KDL/Pinocchio return by default.

Inverse kinematics

Inverse kinematics (IK) is the hard direction: given a desired end-effector pose T*, find joint values q such that FK(q) = T*. Unlike FK, IK can have zero, one, finite, or infinite solutions, and finding them is where real engineering lives.

Analytic vs numerical

Analytic (closed-form) IK solves the kinematic equations algebraically for a specific robot geometry. A 6-DoF arm with a spherical wrist (the last three axes intersect at a point) decouples beautifully: the wrist center's position depends only on the first three joints (solve by geometry — a planar trig problem giving up to 4 position solutions), and orientation falls to the last three (another set, giving up to 8 total). Closed-form IK is fast (microseconds), exact, and returns all solutions — but it must be derived per robot. IKFast (from OpenRAVE) auto-generates C++ closed-form solvers from a kinematic description and is the gold standard when your geometry supports it.

Numerical IK iterates from a seed, using the Jacobian to descend the pose error toward zero. It's general (any chain, any DoF, extra constraints), but it's iterative (slower), needs a seed, finds only one solution (the one nearest the seed), and can fail to converge near singularities or when the target is unreachable.

IK method	Type	Speed	Solutions	Robustness near singularity	When to use
Analytic / IKFast	Closed-form	µs, fastest	All (up to 8 for 6R)	Exact but solution branches collapse	6-DoF arms with spherical wrist; anything real-time and well-conditioned
Jacobian transpose	Numerical	Slow (many iters)	One (near seed)	Stable but slow, no inverse needed	Quick/dirty, embedded, when you can't invert
Jacobian pseudoinverse	Numerical	Medium	One (near seed)	Blows up — velocities → ∞	Well-conditioned redundant arms away from singularities
Damped least squares (DLS / Levenberg)	Numerical	Medium	One (near seed)	Robust — bounded velocities	Default numerical choice; near-singularity safe
bio_ik / optimization	Numerical (global-ish)	Slower	One (best)	Robust; handles many goals	Redundant arms, multiple soft goals, awkward geometries

The Jacobian-based update step

All numerical methods share a loop: compute the pose error, map it to a joint update with some inverse of the Jacobian, step, repeat. The differences are entirely in how you "invert" J.

Jacobian transpose uses Δq = α·Jᵀ·e. It's gradient descent on the squared pose error — provably converges, needs no matrix inverse, but slowly and with a hand-tuned α.

Pseudoinverse uses Δq = J⁺·e where J⁺ = Jᵀ(JJᵀ)⁻¹. It takes the minimum-norm joint step that achieves the desired Cartesian step. It's fast and exact — until you approach a singularity, where JJᵀ becomes ill-conditioned, its inverse explodes, and the solver demands enormous joint velocities that the hardware can't deliver and you wouldn't want if it could.

Damped least squares (DLS), also called the Levenberg–Marquardt method, fixes exactly this by adding a damping term λ²I:

import numpy as np

def ik_dls(fk, jac, q0, T_target, lam=0.05, iters=100, tol=1e-4):
    """Damped least squares IK. fk(q)->4x4 pose, jac(q)->6xn Jacobian."""
    q = np.array(q0, dtype=float)
    for _ in range(iters):
        T = fk(q)
        e = pose_error(T, T_target)        # 6-vec: [pos_err(3); axis_angle_err(3)]
        if np.linalg.norm(e) < tol:
            return q, True
        J = jac(q)                          # 6 x n geometric Jacobian
        # DLS step:  dq = J^T (J J^T + lam^2 I)^-1 e
        JJt = J @ J.T
        dq = J.T @ np.linalg.solve(JJt + (lam**2) * np.eye(6), e)
        q = q + dq
    return q, False

def pose_error(T, T_target):
    p_err = T_target[:3, 3] - T[:3, 3]                 # position error (m)
    R_err = T_target[:3, :3] @ T[:3, :3].T             # orientation error matrix
    # log map of R_err -> axis-angle (rotation vector), small-angle safe
    angle = np.arccos(np.clip((np.trace(R_err) - 1) / 2, -1.0, 1.0))
    if angle < 1e-9:
        w_err = np.zeros(3)
    else:
        w_err = (angle / (2 * np.sin(angle))) * np.array([
            R_err[2,1] - R_err[1,2],
            R_err[0,2] - R_err[2,0],
            R_err[1,0] - R_err[0,1],
        ])
    return np.concatenate([p_err, w_err])

The damping λ is the knob: small λ (≈0.01) means accurate tracking but fragile near singularities; large λ (≈0.1–0.5) means rock-solid stability but sloppy tracking. The clever production trick is adaptive damping — keep λ small when well-conditioned (use the smallest singular value of J as the trigger) and ramp it up only as you approach a singularity. That's what KDL's ChainIkSolverPos_LMA does, and it's why DLS is the sane default for general numerical IK.

Rule: Reach for closed-form (IKFast) when your robot's geometry allows it and you need every solution. Reach for DLS for everything else. Plain pseudoinverse without damping is a footgun — it works in demos and explodes in the field.

Multiple solutions and branch selection

A 6R arm typically has up to 8 IK solutions for a reachable pose: elbow up/down, wrist flip/no-flip, shoulder left/right. Analytic solvers return all of them; you then pick one. The wrong pick produces a "flip" — the robot lunges through a wild reconfiguration to reach the next waypoint because the solver jumped solution branches between points. Standard practice: among valid solutions, choose the one closest (in weighted joint distance) to the current configuration, and keep that branch across a trajectory unless forced off it.

Singularities

A singularity is a configuration where the Jacobian J(q) loses rank — its determinant (for square J) or smallest singular value goes to zero. Physically, the arm cannot instantaneously move the tool in some direction no matter how it moves its joints. Mathematically, IK demands infinite joint velocity to produce finite Cartesian velocity in the lost direction, which is exactly the explosion DLS was invented to tame.

The classic three on a 6-axis arm

Wrist singularity: axes 4 and 6 become collinear (joint 5 at 0° or 180°). The two parallel axes can only contribute the same rotation, so you lose a rotational DoF. This is the most common one in practice — it shows up constantly in welding and machining paths that drag the tool through a "straight" orientation. Symptom: J4 and J6 spin wildly in opposite directions trying to produce a small wrist motion.

Shoulder singularity: the wrist center lies directly above (on the axis of) joint 1. Any tiny lateral move of the tool demands a near-instant 180° flip of the base. Common when a path passes near the robot's central column.

Elbow singularity: the arm is fully outstretched (or fully folded), so the wrist center sits on the boundary of the reachable sphere. The arm can't move the tool further outward; the elbow can't help. This is a boundary (workspace) singularity, distinct from the two interior ones above.

Why they blow up IK and how to handle them

Near a singularity, the smallest singular value σ_min → 0, and the pseudoinverse scales the corresponding direction by 1/σ_min → ∞. Three layers of defense:

1. Damped least squares (covered above) caps the velocity by trading tracking accuracy near the singularity — the robot lags slightly in the degenerate direction rather than convulsing. This is the run-time safety net.
2. Manipulability monitoring. Compute Yoshikawa's manipulability index w = √det(JJᵀ) (proportional to the volume of the velocity ellipsoid). When w drops below a threshold, you're near a singularity — slow the trajectory, warn, or abort.
3. Avoidance at plan time. The real fix: don't path through singular regions. For a redundant arm, use the null space to steer the configuration away (maximize manipulability as a secondary objective). For a 6-DoF arm, choose waypoints and solution branches that keep clear, or re-orient the workpiece so the process path lives in well-conditioned configurations.

Industrial robot arms controllers handle this with vendor-specific tricks — singularity-avoidance modes that subtly deviate the path, or that switch to joint-space interpolation through the singular region — but those introduce path error, which is exactly why process engineers fixture the part to keep the seam out of the wrist-singular orientation in the first place.

Rule: Singularities are a planning problem masquerading as a control problem. DLS keeps you alive when you stumble into one; the right answer is to plan and fixture so you never do.

Redundancy & redundancy resolution

A robot is redundant for a task when it has more joints (n) than the task needs DoF (m). A 7-DoF arm doing a 6-DoF pose task has one degree of redundancy (n − m = 1); a humanoid doing a reaching task with its whole body might have dozens.

Why 7 DoF

Six joints is the minimum to place the tool at an arbitrary position and orientation. So why do many cobots (Franka, KUKA iiwa) and humanoid arms have seven? Because the seventh joint gives you a continuous family of configurations that all reach the same tool pose — the "elbow" can swing on a circle while the hand stays put. That extra freedom buys you: reaching around obstacles, dodging joint limits, keeping the elbow out of a human's way, and avoiding singularities. The cost is that IK now has infinite solutions, so you need a principled way to choose.

The null-space trick

The pseudoinverse solution q̇ = J⁺·v is the minimum-norm joint velocity achieving the task twist v. But you can add any motion that lies in the null space of J — motion that produces zero end-effector velocity — without disturbing the task:

q_dot = J^+ @ v  +  (I - J^+ @ J) @ q_dot_0
        \_______/    \____________________/
         task term     null-space projector @ secondary objective

The projector (I − J⁺J) annihilates anything that would move the tool, so q̇₀ — the gradient of a secondary cost — only moves the redundant DoF. Pick q̇₀ = ∇H(q) for a cost H you want to minimize:

• Joint-limit avoidance: H penalizes proximity to limits → the arm self-centers its joints.
• Manipulability maximization: H = w(q) → the arm steers away from singularities.
• Obstacle avoidance: H is distance-to-obstacle → the elbow tucks away from clutter.

This is the heart of operational-space / whole-body control on humanoids and the reason a Franka can keep its end-effector dead still while you physically push its elbow around — you're injecting null-space motion by hand. bio_ik in MoveIt exposes this as weighted soft goals so you can ask for "reach this pose AND keep joint 3 near zero AND avoid this volume," and it solves the whole stack.

Configuration space & the planning problem

We now leave kinematics and enter planning. The central abstraction is configuration space (C-space): the space of all possible robot configurations. A single point in C-space is a complete description of the robot's geometry — for a 6-axis arm, that's a point in a 6-dimensional space (q₁, …, q₆); for a mobile base, it's (x, y, θ).

Obstacles, free space, and the planning statement

The world's obstacles, plus the robot's own self-collisions and joint limits, carve out a forbidden region C_obs in C-space. What's left is free space C_free. The motion-planning problem is then deceptively clean:

The problem: Find a continuous curve in C_free from the start configuration q_start to the goal configuration q_goal (or to any configuration in a goal set, for task-space goals).

The brutal part is that obstacles in the workspace map to weirdly-shaped, hard-to-describe regions in C-space. A simple box obstacle in front of an arm becomes a curved, non-convex blob in 6D. There is generally no closed-form description of C_free — you can only test whether a given configuration is in collision (a forward-kinematics + collision-check query), not enumerate the free region.

The curse of dimensionality

The obvious approach — grid C-space, mark cells free/occupied, run A* — dies immediately. A 6-DoF arm gridded at a coarse 10 cells per joint is 10⁶ cells; at a usable 100 cells per joint it's 10¹² cells. A 7-DoF arm at 100 cells is 10¹⁴. You cannot store, let alone search, that grid. The volume of C-free shrinks and the number of cells explodes exponentially with DoF — Bellman's curse of dimensionality, and the reason discrete search dominates low-DoF mobile-robot planning (2D/3D grids work great) but is hopeless for arms.

This single fact bifurcates the field. Mobile robots plan in 2D/3D grids or lattices with A*/D*/hybrid-A* (see mobile robots & AMR/AGV). Manipulators must use methods that never build the grid — sampling and optimization.

Sampling-based planning

The insight that broke the dimensionality wall (mid-1990s) is delightfully cheap: don't represent C_free; sample it. Throw random configurations into C-space, keep the collision-free ones, connect nearby ones with collision-free straight segments, and search the resulting graph. You only ever need a fast collision checker and a sampler — both of which scale gently with dimension.

PRM — probabilistic roadmaps

The Probabilistic Roadmap (PRM) builds, offline, a graph (roadmap) of the free space: sample N collision-free configurations (nodes), connect each to its k nearest neighbors with collision-free edges, and you have a reusable map. At query time, connect q_start and q_goal to the roadmap and run graph search (A*/Dijkstra). PRM shines for multi-query problems — a fixed workcell where you plan thousands of motions in the same static environment, amortizing the roadmap construction.

RRT — rapidly-exploring random trees

For single-query problems (the environment changed, plan once, now), the Rapidly-exploring Random Tree (RRT) is the workhorse. Grow a tree from the start: sample a random configuration, find the nearest tree node, and extend a short step toward the sample. RRTs are biased to rapidly explore unexplored space (the Voronoi bias) and find a feasible path fast.

def rrt(q_start, q_goal, sample, collision_free, step=0.1, goal_bias=0.05, max_iter=10000):
    tree = {tuple(q_start): None}          # node -> parent
    for _ in range(max_iter):
        q_rand = q_goal if random() < goal_bias else sample()   # goal-biased sampling
        q_near = nearest(tree, q_rand)
        q_new  = steer(q_near, q_rand, step)                    # step toward q_rand
        if collision_free(q_near, q_new):                       # edge collision check
            tree[tuple(q_new)] = tuple(q_near)
            if distance(q_new, q_goal) < step and collision_free(q_new, q_goal):
                return reconstruct_path(tree, q_new, q_goal)
    return None   # failed within budget

RRT-Connect is the version everyone actually uses: grow two trees, one from start and one from goal, and try to connect them each iteration. It's dramatically faster than single-tree RRT for typical problems and is MoveIt 2's default OMPL planner for good reason.

RRT*, BIT*, and the asymptotic-optimality story

Plain RRT finds a path, not a good one — its solutions are jagged and often far from optimal. RRT* adds two steps (choose the best parent within a neighborhood, then rewire neighbors through the new node) that make it asymptotically optimal: as samples → ∞, the solution cost converges to the true optimum. The catch is "asymptotically" — RRT* needs a lot of samples to look good, and an early RRT* path can be as ugly as RRT's. BIT* (Batch Informed Trees) and Informed RRT* are the modern, sample-efficient successors that focus sampling into the ellipsoidal region that could possibly improve the current solution.

Planner	Query type	Optimality	Speed to first solution	Best for
PRM / PRM*	Multi-query	PRM no, PRM* asymptotic	Slow build, fast query	Static workcells, many plans, same scene
RRT	Single-query	None	Very fast	Quick feasible path, high-DoF, one-shot
RRT-Connect	Single-query	None	Fastest (bidirectional)	Default manipulator planner
RRT*	Single-query	Asymptotic	Fast feasible, slow to converge	When path quality matters and you have compute
BIT* / Informed RRT*	Single-query	Asymptotic, efficient	Good and improving	Modern optimal sampling, anytime use

OMPL (the Open Motion Planning Library) implements all of these and dozens more behind one interface; it is the planning backend MoveIt 2 ships and what 90% of manipulator planning in ROS actually runs. You rarely implement a sampling planner yourself — you choose one in OMPL and tune the time budget, range (step size), and goal bias.

Rule: Single query, just-get-there → RRT-Connect. Need the path to be short/smooth and have a couple hundred milliseconds → RRT* or BIT*. Same static cell, thousands of plans → PRM. Sampling planners give you feasibility, almost never elegance — that's the next section's job.

The two universal weaknesses

Sampling planners share two warts. First, they're probabilistically complete, not complete — they'll find a solution if one exists given enough time, but can't prove none exists, and they struggle with narrow passages (a thin corridor in C-free that random samples rarely hit). Second, the raw output is jerky and redundant — full of unnecessary zig-zags — so it must be shortcut-smoothed (repeatedly try to replace two waypoints with a direct connection) before it's fit to execute. Both warts motivate optimization.

Optimization-based planning

Optimization-based planners flip the script. Instead of searching for any feasible path and cleaning it up, they start from an initial trajectory (often a naïve straight line in C-space that plows through obstacles) and iteratively deform it to minimize a cost that balances smoothness against constraint violation.

CHOMP, STOMP, TrajOpt

CHOMP (Covariant Hamiltonian Optimization for Motion Planning) does gradient descent on a cost = smoothness + obstacle. The obstacle cost comes from a precomputed signed distance field (SDF) of the workspace, so its gradient is cheap to query. CHOMP's "covariant" gradient respects the trajectory's smoothness metric, so updates stay smooth. It's fast and produces lovely trajectories — when it converges. Being a local optimizer, it gets stuck in local minima and can fail on a straight-line seed that's deep inside an obstacle.

STOMP (Stochastic Trajectory Optimization) sidesteps the gradient entirely: it samples noisy perturbations of the current trajectory, evaluates their cost, and combines them weighted by how good they are (a sampling-based update reminiscent of CMA-ES). Because it needs no gradient, it handles non-differentiable costs (e.g., binary collision, torque limits) that CHOMP can't, at the price of more cost evaluations.

TrajOpt formulates planning as sequential convex optimization with hard constraints: it handles collision avoidance as constraints (via continuous-time signed-distance, catching tunneling between waypoints), joint limits, and pose constraints, solving a sequence of convex subproblems. It's the most powerful and constraint-aware of the three, and the closest to how modern MPC-style whole-body controllers think.

Planner	Method	Handles non-smooth costs	Constraint handling	Failure mode
CHOMP	Covariant gradient descent + SDF	No (needs gradients)	Soft (penalty)	Local minima; bad seed plows through obstacle
STOMP	Stochastic sampling update	Yes	Soft (penalty)	Slow (many rollouts); stochastic
TrajOpt	Sequential convex optimization	Partly	Hard constraints	Needs decent init; convex-approx limits

The modern hybrid

The 2026 production pattern isn't "sampling vs optimization" — it's both, in sequence. Sampling-based planning is great at the global, discrete question (which side of the obstacle, which homotopy class — fundamentally a combinatorial choice that gradient methods can't make). Optimization is great at the local, continuous question (make this rough path short, smooth, and clearance-rich). So:

1. Sample (RRT-Connect) to get a feasible path in the right homotopy class, fast.
2. Optimize (CHOMP/TrajOpt) using that path as the seed to polish it into something smooth and short.

This sidesteps optimization's local-minima problem (the sample gives a good seed in the right "tunnel") and sampling's ugliness problem (optimization cleans it up). MoveIt 2 supports exactly this by chaining planning pipelines and adding CHOMP/STOMP as post-processing optimizers after an OMPL plan.

Opinion with a reason: If your scene is cluttered with multiple ways around obstacles, you need a sampling planner in the loop — pure optimization will pick whichever local tunnel its seed happens to start in and refuse to consider the better route on the other side. If your scene is open and you mostly want smoothness and clearance, pure optimization (or even a parametric spline through a few waypoints) is faster and cleaner than sampling. Match the tool to the topology of the free space.

Trajectory generation

The planner handed you a path: an ordered list of collision-free configurations, with no time attached. The robot can't execute that — a motor needs a position reference as a function of time. Trajectory generation (a.k.a. time parameterization) assigns timing to the path under the robot's dynamic limits.

Path vs trajectory, precisely

A path is a geometric curve q(s), s ∈ [0, 1]. A trajectory is q(t), t ∈ [0, T], with well-defined q̇(t) and q̈(t). Going from one to the other is choosing the time-scaling s(t) such that velocity (|q̇| ≤ v_max), acceleration (|q̈| ≤ a_max), and ideally jerk (|q⃛| ≤ j_max) limits are respected at every instant. This is not an afterthought — execute an aggressive trajectory and you get tracking error, vibration, and overshoot; execute a timid one and you waste cycle time.

Trapezoidal vs S-curve profiles

For a single point-to-point move, the simplest time-optimal profile under velocity+acceleration limits is the trapezoidal velocity profile: accelerate at a_max, cruise at v_max, decelerate at a_max. Velocity is a trapezoid, acceleration is a square wave — which means infinite jerk at the corners. Infinite jerk excites structural resonances, wears gearboxes, and makes the tool ring.

The S-curve (a.k.a. seven-segment) profile fixes this by limiting jerk: acceleration ramps up and down smoothly, so velocity has rounded S-shaped transitions. It's gentler on the mechanics (critical for harmonic-drive joints and precision settling) at the cost of being slightly slower and more complex to compute.

def trapezoidal_profile(dist, v_max, a_max):
    """Time-parameterize a 1-DoF move of length `dist`. Returns q(t), v(t)."""
    t_acc = v_max / a_max                       # time to reach cruise speed
    d_acc = 0.5 * a_max * t_acc**2              # distance covered accelerating
    if 2 * d_acc >= dist:                       # triangular: never reach v_max
        t_acc = (dist / a_max) ** 0.5
        v_peak = a_max * t_acc
        t_cruise, T = 0.0, 2 * t_acc
    else:
        v_peak = v_max
        d_cruise = dist - 2 * d_acc
        t_cruise = d_cruise / v_max
        T = 2 * t_acc + t_cruise

    def q(t):
        if t < t_acc:                                   # accel phase
            return 0.5 * a_max * t**2
        elif t < t_acc + t_cruise:                      # cruise phase
            return d_acc + v_peak * (t - t_acc)
        elif t <= T:                                    # decel phase
            td = t - t_acc - t_cruise
            return d_acc + v_peak * t_cruise + v_peak*td - 0.5*a_max*td**2
        return dist
    return q, T

The S-curve adds jerk-limited ramps on each end of the acceleration phase (seven segments: jerk-up, const-accel, jerk-down, const-vel, jerk-down, const-decel, jerk-up). The math is bookkeeping-heavy, which is why you should use a library — Ruckig is the modern, open-source, real-time jerk-limited generator that MoveIt 2 adopted; it computes time-optimal, jerk-bounded trajectories online (even mid-motion when the target changes) in microseconds.

Time-optimal parameterization and blending

For a multi-waypoint path, you don't stop at each point — you blend through them. Two production approaches in MoveIt 2:

• TOTG (Time-Optimal Trajectory Generation, Kunz & Stilman) takes the whole path and computes the time-optimal parameterization respecting velocity and acceleration limits, smoothing the corners with a configurable blend radius. It's the long-time MoveIt default.
• Ruckig does jerk-limited online generation — the better choice when jerk limits matter or when you need to re-time on the fly.
• The Pilz industrial motion planner generates trapezoidal trajectories directly for LIN, PTP, and CIRC commands — the deterministic, industrial-style motion (straight lines and arcs in Cartesian space) that factory programmers expect, as opposed to the free-form sampling-planner output.

Rule: A path that ignores jerk will pass in simulation and ring in hardware. Bound jerk (S-curve / Ruckig) for any precision or high-payload move; trapezoidal is fine for coarse point-to-point where settling time isn't critical. And always validate the executed trajectory's velocity/accel against the datasheet limits — planners get joint-limit configs wrong constantly.

Putting it together in practice

Stack all of the above and you get MoveIt 2, the manipulation framework for ROS 2 and the place where most of these algorithms meet a real robot.

The MoveIt 2 pipeline

A move_group plan request flows through, roughly:

1. Robot model — parsed from URDF (geometry, joints, limits, collision meshes) plus SRDF (semantic info: planning groups, named poses, disabled self-collision pairs). This is your representation, and it's where most bugs originate.
2. IK — for pose goals, an IK plugin maps Cartesian goals to joint goals: KDL (numerical DLS, default, works everywhere), IKFast (closed-form, fast, per-robot), bio_ik or pick_ik (optimization-based, redundancy-aware, increasingly the recommended general choice).
3. Planning — the planning pipeline runs OMPL (RRT-Connect default), CHOMP, STOMP, or Pilz, with FCL (Flexible Collision Library) doing the collision queries against the planning scene (the live world model from sensors + known objects).
4. Collision checking — FCL tests robot-vs-robot (self) and robot-vs-world collisions, accelerated by broad-phase bounding-volume hierarchies. This is called thousands of times per plan and is usually the compute bottleneck.
5. Trajectory processing — the geometric path gets time-parameterized (TOTG or Ruckig) under velocity/accel/jerk limits.
6. Execution — the trajectory streams to a ros2_control JointTrajectoryController, which interpolates and feeds the per-joint position/velocity references to the real-time control layer (see ROS 2).

Manipulator vs mobile-robot planning

These are genuinely different worlds and conflating them costs teams real time:

Aspect	Manipulator (arm)	Mobile robot (AMR/AGV)
C-space	6–7+ DoF, high-dimensional	2D (x,y,θ) or 3D, low-dimensional
Dominant method	Sampling (OMPL) + optimization	Grid/lattice search (A*, hybrid-A*, D*)
Representation	URDF + planning scene	Occupancy/cost grid, costmaps
Framework	MoveIt 2	Nav2
Constraints	Joint limits, singularities, self-collision	Non-holonomic (car-like), footprint, kinodynamic
Replanning rate	Per task (seconds)	Continuous (10–20 Hz, dynamic obstacles)

A mobile base lives in a low-dimensional space where you can grid the world, so mobile robots lean on costmap-based search (Nav2) with continuous local replanning. Arms can't grid their space, so they sample. Use MoveIt for arms, Nav2 for bases, and the right tool for each — a humanoid or mobile manipulator runs both, coordinated.

The practical stack — what actually ships

A representative 2026 manipulation stack:

• Model: URDF/SRDF, validated against the real robot (collision meshes that match reality, limits in the right units).
• Kinematics: Pinocchio or KDL for FK/Jacobian; IKFast or pick_ik for IK.
• Planning: OMPL RRT-Connect for free-space moves; CHOMP/TrajOpt polish or Pilz LIN/PTP for process moves.
• Collision: FCL against a planning scene fused from depth sensors (LiDAR/depth cameras) and known CAD objects.
• Trajectory: Ruckig for jerk-limited timing.
• Control: ros2_control trajectory controller → joint controllers → FOC drives at 1–10 kHz on a real-time kernel.

Opinion with a reason: When a MoveIt plan fails or executes weirdly, resist the urge to swap planners. In my experience the order of likely culprits is: (1) TF/URDF representation error, (2) collision geometry that doesn't match reality (padding too tight, mesh missing), (3) IK landing in the wrong solution branch, (4) joint limits wrong, and only then (5) the planner itself. Fix the model first. Planners are mature; your URDF probably isn't.

Frequently asked questions

What's the difference between forward and inverse kinematics, in one sentence? Forward kinematics computes the tool pose from known joint angles (easy, unique, just matrix multiplication); inverse kinematics computes joint angles from a desired tool pose (hard, possibly zero/multiple/infinite solutions, often iterative).

Why do everyone's controllers use quaternions instead of Euler angles? Because Euler angles suffer gimbal lock — at certain orientations two rotation axes align, you lose a DoF, and the rate equations become singular — while unit quaternions are singularity-free, compose cheaply, and interpolate smoothly via SLERP. Use Euler angles only for human-facing display.

Is the Jacobian transpose really enough for force control? Yes — τ = Jᵀ·F is exact (it follows from the principle of virtual work), not an approximation. It maps a desired end-effector wrench to the joint torques that produce it, and it's the basis of Cartesian impedance control and the leg-force control on legged robots. You don't even need to invert anything.

When should I use closed-form IK vs numerical IK? Use closed-form (IKFast) when your robot has a solvable geometry — classically a 6-DoF arm with a spherical wrist — and you want every solution at microsecond speed. Use numerical (damped least squares) for redundant arms, unusual geometries, extra constraints, or when you can't derive a closed form.

What exactly is a singularity and why does my IK explode there? A singularity is a configuration where the Jacobian loses rank — the arm can't move the tool in some direction regardless of joint motion. The pseudoinverse scales the lost direction by 1/σ_min, and as the smallest singular value σ_min → 0, the commanded joint velocity → ∞. Damped least squares adds a λ² term that caps it.

Why do collaborative robots and humanoids have 7 joints instead of 6? Six is the minimum to reach an arbitrary pose; the seventh joint makes the arm redundant, giving a continuous family of configurations for the same tool pose. You exploit that null space to avoid obstacles, dodge joint limits, keep the elbow clear, and steer away from singularities.

RRT or RRT — which should I use?* RRT (specifically RRT-Connect) when you just need a feasible path fast and will smooth it afterward — it's the manipulator default. RRT* when path quality (length, smoothness) matters and you can afford more computation; it's asymptotically optimal but slow to converge, so consider Informed RRT* or BIT* for sample efficiency.

What's the practical difference between a path and a trajectory? A path is a geometric curve through configuration space with no time — just where to go. A trajectory adds timing: position, velocity, and acceleration as functions of time, respecting velocity/accel/jerk limits. Planners produce paths; trajectory generation (TOTG, Ruckig) turns them into executable trajectories.

Trapezoidal or S-curve velocity profile? Trapezoidal is simpler and slightly faster but has infinite jerk at the corners, which excites vibration and stresses gearboxes. S-curve (jerk-limited) is gentler on the mechanics and better for precision and high-payload moves, at a small cost in cycle time and complexity. For anything precision-critical, use S-curve (or Ruckig).

Should I write my own DH-based forward kinematics? For learning, yes. For production, no — author the robot in URDF and use Pinocchio or KDL. Hand-derived DH tables are error-prone (convention, sign, offset bugs), only describe serial chains, and give you nothing a library doesn't compute faster and with analytical derivatives.

My MoveIt plan keeps failing — is the planner bad? Almost certainly not. Check, in order: the TF tree and URDF (wrong frames/units), collision geometry (meshes that don't match reality, over-tight padding), the IK solution branch, and joint limits. The planner is mature; the representation around it usually isn't.

Do mobile robots and robot arms use the same planning algorithms? No. Mobile bases live in a low-dimensional 2D/3D space you can grid, so they use search-based planners (A*, hybrid-A*, D*) in Nav2. Arms live in a 6–7+ dimensional space where gridding is impossible (curse of dimensionality), so they use sampling-based planners (OMPL) in MoveIt 2. A mobile manipulator runs both.