figure out how to check collisions property

kinodynamic_rrt/README.md

@@ -1 +1,6 @@
-### Kinodynamic RRT
+### Kinodynamic RRT
+
+This implementation is inspired by Jason Nezvadovitz's repositiory [lqrrt](https://github.com/jnez71/lqRRT).
+
+
+![Kinodynamic RRT](KinodynamicRRT.png)

kinodynamic_rrt/kinodynamic_rrt.py

@@ -27,9 +27,12 @@ def __init__(self,xs,cost=0):
 # max number of iterations
 
 class KinodynamicRRTStar:
-    def __init__(self, x_start, x_goal, time_forward,goal_sample_rate, throttle_speed, number_of_motion_primitives, iter_max,grid,min_speed=0.1):
+    def __init__(self, x_start, x_goal, time_forward,goal_sample_rate, throttle_speed, number_of_motion_primitives, iter_max,grid,min_speed=0.1,rrt_star=False):
+        self.rrt_star  = rrt_star
         self.s_start = Node(x_start)
         self.s_goal = Node(x_goal)
+        if(self.rrt_star):
+            self.s_start.cost = 0
 
         # Define the parameters for rrt
 
@@ -47,6 +50,9 @@ def __init__(self, x_start, x_goal, time_forward,goal_sample_rate, throttle_spee
         # Initialize the list of vertices with the start point
         self.list_of_vertices = [self.s_start]
 
+        # This prevents duplicate nodes from being added:
+        self.list_of_xys = []
+
         # The path begins as an empty list
         self.path = []
 
@@ -87,13 +93,18 @@ def planning(self):
                 print("Sample Number: ",k,"Number of vertices",len(self.list_of_vertices))
 
 
+            ## check if this point is already in the tree
+            point = (node_new.x,node_new.y)
+            add = not point in self.list_of_xys
+
+
             # get a new node and if the ray connecting the new node with near node 
             # is collision free then we can add the vertex to our list of nodes
-            if node_new and not self.is_collision(node_near, node_new):
-                # find the closest neighbors this is a list
-                #neighbor_indices = self.find_nearest_neighbors(node_new)
-                # append the new node
+            if node_new and not self.is_collision(node_near, node_new) and add:
+
+                # if the path is not in collision 
                 self.list_of_vertices.append(node_new)
+                self.list_of_xys.append(point)
 
 
             # get the index of the minimum cost vertex within a step length of the goal 
@@ -168,6 +179,10 @@ def new_state(self, node_start, node_goal):
         index = dist.argmin()
         nearest_trajectory = primitives[index]
 
+        # get the distance here too
+        if(self.rrt_star):
+            new_dist = dist[index]
+
         #print(nearest_trajectory)
 
         #print(pts[index],pts)
@@ -181,6 +196,9 @@ def new_state(self, node_start, node_goal):
         # each trajectory technically has a speed but I think I just want to assume that each point has 
         # a speed equally to the minimum speed, not sure how this will work
         node_new = Node(xs=new_state)
+
+        if(self.rrt_star):
+            node_new.cost = node_start.cost + new_dist 
 
         # make the parent of this node the nearest node
         node_new.parent = node_start
@@ -190,7 +208,11 @@ def new_state(self, node_start, node_goal):
 
 
     """function that checks for collisions in the newly sampled point,
-       In this case we can use the trajectory that was used in the euler simulation"""
+       In this case we can use the trajectory that was used in the euler simulation.
+       This currently naive because it just checks if the end point is in collision, which is fine 
+       if the step size is small but not correct if not
+       
+       """
     def is_collision(self,start_node,end_node):
 
         bloat =5 
@@ -267,6 +289,7 @@ def extract_path(self, node_end):
     def search_goal_parent(self,all_paths=False):
         # create a list of distances to the goal
         # random sampling allows you to add a node to the tree more than once (need to figure out how to prevent that)
+        # did that I can now change this to be simpler
         # well we will only consider 
         candidates =[]
         check_for_redundant = []
@@ -347,13 +370,15 @@ def plot_final_all(self):
     x_start = (-0.006356, 0.030406, 0.322701, 0.1)
     x_goal = (1.077466, 0.921832,0.750663, 0.1)
     grid = 'porto_grid.npy'
-    time_forward = 0.2
-    n_samples = 100
+    time_forward = 0.3
+    n_samples = 10000
     goal_sample_rate = 0.10
     throttle_speed = 0.3
     number_of_motion_primitives = 5
 
+    # RRT or RRT*
     kinodynamic_rrt = KinodynamicRRTStar(x_start, x_goal, time_forward,goal_sample_rate, throttle_speed, number_of_motion_primitives, n_samples,grid,min_speed=0.1)
+    #kinodynamic_rrt = KinodynamicRRTStar(x_start, x_goal, time_forward,goal_sample_rate, throttle_speed, number_of_motion_primitives, n_samples,grid,min_speed=0.1,rrt_star=True)
     kinodynamic_rrt.planning()
     kinodynamic_rrt.plot_final()
     #kinodynamic_rrt.plot_final_all()

learning_ompl/planner_and_graph.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python
+
+
+
+# Author: Ryan Luna
+try:
+    # graph-tool and py-OMPL have some minor issues coexisting with each other.  Both modules
+    # define conversions to C++ STL containers (i.e. std::vector), and the module that is imported
+    # first will have its conversions used.  Order doesn't seem to matter on Linux,
+    # but on Apple, graph_tool will not be imported properly if OMPL comes first.
+    import graph_tool.all as gt
+    graphtool = True
+except ImportError:
+    print('Failed to import graph-tool.  PlannerData will not be analyzed or plotted')
+    graphtool = False
+
+try:
+    from ompl import base as ob
+    from ompl import geometric as og
+except ImportError:
+    # if the ompl module is not in the PYTHONPATH assume it is installed in a
+    # subdirectory of the parent directory called "py-bindings."
+    from os.path import abspath, dirname, join
+    import sys
+    sys.path.insert(0, join(dirname(dirname(abspath(__file__))), 'py-bindings'))
+    from ompl import base as ob
+    from ompl import geometric as og
+
+
+# Create a narrow passage between y=[-3,3].  Only a 6x6x6 cube will be valid, centered at origin
+def isStateValid(state):
+    if state.getY() >= -3 and state.getY() <= 3:
+        return state.getX() >= -3 and state.getX() <= 3 and \
+            state.getZ() >= -3 and state.getZ() <= 3
+    return True
+
+def useGraphTool(pd):
+    # Extract the graphml representation of the planner data
+    graphml = pd.printGraphML()
+    f = open("graph.graphml", 'w')
+    f.write(graphml)
+    f.close()
+
+    # Load the graphml data using graph-tool
+    graph = gt.load_graph("graph.graphml", fmt="xml")
+    edgeweights = graph.edge_properties["weight"]
+
+    # Write some interesting statistics
+    avgdeg, stddevdeg = gt.vertex_average(graph, "total")
+    avgwt, stddevwt = gt.edge_average(graph, edgeweights)
+
+    print("---- PLANNER DATA STATISTICS ----")
+    print(str(graph.num_vertices()) + " vertices and " + str(graph.num_edges()) + " edges")
+    print("Average vertex degree (in+out) = " + str(avgdeg) + "  St. Dev = " + str(stddevdeg))
+    print("Average edge weight = " + str(avgwt)  + "  St. Dev = " + str(stddevwt))
+
+    _, hist = gt.label_components(graph)
+    print("Strongly connected components: " + str(len(hist)))
+
+    # Make the graph undirected (for weak components, and a simpler drawing)
+    graph.set_directed(False)
+    _, hist = gt.label_components(graph)
+    print("Weakly connected components: " + str(len(hist)))
+
+    # Plotting the graph
+    gt.remove_parallel_edges(graph) # Removing any superfluous edges
+
+    edgeweights = graph.edge_properties["weight"]
+    colorprops = graph.new_vertex_property("string")
+    vertexsize = graph.new_vertex_property("double")
+
+    start = -1
+    goal = -1
+
+    for v in range(graph.num_vertices()):
+
+        # Color and size vertices by type: start, goal, other
+        if pd.isStartVertex(v):
+            start = v
+            colorprops[graph.vertex(v)] = "cyan"
+            vertexsize[graph.vertex(v)] = 10
+        elif pd.isGoalVertex(v):
+            goal = v
+            colorprops[graph.vertex(v)] = "green"
+            vertexsize[graph.vertex(v)] = 10
+        else:
+            colorprops[graph.vertex(v)] = "yellow"
+            vertexsize[graph.vertex(v)] = 5
+
+    # default edge color is black with size 0.5:
+    edgecolor = graph.new_edge_property("string")
+    edgesize = graph.new_edge_property("double")
+    for e in graph.edges():
+        edgecolor[e] = "black"
+        edgesize[e] = 0.5
+
+    # using A* to find shortest path in planner data
+    if start != -1 and goal != -1:
+        _, pred = gt.astar_search(graph, graph.vertex(start), edgeweights)
+
+        # Color edges along shortest path red with size 3.0
+        v = graph.vertex(goal)
+        while v != graph.vertex(start):
+            p = graph.vertex(pred[v])
+            for e in p.out_edges():
+                if e.target() == v:
+                    edgecolor[e] = "red"
+                    edgesize[e] = 2.0
+            v = p
+
+    # Writing graph to file:
+    # pos indicates the desired vertex positions, and pin=True says that we
+    # really REALLY want the vertices at those positions
+    gt.graph_draw(graph, vertex_size=vertexsize, vertex_fill_color=colorprops,
+                edge_pen_width=edgesize, edge_color=edgecolor,
+                output="graph.png")
+    print('\nGraph written to graph.png')
+
+def plan():
+    # construct the state space we are planning in
+    space = ob.SE3StateSpace()
+
+    # set the bounds for R^3 portion of SE(3)
+    bounds = ob.RealVectorBounds(3)
+    bounds.setLow(-10)
+    bounds.setHigh(10)
+    space.setBounds(bounds)
+
+    # define a simple setup class
+    ss = og.SimpleSetup(space)
+
+    # create a start state
+    start = ob.State(space)
+    start().setX(-9)
+    start().setY(-9)
+    start().setZ(-9)
+    start().rotation().setIdentity()
+
+    # create a goal state
+    goal = ob.State(space)
+    goal().setX(-9)
+    goal().setY(9)
+    goal().setZ(-9)
+    goal().rotation().setIdentity()
+
+    ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
+
+    # set the start and goal states
+    ss.setStartAndGoalStates(start, goal, 0.05)
+
+    # Lets use PRM.  It will have interesting PlannerData
+    planner = og.PRM(ss.getSpaceInformation())
+    ss.setPlanner(planner)
+    ss.setup()
+
+    # attempt to solve the problem
+    solved = ss.solve(20.0)
+
+    if solved:
+        # print the path to screen
+        print("Found solution:\n%s" % ss.getSolutionPath())
+
+        # Extracting planner data from most recent solve attempt
+        pd = ob.PlannerData(ss.getSpaceInformation())
+        ss.getPlannerData(pd)
+
+        # Computing weights of all edges based on state space distance
+        pd.computeEdgeWeights()
+
+        if graphtool:
+            useGraphTool(pd)
+
+if __name__ == "__main__":
+    plan()