§4.2 Geometric actions: inverse kinematics and motion planning

Forward kinematics, then inverse

A serial manipulator with n joints is parameterized by a vector q ∈ R^n of joint angles (or, for prismatic joints, displacements). Forward kinematics is the map from q to the pose of the end effector, written T(q) ∈ SE(3) — a 4×4 homogeneous transform encoding both position and orientation. The map is built by chaining one transform per link, using Denavit–Hartenberg parameters or the more modern product-of- exponentials formulation (Lynch and Park, 2017, Ch. 4). It is closed- form, smooth, and cheap: for a 7-DOF arm, evaluating T(q) is roughly two dozen multiplications. There is no algorithmic difficulty in forward kinematics, only bookkeeping.

Inverse kinematics is the opposite direction: given a target pose T_desired, find a q with T(q) = T_desired. The difficulty here is real. For a generic 6-DOF arm, IK is a system of six nonlinear equations in six unknowns. For arms with three intersecting wrist axes — Puma, Kuka KR series, most industrial designs — Pieper’s solution from 1968 gives a closed-form answer: position decouples from orientation, and each piece is solvable with trigonometric identities. For arms without that geometric niceness — and for any robot with more than six joints, which includes most modern collaborative arms (Franka Panda, KUKA iiwa, Universal Robots) — there is no closed form, and the problem becomes either numerical or underdetermined. A 7-DOF arm reaching a 6-DOF target has a one-dimensional manifold of solutions: a continuous family of elbow positions, the “elbow swivel” parameter. The IK solver has to pick one, and the choice matters — picking poorly puts the elbow into a doorframe.

How an IK solver actually works

In practice almost no one writes their own IK solver anymore; everyone calls into TRAC-IK (Beeson and Ames, 2015), KDL, or IKFast (the analytic generator distributed with OpenRAVE). It is still worth understanding the two underlying recipes, because their failure modes show up at the interfaces.

The closed-form recipe is what IKFast does. It takes the URDF of the robot, symbolically derives the trigonometric equations from the forward- kinematics chain, and emits a C++ file that, given a target pose, returns the up-to-sixteen analytic solutions in microseconds. It is exact, has no hyperparameters, and is the default choice when the arm has the right geometry. It fails when the arm does not — most 7-DOF arms — because the symbolic derivation simply does not terminate.

The numerical recipe handles everything else. It starts from a current guess q_0 and iterates a small update q_{k+1} = q_k + Δq until the error |T(q_k) - T_desired| falls below a tolerance. The update uses the kinematic Jacobian J(q) = ∂T/∂q (a 6×n matrix mapping joint velocities to end-effector twists), inverted in some sense to map the pose error back to a joint update. The simplest version is Δq = J^+ · e, where J^+ is the Moore-Penrose pseudoinverse and e is the 6-vector pose error; in practice everyone uses damped least squares Δq = J^T (JJ^T + λ²I)^{-1} e (Nakamura and Hanafusa, 1986) because it stays well-conditioned near singularities — configurations where J loses rank and the arm momentarily cannot move in some Cartesian direction. TRAC-IK runs damped least squares and sequential quadratic programming in parallel and returns whichever converges first.

The failure modes are worth a paragraph because they are what an executor above the IK layer has to handle. No solution exists: the target pose is outside the workspace, or inside a self-collision. A solution exists but the solver missed it: numerical IK is a local method, and a bad initial guess can sit in a basin that does not reach the global optimum. Multiple solutions exist: which one is returned depends on the seed, and naively re-seeding between calls produces a robot that flips its elbow between consecutive waypoints. Every production system has a wrapper around IK that handles these — seeding with the previous solution, filtering by collision, choosing among solutions by a “distance from current pose” heuristic.

Configuration space and the motion-planning problem

Suppose IK has succeeded and returned a goal configuration q_goal. The robot is currently at q_start. Why not just interpolate the joint angles linearly between them? Because the straight line in joint space typically passes through configurations where the arm is inside the table, inside itself, or inside the block it is trying to grasp.

The cleanest way to think about this is in configuration space, or C-space: the space of all joint vectors q ∈ R^n (with appropriate periodicity for revolute joints). Each pose of the robot is one point in C-space. Each obstacle in the workspace — the table, the second block, the gripper itself — projects to a forbidden region C_obs ⊂ R^n, and the free region C_free = R^n \ C_obs is where the robot is allowed to be. The motion-planning problem becomes: find a continuous curve from q_start to q_goal that lies entirely inside C_free.

C_free is the catch. It is implicitly defined: there is no closed-form description of it, only a black-box collision-check that takes a q and returns true or false. For a 7-DOF arm in a typical tabletop scene, one collision check costs a few hundred microseconds (FCL, the standard library, broadphases AABB intersections then runs GJK on candidate pairs). Motion planners are accordingly judged by how few of these checks they do.

Sampling-based motion planning

The dominant approach for robotic arms is sampling-based motion planning (LaValle, 2006). The idea is so simple it is almost embarrassing: rather than explicitly modeling C_free, sample points uniformly at random from the joint-space box, discard the ones that collide, and build a graph out of the survivors. Two algorithms dominate.

Probabilistic Roadmap (PRM), introduced by Kavraki et al. (1996), samples thousands of configurations once, connects each to its k nearest neighbors with straight-line edges (checking each edge for collisions along its midpoint and recursively), and stores the resulting roadmap. At query time, q_start and q_goal are connected to the roadmap and Dijkstra finds a path. PRM is multi-query: amortize the roadmap build across many planning queries in the same scene.

Rapidly-exploring Random Tree (RRT), introduced by LaValle (1998), builds a tree rooted at q_start. Each iteration samples a random q, finds the nearest node in the tree, and extends the tree by stepping a small distance toward the sample, adding the new node if the edge is collision-free. Bias the sampling toward q_goal with probability 0.05, and the tree finds the goal quickly. RRT-Connect (Kuffner and LaValle, 2000) grows two trees, one from start and one from goal, and connects them — empirically the fastest single-query planner for high-DOF arms and the default in most modern stacks.

Neither RRT nor PRM produces optimal paths. The RRT* and PRM* variants of Karaman and Frazzoli (2011) recover asymptotic optimality by rewiring edges as new nodes arrive; in the limit of infinite samples, the returned path converges to the shortest. In practice, the post-processing step that matters most is shortcutting: take the returned waypoint sequence, pick random pairs of waypoints, and replace the segment between them with a straight line if the line is collision-free. A 200-waypoint RRT output typically shortens to 20 waypoints after shortcutting.

The reference implementation everyone uses is OMPL (Şucan, Moll, and Kavraki, 2012), wrapped in MoveIt for ROS. A typical IK + RRT-Connect + shortcut pipeline for a 7-DOF arm in a known scene plans a pick motion in 100–500 ms.

Optimization-based motion planning

The other family is optimization-based. CHOMP (Ratliff et al., 2009) formulates planning as gradient descent on a trajectory cost — a sum of smoothness (joint acceleration squared) and obstacle cost (the distance field of the scene, smoothed). It starts from a straight-line initialization in joint space and iteratively pushes the trajectory away from obstacles. TrajOpt (Schulman et al., 2013) does the same thing with sequential convex optimization and explicit collision constraints rather than soft penalties. Both are local: they need a reasonable initial trajectory and will return a local optimum, not the global one.

The trade-off with sampling-based planners is straightforward. Sampling-based methods are probabilistically complete — given infinite time, they find a solution if one exists — but produce jagged paths and ignore costs other than path length. Optimization-based methods produce smooth, low-cost paths but get stuck in local minima around obstacles. Production stacks typically run RRT-Connect first to find a path, then hand the result to TrajOpt as a warm start to polish it.

What the geometric layer hands to §4.3

The output of this whole pipeline — IK to get q_goal, motion planning to get a path, shortcutting and smoothing — is a discrete sequence of joint configurations (q_0, q_1, …, q_K) with associated timestamps. This is a kinematic trajectory: it describes positions over time but says nothing about whether the robot’s motors can actually generate those positions under gravity and inertia. Sending the trajectory directly to position-controlled joints often works for slow motions and stiff industrial arms; sending it to a torque-controlled compliant arm moving fast will produce trajectory tracking error of centimeters, more than enough to miss the block.

Closing that gap — turning a kinematic trajectory into joint torques — is the inverse-dynamics problem of §4.3.