def main2():
map = Map("maps/rss_offset.json")
planner = RRT(map)
start = [.5, .5]
goal = [3.5, 2.5]
path = np.array(planner.getPath(start, goal))
r = Controller(start[0], start[1], 0, map, path)
a = input("presse a key to start controller: ")
x, y, v, w, d = r.followPath()
def test6():
obstacles = [Polygon([(2,0), (2, 2), (3, 2), (3, 0)]),
Polygon([(4,4), (4,8), (7,1)]),
Polygon([(2,2), (1,8), (2,4), (3,3)])]
workspace = Workspace(dim=(12,12), obstacles=obstacles)
car_rrt = RRT(workspace, (1, 1, pi), (10, 10, pi), 10000, num_rand_actions=4, max_control_time=1, step_threshold=.05)
results = car_rrt.build_rrt()
print(results)
ax = workspace.graph_results(results, car_rrt, plot_tree=False)
plt.show()
def test22():
obstacles = [Polygon([(2,0), (8, 0), (8, 1), (2, 1)]),
Polygon([(2,10), (8,10), (8,8), (2,8)]),
Polygon([(2,1), (2,8), (3,8), (3,1)])]
workspace = Workspace(dim=(12,12), obstacles=obstacles)
car_rrt = RRT(workspace, (1, 6, pi), (6, 6, pi), 10000, num_rand_actions=6, max_control_time=1, step_threshold=.05)
results = car_rrt.build_rrt()
print(results)
anim = workspace.animate_results(results, car_rrt, plot_tree=True)
plt.show()
def generate_paths(self, algo="a-star"):
if algo == "a-star":
for robot in self.robots:
astar = Astar(robot.pose, robot.goal, self.obstacles)
robot.path = astar.generate_path()
elif algo == "rrt":
for robot in self.robots:
rrt = RRT(robot.pose, robot.goal, self.obstacles)
robot.path = rrt.generate_path()
else:
print "No such algo currently exists."
return
def plan_cb(self, goal):
# Wait for map to be created
while not rospy.is_shutdown():
if self.map is not None:
break
rospy.loginfo_throttle(5, 'Planning: Waiting for map...')
self.wait_rate.sleep()
rospy.loginfo('Planning: map recieved!')
# Create waypoints for the planner to pass through
waypoints = self.create_waypoints(self.map.gates)
start_wp = waypoints.pop(0)
# Run path planning
planner = RRT(self.map)
traj = planner.plan_traj(start_wp, waypoints)
while traj is None and not rospy.is_shutdown():
rospy.logwarn('RRT: Restarting RRT...')
traj = planner.plan_traj(start_wp, waypoints)
# Convert trajectory of Beziers to dd241909_msgs/Trajectory
traj_pieces = []
for bezier in traj:
piece = TrajectoryPolynomialPiece()
for c in bezier.coeffs:
piece.poly_x.append(c.x)
piece.poly_y.append(c.y)
piece.poly_z.append(c.z)
piece.poly_yaw.append(0.0)
piece.duration = rospy.Duration(bezier.T)
traj_pieces.append(piece)
traj_msg = Trajectory()
traj_msg.header.frame_id = 'map'
traj_msg.header.stamp = rospy.Time.now()
traj_msg.pieces = traj_pieces
# Set yaw values of traj_msg inplace
self.set_yaw_gates(traj_msg, start_wp, traj)
# self.set_yaw_rotating(traj_msg, start_wp, waypoints[-1])
# Publish trajectory
self.traj_pub.publish(traj_msg)
# Visualise trajectory in rviz
if self.use_rviz:
publish_traj_to_rviz(traj_msg)
self.plan_server.set_succeeded(SimpleResult(message=''))
class RrtGa:
'''
结合RRT和GA算法来
解决路径规划问题
输入图，遗传算法的一些参数
'''
def __init__(self, my_map):
self.map = my_map
self.pop_num = 20 # 种群数量
self.rrt = RRT(self.map)
self.mutate_rate = 0.0001 # 突变概率
self.iterations = 100
self.cross_rate = 0.9
def init_pop(self):
paths = []
for _ in range(self.pop_num):
path = self.rrt.find_path()
paths.append(path)
return paths
def find_path(self):
self.pop = self.init_pop() # 生成初始种群
self.fits = [self.map.get_fitness(path)
for path in self.pop] # 计算初始种群的fitness值
for iteration in range(self.iterations):
pass
def __init__(self):
# self.command_sub_ = rospy.Subscriber('controller_commands_dev', Controller_Commands, self.cmdCallback, queue_size=5)
# self.command_pub_ = rospy.Publisher('controller_commands', Controller_Commands, queue_size=1)
# self.state_sub_ = rospy.Subscriber('state', State, self.stateCallback, queue_size=1)
ref_lat = rospy.get_param("ref_lat")
ref_lon = rospy.get_param("ref_lon")
ref_h = rospy.get_param("ref_h")
self.ref_pos = [ref_lat, ref_lon, ref_h]
#Get the obstacles, boundaries, and drop location in order to initialize the RRT class
mission_type, self.obstacles, self.boundary_list, self.boundary_poly, drop_location = tools.get_server_data(
JudgeMission.MISSION_TYPE_DROP, self.ref_pos)
self.RRT_planner = RRT(
self.obstacles, self.boundary_list, animate=False
) #Other arguments are available but have been given default values in the RRT constructor
# Advertise the path planning service call
self._plan_server = rospy.Service('plan_path', PlanMissionPoints,
self.waypoints_callback)
#Set up a service call to get waypoints from the mission planner
self.mission_data = rospy.ServiceProxy('plan_mission',
PlanMissionPoints)
def test_rrt_init_args(self):
rrt = RRT(start_state=(0., 0.),
goal_state=(1, 1),
dim_ranges=[(-2, 2), (-2, 2)],
obstacles=None,
step_size=0.1,
max_iter=1000)
def test_build(self):
num_retries = 3
for _ in range(num_retries):
rrt1 = RRT(start_state=(0.2, 0.2),
goal_state=(0.8, 0.8),
dim_ranges=[(0, 1), (0, 1)],
max_iter=1000)
path = rrt1.build()
if path is not None:
break
else:
print("retrying...")
self.assertIsNotNone(path)
self.assertTrue(np.all(path[0] == [0.2, 0.2]))
self.assertTrue(np.all(path[-1] == [0.8, 0.8]))
def rrt_local(self, pe, start, end):
dims = len(start.vertex)
lims = []
for i in range(dims):
if start.vertex[i] < end.vertex[i]:
min = start.vertex[i] - 20
max = end.vertex[i] + 20
else:
max = start.vertex[i] + 20
min = end.vertex[i] - 20
lims.append([min,max])
rrt = RRT(50,
dims,
self.epsilon,
lims=np.array(lims),
connect_prob=.15,
collision_func=pe.test_collisions)
plan = rrt.build_rrt(start.vertex, end.vertex, pe.robot)
return plan
def test_rrt(self):
"""
Tests that the RRT class works
"""
env = StaticEnvironment((100, 100), 100)
rrt = RRT(env)
# Selection of random starting and ending points
start = env.random_free_space()
end = env.random_free_space()
# Trying first the euclidian distance
rrt.set_start(start)
rrt.run(end, 200, metric='euclidian')
# Trying first the distance defined by the local planner
rrt.set_start(start)
rrt.run(end, 200, metric='local')
print('Static RRT OK')
def solution(car):
#boundary, start, goal, obstacles, space_resolution, stepFunction,
# primitives=5, primitive_bounds=[-math.pi/4.0, math.pi/4.0], rounding=4
# Assumption: state = (id,prim,x,y,r,F,g)
boundary = [[car.xlb, car.ylb], [car.xub, car.yub]]
# start = [0,0,car.x0,car.y0,0,0,0]
# gt = math.atan2(car.yt-car.y0, car.xt-car.x0)
# goal = [-1,-1,car.xt,car.yt,gt,0,0]
start = (car.x0, car.y0, 0.0, 0.0, 0, 0) #(x,y,theta,phi,id,parent)
goal = (car.xt, car.yt, 0.0, 0.0, 0, 0)
obstacles = car.obs
#space_resolution = min(obstacles, key = lambda t: t[2])[2]/4.0 #smallest obstacle
space_resolution = 0.01
stepFunction = car.step
print("Resolution: {}".format(space_resolution))
print("Start: {}".format(start))
print("Goal: {}".format(goal))
print("Boundary {}".format(boundary))
# astar = AStar(boundary, start, goal, obstacles, space_resolution, stepFunction)
rrt = RRT(boundary, start, goal, obstacles, space_resolution, stepFunction)
controls = [0, 0, 0]
print("Started Planning")
# controls = astar.run()
controls = rrt.run()
print("End Planning")
assert (controls != None)
times = [0]
for _ in controls:
times.append(times[-1] + car.dt)
return controls, times
def main():
fig = plt.figure()
armax = fig.add_subplot(121,
autoscale_on=False,
xlim=(-15, 15),
ylim=(-15, 15))
confax = fig.add_subplot(122,
autoscale_on=False,
xlim=(-2 * math.pi, 2.5 * math.pi),
ylim=(-2 * math.pi, 2.5 * math.pi))
rrt = RRT(confax, Arm2D())
confax.set_title("Configuration Space")
armax.set_title("Arm")
arm = Arm2DGraph(armax, confax, rrt)
armax.set_aspect('equal')
confax.set_aspect('equal')
oldjoints = arm.arm.joints
cx = []
cy = []
for theta1 in na.arange(-2 * math.pi, 2.5 * math.pi, 0.1):
for theta2 in na.arange(-2 * math.pi, 2.5 * math.pi, 0.1):
arm.arm.update(joints=[theta1, theta2])
if arm.arm.inCollision():
cx.append(theta1)
cy.append(theta2)
confax.scatter(cx, cy, c='r', marker='x')
arm.arm.joints = oldjoints
#ax.axis((-10, 10, -10, 10))
fig.canvas.mpl_connect('button_press_event', arm.on_press)
fig.canvas.mpl_connect('button_release_event', arm.on_release)
fig.canvas.mpl_connect('motion_notify_event', arm.on_motion)
fig.canvas.mpl_connect('motion_notify_event', arm.on_motion)
fig.canvas.mpl_connect('key_press_event', arm.on_key_press)
fig.canvas.mpl_connect('key_release_event', arm.on_key_release)
armax.grid()
confax.grid()
ani = animation.FuncAnimation(fig, arm.animate, interval=10, blit=False)
plt.show()
def setUp(self):
# RRT 1: RRT with 1 node, in 3 dimensions
self.rrt1 = RRT(start_state=(0., 0., 0.),
goal_state=(1, 1, 1),
dim_ranges=[(-2, 2)] * 3,
step_size=0.1)
# RRT 2: RRT with 2 nodes, in 2 dimensions
# (start) --> (node1)
self.rrt2 = RRT(start_state=(0., 0.),
goal_state=(1, 1),
dim_ranges=[(-2, 2), (-2, 2)],
step_size=0.05)
self.rrt2_node1 = self.rrt2.start.add_child(state=(0.5, 0.25))
# RRT 3: RRT with 2 nodes, in 3 dimensions
self.rrt3 = RRT(start_state=(0.25, 0.25, 0.25),
goal_state=(0.75, 0.75, 0.75),
dim_ranges=[(0, 1)] * 3,
step_size=0.1)
self.rrt3_node1 = self.rrt3.start.add_child(state=(0.5, 0.5, 0.5))
self.rrt3_node2 = self.rrt3_node1.add_child(state=(0.75, 0.75, 0.7))
goal_region_radius = 1e-2
def goal_test(node):
global goal
return distance(node,goal) < goal_region_radius
def distance_from_goal(node):
global goal
return max(distance(node,goal)-goal_region_radius,0)
start = np.array([0,0,0])
goal = np.array([100.0,100.0,0])
rrt = RRT(state_ndim=3)
rrt.set_distance(distance)
rrt.set_cost(cost)
rrt.set_steer(steer)
rrt.set_goal_test(goal_test)
rrt.set_distance_from_goal(distance_from_goal)
rrt.set_sample(sample)
rrt.set_collision_check(collision_check)
rrt.set_collision_free(collision_free)
rrt.gamma_rrt = 100.0
rrt.eta = 50.0
rrt.c = 1
goal_config = [500, 500, 500] for i in range(N_ATTEMPT): #start location Qinit_3d = (0, 0, 0) gx = random.choice(range(dd*3, ll+1, dd)) gy = random.choice(range(dd*3, ll+1, dd)) gz = random.choice(range(dd*3, ll+1, dd)) gx = goal_config[0] gy = goal_config[1] gz = goal_config[2] print 'attempt: ',gx,gy,gz Qgoal_3d = (gx, gy, gz) Qgoal_3d = (dd*9, dd*9, dd*9) rrt = RRT(space, my_car, Qinit_3d, Qgoal_3d, QGOAL_BIAS, MAX_TREE_NODES) tree = rrt.build_rrt_3d() if rrt.path: print 'path: ', rrt.path # stats print 'attempted: ',len(rrt.attempts) print 'finalpath: ',len(rrt.path) # draw expanded for value in rrt.attempts: curr = (value[0], value[1], value[2]) sphere(pos=curr, radius=11, color=color.yellow) prev = (0,0,0)
if np.random.rand() < .9:
statespace = np.random.rand(2) * np.array([.4, 2]) - np.array([.2, 1])
time = np.random.randint(0, max_time_horizon, size=1) + 1
#time = np.array(min(np.random.geometric(.06,size=1),max_time_horizon))
time = np.reshape(time, newshape=(1, ))
return np.concatenate((statespace, time))
else: #goal bias
statespace = goal[0:2]
time = np.random.randint(0, max_time_horizon, size=1) + 1
#time = np.array(min(np.random.geometric(.06,size=1),max_time_horizon))
time = np.reshape(time, newshape=(1, ))
return np.concatenate((statespace, time))
start = np.array([0, -1, 0])
rrt = RRT(state_ndim=3, control_ndim=1)
rrt.sample_goal = lambda: goal
rrt.set_distance(lqr_rrt.distance_cache)
rrt.set_same_state(lqr_rrt.same_state)
rrt.set_cost(lqr_rrt.cost)
rrt.set_steer(lqr_rrt.steer_cache)
rrt.set_goal_test(goal_test)
rrt.set_sample(sample)
#rrt.set_collision_check(isStateValid)
rrt.set_collision_free(lqr_rrt.collision_free)
rrt.set_distance_from_goal(distance_from_goal)
MAX_TREE_NODES = 1000
QGOAL_BIAS = 0.33
my_car = Car()
space = ConfigSpace(g)
Qinit_3d = (0, 0, 0)
# x = g.objects[g.reverseLookup[goal]].pos.x
# y = g.objects[g.reverseLookup[goal]].pos.y
# z = g.objects[g.reverseLookup[goal]].pos.z
# Qgoal_3d = (x, y, z)
Qgoal_3d = g.objects[g.reverseLookup[goal]].pos
#print 'GOAL',Qgoal_3d
# RUN
t1 = time.time()
rrt = RRT(space, my_car, Qinit_3d, Qgoal_3d, QGOAL_BIAS,
MAX_TREE_NODES)
tree = rrt.build_rrt_3d()
t2 = time.time()
if rrt.path:
# Order: run#, number of nodes, size, number of obstructions, time, path length, nodes expanded
print "R,%d,%d,%d,%d,%f,%d,%d" % (
run, len(g.locations), size, obstructions,
(100 * (t2 - t1)), len(rrt.path), len(rrt.attempts))
else:
print "R,%d,%d,%d,%d,%f,%d,%d" % (
run, len(g.locations), size, obstructions,
(100 * (t2 - t1)), -1, len(rrt.attempts))
# Draw the resulting path
if draw and rrt.path:
'''
ee = fr.ee(q)
err = np.sum((np.asarray(desired_ee_rp) - np.asarray(ee[3:5]))**2)
grad = np.asarray([
0, 0, 0, 2 * (ee[3] - desired_ee_rp[0]),
2 * (ee[4] - desired_ee_rp[1]), 0
])
return err, grad
print('Desired ee roll and pitch:', desired_ee_rp)
joints_start = fr.home_joints.copy()
joints_start[0] = -np.deg2rad(45)
joints_target = joints_start.copy()
joints_target[0] = np.deg2rad(45)
rrt = RRT(fr, is_in_collision)
constraint = ee_upright_constraint
plan = rrt.plan(joints_start, joints_target, constraint)
i = 0
collision_boxes_publisher = CollisionBoxesPublisher('collision_boxes')
rate = rospy.Rate(10)
while not rospy.is_shutdown():
joints = plan[i % len(plan)]
fr.publish_joints(joints)
fr.publish_collision_boxes(joints)
collision_boxes_publisher.publish_boxes(boxes)
i += 1
rate.sleep()
def main(self):
"""
dims : graph's dimensions (numpy array of tuples)
obstacles : obstacles to be placed in the graph (list of shapely.Polygons)
home : home location (tuple)
init_state : start location (tuple)
goal_state : goal location (tuple)
delta : distance between nodes (int)
k : shrinking ball facotr (int)
n : number of waypoints to push (int)
path : list of waypoints (list of tuples)
case : case number to set behavior (int)
Units: meters
"""
#rospy.init_node('RRTnode')
#rospy.wait_for_service('/control/waypoints')
wp_push = rospy.ServiceProxy('/control/waypoints', WaypointPush)
#rospy.Subscriber('/mavros/home_position/home', HomePosition, self.home_pos_cb)
#rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, self.global_heading)
#rospy.Subscriber('/mavros/global_position/global', NavSatFix, self.global_position)
rate = rospy.Rate(1) # send msgs at 1 Hz
start_time = rospy.get_time()
rate.sleep()
# while loop for gps_check:
while self.gps_status == -1 or self.gps_status is None:
rospy.loginfo("BAD GPS")
rate.sleep()
while not self.home_set:
rospy.loginfo("Home not set!")
rate.sleep()
# E-x N-y D-z
dims = np.array([(-500, 2500), (-100, 2900), (-50, -50)])
obstacle = [(Point(1300, 1300).buffer(500))]
init = self.pos
goal = (2300.0, 2600.0, -50.0)
delta = 100
k = 2
graph = Graph(dims, obstacle)
path = None
trail = []
position = []
start_index = 0
rospy.loginfo("Calculating Trajectory...")
#print('Calculating Trajectory...\n')
while not rospy.is_shutdown():
if graph.num_nodes() == 0:
#t0 = time.time()
rrt = RRT(graph, init, goal, delta, k, path, self.heading)
if graph.num_nodes() <= 250:
path, leaves = rrt.search()
else:
init = path[path.index(rrt.brute_force(self.pos, path)) + 1]
trail.append(path)
position.append(self.pos)
rate.sleep()
graph.clear()
pathNED = np.asarray(path)
wp_msg = []
pathLLA = self.frame.ConvNED2LLA(pathNED.T)
for i in range(len(pathNED)):
wp_point = Waypoint()
wp_point.frame = 3
wp_point.x_lat = pathLLA[0][i]
wp_point.y_long = pathLLA[1][i]
wp_point.z_alt = pathLLA[2][i]
wp_msg += [wp_point]
print(wp_msg, '\n')
resp = wp_push(start_index, wp_msg)
print(resp, '\n')
start_index += path.index(rrt.brute_force(self.pos, path)) + 1
#print('Search Time: {} secs\n'.format(time.time() - t0))
rate.sleep()
if path is not None:
if dist(self.pos, goal) <= delta * 3 or goal in path:
rospy.loginfo("Goal reached")
#print('Goal Reached.\n')
print('trail : ', trail, '\n')
print('position: ', position, '\n')
break
print('EOL')
[0, 0, 0, -np.pi / 4, 0, np.pi / 4, np.pi / 4])
joints_target[0] = -np.deg2rad(45)
elif args.map2:
joints_start = np.array(
[0, np.pi / 6, 0, -2 * np.pi / 3, 0, 5 * np.pi / 6, np.pi / 4])
joints_target = np.array(
[0, 0, 0, -np.pi / 4, 0, np.pi / 4, np.pi / 4])
else:
joints_start = fr.home_joints.copy()
joints_start[0] = -np.deg2rad(45)
joints_target = joints_start.copy()
joints_target[0] = np.deg2rad(45)
if args.rrt:
print("RRT: RRT planner is selected!")
planner = RRT(fr, is_in_collision)
elif args.rrtc:
print("RRTC: RRT Connect planner is selected!")
planner = RRTConnect(fr, is_in_collision)
elif args.prm:
print("PRM: PRM planner is selected!")
planner = PRM(fr, is_in_collision)
elif args.obprm:
print("OB_PRM: OB_PRM planner is selected!")
planner = OBPRM(fr, is_in_collision)
constraint = ee_upright_constraint
if args.prm or args.obprm:
plan = planner.plan(joints_start, joints_target, constraint, args)
else:
plan = planner.plan(joints_start, joints_target, constraint)
class TestRRTSteps(unittest.TestCase):
def setUp(self):
# RRT 1: RRT with 1 node, in 3 dimensions
self.rrt1 = RRT(start_state=(0., 0., 0.),
goal_state=(1, 1, 1),
dim_ranges=[(-2, 2)] * 3,
step_size=0.1)
# RRT 2: RRT with 2 nodes, in 2 dimensions
# (start) --> (node1)
self.rrt2 = RRT(start_state=(0., 0.),
goal_state=(1, 1),
dim_ranges=[(-2, 2), (-2, 2)],
step_size=0.05)
self.rrt2_node1 = self.rrt2.start.add_child(state=(0.5, 0.25))
# RRT 3: RRT with 2 nodes, in 3 dimensions
self.rrt3 = RRT(start_state=(0.25, 0.25, 0.25),
goal_state=(0.75, 0.75, 0.75),
dim_ranges=[(0, 1)] * 3,
step_size=0.1)
self.rrt3_node1 = self.rrt3.start.add_child(state=(0.5, 0.5, 0.5))
self.rrt3_node2 = self.rrt3_node1.add_child(state=(0.75, 0.75, 0.7))
def test_get_random_sample(self):
sample = self.rrt1._get_random_sample()
lower = np.array([d[0] for d in self.rrt1.dim_ranges])
upper = np.array([d[1] for d in self.rrt1.dim_ranges])
self.assertEqual(len(sample), len(self.rrt1.dim_ranges))
self.assertTrue(np.all(sample >= lower))
self.assertTrue(np.all(sample < upper))
def test_get_nearest_neighbor(self):
sample1 = np.array([0.1, 0.2, 0.3])
neighbor1 = self.rrt1._get_nearest_neighbor(sample1)
self.assertIs(neighbor1, self.rrt1.start)
sample2 = np.array([0.25, 0.25])
neighbor2 = self.rrt2._get_nearest_neighbor(sample2)
self.assertIs(neighbor2, self.rrt2_node1)
sample3 = np.array([0.7, 0.7, 0.7])
neighbor3 = self.rrt3._get_nearest_neighbor(sample3)
self.assertIs(neighbor3, self.rrt3_node2)
def test_extend_sample(self):
sample1 = np.array([0, 0, 0.1])
sample1_node = self.rrt1._extend_sample(sample1, self.rrt1.start)
self.assertIs(sample1_node.parent, self.rrt1.start)
self.assertAlmostEqual(np.linalg.norm(sample1 - sample1_node.state),
0.,
places=3)
sample2 = np.array([1, 1, 1])
sample2_ext = np.array([0.0577, 0.0577, 0.0577])
sample2_node = self.rrt1._extend_sample(sample2, self.rrt1.start)
self.assertAlmostEqual(np.linalg.norm(sample2_ext -
sample2_node.state),
0.,
places=3)
def test_check_for_completion(self):
complete1 = self.rrt2._check_for_completion(self.rrt2_node1)
self.assertFalse(complete1)
complete2 = self.rrt3._check_for_completion(self.rrt3_node2)
self.assertTrue(complete2)
def test_trace_path_from_start(self):
path1 = self.rrt1._trace_path_from_start(self.rrt1.start)
self.assertEqual(len(path1), 1)
self.assertTrue(np.all(path1[0] == self.rrt1.start.state))
path2 = self.rrt2._trace_path_from_start(self.rrt2_node1)
self.assertEqual(len(path2), 2)
self.assertTrue(np.all(path2[0] == self.rrt2.start.state))
self.assertTrue(np.all(path2[1] == self.rrt2_node1.state))
self.rrt3_node2.children.append(self.rrt3.goal)
self.rrt3.goal.parent = self.rrt3_node2
path3 = self.rrt3._trace_path_from_start()
self.assertEqual(len(path3), 4)
self.assertTrue(np.all(path3[0] == self.rrt3.start.state))
self.assertTrue(np.all(path3[1] == self.rrt3_node1.state))
self.assertTrue(np.all(path3[2] == self.rrt3_node2.state))
self.assertTrue(np.all(path3[3] == self.rrt3.goal.state))
def test_check_for_collision(self):
sample = np.array([0.5, 0.5, 0.5])
in_collision = self.rrt1._check_for_collision(sample)
self.assertFalse(in_collision)
def main():
rospy.init_node('rrt')
rrt = RRT()
rospy.spin()
def is_collision(self, state):
#-- Casts as a shapely point
state_point = Point(state[0], state[1])
if self.obstacles is None:
return False
else:
#-- For each obstacle checks if the point is contained in the obstacle.
for obstacle in self.obstacles:
if obstacle.contains(state_point):
return True
return False
def graph_results(self, results, car_rrt, plot_tree=False):
return graph_results(self, results, car_rrt, plot_tree)
def animate_results(self, results, car_rrt, plot_tree=False):
return animate_results(self, results, car_rrt, plot_tree)
if __name__ == '__main__':
random.seed(446)
obstacles = [Polygon([(2,0), (2, 2), (3, 2), (3, 0)]),
Polygon([(4,4), (4,8), (7,1)]),
Polygon([(2,2), (1,8), (2,4), (3,3)])]
workspace = Workspace(dim=(12,12), obstacles=obstacles)
car_rrt = RRT(workspace, (1, 1, pi), (10, 10, pi), 10000, num_rand_actions=6, max_control_time=2, step_threshold=.05)
results = car_rrt.build_rrt()
print(results)
ax = workspace.graph_results(results, car_rrt, plot_tree=False)
plt.show()
import json
from apf import APF
from rrt import RRT
def load_config(config_filename: str) -> dict:
with open(config_filename) as f:
config = json.load(f)
return config
def pretty_print_config(config: dict):
print(json.dumps(config, indent=2, sort_keys=False))
if __name__ == '__main__':
config = load_config('config.json')
apf_paraboloid = APF(config, paraboloid=True)
apf_paraboloid.apf()
apf_conical = APF(config, paraboloid=False)
apf_conical.apf()
rrt = RRT(config)
rrt.bidirectional_rrt()
print "Enter your choice: " print "1) RRT" print "2) RRT Star" con = int(input()) start = [X, Y] end = [gX, gY] generator = Generator(n) obstacleList = generator.generateRect() expandDist = 0.7 goalSample = 0.2 if con == 1: rrt = RRT(start, end, obstacleList, expandDist, goalSample) if con == 2: itera = n * 1000 rrt = RRTStar(start, end, obstacleList, expandDist, goalSample, itera) xPath, yPath, xPlot, yPlot = rrt.planner() if xPath != None: fig1 = plt.figure() ax1 = fig1.add_subplot(111, aspect='equal') for (ox, oy, sx, sy) in obstacleList: ax1.add_patch(patches.Rectangle((ox - sx/2.0, oy - sy/2.0), sx, sy, color='black')) plt.plot(xPath, yPath) plt.scatter(xPlot, yPlot, marker='^', color='red')
def test_rrt_init_basic(self):
rrt = RRT(start_state=(0.0, 0.0),
goal_state=(1, 1),
dim_ranges=[(-2, 2), (-2, 2)])
def sample():
if np.random.rand()<.9:
statespace = np.random.rand(2)*np.array([.4,2])-np.array([.2,1])
time = np.random.randint(0,max_time_horizon,size=1) + 1
#time = np.array(min(np.random.geometric(.06,size=1),max_time_horizon))
time = np.reshape(time,newshape=(1,))
return np.concatenate((statespace,time))
else: #goal bias
statespace = goal[0:2]
time = np.random.randint(0,max_time_horizon,size=1) + 1
#time = np.array(min(np.random.geometric(.06,size=1),max_time_horizon))
time = np.reshape(time,newshape=(1,))
return np.concatenate((statespace,time))
start = np.array([0,-1,0])
rrt = RRT(state_ndim=3,control_ndim=1)
rrt.sample_goal = lambda : goal
rrt.set_distance(lqr_rrt.distance_cache)
rrt.set_same_state(lqr_rrt.same_state)
rrt.set_cost(lqr_rrt.cost)
rrt.set_steer(lqr_rrt.steer_cache)
rrt.set_goal_test(goal_test)
rrt.set_sample(sample)
#rrt.set_collision_check(isStateValid)
rrt.set_collision_free(lqr_rrt.collision_free)
rrt.set_distance_from_goal(distance_from_goal)
def main(argv=None):
if (argv == None):
argv = sys.argv[1:]
width = 100.0
height = 100.0
pp = PathPlanningProblem(width, height, 60, 40, 40)
initial, goals = pp.CreateProblemInstance()
fig = plt.figure()
ax = fig.add_subplot(1, 2, 1, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
for o in pp.obstacles:
ax.add_patch(copy.copy(o.patch))
ip = plt.Rectangle((initial[0], initial[1]), 1.0, 1.0, facecolor='#ff0000')
ax.add_patch(ip)
for g in goals:
g = plt.Rectangle((g[0], g[1]), 1.0, 1.0, facecolor='#00ff00')
ax.add_patch(g)
qtd = QuadTreeDecomposition(pp, 0.2, initial, goals)
qtd.Draw(ax)
n = qtd.CountCells()
start = timeit.default_timer()
astar = AStarSearch(
qtd.domain, qtd.root,
Rectangle(qtd.initialCell[0], qtd.initialCell[1], 0.1, 0.1),
Rectangle(qtd.goalsCell[0][0], qtd.goalsCell[0][1], 0.1, 0.1))
stop = timeit.default_timer()
print("A* Running Time: ", stop - start)
print("A* Path Length : ", astar.path_length)
plt.plot([x for (x, y) in astar.path], [y for (x, y) in astar.path], '-')
ax.set_title('Quadtree Decomposition\n{0} cells'.format(n))
ax = fig.add_subplot(1, 2, 2, aspect='equal')
ax.set_xlim(0.0, width)
ax.set_ylim(0.0, height)
for o in pp.obstacles:
ax.add_patch(copy.copy(o.patch))
ip = plt.Rectangle((initial[0], initial[1]), 1, 1, facecolor='#ff0000')
ax.add_patch(ip)
goal = None
for g in goals:
goal = g
g = plt.Rectangle((g[0], g[1]), 1, 1, facecolor='#00ff00')
ax.add_patch(g)
start = timeit.default_timer()
spath = RRT.ExploreDomain(RRT(8), pp, initial, goal, 5000)
path = spath[0]
final = spath[1]
if len(final) == 0:
print("No path found")
RRT.draw(RRT(0), plt, path, final)
stop = timeit.default_timer()
print("RRT Running Time: ", stop - start)
if len(final) > 0:
print("RRT Path Length : ", RRT.pathLen(RRT(0), final))
ax.set_title('RRT')
plt.show()
def test_rrt_init_bad_dims(self):
with self.assertRaises(AssertionError):
rrt = RRT(start_state=(0., 0.),
goal_state=(1, 1, 1),
dim_ranges=[(-2, 2), (-2, 2)])
return 0
def goal_test(node):
goal_region_radius = .01
n = 4
return np.sum(np.abs(node['state'][0:n] -
goal[0:n])) < goal_region_radius #disregards time
return distance(
node, goal
) < goal_region_radius #need to think more carefully about this one
start = np.array([0, 0, 0, 0, 0])
lqr_rrt = LQR_RRT(A, B, Q, R, max_time_horizon)
rrt = RRT(state_ndim=5, control_ndim=2)
lqr_rrt.action_state_valid = action_state_valid
lqr_rrt.max_nodes_per_extension = 5
rrt.sample_goal = lambda: goal
rrt.set_distance(lqr_rrt.distance_cache)
rrt.set_same_state(lqr_rrt.same_state)
rrt.set_cost(lqr_rrt.cost)
rrt.set_steer(lqr_rrt.steer_cache)
rrt.set_goal_test(goal_test)
rrt.set_sample(sample)
rrt.set_collision_free(lqr_rrt.collision_free)
def distance(from_node,to_point):
assert len(to_point)==2
#to_point is an array and from_point is a node
return np.linalg.norm(to_point-from_node['state'])
goal_region_radius = .05
def goal_test(node):
global goal
return distance(node,goal) < goal_region_radius
def distance_from_goal(node):
global goal
return max(distance(node,goal)-goal_region_radius,0)
start = np.array([-1,-1])*1
rrt = RRT(state_ndim=2,control_ndim=2)
rrt.set_same_state(same_state)
rrt.set_distance(distance)
rrt.set_cost(cost)
rrt.set_steer(steer)
rrt.set_goal_test(goal_test)
rrt.set_sample(sample)
rrt.set_collision_check(isStateValid)
rrt.set_collision_free(collision_free)
rrt.set_distance_from_goal(distance_from_goal)
rrt.gamma_rrt = 40.0
rrt.eta = 0.5
if __name__ == "__main__":
start = time.time()
# parse command line args
parser = argparse.ArgumentParser()
parser.add_argument("-path_to_data", default="../results/")
parser.add_argument("-path_to_output", default="../results/")
parser.add_argument("-visualize", action="store_true")
parser.add_argument("-method", choices=["RRT", "PRM"], default="RRT")
parser.add_argument("-step", default=0.05, type=float)
args = parser.parse_args()
if args.method == "RRT":
RRT(data_dir=args.path_to_data,
out_dir=args.path_to_output,
viz_=args.visualize,
step_=args.step)
elif args.method == "PRM":
print("PRM not implemented!")
# PRM(data_dir=args.path_to_data, out_dir=args.path_to_output, viz_=args.visualize, N=500)
else:
print("Choose RRT or PRM for method")
exit()
end = time.time()
print(f"Elapsed Time: {(end-start):.4f}s")
# keep visualization on screen
if args.visualize: input("Press any button to continue...\n")
def rrt_planning(self):
#initialize array
X = []
Y = []
Z_euler=[]
#set start point for orientation
angle_s= self.angle_orientation[0]
#set start point for path
x_s = self.x_array[0]
y_s = self.x_array[0]
l = len(self.order_pic)
maxIter = 1000
obstacleList=self.obstacleList
#Generate Path & Orientation based on given order
for i in range(l):
#get order number
n = self.order_pic[i]
print(n)
#set goal point
x_g = self.x_array[n]
y_g = self.y_array[n]
angle_g = self.angle_orientation[n]
angle_g = angle_g+np.pi
#call RRT from rrt.py
rrt = RRT(start=[x_s, y_s], goal=[x_g, y_g], rand_area=[0, 9], obstacle_list=self.obstacleList)
path = rrt.planning(animation=False)
#print(path)
smoothedPath = rrt.path_smoothing(path, maxIter, obstacleList)
#print(smoothedPath)
X_p,Y_p = map(list,zip(*smoothedPath))
#Reverse X,Y coordinate for each segment
X_p = np.flipud(X_p)
Y_p = np.flipud(Y_p)
m = len(X_p)-1
num_point = 800
#Store the path
for j in range(m):
#linaer interpolation of path
n_x = np.linspace(X_p[j], X_p[j+1], num=num_point)
n_y = np.linspace(Y_p[j], Y_p[j+1], num=num_point)
X = np.concatenate([X,n_x])
Y = np.concatenate([Y,n_y])
#linear interpolation of orientation
n_z_euler=np.linspace(angle_s , angle_g, m*num_point)
Z_euler=np.concatenate([Z_euler,n_z_euler])
#update start point
x_s = x_g
y_s = y_g
angle_s = angle_g
Z = np.ones(len(X))*1.3
X_euler = np.linspace(0, 0 , len(X))
Y_euler = np.linspace(0, 0 , len(X))
#print(X)
self.number_of_points=len(X)
return X, Y, Z, X_euler, Y_euler, Z_euler
time = np.reshape(time,newshape=(1,))
return np.concatenate((statespace,time))
def distance_from_goal(node):
return 0
def goal_test(node):
goal_region_radius = .01
n = 4
return np.sum(np.abs(node['state'][0:n]-goal[0:n])) < goal_region_radius #disregards time
return distance(node,goal) < goal_region_radius #need to think more carefully about this one
start = np.array([0,0,0,0,0])
lqr_rrt = LQR_RRT(A,B,Q,R,max_time_horizon)
rrt = RRT(state_ndim=5,control_ndim=2)
lqr_rrt.action_state_valid = action_state_valid
lqr_rrt.max_nodes_per_extension = 5
rrt.sample_goal = lambda : goal
rrt.set_distance(lqr_rrt.distance_cache)
rrt.set_same_state(lqr_rrt.same_state)
rrt.set_cost(lqr_rrt.cost)
rrt.set_steer(lqr_rrt.steer_cache)
rrt.set_goal_test(goal_test)
rrt.set_sample(sample)
rrt.set_collision_free(lqr_rrt.collision_free)