Code for experimental part of my diploma thesis.
The main body of the code was taken from open source library https://github.com/AtsushiSakai/PythonRobotics#what-is-this and some edits were made to fit my thesis
Brief explanation of algorithms:
RRT*:
- The algorithm initiates with start point, goal point, and list of obstacles with centers of circles. After a planning function is called, a random node from pre-defined range is picked.
- Function get_nearest_node_index then finds index of the node which is the nearest to random node.
- Function steer creates a new node object which is expand_dis far away from the nearest node and contains information about parent and path based on resolution of path. Subsequently, cost of this node object is calculated.
- Function check_collision then checks path colisions. The colisions are checked for components of attribute path in every node. The number of path components in depends on the path resolution.
- Function find_near_nodes then finds nodes in the proximity r. This r is defined by a formula found in Sampling-based algorithms for optimal motion planning by karaman 2011. The nodes are determined using equation for circle.
- Function choose_parent chooses parent for the new node from near nodes found in previous function. Inside, function steer is used without expand_distance argument to get the node and cost from near nodes to new node. Then, the one with minimum cost is determined and finally steer is used once more to yield a node object which is a new node with updated parent.
- If choose_parent yields None, then new_node from very first steer call is added to the list.
- If however it yields some node, then function rewire is run. For every near node, steer is called to compute path and cost from new node, and if its collision free and cost is lower, then near node is adjusted and finally function propagate_cost_to_leaves is called to update parents and costs of nodes in new path.
- This concludes the searching part. After all the iterations, function search_best_goal_node is called which computes distance to all the nodes from the goal, chooses the minimum and collision free one and passes it as an argument for generate_final_course which iterates over all parents until it comes to the start.
A*:
- Algorithm runs using initializing data like start and goal nodes or boundary. In the initialization process, function get_motion_model embodies all possible movements of the robot and calc_obstacle_map creates a binary map with given resolution by checking all the nodes in the grid to list of obstacles and boundary points. Since this works in the grid, which is usually different from the workspace, function calc_grid_position calculates position of grid node in the workspace and then checks colisions.
- Function planning runs the whole process. First goal node and start node are defined. Function calc_xy_index calculates their grid position from their position in the workspace without grid. Two dictionaries are created : open_list and closed_list. The key in these dictionaries is created by calc_grid_index, which assigns every node a unique index.
- While loop keeps iterating the subsequent steps until either goal is found or open_list is empty.
- Current node is chosen based on the lowest cost+heuristic value. Search of neighboors is done based on the motion_model. function verify_node then checks whether the node collides with obstacles or boundary. Then, if node is already in the closed set, it is ignored, if in the open set, cost is improved if possible, and if its not in either, it is added to the open_set.
- The search continues until termination condition is fulfilled. In the end, function calc_final_path returns the path from start to goal by iterating over neighboors of nodes.