robot-path-planning

name	robot-path-planning
description	Robot path planning — configuration space (C-space), RRT/RRT* (rapidly-exploring random trees), PRM (probabilistic roadmap), A* and Dijkstra graph search, potential fields, trajectory optimization (CHOMP, STOMP, TrajOpt), joint-space vs. Cartesian-space planning, minimum-time trajectory (bang-bang, TOPP), singularity avoidance, collision detection (GJK algorithm), motion planning in ROS (MoveIt!, OMPL), and industrial robot offline programming (OLP).
metadata	{"priority":7,"promptSignals":{"phrases":["robot path planning","motion planning","RRT algorithm","trajectory planning robot","configuration space","robot collision avoidance"],"minScore":3}}

Robot Path Planning — Complete Skill

Configuration Space (C-Space)

Definitions

Configuration q: complete specification of robot state; q ∈ C (configuration space) For n-DOF serial robot: C = ℝⁿ or (S¹)ⁿ (angles); dim(C) = n Free space C_free = C \ C_obstacle: configurations without collision

C-space obstacle: for a rigid obstacle O in workspace, C_obs = {q : robot(q) ∩ O ≠ ∅} Configuration space transformation: maps workspace obstacles to C-space (can be non-convex, complex geometry)

Planning problem: Given: q_start, q_goal ∈ C_free; find path π: [0,1] → C_free connecting start to goal

Completeness: planner is complete if it always finds a path when one exists (and reports failure when none exists) Probabilistic completeness: probability of finding path → 1 as computation time → ∞ (sampling-based)

Sampling-Based Planning

RRT (Rapidly-Exploring Random Tree) — LaValle 1998

Algorithm:

1. Initialize tree T with q_start
2. Repeat until goal reached: a. Sample q_rand from C uniformly at random (or toward goal with probability p_goal ≈ 0.05–0.10) b. Find nearest node q_near in T (by Euclidean distance in joint space) c. Extend: q_new = q_near + min(δ, ||q_rand - q_near||) × (q_rand - q_near)/||...|| [step size δ] d. If CollisionFree(q_near, q_new): add q_new to T and edge (q_near, q_new) e. If ||q_new - q_goal|| < ε_goal: path found; trace back to root

Properties: Probabilistically complete; not optimal (path is not minimum length/time) Fast in high-dimensional spaces; no explicit C-space computation needed Step size δ: typically 5–10° for joints; tradeoff between exploration speed and resolution

RRT-Connect (Kuffner-LaValle 2000): Two trees grown simultaneously (from q_start and q_goal); connected when trees meet ~5× faster than single-tree RRT in practice; commonly used in robot planners

RRT* (Karaman-Frazzoli 2011)

Asymptotically optimal RRT: After adding q_new, rewire nearby nodes if shorter path found through q_new: Nearby radius: r = γ × (log(N)/N)^(1/n) [N = number of nodes; γ = problem-dependent constant] For each q_near in ball of radius r: if cost(q_start → q_new → q_near) < cost(q_start → q_near) → rewire

Convergence: as N → ∞, path length converges to optimal; slower than RRT but produces shorter paths InformedRRT:* samples within ellipsoidal subset of C-space containing potentially better paths → faster convergence

Probabilistic Roadmap (PRM) — Kavraki 1996

Two phases: Learning phase: sample N_config configurations in C_free; connect nearby configs → roadmap G = (V, E) Query phase: connect q_start and q_goal to roadmap → search roadmap with Dijkstra/A*

Connectivity: connect q_near if distance < r_connect and edge is collision-free Advantages: reusable roadmap for multiple queries; good for fixed environments Disadvantages: poor in narrow passages (unlikely to sample both sides)

Graph Search Methods

A* Algorithm

For discretized C-space or on roadmap: f(n) = g(n) + h(n) [g = cost from start; h = admissible heuristic (underestimate of remaining cost)] Heuristic: h(q) = ||q - q_goal|| [Euclidean distance in joint space; admissible if metric distance]

Optimality: A* finds optimal path if h is admissible (h(n) ≤ actual cost to goal from n) Completeness: complete on finite graphs

Dijkstra: special case h = 0; finds optimal; slower than A* (explores more nodes)

Trajectory Optimization

CHOMP (Covariant Hamiltonian Optimization for Motion Planning)

Objective: minimize: U(ξ) = λ × U_smooth(ξ) + U_obstacle(ξ) [ξ = trajectory; smooth (jerk minimization) + obstacle avoidance] U_smooth = (1/2) × ∫ ||ξ''(t)||² dt [second-derivative smoothness; discretized as q^T K q] U_obstacle = ∫ c(ξ(t)) × ||ξ'(t)|| dt [c = signed distance field potential; ξ' = velocity]

Update: ξ_{k+1} = ξ_k - η × (K^{-1} × ∇U_obstacle + ∇U_smooth) [gradient descent with metric K] K = finite-difference matrix for second derivative (tridiagonal)

STOMP: stochastic version; samples trajectory perturbations; evaluates cost; updates toward better samples; handles non-differentiable costs (hard constraints)

TrajOpt (Schulman et al. 2013)

Sequential convex optimization: Formulate as NLP: min f(ξ); subject to: collision constraints, joint limits, dynamics Linearize constraints at each iteration → convex QP subproblem → trust region updates Collision constraints: signed distance function d(x) ≥ 0 (no penetration)

Advantages: handles equality and inequality constraints; deterministic; faster convergence than CHOMP for constrained problems Commonly used in manipulation and grasping planning

Minimum-Time Trajectory

Time-Optimal Trajectory (TOPP)

Time-Optimal Path Parameterization (TOPP — Bobrow 1985, Pham 2014): Given geometric path ξ(s), s ∈ [0,1], find optimal time parameterization s(t)

State space: (s, ṡ) (path position and velocity); constraints: Torque limits: τ_min ≤ M(q) × q̈ + C × q̇ + G ≤ τ_max Joint velocity limits: q̇_min ≤ q̇ ≤ q̇_max

Maximum velocity curve (MVC): ṡ_max(s) = maximum ṡ at each s without violating torque limits TOPP algorithm: follow MVC between acceleration and deceleration switching points Bang-bang control: maximum acceleration then maximum deceleration (for simple cases)

Time savings: minimum-time vs. trapezoidal profile: 20–40% faster for constrained robots

Singularity Avoidance

Kinematic Singularities

Singularity: configuration where Jacobian rank drops → loss of one or more Cartesian DOF Types: wrist singularity (three wrist axes coplanar), shoulder singularity, elbow singularity

Detection: det(J × J^T) → 0 near singularity (or smallest singular value σ_min of J → 0) Manipulability measure: w = √(det(J × J^T)) [Yoshikawa]; w = 0 at singularity

Avoidance: Path planning: penalize configurations with low manipulability in cost function Damped least squares: J† = J^T × (J × J^T + λI)^{-1} [λ = damping; increases near singularity → slows down, avoids singularity] Null-space motion: use redundant DOF (7+ DOF robot) to move away from singularity

Collision Detection

GJK Algorithm (Gilbert-Johnson-Keerthi, 1988)

For convex shapes: Distance between convex shape A and B = minimum distance between their Minkowski difference A ⊖ B and origin GJK iterates simplex updates in Minkowski space → O(polyhedra vertices) complexity

EPA (Expanding Polytope Algorithm): When GJK detects penetration (origin inside Minkowski diff) → EPA finds penetration depth and direction

BVH (Bounding Volume Hierarchy): Tree of bounding boxes/spheres enclosing robot geometry; prune non-colliding branches fast OBB (oriented bounding box) or AABB (axis-aligned); AABB faster to check; OBB tighter

FCL (Flexible Collision Library) / Bullet physics: Standard libraries used in MoveIt!, OMPL, Drake for real-time collision checking

ROS MoveIt! and OMPL

MoveIt!: ROS framework for motion planning

• OMPL: open-source library with RRT, RRT*, PRM, KPIECE, etc.
• Collision checking: FCL or Bullet via planning scene
• IK: KDL, TracIK, Bio-IK for inverse kinematics
• Robot model: URDF → SRDF (semantic robot description)

Typical planning call: move_group.plan(trajectory) → runs RRT-connect in joint space; 0.01–1 s typical for industrial 6-DOF

Standards and References

Standard	Scope
ISO 10218-1/-2	Industrial robot safety; safe speed and workspace monitoring
ROS MoveIt! documentation	Practical motion planning implementation
OMPL (Open Motion Planning Library)	Reference for sampling-based algorithms
LaValle "Planning Algorithms" (2006)	Comprehensive textbook; freely available online
Schulman et al. "Motion Planning with Sequential Convex Optimization" (2014)	TrajOpt reference

Output

Provide: planning problem specification (q_start; q_goal; n DOF; joint limits; workspace obstacles), algorithm selected (RRT-Connect/RRT*/PRM/TrajOpt; justification: environment complexity, optimality requirement, computation time budget), C-space dimensionality and key obstacles (approximate C-obs geometry if known), planning result (path found: yes/no; path length [rad or m in joint space]; planning time [s]; number of nodes N), collision checking method (GJK/BVH; collision objects; check rate [kHz]), trajectory optimization (if applied: CHOMP/STOMP/TrajOpt; objective value before/after optimization; path quality metric), time-optimal parameterization (TOPP: total time t_opt [s] vs. trapezoidal profile t_trap [s]; saving [%]; joint torque utilization [%]), singularity avoidance (min manipulability w along path; joints near singularity: which?; mitigation applied), and applicable reference (LaValle Planning Algorithms, ISO 10218, ROS MoveIt!).