Initial diversified RRT*

guided_mrmp/utils/rrt_diversified.py

+"""
+Implements Klampt's various motion planners
+"""
+
+from OpenGL.GL import *
+from OpenGL.GLU import *
+from OpenGL.GLUT import *
+import math
+import random
+from klampt.plan.cspace import CSpace, MotionPlan
+from klampt.vis.glprogram import GLProgram
+from klampt.math import vectorops
+import numpy as np
+
+class Circle:
+    def __init__(self,x=0,y=0,radius=1):
+        self.center = (x,y)
+        self.radius = radius
+        
+    def contains(self,point):
+        return (vectorops.distance(point,self.center) <= self.radius)
+
+    def drawGL(self,res=0.01):
+        numdivs = int(math.ceil(self.radius*math.pi*2/res))
+        glBegin(GL_TRIANGLE_FAN)
+        glVertex2f(*self.center)
+        for i in range(numdivs+1):
+            u = float(i)/float(numdivs)*math.pi*2
+            glVertex2f(self.center[0]+self.radius*math.cos(u),self.center[1]+self.radius*math.sin(u))
+        glEnd()
+
+class Rectangle:
+    def __init__(self,x=0,y=0,width=1,height=1):
+        self.bottom_left = (x,y)
+        self.width = width
+        self.height = height
+    
+    def contains(self,point):
+        x,y = point
+        x0,y0 = self.bottom_left
+        return (x >= x0 and x <= x0+self.width and y >= y0 and y <= y0+self.height)
+    
+    def drawGL(self):
+        x,y = self.bottom_left
+        w = self.width
+        h = self.height
+        glBegin(GL_QUADS)
+        glVertex2f(x,y)
+        glVertex2f(x+w,y)
+        glVertex2f(x+w,y+h)
+        glVertex2f(x,y+h)
+        glEnd()
+
+class CircleRectangleObstacleCSpace(CSpace):
+    def __init__(self, x_bounds=(0.0,1.0), y_bounds=(0.0,1.0), robot_radius=0.05):
+        CSpace.__init__(self)
+        #set bounds
+        # self.bound = [(0.0,1.0),(0.0,1.0)]
+        self.bound = [(x_bounds[0], x_bounds[1]), (y_bounds[0], y_bounds[1])]
+        #set collision checking resolution
+        self.eps = 1e-3
+        #setup a robot with radius 0.05
+        self.robot = Circle(0,0,robot_radius)
+        #set obstacles here
+        self.obstacles = []
+        #store paths here
+        self.paths = []
+
+    def addObstacle(self,obs):
+        self.obstacles.append(obs)
+    
+    def feasible(self,q):
+        #bounds test
+        if not CSpace.feasible(self,q): return False
+        #TODO: Problem 1: implement your feasibility tests here
+        #currently, only the center is checked, so the robot collides
+        #with boundary and obstacles
+        for o in self.obstacles:
+            if o.contains(q): return False
+        return True
+
+    def drawObstaclesGL(self):
+        glColor3f(0.2,0.2,0.2)
+        for o in self.obstacles:
+            o.drawGL()
+
+    def drawRobotGL(self,q):
+        glColor3f(0,0,1)
+        newc = vectorops.add(self.robot.center,q)
+        c = Circle(newc[0],newc[1],self.robot.radius)
+        c.drawGL()
+
+class CSpaceObstacleProgram(GLProgram):
+    def __init__(self, space, start=(0.1,0.5), goal=(0.9,0.5), planner_name="rrt*"):
+        GLProgram.__init__(self)
+        self.space = space
+        #PRM planner
+        # MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=0.1)
+        self.optimizingPlanner = False
+        
+        #FMM* planner
+        #MotionPlan.setOptions(type="fmm*")
+        #self.optimizingPlanner = True
+        
+        #RRT planner
+        MotionPlan.setOptions(type=planner_name, perturbationRadius=0.25, bidirectional=True)
+        self.optimizingPlanner = False
+
+        #RRT* planner
+        #MotionPlan.setOptions(type="rrt*")
+        #self.optimizingPlanner = True
+        
+        #random-restart RRT planner
+        #MotionPlan.setOptions(type="rrt",perturbationRadius=0.25,bidirectional=True,shortcut=True,restart=True,restartTermCond="{foundSolution:1,maxIters:1000}")
+        #self.optimizingPlanner = True
+
+        #OMPL planners:
+        #Tested to work fine with OMPL's prm, lazyprm, prm*, lazyprm*, rrt, rrt*, rrtconnect, lazyrrt, lbtrrt, sbl, bitstar.
+        #Note that lbtrrt doesn't seem to continue after first iteration.
+        #Note that stride, pdst, and fmt do not work properly...
+        #MotionPlan.setOptions(type="ompl:rrt",suboptimalityFactor=0.1,knn=10,connectionThreshold=0.1)
+        #self.optimizingPlanner = True
+        
+        self.planner = MotionPlan(space)
+        self.start=start
+        self.goal=goal
+        self.planner.setEndpoints(start,goal)
+        self.path = []
+        self.G = None
+        
+    def keyboardfunc(self,key,x,y):
+        if key==' ':
+            if self.optimizingPlanner or not self.path:
+                print("Planning 1...")
+                self.planner.planMore(1)
+                self.path = self.planner.getPath()
+                self.G = self.planner.getRoadmap()
+                self.refresh()
+        elif key=='p':
+            if self.optimizingPlanner or not self.path:
+                print("Planning 100...")
+                self.planner.planMore(100)
+                self.path = self.planner.getPath()
+                self.G = self.planner.getRoadmap()
+                self.refresh()
+       
+    def display(self):
+        glMatrixMode(GL_PROJECTION)
+        glLoadIdentity()
+        glOrtho(0,1,1,0,-1,1);
+        glMatrixMode(GL_MODELVIEW)
+        glLoadIdentity()
+
+        glDisable(GL_LIGHTING)
+        self.space.drawObstaclesGL()
+        if self.path:
+            #draw path
+            glColor3f(0,1,0)
+            glBegin(GL_LINE_STRIP)
+            for q in self.path:
+                glVertex2f(q[0],q[1])
+            glEnd()
+            for q in self.path:
+                self.space.drawRobotGL(q)
+        else:
+            self.space.drawRobotGL(self.start)
+            self.space.drawRobotGL(self.goal)
+
+        if self.G:
+            #draw graph
+            V,E = self.G
+            glEnable(GL_BLEND)
+            glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
+            glColor4f(0,0,0,0.5)
+            glPointSize(3.0)
+            glBegin(GL_POINTS)
+            for v in V:
+                glVertex2f(v[0],v[1])
+            glEnd()
+            glColor4f(0.5,0.5,0.5,0.5)
+            glBegin(GL_LINES)
+            for (i,j) in E:
+                glVertex2f(V[i][0],V[i][1])
+                glVertex2f(V[j][0],V[j][1])
+            glEnd()
+            glDisable(GL_BLEND)
+
+if __name__=='__main__':
+    import argparse
+    
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--seed", 
+        type=int, 
+        default=None
+    )
+    args = parser.parse_args()
+    
+    if args.seed:
+        np.random.seed(args.seed)
+        random.seed(args.seed)
+    
+    iter = 1000
+    
+    # robot_starts = [[0.05, 0.05], [4.95, 4.95], [0.05, 4.95], [4.95, 0.05]]
+    # robot_goals = [[4.95, 4.95], [0.05, 0.05], [4.95, 0.05], [0.05, 4.95]]
+    robot_starts = [[0.05, 0.05], [4.95, 4.95]]
+    robot_goals = [[4.95, 4.95], [0.05, 0.05]]
+    circ_obstacles = [(3.5, 2.5, 1)]
+    rect_obstacles = []
+    
+    robot_paths = []
+    
+    def edgeCost(x1,x2):
+        x2 = np.array(x2)
+        sum_dist = 0
+        for path in robot_paths:
+            for loc in path:
+                sum_dist += np.linalg.norm(x2 - np.array(loc))
+        cost = np.log(1 + sum_dist)
+        # print(cost)
+        return cost
+    
+    space = CircleRectangleObstacleCSpace(x_bounds=(0,5), y_bounds=(0,5), robot_radius=0.05)
+    for (x, y, r) in circ_obstacles:
+        space.addObstacle(Circle(x, y, r))
+    for (x, y, w, h) in rect_obstacles:
+        space.addObstacle(Rectangle(x, y, w, h))
+    
+    planner = "rrt*"
+    MotionPlan.setOptions(type=planner, perturbationRadius=0.25, bidirectional=True)
+    
+    for start, goal in zip(robot_starts, robot_goals):
+        
+        mp = MotionPlan(space, type=planner)
+        mp.setEndpoints(start, goal)
+        mp.setCostFunction(edgeCost)
+        
+        mp.planMore(iter)
+        path = mp.getPath()
+        robot_paths.append(path)
+        
+        mp.close()
+
+    # program = CSpaceObstacleProgram(space,start,goal)
+    # program.view.w = program.view.h = 640
+    # program.name = "Motion planning test"
+
+    # if vis:
+    #     program.run()
+    #     path = program.planner.getPath()
+    # else:
+    # program.planner.planMore(iter)
+    # path = program.planner.getPath()
+    
+    import matplotlib.pyplot as plt
+    import matplotlib.patches as patches
+    
+    fig, ax = plt.subplots(1)
+    
+    for (x, y, r) in circ_obstacles:
+        circle = patches.Circle((x, y), r, facecolor='black', edgecolor=None)
+        ax.add_patch(circle)
+    for (x, y, w, h) in rect_obstacles:
+        rect = patches.Rectangle((x, y), w, h, facecolor='black', edgecolor=None)
+        ax.add_patch(rect)
+    
+    # colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd', '#8c564b', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf']
+    for i, path in enumerate(robot_paths):
+        path = np.array(path)
+        print(f"Path {i}: {path}")
+        x_coords = path[:,0]
+        y_coords = path[:,1]
+        plt.plot(x_coords, y_coords, label=f'Robot {i}')
+
+    plt.legend(loc='upper left')
+    ax.set_aspect('equal')
+
+    plt.show()
+        
+    
\ No newline at end of file