Implementing A* and RRT Motion Planning for Robotics

Two classic planning approaches are A* and RRT (Rapidly-exploring Random Tree).

A* is particularly useful when the environment can be represented as a graph or grid. RRT is useful when planning in continuous or high-dimensional configuration spaces.

A* Planning

A* combines the cost already traveled with an estimate of the remaining cost.

Conceptually:

f(n) = g(n) + h(n)

Where:

• g(n) is the cost from the start.
• h(n) estimates the cost to the goal.
• f(n) ranks candidate nodes.

Grid Example

S . . # . . .
. . . # . . .
. . . . . # .
. # # # . # .
. . . . . . G

The planner explores promising cells while avoiding blocked cells.

Python Implementation Skeleton

import heapq

def astar(graph, start, goal, heuristic):
    queue = [(0, start)]
    cost = {start: 0}
    parent = {start: None}

    while queue:
        _, current = heapq.heappop(queue)

        if current == goal:
            break

        for neighbor in graph[current]:
            new_cost = cost[current] + 1

            if neighbor not in cost or new_cost < cost[neighbor]:
                cost[neighbor] = new_cost
                priority = new_cost + heuristic(neighbor, goal)
                heapq.heappush(queue, (priority, neighbor))
                parent[neighbor] = current

    return parent

RRT Planning

RRT works differently.

Instead of systematically exploring grid cells, it samples points and gradually grows a tree.

                  x
                /
        x------x
       /
S-----x
               x----x------G

A typical loop is:

1. Sample a random configuration.
2. Find the nearest existing node.
3. Steer toward the sample.
4. Check collision.
5. Add the new node if valid.
6. Repeat until the goal is reached.

RRT Skeleton

for _ in range(max_iterations):
    sample = random_configuration()
    nearest = nearest_node(tree, sample)
    new_node = steer(nearest, sample)

    if collision_free(nearest, new_node):
        tree.add(new_node)
        tree.connect(nearest, new_node)

        if reached_goal(new_node):
            return extract_path(tree, new_node)

A* vs RRT

Property	A*	RRT
Representation	Grid/graph	Continuous space
Search	Deterministic	Sampling-based
High-dimensional spaces	Less suitable	More suitable
Exact grid path	Yes	No
Typical use	Mobile robot maps	Manipulators / complex spaces

Improving RRT

Basic RRT can produce inefficient paths.

Common improvements include:

• RRT*
• Goal-biased sampling
• Better steering
• Path smoothing
• Collision-aware sampling

Robot Configuration Space

For a robotic arm, a state may be:

q = [joint1, joint2, joint3, ...]

Planning directly in joint space is often more practical than planning the end-effector through raw Cartesian space.

Production Checklist

Always validate:

• Collision model
• Robot dimensions
• Joint limits
• Velocity limits
• Acceleration limits
• Goal tolerance
• Planning timeout

Planning algorithms produce candidate solutions. A controller still needs to execute those solutions safely.

Useful Links

• Website: https://www.v-modal.com
• SDK Flutter: https://github.com/v-modal/vmodal_sdk_flutter
• SDK Android: https://github.com/v-modal/vmodal_sdk_android
• Discord: https://discord.gg/K72z28KU