def rrt_connect_planning(map_info, display=False):
rrt_start = RRT(map_info.start)
rrt_end = RRT(map_info.end)
rrt_a = rrt_start
rrt_b = rrt_end
okdtree = cKDTree(map_info.obstacle)
while True:
q_rand = (randint(1, map_info.width - 1), randint(1, map_info.height - 1))
if q_rand == map_info.start or q_rand in map_info.obstacle or rrt_a.is_contain(q_rand):
continue
q_new = rrt_a.extend(q_rand, okdtree)
if not q_new:
continue
while True:
q_new_ = rrt_b.extend(q_new, okdtree)
if display:
map_info.set_rand(q_rand)
map_info.set_rrt_connect(rrt_a.get_rrt(), rrt_b.get_rrt())
# 2 rrts reached
if q_new == q_new_:
return reconstruct_path(rrt_a, rrt_b, q_new, map_info)
elif q_new_ == None:
break
# swap 2 rrts
rrt_a, rrt_b = rrt_b, rrt_a
def __init__(self):
#Initialising messages and variables
self.position = Point()
self.path = Path()
self.obstacles = []
self.start = Point()
self.goal = Point()
self.scan = LaserScan()
self.goal_recieved = False
#Publishers and Subscribers
self.path_pub = rospy.Publisher('/path', Path, queue_size=10)
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.__odom_update)
self.scan_pub = rospy.Subscriber('/scan', LaserScan, self.__laser_sub)
self.goal_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped,
self.__goal_sub)
#Flag to check if final goal is reached
self.not_reached = True
#Initialising path planner
self.path_planner = RRT(1, 1000)
self.path_points = []
#Initialise transform listener
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
self.rate = rospy.Rate(10)
def main():
# ====Search Path with RRT====
# Parameter
obstacleList = [
(5, 5, 1),
(3, 6, 2),
(3, 8, 2),
(3, 10, 2),
(7, 5, 2),
(9, 5, 2)
] # [x,y,size]
rrt = RRT(start=[0, 0], goal=[6, 10],
rand_area=[-2, 15], obstacle_list=obstacleList)
path = rrt.planning(animation=show_animation)
# Path smoothing
maxIter = 1000
smoothedPath = path_smoothing(path, maxIter, obstacleList)
# Draw final path
if show_animation:
rrt.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.plot([x for (x, y) in smoothedPath], [
y for (x, y) in smoothedPath], '-c')
plt.grid(True)
plt.pause(0.01) # Need for Mac
plt.show()
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 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_dynamic_env(self):
"""
Tests that the RRT works in a dynamic environement
"""
env = DynamicEnvironment((100, 100), 5)
rrt = RRT(env)
start = (50, 1, 1.57)
end = (50, 99, 1.57)
# Initialisation of the tree, to have a first edge
rrt.set_start(start)
rrt.run(end, 200, metric='local')
# Initialisation of the position of the vehicle
position = start[:2]
current_edge = rrt.select_best_edge()
for i in range(60):
# We check if we are on an edge or if we have to choose a new edge
if not current_edge.path:
current_edge = rrt.select_best_edge()
# Update the position of the vehicle
position = current_edge.path.popleft()
# Update the environment
# The frontiers of the sampling and the obstacles
env.update(position)
# The position of the goal
end = (50, position[1]+90, 1.57)
# Continue the growth of the tree
rrt.run(end, 2, metric='local')
env.plot(display=False)
rrt.plot(file_name='static'+str(i)+'.png', close=True)
print('Dynamic RRT OK')
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 main():
# ====Search Path with RRT====
# Parameter
obstacleList = [(6.23, 1.9, 1.2), (4.48, 3.44, 1.02), (7.51, 7.14, 0.96),
(0.59, 8.35, 0.75), (2.19, 7.31, 0.6), (1.43, 2.5, 0.48),
(5.8, 6.53, 0.45)] # [x,y,size]
rrt = RRT(start=[x_array[start], y_array[start]],
goal=[x_array[end], y_array[end]],
rand_area=[0, 9],
obstacle_list=obstacleList)
path = rrt.planning(animation=show_animation)
# Path smoothing
maxIter = 1000
smoothedPath = path_smoothing(path, maxIter, obstacleList)
# Draw final path
if show_animation:
rrt.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.plot([x for (x, y) in smoothedPath],
[y for (x, y) in smoothedPath], '-c')
plt.grid(True)
plt.pause(0.01) # Need for Mac
plt.show()
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 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 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=''))
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 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 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 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()
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)])
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:
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()
def main():
rospy.init_node('rrt')
rrt = RRT()
rospy.spin()
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')
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 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()
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 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
[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)
