Implementing A* and RRT Motion Planning for Robotics
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: Sample a random configuration. Find the nearest existing node. Steer toward the sample. Check collision. Add the new node if valid. 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 Determinis