Introduction to Motion Planning in Continuous Spaces

Abdulrahman Hamdy Ahmad

201

www.avlab.io

Khalifa University

ROBO

Course Structure

Module 1: Algorithm Fundamentals

• Analysis of algorithms (4 Lectures)
• Complexity and Intractability
• ADT and Unsorted Lists
• Sorted Lists
• Stacks and Queues
• Graph Preliminaries
• Priority Queue (Heap)
• Sorting Algorithms: Selection, Bubble; Heap Sort
• Sorting Algorithms: Quick Sort, Merge Sort

Module 2: Path Planning in Discrete Spaces

• Dijkstra’s Algorithm;
• Uninformed Graph Search: DFS, BFS, Uniform Cost Search;
• Informed Graph Search: A*
• State / Configuration Space

Module 3: Path Planning in Continuous Spaces

• Today:
• Introduction to Continuous C-space
• Intro Sampling-Based Planning
• Probabilistic Roadmaps (PRM)
• Rapidly-Exploring Random Trees (RRT).

2

ku.ac.ae

Outline

• Background
• Introduction to C-Space
• Introduction to Sampling-based Motion Planning
• Probabilistic Roadmaps (PRM)
• Rapidly-Exploring Random Trees (RRT)

​

3

ku.ac.ae

Background

4

Recap: Motion planning problem

• What is motion planning problem? (manipulator, mobile robot, drones,...etc?

​

5

ku.ac.ae

Motion planning methods

• Motion planning can be divided into two classes:
• Search-based methods
• These algorithms discretize the space into a graph and use graph-searching algorithms like A* or Dijkstra to find a path. They are complete and optimal in the discretized space but can suffer from the curse of dimensionality.
• Sampling-based methods
• These algorithms randomly sample the continuous space to generate a set of possible paths. Common examples include the Rapidly-exploring Random Tree (RRT) and Probabilistic Roadmap (PRM).

6

ku.ac.ae

Convex Domain / region

• Definition:
• A region is convex if the line segment connecting any two points within the region lies entirely inside the region.
• Key Idea:
• No “indentations” or “holes” in the shape.
• Every internal point is reachable without leaving the region.
• Why it matters in path planning?
• Collision-free regions are often non-convex.
• Many planners (like PRM or RRT) decompose complex, non-convex spaces into smaller convex regions to simplify planning.

7

ku.ac.ae

Configuration Space

8

Introduction to Continuous C-space

• Continuous: Infinite configurations (position + orientation)
• Configuration Space (C-space)
• Definition: The set of all possible robot configurations (all variables) depending on the type of the robot.
• Obstacles in C-space: Mapping workspace obstacles into configuration space.
• Examples:
• Point robot in 2D: C = ℝ²
• Rigid robot in 2D: C = ℝ² × S¹ (x, y, θ)
• Challenge: Impossible to discretize finely enough for large spaces.

9

ku.ac.ae

Workspace vs configuration space

1. Workspace (W-space):
• The physical environment where the robot operates.
• Defined by real-world coordinates (e.g., x, y, z).
• Includes obstacle space and free space in the environment.
• Example: A 2D map where a TurtleBot moves around walls.
2. Configuration Space (C-space):
• The space of all possible robot configurations.
• Each point = one unique robot pose (position + orientation).
• Obstacles are expanded (inflated) to account for robot size and shape.
• Represents: The feasibility of robot configurations.
3. Obstacle space configuration
• All robot configurations where it collides with obstacles.
4. Collision-free configuration
• All configurations where the robot can move without collisions.

10

Notice: Obstacle Inflation in Configuration Space

​

When moving from the workspace (W-space) to the configuration space (C-space), obstacles must be inflated to account for the robot’s size and shape.

Simple Example:

• Suppose the robot is a circle with radius 0.2 m.
• In the workspace, there’s a square obstacle that the robot cannot touch.
• In C-space, that square becomes a larger square, expanded outward by 0.2 m on all sides.
• Now, if the robot’s center touches the inflated boundary, the robot’s body would just touch the real obstacle in the workspace.

ku.ac.ae

Workspace vs configuration space

​

• For a differential‐drive mobile robot (like TurtleBot3 Burger), you can define:
• Forward Kinematics (FK):
• From wheel speeds (or linear v and angular ω velocities) → robot pose (x,y,θ) in the workspace.
• Inverse Kinematics (IK):
• Given a desired robot pose change or path in the workspace → required wheel velocities (or v,ω).
• How this maps W-space ↔ C-space
• Workspace (W‐space): The physical plane (x,y) where the robot moves.
• Configuration Space (C‐space): The set of all possible robot configurations q=(x,y,θ).
• FK and IK provide mappings between these spaces:
• FK: q↦(x,y,θ) (C‐space → W‐space)
• IK: (x,y,θ) (or desired change) → appropriate q (or wheel inputs) (W‐space → C‐space)

​

11

workspace

C-Space

IK

FK

Reading:

https://www.roboticsbook.org/S52_diffdrive_actions.html

ku.ac.ae

Exercise (1)

• Scenario
• You are programming a TurtleBot3 Burger in a 2D Gazebo world.
• The robot must navigate from its start pose near a wall to a goal pose on the other side of the map while avoiding static obstacles (boxes and walls).

1. Workspace (W-space)�
2. Configuration Space (C-space)�
3. Obstacle-Space Configuration�
4. Collision-Free Configuration Space�
5. State Space (X)

• All robot configurations where it collides with obstacles. Created by inflating obstacles by the robot’s radius (~0.105 m for TurtleBot3 Burger).
• (x, y) coordinates in meters, matching the map dimensions.
• X=(x,y,θ,v,ω)
• C_free = C \ C_obs
• (x, y, and θ)

12

Q. Match with the correct definition.

ku.ac.ae

Exercise (2): Workspace vs configuration space

13

Workspace

C-space

B

A

C

Q. select the correct c-space corresponding to each workspace.

ku.ac.ae

SAMPLING-BASED MOTION PLANNING

14

Motivation

• The previous methods rely on an explicit representation of the obstacles in configuration space�
• This may result in an excessive computational burden in:
• (a) high-dimensions
• (b) environments with a large number of obstacles

15

ku.ac.ae

Motivation

• Avoiding such a representation is the main underlying idea of sampling-based methods�
• Instead of using an explicit representation of the environment, sampling-based rely on a collision-checking module after Randomly sample configurations!

16

ku.ac.ae

Sampling-Based Planning Overview

• Motivation
• Avoid explicit representation of the full C-space.
• Core Idea
• Randomly sample configurations, test for validity, and connect them if feasible.
• Advantages
• Scalable to high-dimensional spaces.
• Easy to extend with robot-specific constraints.
• Types
• Probabilistic Roadmap (PRM):
• Roadmap construction
• Search
• Rapidly-exploring Random Tree (RRT):
• Roadmap construction and search online

17

Recent Progress on Sampling Based Dynamic Motion Planning Algorithms, Short et al, AIM 2016

ku.ac.ae

Introduction to Probabilistic Roadmap (PRM)

18

Probabilistic Roadmaps (PRM)

​

• The steps of this algorithm are:
• Populate the two-dimensional configuration space with random samples serving as nodes.
• Create a candidate PRM by connecting nearby samples (e.g., k-nearest neighbors or a distance threshold) with collision-free straight lines
• Enhance the PRM by connecting disjoint components of the network (add extra safe edges to improve connectivity).
• Connect 𝑞_start and 𝑞_goal to the roadmap (and run a graph search such as Dijkstra/A* to extract a path).

19

ku.ac.ae

Increasing the number of sample nodes

20

N=10

N=20

N=100

ku.ac.ae

Python Tutorial for PRM

21

https://colab.research.google.com/drive/1tCAJkTHGOTPBljCltyGm47S76UoAeq7D?usp=sharing

• Try to change the number of samples and observe the connections each time.
• Try to change the k in the k-nn method.
• Try to change the d_max in the k-nn.
• Report the results and show if some cases where there is no path from the start to the goal? why?

ku.ac.ae

Narrow passage problem and sampling methods

• Uniform Random Sampling
• Easiest to implement.
• Samples are evenly distributed throughout the C-space.
• Works well for open spaces, but struggles in narrow passages.

22

Sampling-based Motion Planning: Analysis and Path Quality, R. Geraerts PhD Thesis, 2006

ku.ac.ae

Exercise (3)

• Choose C-space or W-space? Why?
• (a) Where Do We Sample?
• (b) Where Do We Check for Collisions?
• Answer (a):
• We always sample in the Configuration Space (C-space). Each sample represents a possible robot configuration, i.e.,� q=(x,y,θ,…)
• The goal is to find a path through valid configurations (where the robot does not collide).
• In the workspace, we only see positions - but configurations also include orientation, joint angles, etc.
• Answer (b)
• Collision checking is performed in the Workspace (W-space).
• Steps:
• You sample a configuration q in C-space.
• The planner uses Forward Kinematics (FK) to compute the robot’s shape and position in W-space.
• Then it checks if the robot (in that pose) intersects any obstacles in the workspace.
• If no collision, the configuration is valid (belongs to C_free.
• If collision, it’s invalid (belongs to C_obs.

23

ku.ac.ae

Introduction to Rapidly-Exploring Random Trees (RRT)

24

Rapidly-Exploring Random Trees (RRT)

​

• The Idea Behind RRT
• Instead of searching exhaustively, sample the space randomly.
• Grow a tree of feasible configurations outward from the start point.
• Each new sample “pulls” the tree toward unexplored areas of the configuration space.
• The tree rapidly explores large, unknown, or complex regions.

25

https://www.youtube.com/watch?v=YKiQTJpPFkA

ku.ac.ae

RRT Algorithm

26

ku.ac.ae

Rapidly-Exploring Random Trees (RRT)

• Q: the configuration space (all configurations).
• Q_free ⊂ Q: the collision-free subset (no overlap with obstacles).
• T=(V,E): the tree with vertices V (configurations) and edges E (local, straight-line motions).
• ∥ ⋅ ∥: a distance metric in Q (usually Euclidean (2D or 3D, …etc.)
• q_start: The root of the tree. Put it exactly at your known start pose.
• q_goal: The target configuration. Often you accept reaching any state inside a goal region (a small ball around it) to be robust.
• n (number of iterations / nodes to try) More iterations → more coverage and higher chance to find a path, but more compute.
• Rule of thumb: Start modest (hundreds to a few thousands), increase if the space is large or cluttered.
• α (step size) How far each extension moves.
• Too large: the planner may hop over narrow passages and collide more.
• Too small: tree grows very slowly (many nodes needed).� Heuristics: pick α around the robot size to a few times that, or ~1–3% of the workspace diameter. Tune per map.
• β (goal bias, 0≤β≤1) Probability of sampling the goal instead of a random state.
• Small β (e.g., 0.05–0.15): keeps good exploration, still nudges toward the goal.
• Large β: can cause the tree to repeatedly stab toward the goal and get stuck on obstacles.� Typical: 0.05≤β≤0.2.

​

27

ku.ac.ae

RRT Notes

• The main idea is to incrementally probe an explore the C-space.
• In the early iterations the RRT quickly reaches the unexplored parts.
• However, RRT is dense in the limit, which means that it will eventually get to any point in the space.
• Builds a tree, not a graph (like PRM).
• Single-query planner: good for dynamic, one-time planning problems.
• Naturally biased toward unexplored space (or the goal), providing fast coverage.

Question: what is the difference between graph and tree?

​

• In general, a tree is any acyclic connected structure where each node (except the root) has exactly one parent, but it can have any number of children.
• A graph is a general network of nodes (vertices) connected by edges, where:Nodes can have any number of connections (no parent-child restriction).It can contain cycles (closed loops). Edges may be directed or undirected. It can be connected or disconnected.
• In short: a tree is a special type of graph - one that is connected and has no cycles.
• ​

28

ku.ac.ae

Python tutorial for the RRT

https://colab.research.google.com/drive/1EntYj-wRIvZNtb1CDCvjmnzxEENUlcNT?usp=sharing

29

ku.ac.ae

Thanks

Any questions?

30