| 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:
- Initialize tree T with q_start
- 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!).
