public void generateRRTObstaclesWithPath(int numNodes, OccupancyGrid occupancy, int pathSearchDepth) { // Create a new tree which we will populate with nodes, with the root of the tree // set to the initial configuration tree = new Tree(initialConfiguration); // Loop until the graph is the requested size for (int i = 0; i < numNodes; i++) { bool clearConf = false; StateConfig randConf = null; StateConfig newConf = null; TreeNode<StateConfig> closestNode = null; // Generate a random configuration within the configuration space until one is // found which does not fall in any occupied cell in the occupancy grid while (!clearConf){ randConf = getRandomConfiguration(limits); Tuple<float,float> pos = randConf.getPosition(); // If the random configuration is occupied, generate a new one. if(occupancy.isOccupied(pos.first, pos.second)) continue; // Find the closest node on the current graph to the point that was generated closestNode = tree.findClosest(randConf, Tree.SearchMethod.BFS); StateConfig closestConf = closestNode.content; // Generate a new configuration by moving along the hyper-edge connecting // the closest configuration and the randomly generated one. newConf = closestConf + (randConf - closestConf) * increment; Tuple<float,float> newPos = newConf.getPosition(); if (occupancy.isOccupied(newPos.first, newPos.second)) continue; if (!occupancy.obstacleFreeStraightPath(closestConf.getPosition(), newConf.getPosition(), pathSearchDepth)) continue; clearConf = true; } // Add the new configuration as a child of the closest node. TreeNode<StateConfig> newNode = tree.addNode(newConf, closestNode); // If we are within range of the goal with the new node, stop expanding the tree. if (checkGoalDist(newNode)) break; } }
/* * Generates an RRT with the specified number of nodes. The goalBias * parameter defines how biased the RRT should be towards choosing the * goal as the configuration to go towards. It should be a value between * 0 and 1. */ public void generateRRTBasic(int numNodes) { // Create a new tree which we will populate with nodes, with the // initial configuration as the root of the tree. tree = new Tree(initialConfiguration); // Loop until the tree has the requested number of nodes for (int i = 0; i < numNodes; i++) { // Generate a random configuration within the configuration space StateConfig randConf = getRandomConfiguration(limits); // Find the closest node on the current graph to the point that was generated TreeNode<StateConfig> closestNode = tree.findClosest(randConf, Tree.SearchMethod.BFS); StateConfig closestConf = closestNode.content; // Generate a new configuration by moving along the hyper-edge connecting // the closest configuration and the randomly generated one. StateConfig newConf = closestConf + (randConf - closestConf) * increment; // Add the new configuration as a child of the closest node. TreeNode<StateConfig> newNode = tree.addNode(newConf, closestNode); // If we are within range of the goal with the new node, stop expanding the tree. if (checkGoalDist(newNode)) break; } }