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." That sentence is a lie of omission. Between it and a stream of joint torques sits a stack of mathematics that has been refined for seventy years (Denavit and Hartenberg published their link-frame convention in 1955) and is still, in 2026, where most of the hard bugs live. The mug has a pose in SE(3), the six-dimensional group of rigid motions. The arm has six or seven joints, so its configuration is a point in an n-dimensional torus. "Pick it up" is a request to find a continuous curve in that torus, avoiding a forbidden set with no closed-form description, and trace it in time without exceeding a jerk limit, all before the human notices lag. 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. The tragedy is that the layers fail silently up the stack: a frame error in layer one surfaces as an inexplicable "planner bug" three layers later, and the engineer spends a week tuning the wrong knob.

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 remains unsolved: 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 transform bugs wearing a costume all along.

Companion reading: industrial robot arms, real-time control systems, ROS 2, and collaborative robots (cobots). If you want the broader map of the field this sits in, start with the robotics canon.

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 to 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), the special Euclidean group of rigid-body motions. SE(3) is a 6-dimensional Lie group: it is curved (you cannot add two poses), but near any point it looks locally like the flat 6-vector "velocity" space se(3), the algebra of twists. That local-flat / globally-curved duality is the source of nearly every orientation headache in this guide. The 4×4 homogeneous matrix below is the matrix representation of SE(3); the exponential map T = exp(ξ̂) turns a constant twist ξ = (v, ω) into the pose reached by screwing along it for unit time, and log(T) recovers the twist. Screw theory (Ball, 1900; modernized in Murray, Li & Sastry's A Mathematical Introduction to Robotic Manipulation, 1994) is the coordinate-free version of everything that follows.

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)·ê). The half-angle is not a typo: quaternions live on the unit 3-sphere S³, which double-covers SO(3), so a full 360° physical rotation traverses only 180° on the quaternion sphere, landing on the antipode −q; you need a 720° physical rotation to traverse a full 360° loop and return to the same quaternion. Composition is the quaternion product (≈16 multiplies and 12 adds, cheaper than the 27 multiplies of a 3×3 matmul), rotating a vector is v' = q ⊗ v ⊗ q*, and (crucially) interpolating between two orientations is SLERP (spherical linear interpolation, introduced by Ken Shoemake at SIGGRAPH 1985), which traces the shortest geodesic on S³ at constant angular velocity:

SLERP(q0, q1, t) = ( sin((1-t)Ω) / sinΩ )·q0 + ( sin(tΩ) / sinΩ )·q1
                   where cosΩ = q0 · q1  (the 4-D dot product)

There is no equivalent of gimbal lock anywhere on the sphere, the metric is uniform in every direction, which is exactly what "no singular parameterization" means geometrically.

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), from Jacques Denavit and Richard Hartenberg's 1955 ASME Journal of Applied Mechanics paper: four parameters per joint that fully describe the relationship between consecutive link frames. Why exactly four, not six? Because the DH construction spends two of a general frame's six degrees of freedom by convention (aligning each x-axis along the common normal between successive joint axes and each z-axis along the joint axis), leaving four free parameters. That is the minimal number that can encode a 1-DoF joint's geometry, which is precisely why the convention is fragile: there is no slack to absorb a sign error. Using the modified DH convention (Craig's Introduction to Robotics, which puts the frame at the proximal rather than distal joint), 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. Standard and modified DH tables for the same robot look different and are not interchangeable: mixing them is a rite-of-passage bug.

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 the recursive spatial-vector algorithms Roy Featherstone formalized in Rigid Body Dynamics Algorithms (2008). The key property: these algorithms are O(n) in the number of joints, not O(n³), because they propagate 6-D spatial velocities and forces link-by-link along the tree instead of assembling and inverting a mass matrix. Pinocchio is the faster, more modern choice: it computes FK for a humanoid in single-digit microseconds and gives you analytical derivatives of FK and dynamics with respect to q, which matters enormously for optimization-based planning and MPC: a numerically-differenced Jacobian costs n extra FK calls and injects ~1e-8-scale noise that wrecks a second-order optimizer. 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). This is the foundation of Daniel Whitney's 1969 resolved motion rate control, the insight that you can command a robot in Cartesian velocity by inverting this linear map at each instant, which quietly underlies every "jog the tool in +Z" button ever built:

| 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. It falls straight out of the principle of virtual work: a virtual joint displacement δq does work τᵀ·δq, the resulting end-effector displacement is δx = J·δq, and the wrench does work Fᵀ·δx = Fᵀ·J·δq. Energy is conserved for any δq, so τᵀ = Fᵀ·J, and transposing gives the joint torques τ required to exert an end-effector wrench F (force + moment):

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) (introduced to robotics independently by Nakamura & Hanafusa and by Wampler, both in 1986, as the singularity-robust inverse) is the Levenberg-Marquardt idea transplanted from nonlinear least squares. It fixes exactly this by adding a damping term λ²I. The step it computes is the exact minimizer of a regularized objective, Δq = argmin ‖J·Δq − e‖² + λ²‖Δq‖², so λ is literally the price (in units of joint-motion-squared) you charge the solver for each unit of Cartesian error it chases: near a singularity that price finally exceeds the payoff and the velocities stay bounded:

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 to 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. That count comes straight from a theorem. Pieper's 1968 result (for wrist-partitioned 6R arms) and the general Raghavan-Roth elimination (1993) prove a generic 6R inverse reduces to a degree-16 polynomial, so at most 16 real solutions exist for the fully general geometry, collapsing to 8 when the last three axes intersect. 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.

War story: A palletizing cell ran flawlessly for months, then started slamming the arm into an E-stop roughly once a shift, always near the same corner of the pallet. The path was fine; the planner was fine. The bug was branch hysteresis: at that corner the tool pose sat exactly on the boundary between two wrist solutions, and floating-point noise in the pose target flipped the IK seed between "wrist up" and "wrist down" on consecutive cycles. Each flip commanded a 300°/s reconfiguration the trajectory controller dutifully tried to execute. The fix was three lines (reject any IK solution more than a fixed joint-distance from the previous one), not a new planner. The representation, again, was wearing the costume.

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 (Tsuneo Yoshikawa, IJRR 1985) w = √det(JJᵀ) = σ₁·σ₂·…·σ₆, the product of the Jacobian's singular values and proportional to the volume of the velocity ellipsoid {ẋ = J·q̇ : ‖q̇‖ ≤ 1}. When w drops below a threshold, you're near a singularity. A subtler and often better trigger is the condition number κ(J) = σ_max / σ_min ∈ [1, ∞): w can be large yet κ enormous (a very eccentric ellipsoid, good in five directions, hopeless in one), which is the case that actually burns you. κ is also unit-consistency-sensitive, so weight the linear and angular blocks (a "characteristic length") before comparing: a raw 6×6 Jacobian mixes m/s and rad/s and its condition number is meaningless without that scaling. When κ → ∞, 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: it minimizes ‖q̇‖ subject to J·q̇ = v, which is what the Moore-Penrose pseudoinverse means. But the general solution of an underdetermined linear system is the particular solution plus anything in the kernel, so you can add any motion that lies in the null space of J (motion that produces zero end-effector velocity) without disturbing the task. Alain Liégeois formalized this gradient-projection scheme in 1977 (IEEE Trans. Systems, Man, and Cybernetics), and it remains the backbone of redundancy resolution:

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 control (Oussama Khatib's 1987 formulation, the foundation of 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. When you stack several such objectives by strict priority (task first, then a subtask projected into the task's null space, then a third into that one's null space), you get the task-priority framework (Nakamura, Hanafusa & Yoshikawa, 1987), the algebra every modern humanoid controller runs on.

The take: The seventh joint is a controls superpower you pay for once in hardware and spend forever in software. A 6-DoF arm forces a choice between reaching the pose and everything else; a 7-DoF arm reaches the pose and dodges the human and stays well-conditioned at once, because those secondary goals live in a subspace the task never touches. Teams that treat redundancy as an IK nuisance to nail down are leaving the whole point on the table.

Configuration space & the planning problem

We now leave kinematics and enter planning. The central abstraction is configuration space (C-space), Tomás Lozano-Pérez's 1983 formalization that reduced the whole "move a complicated rigid body among obstacles" problem to "move a point through a transformed free space." 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, θ). Note the topology: revolute joints make C-space a torus, not a box, so q = 0.01 and q = 2π − 0.01 are neighbors, and a planner that treats joint angles as flat Euclidean coordinates will refuse a 0.02-rad move because it "sees" a 6.26-rad one.

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. The cell count scales as N = k^n for k cells per joint and n joints: a 6-DoF arm gridded at a coarse k = 10 is 10⁶ cells; at a usable k = 100 it's 10¹² cells. A 7-DoF arm at k = 100 is 10¹⁴. At one byte per cell and one nanosecond per collision check, that is 100 TB of memory and over a day just to label the grid, before any search. You cannot store it, let alone search it. This is Bellman's curse of dimensionality, and it has a hard theoretical floor: John Reif proved in 1979 that the generalized mover's problem (motion planning for a many-jointed robot) is PSPACE-hard, so no clever data structure rescues the worst case. That hardness is why the field abandoned complete methods for arms and embraced randomization, trading the guarantee of an answer for the near-certainty of one in reasonable time. It is also why discrete search dominates low-DoF mobile-robot planning 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. The guarantee you keep is probabilistic completeness: the probability of finding a solution, if one exists, converges to 1 as samples → ∞, and for many problems the failure probability decays exponentially in the number of samples. You surrender worst-case completeness (the PSPACE-hardness never went away: it went to the narrow-passage problem, below) in exchange for algorithms that are trivial to implement and empirically superb.

PRM: probabilistic roadmaps

The Probabilistic Roadmap (PRM) (Kavraki, Švestka, Latombe & Overmars, 1996) 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) (Steven LaValle, 1998) 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. The mechanism behind the name is the Voronoi bias: the probability that a node is selected for expansion is proportional to the volume of its Voronoi cell, and the nodes on the frontier of the explored region own the largest cells, so the tree is stochastically pulled outward into unexplored space. That is why RRTs find a feasible path fast and cover a space with startlingly few samples.

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 (Kuffner & LaValle, ICRA 2000) is the version everyone actually uses: grow two trees, one from start and one from goal, and greedily try to connect them each iteration with a CONNECT step that extends repeatedly until it reaches the sample or hits an obstacle. It's dramatically faster than single-tree RRT for typical problems (the bidirectional search roughly halves the effective depth, and the greedy connect heuristic collapses easy free-space corridors in a few iterations), which is why it is MoveIt 2's default OMPL planner.

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

Plain RRT finds a feasible path with no guarantee of quality: its solutions are jagged and provably converge to a suboptimal cost with probability 1 (a result that surprised the field). RRT* (Sertac Karaman & Emilio Frazzoli, IJRR 2011) 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 almost surely to the true optimum. The key to keeping that cheap is the shrinking connection radius r(n) = γ·(log n / n)^(1/d), where n is the sample count and d the C-space dimension: the log n / n decay is precisely the rate that keeps the graph connected while bounding the average node degree, so RRT* stays near-linear in cost per sample instead of degenerating to a dense O(n²) graph. The catch is "asymptotically": an early RRT* path can be as ugly as RRT's. Informed RRT* and BIT* (Batch Informed Trees), both from Jonathan Gammell, Siddhartha Srinivasa & Timothy Barfoot (2014 to 2015), are the sample-efficient successors: once a solution of cost c_best exists, every point that could improve it lies inside a prolate-hyperspheroid (an ellipsoid with the start and goal as foci and c_best as the transverse diameter), so they sample only that shrinking ellipse and stop wasting probes on regions that provably cannot help.

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. The math is unforgiving: if a corridor occupies a fraction μ of C-free's volume, the expected number of uniform samples to land one inside is 1/μ, and μ itself shrinks geometrically with the corridor's aspect ratio and the dimension. A passage that is a comfortable 5% of the volume in 2D can be a 10⁻⁶ sliver in 7D, the same object, exponentially harder to hit. This is exactly the regime where bridge sampling, Gaussian sampling near obstacles, and workspace-informed samplers earn their complexity. 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; Ratliff, Zucker, Bagnell & Srinivasa, ICRA 2009) does gradient descent on a cost = smoothness + obstacle, where the smoothness term is a quadratic form ½·ξᵀAξ (A the finite-difference acceleration or jerk operator) that has a closed-form gradient. The obstacle cost comes from a precomputed signed distance field (SDF) of the workspace, so its gradient is cheap to query at every body point. The word "covariant" is load-bearing: instead of a Euclidean gradient step, CHOMP preconditions the update by A⁻¹, so a perturbation at one waypoint spreads smoothly to its neighbors and the trajectory stays smooth by construction rather than by penalty. 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, because the SDF gradient at the bottom of a collision valley is zero.

STOMP (Stochastic Trajectory Optimization for Motion Planning; Kalakrishnan, Chitta, Theodorou, Pastor & Schaal, ICRA 2011) sidesteps the gradient entirely: it samples noisy perturbations of the current trajectory, evaluates their cost, and combines them in a cost-weighted average (a derivative-free update in the same family as CMA-ES and path-integral policy improvement). Because it needs no gradient, it handles non-differentiable and discontinuous costs (binary collision, torque limits, constraint indicators) that CHOMP structurally cannot, at the price of many more cost evaluations per iteration.

TrajOpt (Schulman, Duan, Ho, Lee, Awwal, Bradlow, Pan, Patil, Goldberg & Abbeel, IJRR 2014) formulates planning as sequential convex optimization with hard constraints: it linearizes the problem, solves a convex subproblem inside a trust region, and iterates, handling collision avoidance as an ℓ₁ penalty on continuous-time signed-distance (the swept volume between waypoints, which catches the tunneling failure where two collision-free knots straddle a thin obstacle), plus joint limits and pose constraints. 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 is 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). By the chain rule q̇ = q'(s)·ṡ and q̈ = q'(s)·s̈ + q''(s)·ṡ², so the whole problem collapses to a search over the scalar path velocity ṡ and acceleration s̈ such that velocity (|q̇| ≤ v_max), acceleration (|q̈| ≤ a_max), and ideally jerk (|q⃛| ≤ j_max) limits hold at every instant. That reduction (a full n-joint timing problem becomes a 2-D phase-plane problem in (s, ṡ)) is the classic result of Bobrow, Dubowsky & Gibson (1985), and the time-optimal solution is bang-bang: at every instant some actuator is saturated on either its acceleration or deceleration limit. 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.

There is also a limit the datasheet's peak numbers hide. A motor's continuous torque is bounded not by peak current but by heating, I²R, so the constraint that actually governs a repeating duty cycle is the RMS torque:

τ_RMS = sqrt( (1/T) ∫₀ᵀ τ(t)² dt )   must stay below the motor's continuous rating

A trajectory can respect every instantaneous τ_max and still cook a joint if its RMS over the cycle exceeds continuous rating: the failure shows up an hour into a production run as a thermal fault, not in simulation. Time-optimal bang-bang trajectories are the worst offenders, one more reason production robots leave cycle time on the table.

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, RSS 2012) 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. TOPP-RA (Pham & Pham, 2018) is the modern reachability-analysis variant (numerically robust where the older phase-plane switching-point search is fiddly) and is what many newer stacks reach for.
• 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.

A reality check on what all this precision buys you: the kinematic chain you compute in double precision meets a machine whose pose repeatability (quantified per ISO 9283, the standard that defines how you actually measure a manipulator's accuracy and repeatability) is typically ±0.02 to 0.1 mm for a good industrial arm, while its absolute accuracy (does the tool land where the model says?) is often an order of magnitude worse, dominated by unmodeled link compliance, gear backlash, and thermal drift. This is why serious deployments run a kinematic calibration step (identifying the true DH-plus-error parameters from a measured cloud of poses) before the model is trusted: the nominal URDF is only a first draft. (ISO 8373 pins down the vocabulary precisely enough that a spec sheet's "repeatability" is not silently swapped for the rosier "resolution.")

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 to 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 to 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 to 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. 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 to 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.