Skip to content
Snippets Groups Projects
Commit 4ce16de4 authored by Adam Sitabkhan's avatar Adam Sitabkhan
Browse files

Initial diversified RRT*

parent 5e4af514
No related branches found
No related tags found
No related merge requests found
"""
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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment