From fcc1044434dec624e9f86a91866083a649000276 Mon Sep 17 00:00:00 2001
From: rachelmoan <moanrachel516@gmail.com>
Date: Sat, 8 Feb 2025 13:32:09 -0600
Subject: [PATCH] use klampt motion planner to get decoupled paths
---
.../controllers/multi_path_tracking.py | 3 +-
.../controllers/multi_path_tracking_db.py | 40 +--
guided_mrmp/controllers/utils.py | 3 +-
guided_mrmp/main.py | 22 +-
guided_mrmp/planners/RRT.py | 237 ------------------
guided_mrmp/planners/RRTStar.py | 50 ----
guided_mrmp/planners/__init__.py | 2 -
guided_mrmp/utils/helpers.py | 50 ++--
guided_mrmp/utils/klampt_planner.py | 210 ++++++++++++++++
9 files changed, 270 insertions(+), 347 deletions(-)
delete mode 100644 guided_mrmp/planners/RRT.py
delete mode 100644 guided_mrmp/planners/RRTStar.py
delete mode 100644 guided_mrmp/planners/__init__.py
create mode 100644 guided_mrmp/utils/klampt_planner.py
diff --git a/guided_mrmp/controllers/multi_path_tracking.py b/guided_mrmp/controllers/multi_path_tracking.py
index cfadef7..5c36a06 100644
--- a/guided_mrmp/controllers/multi_path_tracking.py
+++ b/guided_mrmp/controllers/multi_path_tracking.py
@@ -6,7 +6,7 @@ from guided_mrmp.controllers.multi_mpc import MultiMPC
from guided_mrmp.controllers.mpc import MPC
class MultiPathTracker:
- def __init__(self, env, initial_positions, dynamics, target_v, T, DT, waypoints, settings):
+ def __init__(self, env, initial_positions, dynamics, target_v, T, DT, waypoints, trees, settings):
"""
Initializes the PathTracker object.
Parameters:
@@ -27,6 +27,7 @@ class MultiPathTracker:
self.T = T
self.DT = DT
self.target_v = target_v
+ self.trees
self.radius = dynamics.radius
diff --git a/guided_mrmp/controllers/multi_path_tracking_db.py b/guided_mrmp/controllers/multi_path_tracking_db.py
index 1eef79d..957809b 100644
--- a/guided_mrmp/controllers/multi_path_tracking_db.py
+++ b/guided_mrmp/controllers/multi_path_tracking_db.py
@@ -1,6 +1,3 @@
-from guided_mrmp.planners.RRTStar import RRTStar
-
-
import numpy as np
import matplotlib.pyplot as plt
@@ -456,25 +453,30 @@ class MultiPathTrackerDB(MultiPathTracker):
# plan an RRT path from the current state to the goal
start = (new_ref[:, -1][0], new_ref[:, -1][1])
goal = (old_paths[i][:, -1][0], old_paths[i][:, -1][1])
-
- rrtpath, tree = plan_decoupled_path(self.env,
- start,
- goal,
- solver=self.settings["sampling_based_planner"]["name"],
- step_length=self.settings["sampling_based_planner"]["step_length"],
- goal_sample_rate=self.settings["sampling_based_planner"]["goal_sample_rate"],
- num_samples=self.settings["sampling_based_planner"]["num_samples"],
- r=self.settings["sampling_based_planner"]["r"])
+ print(f"planning from {start} to {goal}")
+ print(f"new_ref = {new_ref}")
+
+ print(f"other new ref = {continuous_soln[(robot_idx+1)*3:(robot_idx+1)*3+3, :]}")
+
+ rrtpath, tree = plan_decoupled_path(self.settings["sampling_based_planner"]["name"],
+ start,
+ goal,
+ self.env,
+ self.radius,
+ self.env.circle_obs,
+ self.env.rect_obs,
+ vis=False,
+ iter=self.settings["sampling_based_planner"]["num_samples"])
- if rrtpath :
- xs = new_ref[0, :].tolist()
- ys = new_ref[1, :].tolist()
+
+ xs = new_ref[0, :].tolist()
+ ys = new_ref[1, :].tolist()
- for node in rrtpath:
- xs.append(node[0])
- ys.append(node[1])
+ for node in rrtpath:
+ xs.append(node[0])
+ ys.append(node[1])
- wp = [xs,ys]
+ wp = [xs,ys]
# Path from waypoint interpolation
path = compute_path_from_wp(wp[0], wp[1], 0.05)
diff --git a/guided_mrmp/controllers/utils.py b/guided_mrmp/controllers/utils.py
index dfe4aad..ca75e31 100644
--- a/guided_mrmp/controllers/utils.py
+++ b/guided_mrmp/controllers/utils.py
@@ -110,7 +110,8 @@ def get_ref_trajectory(state, path, target_v, T, DT, path_visited_points=[]):
last_visited_idx = 0 if path_visited_points == [] else path_visited_points[-1]
# Find the spatially closest point after the last visited point
- next_ind = last_visited_idx + 2
+ next_ind = last_visited_idx + 1
+
ind = next_ind
# ind = get_nn_idx(state, path, path_visited_points)
diff --git a/guided_mrmp/main.py b/guided_mrmp/main.py
index 82f0a8f..a075077 100644
--- a/guided_mrmp/main.py
+++ b/guided_mrmp/main.py
@@ -35,14 +35,15 @@ def initialize_robots(starts, goals, dynamics_models, radii, target_v, env, sett
colors = [list(np.random.choice(range(256), size=3)) for i in range(len(starts))]
for i, (start, goal, dynamics_model, radius, color) in enumerate(zip(starts, goals, dynamics_models, radii, colors)):
- rrtpath, tree = plan_decoupled_path(env,
- (start[0],start[1]),
- (goal[0],goal[1]),
- solver=settings["sampling_based_planner"]["name"],
- step_length=settings["sampling_based_planner"]["step_length"],
- goal_sample_rate=settings["sampling_based_planner"]["goal_sample_rate"],
- num_samples=settings["sampling_based_planner"]["num_samples"],
- r=settings["sampling_based_planner"]["r"])
+ rrtpath, tree = plan_decoupled_path(settings["sampling_based_planner"]["name"],
+ (start[0],start[1]),
+ (goal[0],goal[1]),
+ env,
+ radius,
+ env.circle_obs,
+ env.rect_obs,
+ vis=False,
+ iter=settings["sampling_based_planner"]["num_samples"])
xs = []
ys = []
@@ -122,6 +123,8 @@ if __name__ == "__main__":
# Create the Guided MRMP policy
T = settings['prediction_horizon']
DT = settings['discretization_step']
+
+ rrt_trees = [robot.tree for robot in robots]
policy = MultiPathTrackerDB(env=env,
initial_positions=robot_starts,
dynamics=dynamics_models[0], # NOTE: Using the same dynamics model for all robots for now
@@ -129,6 +132,7 @@ if __name__ == "__main__":
T=T,
DT=DT,
waypoints=[robot.waypoints for robot in robots],
+ trees = rrt_trees,
settings=settings
)
@@ -138,7 +142,7 @@ if __name__ == "__main__":
# Run the simulation
start = time.time()
- sim.run(libs, show_vis)
+ x_hist, y_hist, h_hist = sim.run(libs, show_vis)
end = time.time()
print(f"Simulation took {end-start} seconds")
diff --git a/guided_mrmp/planners/RRT.py b/guided_mrmp/planners/RRT.py
deleted file mode 100644
index b56d765..0000000
--- a/guided_mrmp/planners/RRT.py
+++ /dev/null
@@ -1,237 +0,0 @@
-"""
-RRT implementation
-"""
-import math
-import numpy as np
-
-from guided_mrmp.utils import Node, Env
-
-class RRT:
- def __init__(self, env, s_start, s_goal, step_len, goal_sample_rate, iter_max, sampled_vertices=None):
- self.s_start = Node(s_start,s_start,0,0)
- self.s_goal = Node(s_goal,s_goal, 0,0)
- self.step_len = step_len
- self.goal_sample_rate = goal_sample_rate
- self.iter_max = iter_max
-
- if sampled_vertices is None:
- self.sampled_vertices = [self.s_start]
- else:
- self.sampled_vertices = sampled_vertices
-
- self.env = env
- # self.plotting = Plotting(self.env)
- # self.utils = utils.Utils()
-
- self.x_range = self.env.boundary[0]
- self.y_range = self.env.boundary[1]
- self.obs_circle = self.env.circle_obs
- self.obs_rectangle = self.env.rect_obs
-
- def dist(self, node1, node2):
- return math.hypot(node2.x - node1.x, node2.y - node1.y)
-
- def angle(self, node1, node2):
- return math.atan2(node2.y - node1.y, node2.x - node1.x)
-
- def is_collision(self, start, end):
- """
- determine if the line from start to end is in collision with obstacles
- """
- if self.env.is_inside_obs(start) or self.env.is_inside_obs(end):
- return True
-
- o, d = self.env.get_ray(start, end)
- # obs_vertex = self.env.get_obs_vertex()
-
- # for (v1, v2, v3, v4) in obs_vertex:
- # if self.env.is_intersect_rec(start, end, o, d, v1, v2):
- # return True
- # if self.env.is_intersect_rec(start, end, o, d, v2, v3):
- # return True
- # if self.env.is_intersect_rec(start, end, o, d, v3, v4):
- # return True
- # if self.env.is_intersect_rec(start, end, o, d, v4, v1):
- # return True
-
- for (x, y, r) in self.obs_circle:
- if self.env.is_intersect_circle(o, d, [x, y], r):
- return True
-
- from shapely.geometry import LineString, Polygon
- line = LineString([(start.x, start.y), (end.x, end.y)])
- for (x, y, w, h) in self.obs_rectangle:
- poly = Polygon([(x, y), (x+w, y), (x+w, y+h), (x, y+h)])
- if line.intersects(poly):
- return True
-
- return False
-
-
- def get_nearest_node(self, node_list, node):
- # find nearest neighbor
- dist = [self.dist(node, nd) for nd in node_list]
- node_near = node_list[int(np.argmin(dist))]
-
- # regular and generate new node
- dist, theta = self.dist(node_near, node), self.angle(node_near, node)
- dist = min(self.max_dist, dist)
- node_new = Node((node_near.x + dist * math.cos(theta),
- (node_near.y + dist * math.sin(theta))),
- node_near.current, node_near.g + dist, 0)
-
- # obstacle check
- if not self.is_collision(node_new, node_near):
- # rewire optimization
- for node_n in node_list:
- # check if the node is within optimization distance
- dist_to_new_node = self.dist(node_n, node_new)
- if dist_to_new_node < self.r:
- cost = node_n.g + dist_to_new_node
- # update new sample node's cost and parent
- if node_new.g > cost and not self.is_collision(node_n, node_new):
- node_new.parent = node_n.current
- node_new.g = cost
- else:
- # update node's cost inside the radius
- cost = node_new.g + dist_to_new_node
- if node_n.g > cost and not self.is_collision(node_n, node_new):
- node_n.parent = node_new.current
- node_n.g = cost
- else:
- continue
- return node_new
-
-
- def generate_random_node(self, goal_sample_rate):
- delta = self.env.delta
-
- if np.random.random() > goal_sample_rate:
- # return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
- # np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
- return Node((np.random.randint(self.x_range[0] + delta, self.x_range[1]),
- np.random.randint(self.y_range[0] + delta, self.y_range[1])))
-
- return self.s_goal
-
- def nearest_neighbor(self, node_list, node):
- # find nearest neighbor
- dist = [self.dist(node, nd) for nd in node_list]
- node_near = node_list[int(np.argmin(dist))]
-
- # regular and generate new node
- dist, theta = self.dist(node_near, node), self.angle(node_near, node)
- dist = min(self.step_len, dist)
- node_new = Node((node_near.x + dist * math.cos(theta),
- (node_near.y + dist * math.sin(theta))),
- node_near.current, node_near.g + dist, 0)
-
- # obstacle check
- if self.is_collision(node_new, node_near):
- return None
- return node_new
-
- def new_state(self, node_start, node_end):
- dist, theta = self.get_distance_and_angle(node_start, node_end)
-
- dist = min(self.step_len, dist)
- node_new = Node((node_start.x + dist * math.cos(theta),
- node_start.y + dist * math.sin(theta)))
- node_new.parent = node_start.current
-
- return node_new
-
- def extract_path(self, node_end):
- path = [(self.s_goal.x, self.s_goal.y)]
- node_now = node_end
-
- while node_now.parent is not None:
- node_now = node_now.parent
- path.append((node_now.x, node_now.y))
-
- return path
-
- def get_distance_and_angle(self, node_start, node_end):
- dx = node_end.x - node_start.x
- dy = node_end.y - node_start.y
- return math.hypot(dx, dy), math.atan2(dy, dx)
-
- def extractPath(self, closed_set):
- """
- Extract the path based on the CLOSED set.
-
- Parameters:
- closed_set (list): CLOSED set
-
- Returns
- cost (float): the cost of planning path
- path (list): the planning path
- """
- node = closed_set[closed_set.index(self.s_goal)]
- path = [node.current]
- cost = node.g
- while node != self.s_start:
- node_parent = closed_set[closed_set.index(Node(node.parent, None, None, None))]
- node = node_parent
-
- path.append(node.current)
-
-
- # return the cost, path, and the tree of the sampled vertices
- return cost, path, closed_set
-
- def get_distance_and_angle(self, node_start, node_end):
- dx = node_end.x - node_start.x
- dy = node_end.y - node_start.y
- return math.hypot(dx, dy), math.atan2(dy, dx)
-
- def plan(self):
- for _ in range(self.iter_max):
-
- # generate a random node in the map
- node_rand = self.generate_random_node(self.goal_sample_rate)
-
- if node_rand in self.sampled_vertices:
- continue
-
- node_new = self.nearest_neighbor(self.sampled_vertices, node_rand)
- # print(f"node new = {node_new}")
- # node_new = self.new_state(node_near, node_rand)
-
- if node_new:
-
- self.sampled_vertices.append(node_new)
- dist, __ = self.get_distance_and_angle(node_new, self.s_goal)
-
- if dist <= self.step_len and not self.is_collision(node_new, self.s_goal):
- self.s_goal.parent = node_new.current
- self.s_goal.g = node_new.g + self.dist(self.s_goal, node_new)
- self.sampled_vertices.append(self.s_goal)
- return self.extractPath(self.sampled_vertices)
-
-
- return 0, None, None
-
- def run(self):
- cost, path, tree = self.plan()
- # self.plotting.animation([path], "RRT", cost, self.sampled_vertices)
- # print(f"num of sampled vertices = {len(self.sampled_vertices)}")
-
- # for node in self.sampled_vertices:
- # print(f"{node.current}")
- return path, tree
-
-
-if __name__ == "__main__":
- x_start = (6.394267984578837, 0.25010755222666936) # Starting node
- x_goal = (2.7502931836911926, 2.2321073814882277) # Goal node
-
- rrt = RRT(x_start, x_goal, 0.5, 0.05, 50000)
- path = rrt.run()
-
- print(path)
-
-
-
-
-
diff --git a/guided_mrmp/planners/RRTStar.py b/guided_mrmp/planners/RRTStar.py
deleted file mode 100644
index 2efdcfd..0000000
--- a/guided_mrmp/planners/RRTStar.py
+++ /dev/null
@@ -1,50 +0,0 @@
-"""
-RRT*
-"""
-from guided_mrmp.planners.RRT import RRT
-
-class RRTStar(RRT):
- def __init__(self, env, s_start, s_goal, step_len, goal_sample_rate, iter_max, r, sampled_vertices=None):
- super().__init__(env, s_start, s_goal, step_len, goal_sample_rate, iter_max, sampled_vertices=None)
- self.r = r
- self.name="RRT*"
-
- def nearest_neighbor(self, node_list, node):
- # print("using correct nn funct")
- node_new = super().nearest_neighbor(node_list, node)
- # node_new = self.new_state(node_nearest, node)
- if node_new:
- # rewire optimization
- for node_n in node_list:
- # inside the optimization circle
- new_dist = self.dist(node_n, node_new)
- if new_dist < self.r:
- cost = node_n.g + new_dist
- # update new sample node's cost and parent
- if node_new.g > cost and not self.is_collision(node_n, node_new):
- node_new.parent = node_n.current
- node_new.g = cost
- else:
- # update nodes' cost inside the radius
- cost = node_new.g + new_dist
- if node_n.g > cost and not self.is_collision(node_n, node_new):
- node_n.parent = node_new.current
- node_n.g = cost
- else:
- continue
- return node_new
- else:
- return None
-
-if __name__ == "__main__":
- # x_start = (18, 8) # Starting node
- # x_goal = (37, 18) # Goal node
- x_start = (6.394267984578837, 0.25010755222666936) # Starting node
- x_goal = (2.7502931836911926, 2.2321073814882277) # Goal node
-
- # rrt = RRT(x_start, x_goal, 0.5, 0.05, 10000)
- # path = rrt.run()
-
- rrtstar = RRTStar(x_start, x_goal, 0.5, 0.05, 10000, r=10.0)
- path = rrtstar.run()
- print(path)
diff --git a/guided_mrmp/planners/__init__.py b/guided_mrmp/planners/__init__.py
deleted file mode 100644
index d62e648..0000000
--- a/guided_mrmp/planners/__init__.py
+++ /dev/null
@@ -1,2 +0,0 @@
-from guided_mrmp.planners.RRT import RRT
-from guided_mrmp.planners.RRTStar import RRTStar
\ No newline at end of file
diff --git a/guided_mrmp/utils/helpers.py b/guided_mrmp/utils/helpers.py
index 2af6476..64fd00f 100644
--- a/guided_mrmp/utils/helpers.py
+++ b/guided_mrmp/utils/helpers.py
@@ -1,6 +1,5 @@
import random
-from guided_mrmp.planners import RRT
-from guided_mrmp.planners import RRTStar
+from guided_mrmp.utils.klampt_planner import CircleRectangleObstacleCSpace, CSpaceObstacleProgram, Circle, Rectangle
"""
Helper Functions that are commonly used in the guided_mrmp package
@@ -58,34 +57,29 @@ def initialize_libraries(library_fnames=["guided_mrmp/database/2x3_library","gui
return lib_2x3, lib_3x3, lib_2x5
-def plan_decoupled_path(env, start, goal, solver="RRT*",
- step_length=.5, goal_sample_rate=.05, num_samples=10000, r=3):
- """
- Plan decoupled path from a given start to a given goal, using a single-agent solver.
-
- inputs:
- - start (tuple): (x,y) location of start
- - goal (tuple): (x,y) location of goal
- - solver (string): Name of single-agent solver to be used
- - step_length (float):
- - goal_sample_rate (float):
- - num_samples (int):
- - r (float):
- output:
- - path (list): list of nodes in path
- """
- if solver == "RRT":
- rrt = RRT(env, start, goal, step_length, goal_sample_rate, num_samples)
- path, tree = rrt.run()
- elif solver == "RRT*":
- rrtstar = RRTStar(env, start, goal, step_length, goal_sample_rate, num_samples,r)
- path, tree = rrtstar.run()
- else:
- print(f"Solver {solver} is not yet implemented. Choose something else.")
- return None
+def plan_decoupled_path(solver_name, start, goal, env, robot_radius,circle_obs, rectangle_obs,vis=False, iter=100):
+ x_bounds = env.boundary[0]
+ y_bounds = env.boundary[1]
+ space = CircleRectangleObstacleCSpace(x_bounds,y_bounds,robot_radius)
- return list(reversed(path)), tree
+ for obs in circle_obs:
+ space.addObstacle(Circle(obs[0],obs[1],obs[2]))
+ for obs in rectangle_obs:
+ space.addObstacle(Rectangle(obs[0],obs[1],obs[2],obs[3]))
+
+ program = CSpaceObstacleProgram(space,start,goal,solver_name)
+ 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()
+ tree = program.planner.getRoadmap()
+ return path, tree
def read_agents_from_file(fname):
pass
diff --git a/guided_mrmp/utils/klampt_planner.py b/guided_mrmp/utils/klampt_planner.py
new file mode 100644
index 0000000..cf6efdb
--- /dev/null
+++ b/guided_mrmp/utils/klampt_planner.py
@@ -0,0 +1,210 @@
+"""
+Implements Klampt's various motion planners
+"""
+
+from OpenGL.GL import *
+from OpenGL.GLU import *
+from OpenGL.GLUT import *
+import math
+from klampt.plan.cspace import CSpace,MotionPlan
+from klampt.vis.glprogram import GLProgram
+from klampt.math import vectorops
+
+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 = []
+
+ 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__':
+ space = None
+ start = None
+ goal = None
+ vis = True
+ iter = 100
+
+ space = CircleRectangleObstacleCSpace()
+ space.addObstacle(Circle(0.5,0.5,0.1))
+ space.addObstacle(Rectangle(0.2,0.2,0.1,0.1))
+ start=(0.06,0.5)
+ goal=(0.94,0.5)
+
+
+ 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()
+
+ # print("Path:",path)
+
\ No newline at end of file
--
GitLab