def generateNewNode(self,V,V_theta,E,Other,regionPoly,stuck,append_after_latest_node =False):
"""
Generate a new node on the current tree matrix
V : the node matrix
V_theta : the orientation matrix
E : the tree matrix (or edge matrix)
Other : the matrix containing the velocity and angular velocity(omega) information
regionPoly: the polygon of current region
stuck : count on the number of times failed to generate new node
append_after_latest_node : append new nodes to the latest node (True only if the previous node addition is successful)
"""
if self.system_print == True:
print "In control space generating path,stuck = " + str(stuck)
connection_to_tree = False # True when connection to the tree is successful
if stuck > self.stuck_thres:
# increase the range of omega since path cannot ge generated
omega = random.choice(self.omega_range_escape)
else:
#!!!! CONTROL SPACE STEP 1 - generate random omega
omega = random.choice(self.omega_range)
#!!!! CONTROL SPACE STEP 2 - pick a random point on the tree
if append_after_latest_node:
tree_index = shape(V)[1]-1
else:
if random.choice([1,2]) == 1:
tree_index = random.choice(array(V[0])[0])
else:
tree_index = shape(V)[1]-1
xPrev = V[1,tree_index]
yPrev = V[2,tree_index]
thetaPrev = V_theta[tree_index]
j = 1
#!!!! CONTROL SPACE STEP 3 - Check path of the robot
path_robot = PolyShapes.Circle(self.radius,(xPrev,yPrev))
while j <= self.timeStep:
xOrg = xPrev
yOrg = yPrev
xPrev = xPrev + self.velocity/omega*(sin(omega* 1 + thetaPrev)-sin(thetaPrev))
yPrev = yPrev - self.velocity/omega*(cos(omega* 1 + thetaPrev)-cos(thetaPrev))
thetaPrev = omega* 1 + thetaPrev
path_robot = path_robot + PolyShapes.Circle(self.radius,(xPrev,yPrev))
j = j + 1
thetaPrev = self.orientation_bound(thetaPrev)
path_all = PolyUtils.convexHull(path_robot)
in_bound = regionPoly.covers(path_all)
"""
# plotting
if plotting == True:
self.plotPoly(path_all,'r',1)
"""
stuck = stuck + 1
if in_bound:
robot_new_node = PolyShapes.Circle(self.radius,(xPrev,yPrev))
# check how many nodes on the tree does the new node overlaps with
nodes_overlap_count = 0
for k in range(shape(V)[1]-1):
robot_old_node = PolyShapes.Circle(self.radius,(V[1,k],V[2,k]))
if robot_new_node.overlaps(robot_old_node):
if abs(thetaPrev - V_theta[k]) < self.max_angle_overlap:
nodes_overlap_count += 1
if nodes_overlap_count == 0 or (stuck > self.stuck_thres+1 and nodes_overlap_count < 2) or (stuck > self.stuck_thres+500):
if stuck > self.stuck_thres+1:
append_after_latest_node = False
if (stuck > self.stuck_thres+500):
stuck = 0
stuck = stuck - 20
# plotting
if self.plotting == True:
self.plotPoly(path_all,'b',1)
if self.system_print == True:
print "node connected"
V = hstack((V,vstack((shape(V)[1],xPrev,yPrev))))
V_theta = hstack((V_theta,thetaPrev))
E = hstack((E,vstack((tree_index ,shape(V)[1]-1))))
Other = hstack((Other,vstack((self.velocity,omega))))
##################### E should add omega and velocity
connection_to_tree = True
append_after_latest_node = True
else:
append_after_latest_node = False
if self.system_print == True:
print "node not connected. check goal point"
else:
append_after_latest_node = False
return V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree
def gotoRegion(self, current_reg, next_reg, last=False):
"""
If ``last`` is True, we will move to the center of the destination region.
Returns ``True`` if we've reached the destination region.
"""
q_gBundle = [[], []] # goal bundle
q_overlap = [[], []] # overlapping points with robot range
pose = self.pose_handler.getPose() # Find our current configuration
#Plot the robot on the map in figure 1
if self.PLOT == True:
plt.figure(self.original_figure)
plt.clf()
self.plotPoly(self.realRobot, 'r')
self.plotPoly(self.robot, 'b')
plt.plot(pose[0], pose[1], 'bo')
self.plotPioneer(self.original_figure)
if current_reg == next_reg and not last:
# No need to move!
self.drive_handler.setVelocity(0, 0) # So let's stop
return False
# Check if Vicon has cut out
if math.isnan(pose[2]):
print "no vicon pose"
print "WARNING: No Vicon data! Pausing."
#self.drive_handler.setVelocity(0, 0) # So let's stop
time.sleep(1)
return False
###This part is run when the robot goes to a new region, otherwise, the original map will be used.
if not self.previous_current_reg == current_reg:
#print 'getting into bug alogorithm'
#clean up the previous self.map_work
self.map_work = Polygon.Polygon()
# NOTE: Information about region geometry can be found in self.proj.rfi.regions
# create polygon list for regions other than the current_reg and the next_reg
self.map_work += self.map[self.proj.rfi.regions[current_reg].name]
self.map_work += self.map[self.proj.rfi.regions[next_reg].name]
# building current polygon and destination polygon
self.nextRegionPoly = self.map[
self.proj.rfi.regions[next_reg].name]
self.currentRegionPoly = self.map[
self.proj.rfi.regions[current_reg].name]
#set to zero velocity before finding the tranFace
self.drive_handler.setVelocity(0, 0)
if last:
transFace = None
else:
print "Current reg is " + str(
self.proj.rfi.regions[current_reg].name.lower())
print "Next reg is " + str(
self.proj.rfi.regions[next_reg].name.lower())
for i in range(
len(self.proj.rfi.transitions[current_reg][next_reg])):
pointArray_transface = [
x for x in self.proj.rfi.transitions[current_reg]
[next_reg][i]
]
transFace = asarray(
map(self.coordmap_map2lab, pointArray_transface))
bundle_x = (transFace[0, 0] +
transFace[1, 0]) / 2 #mid-point coordinate x
bundle_y = (transFace[0, 1] +
transFace[1, 1]) / 2 #mid-point coordinate y
q_gBundle = hstack((q_gBundle, vstack(
(bundle_x, bundle_y))))
q_gBundle = q_gBundle.transpose()
# Find the closest face to the current position
max_magsq = 1000000
for tf in q_gBundle:
magsq = (tf[0] - pose[0])**2 + (tf[1] - pose[1])**2
if magsq < max_magsq:
connection = 0
tf = tf + (tf - asarray(self.currentRegionPoly.center(
))) / norm(tf - asarray(self.currentRegionPoly.center(
))) * 2.1 * self.PioneerLengthHalf
if not self.nextRegionPoly.covers(
PolyShapes.Circle(self.PioneerLengthHalf * 2,
(tf[0], tf[1]))):
tf = tf - (tf - asarray(
self.currentRegionPoly.center())) / norm(
tf - asarray(self.currentRegionPoly.center(
))) * 4.2 * self.PioneerLengthHalf
if self.nextRegionPoly.covers(
PolyShapes.Circle(
self.PioneerLengthHalf * 2,
(tf[0], tf[1]))):
connection = 1
else:
connection = 1
if connection == 1:
pt1 = tf
max_magsq = magsq
transFace = 1
self.q_g[0] = pt1[0]
self.q_g[1] = pt1[1]
else:
sample = False
while not sample:
self.q_g[0], self.q_g[
1] = self.nextRegionPoly.sample(
random.random)
robo = PolyShapes.Circle(
self.PioneerLengthHalf,
(self.q_g[0], self.q_g[1]))
if not bool(robo - self.nextRegionPoly):
sample = True
"""
# Push the goal point to somewhere inside the next region to ensure the robot will get there.(CHECK!!)
self.q_g = self.q_g+(self.q_g-asarray(self.currentRegionPoly.center()))/norm(self.q_g-asarray(self.currentRegionPoly.center()))*3*self.PioneerLengthHalf
if not self.nextRegionPoly.isInside(self.q_g[0],self.q_g[1]):
self.q_g = self.q_g-(self.q_g-asarray(self.currentRegionPoly.center()))/norm(self.q_g-asarray(self.currentRegionPoly.center()))*6*self.PioneerLengthHalf
"""
#plot exiting point
if self.PLOT_EXIT == True:
plt.figure(self.overlap_figure)
plt.clf()
plt.plot(q_gBundle[:, 0], q_gBundle[:, 1], 'ko')
plt.plot(self.q_g[0], self.q_g[1], 'ro')
plt.plot(pose[0], pose[1], 'bo')
self.plotPioneer(self.overlap_figure, 0)
if transFace is None:
print "ERROR: Unable to find transition face between regions %s and %s. Please check the decomposition (try viewing projectname_decomposed.regions in RegionEditor or a text editor)." % (
self.proj.rfi.regions[current_reg].name,
self.proj.rfi.regions[next_reg].name)
##################################################
#######check whether obstacle is detected#########
##################################################
#Update pose,update self.robot, self.realRobot orientation
self.robot.shift(pose[0] - self.prev_pose[0],
pose[1] - self.prev_pose[1])
self.realRobot.shift(pose[0] - self.prev_pose[0],
pose[1] - self.prev_pose[1])
self.robot.rotate(pose[2] - self.prev_pose[2], pose[0], pose[1])
self.realRobot.rotate(pose[2] - self.prev_pose[2], pose[0], pose[1])
self.prev_pose = pose
############################
########### STEP 1##########
############################
##Check whether obsRange overlaps with obstacle or the boundary (overlap returns the part of robot not covered by the region
# for real Pioneer robot
if self.system == 1:
# motion controller is not in boundary following mode
if self.boundary_following == False:
if self.robocomm.getReceiveObs() == False:
overlap = self.robot - (self.map_work)
else:
overlap = self.robot - (self.map_work -
self.robocomm.getObsPoly())
else: #TRUE
# use a robot with full range all around it
Robot = PolyShapes.Circle(self.obsRange, (pose[0], pose[1]))
Robot.shift(self.shift * cos(pose[2]),
self.shift * sin(pose[2]))
if self.robocomm.getReceiveObs() == False:
overlap = Robot - (self.map_work)
else:
overlap = Robot - (self.map_work -
self.robocomm.getObsPoly())
# for ODE
else:
if self.boundary_following == False:
overlap = self.robot - (self.map_work)
else: #TRUE
overlap = self.robot - (self.map_work)
if self.boundary_following == False:
if bool(overlap): ## overlap of obstacles
#print "There MAYBE overlap~~ check connection to goal"
# check whether the real robot or and path to goal overlap with the obstacle
QGoalPoly = PolyShapes.Circle(self.PioneerLengthHalf,
(self.q_g[0], self.q_g[1]))
path = PolyUtils.convexHull(self.realRobot + QGoalPoly)
if self.system == 1:
if self.robocomm.getReceiveObs() == False:
pathOverlap = path - (self.map_work)
else:
pathOverlap = path - (self.map_work -
self.robocomm.getObsPoly())
else:
pathOverlap = path - (self.map_work)
if bool(
pathOverlap
): # there is overlapping, go into bounding following mode
#print "There IS overlap"
self.q_hit = mat([pose[0], pose[1]]).T
self.boundary_following = True
#Generate m-line polygon
QHitPoly = PolyShapes.Circle(self.PioneerLengthHalf / 4,
(pose[0], pose[1]))
QGoalPoly = PolyShapes.Circle(self.PioneerLengthHalf / 4,
(self.q_g[0], self.q_g[1]))
self.m_line = PolyUtils.convexHull(QHitPoly + QGoalPoly)
#plot the first overlap
if self.PLOT_M_LINE == True:
plt.figure(self.overlap_figure)
plt.clf()
self.plotPoly(QHitPoly, 'k')
self.plotPoly(QGoalPoly, 'k')
self.plotPoly(overlap, 'g')
self.plotPoly(self.m_line, 'b')
plt.plot(pose[0], pose[1], 'bo')
self.plotPioneer(self.overlap_figure, 0)
else: ##head towards the q_goal
if self.system == 1:
if self.robocomm.getReceiveObs(
) == False: # wait for obstacles from Pioneer
vx = 0
vy = 0
else:
dis_cur = vstack((self.q_g[0], self.q_g[1])) - mat(
[pose[0], pose[1]]).T
vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
else:
dis_cur = vstack((self.q_g[0], self.q_g[1])) - mat(
[pose[0], pose[1]]).T
vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
#print "no obstacles 2-ODE true"
else: ##head towards the q_goal
if self.system == 1:
if self.robocomm.getReceiveObs(
) == False: # wait for obstacles from Pioneer
vx = 0
vy = 0
else:
dis_cur = vstack((self.q_g[0], self.q_g[1])) - mat(
[pose[0], pose[1]]).T
vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
else:
dis_cur = vstack(
(self.q_g[0], self.q_g[1])) - mat([pose[0], pose[1]]).T
vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
#print "no obstacles 1-ODE true"
if self.boundary_following == True:
self.q_hit_count += 1
# finding the point to go normal to (closest overlapping point)
j = 0
recheck = 0
while not bool(overlap):
# cannot see the obstacle. Based on the location of the previous point blow up the range of the robot on the left or on the right
j += 1
# finding whether the previous obstacle point is on the left side or the right side of the robot
# angle = angle of the previous point from the x-axis of the field
# omega = angle of the current Pioneer orientation from the x-axis of the field
# cc = differnece between angle and omega ( < pi = previous point on the left of robot, else on the right of robot)
x = self.prev_follow[0] - pose[0]
y = self.prev_follow[1] - pose[1]
angle = atan(y / x)
# convert angle to 2pi
if x > 0 and y > 0:
angle = angle
elif x < 0 and y > 0:
angle = pi + angle
elif x < 0 and y < 0:
angle = pi + angle
else:
angle = 2 * pi + angle
# convert pose to 2pi
if pose[2] < 0:
omega = (2 * pi + pose[2])
else:
omega = pose[2]
if omega > angle:
cc = 2 * pi - (omega - angle)
else:
cc = angle - omega
# on the left
#if angle - omega > 0 and angle - omega < pi:
if cc < pi:
#print "on the left, angle: "+ str(angle) + " omega: "+ str(omega)+ " angle-omega: "+ str(angle-omega)
Robot = PolyShapes.Rectangle(self.range * 2 * j,
self.range * 2 * j)
Robot.shift(pose[0] - self.range * j * 2,
pose[1] - self.range * j)
Robot.rotate(pose[2] - pi / 2, pose[0], pose[1])
# on the right
else:
#print "on the right, angle: "+ str(angle) + " omega: "+ str(omega)+ " angle-omega: "+ str(angle-omega)
Robot = PolyShapes.Rectangle(self.range * 2 * j,
self.range * 2 * j)
Robot.shift(pose[0], pose[1] - self.range * j)
Robot.rotate(pose[2] - pi / 2, pose[0], pose[1])
if self.system == 1:
overlap = Robot - (self.map_work -
self.robocomm.getObsPoly())
else:
overlap = Robot - (self.map_work)
#self.plotPoly(Robot, 'm',2)
#determines as dynamic obstacles and can be go striaight to the goal point
if j >= 2:
dis_cur = vstack(
(self.q_g[0], self.q_g[1])) - mat([pose[0], pose[1]]).T
vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
overlap = None
self.overlap = overlap
self.q_hit_count = 0
self.boundary_following = False
self.m_line = None
self.drive_handler.setVelocity(vx, vy, pose[2])
RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf +
0.06,
(pose[0], pose[1])) ###0.05
departed = not self.currentRegionPoly.overlaps(
self.realRobot)
#departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
arrived = self.nextRegionPoly.covers(self.realRobot)
return arrived
##extra box plotting in figure 1#
if self.PLOT_OVERLAP == True:
plt.figure(self.original_figure)
plt.clf()
self.plotPoly(self.realRobot, 'r')
self.plotPoly(self.robot, 'b')
self.plotPoly(overlap, 'g', 3)
plt.plot(pose[0], pose[1], 'bo')
self.plotPioneer(self.original_figure)
# find the closest point on the obstacle to the robot
overlap_len = len(overlap)
for j in range(overlap_len):
BoundPolyPoints = asarray(overlap[j])
for i in range(len(BoundPolyPoints) - 1):
bundle_x = (
BoundPolyPoints[i, 0] +
BoundPolyPoints[1 + i, 0]) / 2 #mid-point coordinate x
bundle_y = (
BoundPolyPoints[i, 1] +
BoundPolyPoints[1 + i, 1]) / 2 #mid-point coordinate y
q_overlap = hstack((q_overlap, vstack(
(bundle_x, bundle_y))))
bundle_x = (BoundPolyPoints[len(BoundPolyPoints) - 1, 0] +
BoundPolyPoints[0, 0]) / 2 #mid-point coordinate x
bundle_y = (BoundPolyPoints[len(BoundPolyPoints) - 1, 1] +
BoundPolyPoints[0, 1]) / 2 #mid-point coordinate y
q_overlap = hstack((q_overlap, vstack((bundle_x, bundle_y))))
q_overlap = q_overlap.transpose()
pt = self.closest_pt([pose[0], pose[1]],
vstack(
(q_overlap,
asarray(PolyUtils.pointList(overlap)))))
self.prev_follow = pt
#calculate the vector to follow the obstacle
normal = mat([pose[0], pose[1]] - pt)
#find the distance from the closest point
distance = norm(normal)
velocity = normal * self.trans_matrix
vx = (velocity / norm(velocity) / 3)[0, 0]
vy = (velocity / norm(velocity) / 3)[0, 1]
# push or pull the robot towards the obstacle depending on whether the robot is close or far from the obstacle.
turn = pi / 4 * (distance - 0.5 * self.obsRange) / (
self.obsRange
) ### change to 0.6 from 0.5 for more allowance in following
corr_matrix = mat([[cos(turn), -sin(turn)], [sin(turn),
cos(turn)]])
v = corr_matrix * mat([[vx], [vy]])
vx = v[0, 0]
vy = v[1, 0]
##plotting overlap on figure 2
if self.PLOT_OVERLAP == True:
plt.figure(self.overlap_figure)
plt.clf()
self.plotPoly(self.m_line, 'b')
self.plotPoly(overlap, 'r')
plt.plot(pt[0], pt[1], 'ro')
plt.plot(pose[0], pose[1], 'bo')
self.plotPioneer(self.overlap_figure, 0)
## conditions that the loop will end
#for 11111
RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf + 0.06,
(pose[0], pose[1])) ####0.05
departed = not self.currentRegionPoly.overlaps(self.realRobot)
#departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
arrived = self.nextRegionPoly.covers(self.realRobot)
#for 33333
reachMLine = self.m_line.overlaps(RobotPoly)
# 1.reached the next region
if arrived:
self.boundary_following = False
self.m_line = None
self.q_hit_count = 0
print "arriving at the next region. Exit boundary following mode"
vx = 0
vy = 0
"""
# 2.q_hit is reencountered
elif norm(self.q_hit-mat([pose[0],pose[1]]).T) < 0.05 and self.q_hit_count > self.q_hit_Thres:
print "reencounter q_hit. cannot reach q_goal"
vx = 0
vy = 0
"""
# 3.m-line reencountered
elif reachMLine:
#print >>sys.__stdout__, "m-line overlaps RoboPoly, m-line" + str(norm(self.q_g-self.q_hit)-2*self.obsRange) + " distance: " + str(norm(self.q_g-mat([pose[0],pose[1]]).T))
if norm(self.q_g - mat([pose[0], pose[1]]).T
) < norm(self.q_g - self.q_hit) - 2 * self.obsRange:
#print "m-line overlaps RoboPoly, m-line" + str(norm(self.q_g-self.q_hit)-2*self.obsRange) + " distance: " + str(norm(self.q_g-mat([pose[0],pose[1]]).T))
#print "leaving boundary following mode"
self.boundary_following = False
self.m_line = None
self.q_hit_count = 0
leaving = False
# turn the robot till it is facing the goal
while not leaving:
x = self.q_g[0] - self.pose_handler.getPose()[0]
y = self.q_g[1] - self.pose_handler.getPose()[1]
angle = atan(y / x)
if x > 0 and y > 0:
angle = angle
elif x < 0 and y > 0:
angle = pi + angle
elif x < 0 and y < 0:
angle = pi + angle
else:
angle = 2 * pi + angle
if self.pose_handler.getPose()[2] < 0:
omega = (2 * pi + self.pose_handler.getPose()[2])
#print >>sys.__stdout__,"omega<0: "+ str(omega)
else:
omega = self.pose_handler.getPose()[2]
#print >>sys.__stdout__,"omega: "+ str(omega)
if omega > angle:
cc = 2 * pi - (omega - angle)
else:
cc = angle - omega
# angle(goal point orientation) on the left of omega(robot orientation)
#if angle - omega > 0 and angle - omega < pi:
if cc < pi:
#print>>sys.__stdout__, "turn left"
vx, vy = self.turnLeft(cc)
# on the right
else:
#print>>sys.__stdout__, "turn right"
vx, vy = self.turnRight(2 * pi - cc)
#print>>sys.__stdout__, "omega: "+ str(omega) + " angle: "+ str(angle) + " (omega-angle): " + str(omega-angle)
self.drive_handler.setVelocity(
vx, vy,
self.pose_handler.getPose()[2])
if omega - angle < pi / 6 and omega - angle > -pi / 6:
leaving = True
#Check whether the robot can leave now (the robot has to be closer to the goal than when it is at q_hit to leave)
QGoalPoly = PolyShapes.Circle(self.PioneerLengthHalf,
(self.q_g[0], self.q_g[1]))
path = PolyUtils.convexHull(self.realRobot + QGoalPoly)
if self.system == 1:
if self.robocomm.getReceiveObs() == False:
pathOverlap = path - (self.map_work)
else:
pathOverlap = path - (self.map_work -
self.robocomm.getObsPoly())
else:
pathOverlap = path - (self.map_work)
if not bool(pathOverlap):
#print "There is NO MORE obstacles in front for now."
# check if the robot is closer to the goal compared with q_hit
if norm(self.q_hit - mat(self.q_g).T) > norm(
mat([pose[0], pose[1]]).T - mat(self.q_g).T):
#print "The robot is closer than the leaving point. The robot can leave"
self.boundary_following = False
self.m_line = None
self.q_hit_count = 0
dis_cur = vstack(
(self.q_g[0], self.q_g[1])) - mat([pose[0], pose[1]]).T
vx = (dis_cur / norm(dis_cur) / 3)[0, 0]
vy = (dis_cur / norm(dis_cur) / 3)[1, 0]
else:
lala = 1
#print "not leaving bug algorithm. difference(-farther) =" + str(norm(self.q_hit-mat(self.q_g).T) - norm(mat([pose[0],pose[1]]).T-mat(self.q_g).T))
"""
# Pass this desired velocity on to the drive handler
# Check if there are obstacles within 0.35m of the robot, if so, stop the robot
if self.system == 1:
if self.robocomm.getSTOP() == True:
vx = 0
vy = 0
"""
#vx = 0
#vy = 0
self.overlap = overlap
self.drive_handler.setVelocity(vx, vy, pose[2])
# Set the current region as the previous current region(for checking whether the robot has arrived at the next region)
self.previous_current_reg = current_reg
# check whether robot has arrived at the next region
RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf + 0.06,
(pose[0], pose[1])) ###0.05
#departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
departed = not self.currentRegionPoly.overlaps(self.realRobot)
arrived = self.nextRegionPoly.covers(self.realRobot)
if arrived:
self.q_hit_count = 0
self.boundary_following = False
self.m_line = None
if departed and (
not arrived) and (time.time() - self.last_warning) > 0.5:
print "WARNING: Left current region but not in expected destination region"
# Figure out what region we think we stumbled into
for r in self.proj.rfi.regions:
pointArray = [self.coordmap_map2lab(x) for x in r.getPoints()]
vertices = mat(pointArray).T
if is_inside([pose[0], pose[1]], vertices):
#print "I think I'm in " + r.name
#print pose
break
self.last_warning = time.time()
return arrived
def buildTree(self,p,theta,regionPoly,nextRegionPoly,q_gBundle,face_normal, last=False):
"""
This function builds the RRT tree.
p : x,y position of the robot
theta : current orientation of the robot
regionPoly : current region polygon
nextRegionPoly : next region polygon
q_gBundle : coordinates of q_goals that the robot can reach
face_normal : the normal vector of each face corresponding to each goal point in q_gBundle
"""
q_init = mat(p).T
V = vstack((0,q_init))
theta = self.orientation_bound(theta)
V_theta = array([theta])
#!!! CONTROL SPACE: generate a list of omega for random sampling
omegaLowerBound = -math.pi/20 # upper bound for the value of omega
omegaUpperBound = math.pi/20 # lower bound for the value of omega
omegaNoOfSteps = 20
self.omega_range = linspace(omegaLowerBound,omegaUpperBound,omegaNoOfSteps)
self.omega_range_escape = linspace(omegaLowerBound*4,omegaUpperBound*4,omegaNoOfSteps*4) # range used when stuck > stuck_thres
regionPolyOld = Polygon.Polygon(regionPoly)
regionPoly += PolyShapes.Circle(self.radius*2.5,(q_init[0,0],q_init[1,0]))
# check faces of the current region for goal points
E = [[],[]] # the tree matrix
Other = [[],[]]
path = False # if path formed then = 1
stuck = 0 # count for changing the range of sampling omega
append_after_latest_node = False # append new nodes to the latest node
if self.system_print == True:
print "plotting in buildTree is " + str(self.plotting)
if self.plotting == True:
if not plt.isinteractive():
plt.ion()
plt.hold(True)
while not path:
#step -1: try connection to q_goal (generate path to goal)
i = 0
if self.system_print == True:
print "Try Connection to the goal points"
# pushing possible q_goals into the current region (ensure path is covered by the current region polygon)
q_pass = [[],[],[]]
q_pass_dist = []
q_gBundle = mat(q_gBundle)
face_normal = mat(face_normal)
while i < q_gBundle.shape[1]:
q_g_original = q_gBundle[:,i]
q_g = q_gBundle[:,i]+face_normal[:,i]*1.5*self.radius ##original 2*self.radius
#q_g = q_gBundle[:,i]+(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*1.5*self.radius ##original 2*self.radius
if not regionPolyOld.isInside(q_g[0],q_g[1]):
#q_g = q_gBundle[:,i]-(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*1.5*self.radius ##original 2*self.radius
q_g = q_gBundle[:,i]-face_normal[:,i]*1.5*self.radius ##original 2*self.radius
#forming polygon for path checking
EdgePolyGoal = PolyShapes.Circle(self.radius,(q_g[0,0],q_g[1,0])) + PolyShapes.Circle(self.radius,(V[1,shape(V)[1]-1],V[2:,shape(V)[1]-1]))
EdgePolyGoal = PolyUtils.convexHull(EdgePolyGoal)
dist = norm(q_g - V[1:,shape(V)[1]-1])
#check connection to goal
connect_goal = regionPoly.covers(EdgePolyGoal) #check coverage of path from new point to goal
# compare orientation difference
thetaPrev = V_theta[shape(V)[1]-1]
theta_orientation = abs(arctan((q_g[1,0]- V[2,shape(V)[1]-1])/(q_g[0,0]- V[1,shape(V)[1]-1])))
if q_g[1,0] > V[2,shape(V)[1]-1]:
if q_g[0,0] < V[1,shape(V)[1]-1]: # second quadrant
theta_orientation = pi - theta_orientation
elif q_g[0,0] > V[1,shape(V)[1]-1]: # first quadrant
theta_orientation = theta_orientation
elif q_g[1,0] < V[2,shape(V)[1]-1]:
if q_g[0,0] < V[1,shape(V)[1]-1]: #third quadrant
theta_orientation = pi + theta_orientation
elif q_g[0,0] > V[1,shape(V)[1]-1]: # foruth quadrant
theta_orientation = 2*pi - theta_orientation
# check the angle between vector(new goal to goal_original ) and vector( latest node to new goal)
Goal_to_GoalOriginal = q_g_original - q_g
LatestNode_to_Goal = q_g - V[1:,shape(V)[1]-1]
Angle_Goal_LatestNode= arccos(vdot(array(Goal_to_GoalOriginal), array(LatestNode_to_Goal))/norm(Goal_to_GoalOriginal)/norm(LatestNode_to_Goal))
# if connection to goal can be established and the max change in orientation of the robot is smaller than max_angle, tree is said to be completed.
if self.orientation_print == True:
print "theta_orientation is " + str(theta_orientation)
print "thetaPrev is " + str(thetaPrev)
print "(theta_orientation - thetaPrev) is " + str(abs(theta_orientation - thetaPrev))
print "self.max_angle_allowed is " + str(self.max_angle_allowed)
print "abs(theta_orientation - thetaPrev) < self.max_angle_allowed" + str(abs(theta_orientation - thetaPrev) < self.max_angle_allowed)
print"Goal_to_GoalOriginal: " + str( array(Goal_to_GoalOriginal)) + "; LatestNode_to_Goal: " + str( array(LatestNode_to_Goal))
print vdot(array(Goal_to_GoalOriginal), array(LatestNode_to_Goal))
print "Angle_Goal_LatestNode" + str(Angle_Goal_LatestNode)
if connect_goal and (abs(theta_orientation - thetaPrev) < self.max_angle_allowed) and (Angle_Goal_LatestNode < self.max_angle_allowed):
path = True
q_pass = hstack((q_pass,vstack((i,q_g))))
q_pass_dist = hstack((q_pass_dist,dist))
i = i + 1
if self.system_print == True:
print "checked goal points"
self.E = E
self.V = V
# connection to goal has established
# Obtain the closest goal point that path can be formed.
if path:
if shape(q_pass_dist)[0] == 1:
cols = 0
else:
(cols,) = nonzero(q_pass_dist == min(q_pass_dist))
cols = asarray(cols)[0]
q_g = q_pass[1:,cols]
"""
q_g = q_g-(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*3*self.radius #org 3
if not nextRegionPoly.isInside(q_g[0],q_g[1]):
q_g = q_g+(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*6*self.radius #org 3
"""
if self.plotting == True :
if self.operate_system == 1:
plt.suptitle('Rapidly-exploring Random Tree', fontsize=12)
plt.xlabel('x')
plt.ylabel('y')
if shape(V)[1] <= 2:
plt.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
else:
plt.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')
plt.plot(q_g[0,0],q_g[1,0],'ko')
plt.figure(1).canvas.draw()
else:
BoundPolyPoints = asarray(PolyUtils.pointList(regionPoly))
self.ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
if shape(V)[1] <= 2:
self.ax.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
else:
self.ax.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')
self.ax.plot(q_g[0,0],q_g[1,0],'ko')
# trim the path connecting current node to goal point into pieces if the path is too long now
numOfPoint = floor(norm(V[1:,shape(V)[1]-1]- q_g)/self.step_size)
if numOfPoint < 3:
numOfPoint = 3
x = linspace( V[1,shape(V)[1]-1], q_g[0,0], numOfPoint )
y = linspace( V[2,shape(V)[1]-1], q_g[1,0], numOfPoint )
for i in range(x.shape[0]):
if i != 0:
V = hstack((V,vstack((shape(V)[1],x[i],y[i]))))
E = hstack((E,vstack((shape(V)[1]-2,shape(V)[1]-1))))
#push the goal point to the next region
q_g = q_g+face_normal[:,q_pass[0,cols]]*3*self.radius ##original 2*self.radius
if not nextRegionPoly.isInside(q_g[0],q_g[1]):
q_g = q_g-face_normal[:,q_pass[0,cols]]*6*self.radius ##original 2*self.radius
V = hstack((V,vstack((shape(V)[1],q_g[0,0],q_g[1,0]))))
E = hstack((E,vstack((shape(V)[1]-2 ,shape(V)[1]-1))))
if self.plotting == True :
if self.operate_system == 1:
plt.plot(q_g[0,0],q_g[1,0],'ko')
plt.plot(( V[1,shape(V)[1]-1],V[1,shape(V)[1]-2]),( V[2,shape(V)[1]-1],V[2,shape(V)[1]-2]),'b')
plt.figure(1).canvas.draw()
else:
self.ax.plot(q_g[0,0],q_g[1,0],'ko')
self.ax.plot(( V[1,shape(V)[1]-1],V[1,shape(V)[1]-2]),( V[2,shape(V)[1]-1],V[2,shape(V)[1]-2]),'b')
# path is not formed, try to append points onto the tree
if not path:
# connection_to_tree : connection to the tree is successful
if append_after_latest_node:
V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree = self.generateNewNode(V,V_theta,E,Other,regionPoly,stuck, append_after_latest_node)
else:
connection_to_tree = False
while not connection_to_tree:
V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree = self.generateNewNode (V,V_theta,E,Other,regionPoly,stuck)
if self.finish_print:
print 'Here is the V matrix:', V, 'Here is the E matrix:',E
print >>sys.__stdout__, 'Here is the V matrix:\n', V, '\nHere is the E matrix:\n',E
#B: trim to a single path
single = 0
while single == 0:
trim = 0
for j in range(shape(V)[1]-3):
(row,col) = nonzero(E == j+1)
if len(col) == 1:
E = delete(E, col[0], 1)
trim = 1
if trim == 0:
single = 1;
####print with matlib
if self.plotting ==True :
if self.operate_system == 1:
plt.plot(V[1,:],V[2,:],'b')
for i in range(shape(E)[1]):
plt.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
plt.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)
plt.figure(1).canvas.draw()
else:
BoundPolyPoints = asarray(PolyUtils.pointList(regionPoly))
self.ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
self.ax.plot(V[1,:],V[2,:],'b')
for i in range(shape(E)[1]):
self.ax.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
self.ax.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)
#return V, E, and the current node number on the tree
V = array(V)
return V, E, 0
def gotoRegion(self, current_reg, next_reg, last=False):
"""
If ``last`` is True, we will move to the center of the destination region.
Returns ``True`` if we've reached the destination region.
"""
q_gBundle = [[],[]] # goal bundle
q_overlap = [[],[]] # overlapping points with robot range
pose = self.pose_handler.getPose() # Find our current configuration
#Plot the robot on the map in figure 1
if self.PLOT == True:
plt.figure(self.original_figure)
plt.clf()
self.plotPoly(self.realRobot, 'r')
self.plotPoly(self.robot, 'b')
plt.plot(pose[0],pose[1],'bo')
self.plotPioneer(self.original_figure)
if current_reg == next_reg and not last:
# No need to move!
self.drive_handler.setVelocity(0, 0) # So let's stop
return False
# Check if Vicon has cut out
if math.isnan(pose[2]):
print "no vicon pose"
print "WARNING: No Vicon data! Pausing."
#self.drive_handler.setVelocity(0, 0) # So let's stop
time.sleep(1)
return False
###This part is run when the robot goes to a new region, otherwise, the original map will be used.
if not self.previous_current_reg == current_reg:
#print 'getting into bug alogorithm'
#clean up the previous self.map_work
self.map_work = Polygon.Polygon()
# NOTE: Information about region geometry can be found in self.proj.rfi.regions
# create polygon list for regions other than the current_reg and the next_reg
self.map_work += self.map[self.proj.rfi.regions[current_reg].name]
self.map_work += self.map[self.proj.rfi.regions[next_reg].name]
# building current polygon and destination polygon
self.nextRegionPoly = self.map[self.proj.rfi.regions[next_reg].name]
self.currentRegionPoly = self.map[self.proj.rfi.regions[current_reg].name]
#set to zero velocity before finding the tranFace
self.drive_handler.setVelocity(0, 0)
if last:
transFace = None
else:
print "Current reg is " + str(self.proj.rfi.regions[current_reg].name.lower())
print "Next reg is "+ str(self.proj.rfi.regions[next_reg].name.lower())
for i in range(len(self.proj.rfi.transitions[current_reg][next_reg])):
pointArray_transface = [x for x in self.proj.rfi.transitions[current_reg][next_reg][i]]
transFace = asarray(map(self.coordmap_map2lab,pointArray_transface))
bundle_x = (transFace[0,0] +transFace[1,0])/2 #mid-point coordinate x
bundle_y = (transFace[0,1] +transFace[1,1])/2 #mid-point coordinate y
q_gBundle = hstack((q_gBundle,vstack((bundle_x,bundle_y))))
q_gBundle = q_gBundle.transpose()
# Find the closest face to the current position
max_magsq = 1000000
for tf in q_gBundle:
magsq = (tf[0] - pose[0])**2 + (tf[1] - pose[1])**2
if magsq < max_magsq:
connection = 0
tf = tf+(tf-asarray(self.currentRegionPoly.center()))/norm(tf-asarray(self.currentRegionPoly.center()))*2.1*self.PioneerLengthHalf
if not self.nextRegionPoly.covers(PolyShapes.Circle(self.PioneerLengthHalf*2,(tf[0],tf[1]))):
tf = tf-(tf-asarray(self.currentRegionPoly.center()))/norm(tf-asarray(self.currentRegionPoly.center()))*4.2*self.PioneerLengthHalf
if self.nextRegionPoly.covers(PolyShapes.Circle(self.PioneerLengthHalf*2,(tf[0],tf[1]))):
connection = 1
else:
connection = 1
if connection == 1:
pt1 = tf
max_magsq = magsq
transFace = 1
self.q_g[0] = pt1[0]
self.q_g[1] = pt1[1]
else:
sample = False
while not sample:
self.q_g[0],self.q_g[1] = self.nextRegionPoly.sample(random.random)
robo = PolyShapes.Circle(self.PioneerLengthHalf,(self.q_g[0],self.q_g[1]))
if not bool(robo - self.nextRegionPoly):
sample = True
"""
# Push the goal point to somewhere inside the next region to ensure the robot will get there.(CHECK!!)
self.q_g = self.q_g+(self.q_g-asarray(self.currentRegionPoly.center()))/norm(self.q_g-asarray(self.currentRegionPoly.center()))*3*self.PioneerLengthHalf
if not self.nextRegionPoly.isInside(self.q_g[0],self.q_g[1]):
self.q_g = self.q_g-(self.q_g-asarray(self.currentRegionPoly.center()))/norm(self.q_g-asarray(self.currentRegionPoly.center()))*6*self.PioneerLengthHalf
"""
#plot exiting point
if self.PLOT_EXIT == True:
plt.figure(self.overlap_figure)
plt.clf()
plt.plot(q_gBundle[:,0],q_gBundle[:,1],'ko' )
plt.plot(self.q_g[0],self.q_g[1],'ro')
plt.plot(pose[0],pose[1],'bo')
self.plotPioneer(self.overlap_figure,0)
if transFace is None:
print "ERROR: Unable to find transition face between regions %s and %s. Please check the decomposition (try viewing projectname_decomposed.regions in RegionEditor or a text editor)." % (self.proj.rfi.regions[current_reg].name, self.proj.rfi.regions[next_reg].name)
##################################################
#######check whether obstacle is detected#########
##################################################
#Update pose,update self.robot, self.realRobot orientation
self.robot.shift(pose[0]-self.prev_pose[0],pose[1]-self.prev_pose[1])
self.realRobot.shift(pose[0]-self.prev_pose[0],pose[1]-self.prev_pose[1])
self.robot.rotate(pose[2]-self.prev_pose[2],pose[0],pose[1])
self.realRobot.rotate(pose[2]-self.prev_pose[2],pose[0],pose[1])
self.prev_pose = pose
############################
########### STEP 1##########
############################
##Check whether obsRange overlaps with obstacle or the boundary (overlap returns the part of robot not covered by the region
# for real Pioneer robot
if self.system == 1:
# motion controller is not in boundary following mode
if self.boundary_following == False:
if self.robocomm.getReceiveObs() == False:
overlap = self.robot - ( self.map_work)
else:
overlap = self.robot - ( self.map_work - self.robocomm.getObsPoly())
else: #TRUE
# use a robot with full range all around it
Robot = PolyShapes.Circle(self.obsRange,(pose[0],pose[1]))
Robot.shift(self.shift*cos(pose[2]),self.shift*sin(pose[2]))
if self.robocomm.getReceiveObs() == False:
overlap = Robot - ( self.map_work)
else:
overlap = Robot - ( self.map_work - self.robocomm.getObsPoly())
# for ODE
else:
if self.boundary_following == False:
overlap = self.robot - (self.map_work)
else:#TRUE
overlap = self.robot - (self.map_work)
if self.boundary_following == False:
if bool(overlap): ## overlap of obstacles
#print "There MAYBE overlap~~ check connection to goal"
# check whether the real robot or and path to goal overlap with the obstacle
QGoalPoly= PolyShapes.Circle(self.PioneerLengthHalf,(self.q_g[0],self.q_g[1]))
path = PolyUtils.convexHull(self.realRobot + QGoalPoly)
if self.system == 1:
if self.robocomm.getReceiveObs() == False:
pathOverlap = path - ( self.map_work)
else:
pathOverlap = path - ( self.map_work - self.robocomm.getObsPoly())
else:
pathOverlap = path - ( self.map_work)
if bool(pathOverlap): # there is overlapping, go into bounding following mode
#print "There IS overlap"
self.q_hit = mat([pose[0],pose[1]]).T
self.boundary_following = True
#Generate m-line polygon
QHitPoly = PolyShapes.Circle(self.PioneerLengthHalf/4,(pose[0],pose[1]))
QGoalPoly= PolyShapes.Circle(self.PioneerLengthHalf/4,(self.q_g[0],self.q_g[1]))
self.m_line = PolyUtils.convexHull(QHitPoly + QGoalPoly)
#plot the first overlap
if self.PLOT_M_LINE == True:
plt.figure(self.overlap_figure)
plt.clf()
self.plotPoly(QHitPoly,'k')
self.plotPoly(QGoalPoly,'k')
self.plotPoly(overlap,'g')
self.plotPoly(self.m_line,'b')
plt.plot(pose[0],pose[1],'bo')
self.plotPioneer(self.overlap_figure,0)
else: ##head towards the q_goal
if self.system == 1:
if self.robocomm.getReceiveObs() == False: # wait for obstacles from Pioneer
vx = 0
vy = 0
else:
dis_cur = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
vx = (dis_cur/norm(dis_cur)/3)[0,0]
vy = (dis_cur/norm(dis_cur)/3)[1,0]
else:
dis_cur = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
vx = (dis_cur/norm(dis_cur)/3)[0,0]
vy = (dis_cur/norm(dis_cur)/3)[1,0]
#print "no obstacles 2-ODE true"
else: ##head towards the q_goal
if self.system == 1:
if self.robocomm.getReceiveObs() == False: # wait for obstacles from Pioneer
vx = 0
vy = 0
else:
dis_cur = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
vx = (dis_cur/norm(dis_cur)/3)[0,0]
vy = (dis_cur/norm(dis_cur)/3)[1,0]
else:
dis_cur = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
vx = (dis_cur/norm(dis_cur)/3)[0,0]
vy = (dis_cur/norm(dis_cur)/3)[1,0]
#print "no obstacles 1-ODE true"
if self.boundary_following == True:
self.q_hit_count += 1
# finding the point to go normal to (closest overlapping point)
j = 0
recheck = 0
while not bool(overlap):
# cannot see the obstacle. Based on the location of the previous point blow up the range of the robot on the left or on the right
j += 1
# finding whether the previous obstacle point is on the left side or the right side of the robot
# angle = angle of the previous point from the x-axis of the field
# omega = angle of the current Pioneer orientation from the x-axis of the field
# cc = differnece between angle and omega ( < pi = previous point on the left of robot, else on the right of robot)
x = self.prev_follow[0] -pose[0]
y = self.prev_follow[1] -pose[1]
angle = atan(y/x)
# convert angle to 2pi
if x > 0 and y > 0:
angle = angle
elif x < 0 and y > 0:
angle = pi + angle
elif x <0 and y < 0:
angle = pi + angle
else:
angle = 2*pi + angle
# convert pose to 2pi
if pose[2] < 0:
omega = (2*pi + pose[2])
else:
omega = pose[2]
if omega > angle:
cc = 2*pi - (omega - angle)
else:
cc = angle - omega
# on the left
#if angle - omega > 0 and angle - omega < pi:
if cc < pi:
#print "on the left, angle: "+ str(angle) + " omega: "+ str(omega)+ " angle-omega: "+ str(angle-omega)
Robot = PolyShapes.Rectangle(self.range*2*j,self.range*2*j)
Robot.shift(pose[0]-self.range*j*2,pose[1]-self.range*j)
Robot.rotate(pose[2]-pi/2,pose[0],pose[1])
# on the right
else:
#print "on the right, angle: "+ str(angle) + " omega: "+ str(omega)+ " angle-omega: "+ str(angle-omega)
Robot = PolyShapes.Rectangle(self.range*2*j,self.range*2*j)
Robot.shift(pose[0],pose[1]-self.range*j)
Robot.rotate(pose[2]-pi/2,pose[0],pose[1])
if self.system == 1:
overlap = Robot - ( self.map_work - self.robocomm.getObsPoly())
else:
overlap = Robot - ( self.map_work)
#self.plotPoly(Robot, 'm',2)
#determines as dynamic obstacles and can be go striaight to the goal point
if j >= 2:
dis_cur = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
vx = (dis_cur/norm(dis_cur)/3)[0,0]
vy = (dis_cur/norm(dis_cur)/3)[1,0]
overlap = None
self.overlap = overlap
self.q_hit_count = 0
self.boundary_following = False
self.m_line = None
self.drive_handler.setVelocity(vx,vy, pose[2])
RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf+0.06,(pose[0],pose[1])) ###0.05
departed = not self.currentRegionPoly.overlaps(self.realRobot)
#departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
arrived = self.nextRegionPoly.covers(self.realRobot)
return arrived
##extra box plotting in figure 1#
if self.PLOT_OVERLAP == True:
plt.figure(self.original_figure)
plt.clf()
self.plotPoly(self.realRobot, 'r')
self.plotPoly(self.robot, 'b')
self.plotPoly(overlap,'g',3)
plt.plot(pose[0],pose[1],'bo')
self.plotPioneer(self.original_figure)
# find the closest point on the obstacle to the robot
overlap_len = len(overlap)
for j in range(overlap_len):
BoundPolyPoints = asarray(overlap[j])
for i in range(len(BoundPolyPoints)-1):
bundle_x = (BoundPolyPoints[i,0] +BoundPolyPoints[1+i,0])/2 #mid-point coordinate x
bundle_y = (BoundPolyPoints[i,1] +BoundPolyPoints[1+i,1])/2 #mid-point coordinate y
q_overlap = hstack((q_overlap,vstack((bundle_x,bundle_y))))
bundle_x = (BoundPolyPoints[len(BoundPolyPoints)-1,0] +BoundPolyPoints[0,0])/2 #mid-point coordinate x
bundle_y = (BoundPolyPoints[len(BoundPolyPoints)-1,1] +BoundPolyPoints[0,1])/2 #mid-point coordinate y
q_overlap = hstack((q_overlap,vstack((bundle_x,bundle_y))))
q_overlap = q_overlap.transpose()
pt = self.closest_pt([pose[0],pose[1]], vstack((q_overlap,asarray(PolyUtils.pointList(overlap)))))
self.prev_follow = pt
#calculate the vector to follow the obstacle
normal = mat([pose[0],pose[1]] - pt)
#find the distance from the closest point
distance = norm(normal)
velocity = normal * self.trans_matrix
vx = (velocity/norm(velocity)/3)[0,0]
vy = (velocity/norm(velocity)/3)[0,1]
# push or pull the robot towards the obstacle depending on whether the robot is close or far from the obstacle.
turn = pi/4*(distance-0.5*self.obsRange)/(self.obsRange) ### change to 0.6 from 0.5 for more allowance in following
corr_matrix = mat([[cos(turn),-sin(turn)],[sin(turn),cos(turn)]])
v = corr_matrix*mat([[vx],[vy]])
vx = v[0,0]
vy = v[1,0]
##plotting overlap on figure 2
if self.PLOT_OVERLAP == True:
plt.figure(self.overlap_figure)
plt.clf()
self.plotPoly(self.m_line,'b');
self.plotPoly(overlap,'r');
plt.plot(pt[0],pt[1],'ro')
plt.plot(pose[0],pose[1],'bo')
self.plotPioneer(self.overlap_figure,0)
## conditions that the loop will end
#for 11111
RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf+0.06,(pose[0],pose[1])) ####0.05
departed = not self.currentRegionPoly.overlaps(self.realRobot)
#departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
arrived = self.nextRegionPoly.covers(self.realRobot)
#for 33333
reachMLine= self.m_line.overlaps(RobotPoly)
# 1.reached the next region
if arrived:
self.boundary_following = False
self.m_line = None
self.q_hit_count = 0
print "arriving at the next region. Exit boundary following mode"
vx = 0
vy = 0
"""
# 2.q_hit is reencountered
elif norm(self.q_hit-mat([pose[0],pose[1]]).T) < 0.05 and self.q_hit_count > self.q_hit_Thres:
print "reencounter q_hit. cannot reach q_goal"
vx = 0
vy = 0
"""
# 3.m-line reencountered
elif reachMLine:
#print >>sys.__stdout__, "m-line overlaps RoboPoly, m-line" + str(norm(self.q_g-self.q_hit)-2*self.obsRange) + " distance: " + str(norm(self.q_g-mat([pose[0],pose[1]]).T))
if norm(self.q_g-mat([pose[0],pose[1]]).T) < norm(self.q_g-self.q_hit)-2*self.obsRange:
#print "m-line overlaps RoboPoly, m-line" + str(norm(self.q_g-self.q_hit)-2*self.obsRange) + " distance: " + str(norm(self.q_g-mat([pose[0],pose[1]]).T))
#print "leaving boundary following mode"
self.boundary_following = False
self.m_line = None
self.q_hit_count = 0
leaving = False
# turn the robot till it is facing the goal
while not leaving:
x = self.q_g[0] -self.pose_handler.getPose()[0]
y = self.q_g[1] -self.pose_handler.getPose()[1]
angle = atan(y/x)
if x > 0 and y > 0:
angle = angle
elif x < 0 and y > 0:
angle = pi + angle
elif x <0 and y < 0:
angle = pi + angle
else:
angle = 2*pi + angle
if self.pose_handler.getPose()[2] < 0:
omega = (2*pi + self.pose_handler.getPose()[2])
#print >>sys.__stdout__,"omega<0: "+ str(omega)
else:
omega = self.pose_handler.getPose()[2]
#print >>sys.__stdout__,"omega: "+ str(omega)
if omega > angle:
cc = 2*pi - (omega - angle)
else:
cc = angle - omega
# angle(goal point orientation) on the left of omega(robot orientation)
#if angle - omega > 0 and angle - omega < pi:
if cc < pi:
#print>>sys.__stdout__, "turn left"
vx,vy = self.turnLeft(cc)
# on the right
else:
#print>>sys.__stdout__, "turn right"
vx, vy = self.turnRight(2*pi-cc)
#print>>sys.__stdout__, "omega: "+ str(omega) + " angle: "+ str(angle) + " (omega-angle): " + str(omega-angle)
self.drive_handler.setVelocity(vx,vy, self.pose_handler.getPose()[2])
if omega - angle < pi/6 and omega - angle > -pi/6:
leaving = True
#Check whether the robot can leave now (the robot has to be closer to the goal than when it is at q_hit to leave)
QGoalPoly= PolyShapes.Circle(self.PioneerLengthHalf,(self.q_g[0],self.q_g[1]))
path = PolyUtils.convexHull(self.realRobot + QGoalPoly)
if self.system == 1:
if self.robocomm.getReceiveObs() == False:
pathOverlap = path - ( self.map_work)
else:
pathOverlap = path - ( self.map_work - self.robocomm.getObsPoly())
else:
pathOverlap = path - ( self.map_work)
if not bool(pathOverlap):
#print "There is NO MORE obstacles in front for now."
# check if the robot is closer to the goal compared with q_hit
if norm(self.q_hit-mat(self.q_g).T) > norm(mat([pose[0],pose[1]]).T-mat(self.q_g).T) :
#print "The robot is closer than the leaving point. The robot can leave"
self.boundary_following = False
self.m_line = None
self.q_hit_count = 0
dis_cur = vstack((self.q_g[0],self.q_g[1]))- mat([pose[0],pose[1]]).T
vx = (dis_cur/norm(dis_cur)/3)[0,0]
vy = (dis_cur/norm(dis_cur)/3)[1,0]
else:
lala = 1
#print "not leaving bug algorithm. difference(-farther) =" + str(norm(self.q_hit-mat(self.q_g).T) - norm(mat([pose[0],pose[1]]).T-mat(self.q_g).T))
"""
# Pass this desired velocity on to the drive handler
# Check if there are obstacles within 0.35m of the robot, if so, stop the robot
if self.system == 1:
if self.robocomm.getSTOP() == True:
vx = 0
vy = 0
"""
#vx = 0
#vy = 0
self.overlap = overlap
self.drive_handler.setVelocity(vx,vy, pose[2])
# Set the current region as the previous current region(for checking whether the robot has arrived at the next region)
self.previous_current_reg = current_reg
# check whether robot has arrived at the next region
RobotPoly = PolyShapes.Circle(self.PioneerLengthHalf+0.06,(pose[0],pose[1])) ###0.05
#departed = not self.currentRegionPoly.overlaps(self.realRobot) and (not (self.nextRegionPoly.overlaps(self.realRobot) and not self.nextRegionPoly.covers(self.realRobot)))
departed = not self.currentRegionPoly.overlaps(self.realRobot)
arrived = self.nextRegionPoly.covers(self.realRobot)
if arrived:
self.q_hit_count = 0
self.boundary_following = False
self.m_line = None
if departed and (not arrived) and (time.time()-self.last_warning) > 0.5:
print "WARNING: Left current region but not in expected destination region"
# Figure out what region we think we stumbled into
for r in self.proj.rfi.regions:
pointArray = [self.coordmap_map2lab(x) for x in r.getPoints()]
vertices = mat(pointArray).T
if is_inside([pose[0], pose[1]], vertices):
#print "I think I'm in " + r.name
#print pose
break
self.last_warning = time.time()
return arrived
def buildTree(p,theta,vert, R, system, regionPoly,nextRegionPoly,q_gBundle,\
mappedRegions,allRegions,max_angle_allowed, plotting, operate_system,ax, last=False):
"""
This function builds the RRT tree
pose: x,y position of the robot
theta: current orientation of the robot
R: radius of the robot
system: determine step_size and radius for the example
regionPoly: current region polygon
nextRegionPoly: next region polygon
q_gBundle: coordinates of q_goals that the robot can reach
mappedRegions: region polygons
allRegions: polygon that includes all the region
max_angle_allowed: the difference in angle between the two nodes allowed (between 0 to 2*pi)
plotting: 1 if plotting is enabled, 0- disabled
operate_system: Which operating system is used for execution. Ubuntu and Mac is 1, Windows is 2
ax: plot for windows
"""
max_angle = max_angle_allowed
"""
if operate_system == 2:
# start using anmination to plot Pioneer
self.fig = plt.figure()
self.ax = self.fig.add_subplot(111)
self.scope = _Scope(self.ax,self)
thread.start_new_thread(self.jplot,())
"""
## 1: Nao ; 2: STAGE ; 3: ODE
if system == 1: ## Nao
step_size = 0.2 #set the step_size for points be 1/5 of the norm from ORIGINAL = 0.4
elif system == 2:
step_size = 0.5
elif system == 3:
step_size = 15
if system == 1: ## Nao
timeStep = 5 #time step for calculation of x, y position
elif system == 2:
timeStep = 4 #time step for calculation of x, y position
elif system == 3:
timeStep = 10 #time step for calculation of x, y position #10
#############tune velocity OMEGA, TIME STEP
#fix velocity
## 1: Nao ; 2: STAGE ; 3: ODE
if system == 1: ## Nao
velocity = 0.05 #set the step_size for points be 1/5 of the norm from
elif system == 2:
velocity = 0.06
elif system == 3:
velocity = 2 # what is used in RRTControllerHelper.setVelocity #1.5
#############tune velocity OMEGA, TIME STEP
BoundPoly = regionPoly # Boundary polygon = current region polygon
radius = R
q_init = mat(p).T
V = vstack((0, q_init))
V_theta = array([theta])
original_figure = 1
#!!! CONTROL SPACE: generate a list of omega for random sampling
omegaLowerBound = -math.pi / 20 # upper bound for the value of omega
omegaUpperBound = math.pi / 20 # lower bound for the value of omega
omegaStepSize = 20
omega_range = linspace(omegaLowerBound, omegaUpperBound, omegaStepSize)
omega_range_abso = linspace(omegaLowerBound * 4, omegaUpperBound * 4,
omegaStepSize *
4) # range used when stuck > stuck_thres
edgeX = []
edgeY = []
# check faces of the current region for goal points
E = [[], []]
Other = [[], []]
path = 0 # if path formed then = 1
stuck = 0 # count for changing the range of sampling omega
stuck_thres = 300 # threshold for changing the range of sampling omega
if not plt.isinteractive():
plt.ion()
plt.hold(True)
while path == 0:
#step -1: try connection to q_goal (generate path to goal)
goalCheck = 0
i = 0
# pushing possible q_goals into the current region (ensure path is covered by the current region polygon)
q_pass = [[], [], []]
q_pass_dist = []
q_gBundle = mat(q_gBundle)
while i < q_gBundle.shape[1]: ###not sure about shape
q_g = q_gBundle[:, i] + (q_gBundle[:, i] - V[1:, (
shape(V)[1] - 1)]) / norm(q_gBundle[:, i] - V[1:, (
shape(V)[1] - 1)]) * radius ##original 2*radius
trial = 1
if not BoundPoly.isInside(q_g[0], q_g[1]):
trial = 2
q_g = q_gBundle[:, i] - (q_gBundle[:, i] - V[1:, (
shape(V)[1] - 1)]) / norm(q_gBundle[:, i] - V[1:, (
shape(V)[1] - 1)]) * radius ##original 2*radius
#forming polygon for path checking
cross_goal = cross(
vstack((q_g - vstack(
(V[1, shape(V)[1] - 1], V[2, shape(V)[1] - 1])), 0)).T,
hstack((0, 0, 1)))
cross_goal = cross_goal.T
move_vector_goal = radius * cross_goal[0:2] / sqrt(
(cross_goal[0, 0]**2 + cross_goal[1, 0]**2))
upperEdgeG = hstack((vstack(
(V[1,
shape(V)[1] - 1], V[2, shape(V)[1] - 1])), q_g)) + hstack(
(move_vector_goal, move_vector_goal))
lowerEdgeG = hstack((vstack(
(V[1,
shape(V)[1] - 1], V[2, shape(V)[1] - 1])), q_g)) - hstack(
(move_vector_goal, move_vector_goal))
EdgePolyGoal = Polygon.Polygon(
(tuple(array(lowerEdgeG[:, 0].T)[0]),
tuple(array(upperEdgeG[:, 0].T)[0]),
tuple(array(upperEdgeG[:, 1].T)[0]),
tuple(array(lowerEdgeG[:, 1].T)[0])))
dist = norm(q_g - V[1:, shape(V)[1] - 1])
connect_goal = BoundPoly.covers(
EdgePolyGoal) #check coverage of path from new point to goal
#check connection to goal
"""
if connect_goal:
print "connection is true"
path = 1
q_pass = hstack((q_pass,vstack((i,q_g))))
q_pass_dist = hstack((q_pass_dist,dist))
"""
# compare orientation difference
thetaPrev = V_theta[shape(V)[1] - 1]
theta_orientation = abs(
arctan((q_g[1, 0] - V[2, shape(V)[1] - 1]) /
(q_g[0, 0] - V[1, shape(V)[1] - 1])))
if thetaPrev < 0:
if q_g[1, 0] > V[2, shape(V)[1] - 1]:
if q_g[0, 0] < V[1, shape(V)[1] - 1]: # second quadrant
theta_orientation = -2 * pi + theta_orientation
elif q_g[0, 0] > V[1, shape(V)[1] - 1]: # first quadrant
theta_orientation = -pi - theta_orientation
elif q_g[1, 0] < V[2, shape(V)[1] - 1]:
if q_g[0, 0] < V[1, shape(V)[1] - 1]: #third quadrant
theta_orientation = -pi + theta_orientation
elif q_g[0, 0] > V[1, shape(V)[1] - 1]: # foruth quadrant
theta_orientation = -theta_orientation
else:
if q_g[1, 0] > V[2, shape(V)[1] - 1]:
if q_g[0, 0] < V[1, shape(V)[1] - 1]: # second quadrant
theta_orientation = pi - theta_orientation
elif q_g[0, 0] > V[1, shape(V)[1] - 1]: # first quadrant
theta_orientation = theta_orientation
elif q_g[1, 0] < V[2, shape(V)[1] - 1]:
if q_g[0, 0] < V[1, shape(V)[1] - 1]: #third quadrant
theta_orientation = pi + theta_orientation
elif q_g[0, 0] > V[1, shape(V)[1] - 1]: # foruth quadrant
theta_orientation = 2 * pi - theta_orientation
################################## PRINT PLT #################
if connect_goal:
if plotting == True:
if operate_system == 1:
plt.suptitle('Randomly-exploring rapid tree',
fontsize=12)
BoundPolyPoints = asarray(
PolyUtils.pointList(BoundPoly))
plt.plot(BoundPolyPoints[:, 0], BoundPolyPoints[:, 1],
'k')
plt.xlabel('x')
plt.ylabel('y')
if shape(V)[1] <= 2:
plt.plot((V[1, shape(V)[1] - 1], q_g[0, 0]),
(V[2, shape(V)[1] - 1], q_g[1, 0]), 'b')
else:
plt.plot((V[1, E[0, shape(E)[1] - 1]],
V[1, shape(V)[1] - 1], q_g[0, 0]),
(V[2, E[0, shape(E)[1] - 1]],
V[2, shape(V)[1] - 1], q_g[1, 0]), 'b')
plt.figure(original_figure).canvas.draw()
else:
ax.suptitle('Randomly-exploring rapid tree',
fontsize=12)
BoundPolyPoints = asarray(
PolyUtils.pointList(BoundPoly))
ax.plot(BoundPolyPoints[:, 0], BoundPolyPoints[:, 1],
'k')
ax.xlabel('x')
ax.ylabel('y')
if shape(V)[1] <= 2:
ax.plot((V[1, shape(V)[1] - 1], q_g[0, 0]),
(V[2, shape(V)[1] - 1], q_g[1, 0]), 'b')
else:
ax.plot((V[1, E[0, shape(E)[1] - 1]],
V[1, shape(V)[1] - 1], q_g[0, 0]),
(V[2, E[0, shape(E)[1] - 1]],
V[2, shape(V)[1] - 1], q_g[1, 0]), 'b')
if connect_goal and abs(theta_orientation - thetaPrev) < max_angle:
#if connect_goal and abs(theta_orientation - thetaPrev) < pi/3:
print "connection is true.Path = 1"
path = 1
q_pass = hstack((q_pass, vstack((i, q_g))))
q_pass_dist = hstack((q_pass_dist, dist))
i = i + 1
# connection to goal has established
if path == 1:
if shape(q_pass_dist)[0] == 1:
cols = 0
else:
(cols, ) = nonzero(q_pass_dist == min(q_pass_dist))
cols = asarray(cols)[0]
q_g = q_pass[1:, cols] ###Catherine
q_g = q_g - (q_gBundle[:, q_pass[0, cols]] - V[1:,
(shape(V)[1] - 1)]
) / norm(q_gBundle[:, q_pass[0, cols]] -
V[1:,
(shape(V)[1] - 1)]) * 3 * radius #org 3
if not nextRegionPoly.isInside(q_g[0], q_g[1]):
q_g = q_g + (q_gBundle[:, q_pass[0, cols]] -
V[1:, (shape(V)[1] - 1)]) / norm(
q_gBundle[:, q_pass[0, cols]] -
V[1:, (shape(V)[1] - 1)]) * 6 * radius #org 3
if plotting == True:
if operate_system == 1:
plt.plot(q_g[0, 0], q_g[1, 0], 'ko')
plt.figure(original_figure).canvas.draw()
else:
ax.plot(q_g[0, 0], q_g[1, 0], 'ko')
numOfPoint = floor(norm(V[1:, shape(V)[1] - 1] - q_g) / step_size)
if numOfPoint < 3:
numOfPoint = 3
x = linspace(V[1, shape(V)[1] - 1], q_g[0, 0], numOfPoint)
y = linspace(V[2, shape(V)[1] - 1], q_g[1, 0], numOfPoint)
for i in range(x.shape[0]):
if i != 0:
V = hstack((V, vstack((shape(V)[1], x[i], y[i]))))
E = hstack((E, vstack((shape(V)[1] - 2, shape(V)[1] - 1))))
# path is not formed, try to append points onto the tree
if path == 0:
success = 0 # whether adding a new point is successful
hit_count = 0 # count for regenerating new edge with the same q_rand
Icurrent = [
] # to keep track of the index of the closest point to q_n
while success == 0 and hit_count <= 2:
if stuck > stuck_thres:
omega = random.choice(omega_range_abso)
else:
#!!!! CONTROL SPACE STEP 1 - generate random omega
omega = random.choice(omega_range)
#!!!! CONTROL SPACE STEP 2 - pick a random point on the tree
tree_index = random.choice(array(V[0])[0])
xPrev = V[1, tree_index]
yPrev = V[2, tree_index]
thetaPrev = V_theta[tree_index]
j = 1
#!!!! CONTROL SPACE STEP 3 - Check path of the robot
path_robot = PolyShapes.Circle(radius, (xPrev, yPrev))
while j <= timeStep:
xOrg = xPrev
yOrg = yPrev
xPrev = xPrev + velocity / omega * (
sin(omega * 1 + thetaPrev) - sin(thetaPrev))
yPrev = yPrev - velocity / omega * (
cos(omega * 1 + thetaPrev) - cos(thetaPrev))
thetaPrev = omega * 1 + thetaPrev
path_robot = path_robot + PolyShapes.Circle(
radius, (xPrev, yPrev))
j = j + 1
path_all = PolyUtils.convexHull(path_robot)
in_bound = BoundPoly.covers(path_all)
# plotting
plotPoly(path_all, 'r', 1)
plotMap(original_figure, BoundPoly, allRegions)
stuck = stuck + 1
if in_bound:
stuck = stuck - 5
x = []
y = []
for k in PolyUtils.pointList(path_all):
x = hstack((x, k[0]))
y = hstack((y, k[1]))
V = hstack((V, vstack((shape(V)[1], xPrev, yPrev))))
V_theta = hstack((V_theta, thetaPrev))
E = hstack((E, vstack((tree_index, shape(V)[1] - 1))))
Other = hstack((Other, vstack((velocity, omega))))
##################### E should add omega and velocity
success = 1
if finish_print == 1:
print 'Here is the V matrix:', V, 'Here is the E matrix:', E
print >> sys.__stdout__, 'Here is the V matrix:\n', V, '\nHere is the E matrix:\n', E
#B: trim to a single path
single = 0
while single == 0:
trim = 0
for j in range(shape(V)[1] - 3):
(row, col) = nonzero(E == j + 1)
if len(col) == 1:
E = delete(E, col[0], 1)
trim = 1
if trim == 0:
single = 1
# generate V to be sent to SimGUI to plot
V_toPass = V[0:, 0]
E_toPass = [[], []]
for i in range(shape(E)[1]):
V_toPass = hstack(
(V_toPass, vstack((i, V[1, E[1, i - 1]], V[2, E[1, i - 1]]))))
E_toPass = hstack((E_toPass, vstack((i - 1, i))))
####print with matlib
if plotting == True:
if operate_system == 1:
BoundPolyPoints = asarray(PolyUtils.pointList(BoundPoly))
plt.plot(BoundPolyPoints[:, 0], BoundPolyPoints[:, 1], 'k')
plt.plot(V[1, :], V[2, :], 'b')
for i in range(shape(E)[1] - 1):
plt.text(V[1, E[0, i]],
V[2, E[0, i]],
V[0, E[0, i]],
fontsize=12)
plt.text(V[1, E[1, i]],
V[2, E[1, i]],
V[0, E[1, i]],
fontsize=12)
plt.figure(original_figure).canvas.draw()
else:
BoundPolyPoints = asarray(PolyUtils.pointList(BoundPoly))
ax.plot(BoundPolyPoints[:, 0], BoundPolyPoints[:, 1], 'k')
ax.plot(V[1, :], V[2, :], 'b')
for i in range(shape(E)[1] - 1):
ax.text(V[1, E[0, i]],
V[2, E[0, i]],
V[0, E[0, i]],
fontsize=12)
ax.text(V[1, E[1, i]],
V[2, E[1, i]],
V[0, E[1, i]],
fontsize=12)
heading = E[0, 0]
# parse string for RRT printing in GUI (in format: RRT:E[[1,2,3]]:V[[1,2,3]])
V = array(V)
V_toPass = array(V_toPass)
E_toPass = array(E_toPass)
return V, E, heading, 0, V_toPass, E_toPass
def generateNewNode(self,V,V_theta,E,Other,regionPoly,stuck,append_after_latest_node =False):
"""
Generate a new node on the current tree matrix
V : the node matrix
V_theta : the orientation matrix
E : the tree matrix (or edge matrix)
Other : the matrix containing the velocity and angular velocity(omega) information
regionPoly: the polygon of current region
stuck : count on the number of times failed to generate new node
append_after_latest_node : append new nodes to the latest node (True only if the previous node addition is successful)
"""
if self.system_print == True:
print "In control space generating path,stuck = " + str(stuck)
connection_to_tree = False # True when connection to the tree is successful
if stuck > self.stuck_thres:
# increase the range of omega since path cannot ge generated
omega = random.choice(self.omega_range_escape)
else:
#!!!! CONTROL SPACE STEP 1 - generate random omega
omega = random.choice(self.omega_range)
#!!!! CONTROL SPACE STEP 2 - pick a random point on the tree
if append_after_latest_node:
tree_index = shape(V)[1]-1
else:
if random.choice([1,2]) == 1:
tree_index = random.choice(array(V[0])[0])
else:
tree_index = shape(V)[1]-1
xPrev = V[1,tree_index]
yPrev = V[2,tree_index]
thetaPrev = V_theta[tree_index]
j = 1
#!!!! CONTROL SPACE STEP 3 - Check path of the robot
path_robot = PolyShapes.Circle(self.radius,(xPrev,yPrev))
while j <= self.timeStep:
xOrg = xPrev
yOrg = yPrev
xPrev = xPrev + self.velocity/omega*(sin(omega* 1 + thetaPrev)-sin(thetaPrev))
yPrev = yPrev - self.velocity/omega*(cos(omega* 1 + thetaPrev)-cos(thetaPrev))
thetaPrev = omega* 1 + thetaPrev
path_robot = path_robot + PolyShapes.Circle(self.radius,(xPrev,yPrev))
j = j + 1
thetaPrev = self.orientation_bound(thetaPrev)
path_all = PolyUtils.convexHull(path_robot)
in_bound = regionPoly.covers(path_all)
# plotting
if self.plotting == True:
self.plotPoly(path_all,'r',1)
stuck = stuck + 1
if in_bound:
robot_new_node = PolyShapes.Circle(self.radius,(xPrev,yPrev))
# check how many nodes on the tree does the new node overlaps with
nodes_overlap_count = 0
for k in range(shape(V)[1]-1):
robot_old_node = PolyShapes.Circle(self.radius,(V[1,k],V[2,k]))
if robot_new_node.overlaps(robot_old_node):
if abs(thetaPrev - V_theta[k]) < self.max_angle_overlap:
nodes_overlap_count += 1
if nodes_overlap_count == 0 or (stuck > self.stuck_thres+1 and nodes_overlap_count < 2) or (stuck > self.stuck_thres+500):
if stuck > self.stuck_thres+1:
append_after_latest_node = False
if (stuck > self.stuck_thres+500):
stuck = 0
stuck = stuck - 20
# plotting
if self.plotting == True:
self.plotPoly(path_all,'b',1)
if self.system_print == True:
print "node connected"
V = hstack((V,vstack((shape(V)[1],xPrev,yPrev))))
V_theta = hstack((V_theta,thetaPrev))
E = hstack((E,vstack((tree_index ,shape(V)[1]-1))))
Other = hstack((Other,vstack((self.velocity,omega))))
##################### E should add omega and velocity
connection_to_tree = True
append_after_latest_node = True
else:
append_after_latest_node = False
if self.system_print == True:
print "node not connected. check goal point"
else:
append_after_latest_node = False
return V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree
def buildTree(self,p,theta,regionPoly,nextRegionPoly,q_gBundle,face_normal, last=False):
"""
This function builds the RRT tree.
p : x,y position of the robot
theta : current orientation of the robot
regionPoly : current region polygon
nextRegionPoly : next region polygon
q_gBundle : coordinates of q_goals that the robot can reach
face_normal : the normal vector of each face corresponding to each goal point in q_gBundle
"""
q_init = mat(p).T
V = vstack((0,q_init))
theta = self.orientation_bound(theta)
V_theta = array([theta])
#!!! CONTROL SPACE: generate a list of omega for random sampling
omegaLowerBound = -math.pi/20 # upper bound for the value of omega
omegaUpperBound = math.pi/20 # lower bound for the value of omega
omegaNoOfSteps = 20
self.omega_range = linspace(omegaLowerBound,omegaUpperBound,omegaNoOfSteps)
self.omega_range_escape = linspace(omegaLowerBound*4,omegaUpperBound*4,omegaNoOfSteps*4) # range used when stuck > stuck_thres
regionPolyOld = Polygon.Polygon(regionPoly)
#regionPoly += PolyShapes.Circle(self.radius*2.5,(q_init[0,0],q_init[1,0]))
regionPoly += PolyShapes.Circle(self.radius*4,(q_init[0,0],q_init[1,0]))
# check faces of the current region for goal points
E = [[],[]] # the tree matrix
Other = [[],[]]
path = False # if path formed then = 1
stuck = 0 # count for changing the range of sampling omega
append_after_latest_node = False # append new nodes to the latest node
if self.system_print == True:
print "plotting in buildTree is " + str(self.plotting)
if self.plotting == True:
if not plt.isinteractive():
plt.ion()
plt.hold(True)
while not path:
#step -1: try connection to q_goal (generate path to goal)
i = 0
ltlmop_logger.debug("finding RRT Path...")
if self.system_print == True:
print "Try Connection to the goal points"
# pushing possible q_goals into the current region (ensure path is covered by the current region polygon)
q_pass = [[],[],[]]
q_pass_dist = []
q_gBundle = mat(q_gBundle)
face_normal = mat(face_normal)
while i < q_gBundle.shape[1]:
q_g_original = q_gBundle[:,i]
q_g = q_gBundle[:,i]+face_normal[:,i]*1.5*self.radius ##original 2*self.radius
#q_g = q_gBundle[:,i]+(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*1.5*self.radius ##original 2*self.radius
if not regionPolyOld.isInside(q_g[0],q_g[1]):
#q_g = q_gBundle[:,i]-(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*1.5*self.radius ##original 2*self.radius
q_g = q_gBundle[:,i]-face_normal[:,i]*1.5*self.radius ##original 2*self.radius
#forming polygon for path checking
EdgePolyGoal = PolyShapes.Circle(self.radius,(q_g[0,0],q_g[1,0])) + PolyShapes.Circle(self.radius,(V[1,shape(V)[1]-1],V[2:,shape(V)[1]-1]))
EdgePolyGoal = PolyUtils.convexHull(EdgePolyGoal)
dist = norm(q_g - V[1:,shape(V)[1]-1])
#check connection to goal
connect_goal = regionPoly.covers(EdgePolyGoal) #check coverage of path from new point to goal
# compare orientation difference
thetaPrev = V_theta[shape(V)[1]-1]
if not (q_g[0,0]- V[1,shape(V)[1]-1]) == 0:
theta_orientation = abs(arctan((q_g[1,0]- V[2,shape(V)[1]-1])/(q_g[0,0]- V[1,shape(V)[1]-1])))
else:
theta_orientation = 0
if q_g[1,0] > V[2,shape(V)[1]-1]:
if q_g[0,0] < V[1,shape(V)[1]-1]: # second quadrant
theta_orientation = pi - theta_orientation
elif q_g[0,0] > V[1,shape(V)[1]-1]: # first quadrant
theta_orientation = theta_orientation
elif q_g[1,0] < V[2,shape(V)[1]-1]:
if q_g[0,0] < V[1,shape(V)[1]-1]: #third quadrant
theta_orientation = pi + theta_orientation
elif q_g[0,0] > V[1,shape(V)[1]-1]: # foruth quadrant
theta_orientation = 2*pi - theta_orientation
# check the angle between vector(new goal to goal_original ) and vector( latest node to new goal)
Goal_to_GoalOriginal = q_g_original - q_g
LatestNode_to_Goal = q_g - V[1:,shape(V)[1]-1]
Angle_Goal_LatestNode= arccos(vdot(array(Goal_to_GoalOriginal), array(LatestNode_to_Goal))/norm(Goal_to_GoalOriginal)/norm(LatestNode_to_Goal))
# if connection to goal can be established and the max change in orientation of the robot is smaller than max_angle, tree is said to be completed.
if self.orientation_print == True:
print "theta_orientation is " + str(theta_orientation)
print "thetaPrev is " + str(thetaPrev)
print "(theta_orientation - thetaPrev) is " + str(abs(theta_orientation - thetaPrev))
print "self.max_angle_allowed is " + str(self.max_angle_allowed)
print "abs(theta_orientation - thetaPrev) < self.max_angle_allowed" + str(abs(theta_orientation - thetaPrev) < self.max_angle_allowed)
print"Goal_to_GoalOriginal: " + str( array(Goal_to_GoalOriginal)) + "; LatestNode_to_Goal: " + str( array(LatestNode_to_Goal))
print vdot(array(Goal_to_GoalOriginal), array(LatestNode_to_Goal))
print "Angle_Goal_LatestNode" + str(Angle_Goal_LatestNode)
if connect_goal and (abs(theta_orientation - thetaPrev) < self.max_angle_allowed) and (Angle_Goal_LatestNode < self.max_angle_allowed):
path = True
q_pass = hstack((q_pass,vstack((i,q_g))))
q_pass_dist = hstack((q_pass_dist,dist))
i = i + 1
if self.system_print == True:
print "checked goal points"
self.E = E
self.V = V
# connection to goal has established
# Obtain the closest goal point that path can be formed.
if path:
if shape(q_pass_dist)[0] == 1:
cols = 0
else:
(cols,) = nonzero(q_pass_dist == min(q_pass_dist))
cols = asarray(cols)[0]
q_g = q_pass[1:,cols]
"""
q_g = q_g-(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*3*self.radius #org 3
if not nextRegionPoly.isInside(q_g[0],q_g[1]):
q_g = q_g+(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*6*self.radius #org 3
"""
if self.plotting == True :
if self.operate_system == 1:
plt.suptitle('Rapidly-exploring Random Tree', fontsize=12)
plt.xlabel('x')
plt.ylabel('y')
if shape(V)[1] <= 2:
plt.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
else:
plt.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')
plt.plot(q_g[0,0],q_g[1,0],'ko')
plt.figure(1).canvas.draw()
plt.pause(0.0001)
else:
BoundPolyPoints = asarray(PolyUtils.pointList(regionPoly))
self.ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
if shape(V)[1] <= 2:
self.ax.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
else:
self.ax.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')
self.ax.plot(q_g[0,0],q_g[1,0],'ko')
# trim the path connecting current node to goal point into pieces if the path is too long now
numOfPoint = floor(norm(V[1:,shape(V)[1]-1]- q_g)/self.step_size)
if numOfPoint < 3:
numOfPoint = 3
x = linspace( V[1,shape(V)[1]-1], q_g[0,0], numOfPoint )
y = linspace( V[2,shape(V)[1]-1], q_g[1,0], numOfPoint )
for i in range(x.shape[0]):
if i != 0:
V = hstack((V,vstack((shape(V)[1],x[i],y[i]))))
E = hstack((E,vstack((shape(V)[1]-2,shape(V)[1]-1))))
#push the goal point to the next region
if self.system == 1:
q_g = q_g+face_normal[:,q_pass[0,cols]]*4*self.radius ##original 3*self.radius
else:
q_g = q_g+face_normal[:,q_pass[0,cols]]*3*self.radius ##original 3*self.radius
if not nextRegionPoly.isInside(q_g[0],q_g[1]):
if self.system == 1:
q_g = q_g-face_normal[:,q_pass[0,cols]]*8*self.radius ##original 6*self.radius
else:
q_g = q_g-face_normal[:,q_pass[0,cols]]*6*self.radius ##original 6*self.radius
V = hstack((V,vstack((shape(V)[1],q_g[0,0],q_g[1,0]))))
E = hstack((E,vstack((shape(V)[1]-2 ,shape(V)[1]-1))))
if self.plotting == True :
if self.operate_system == 1:
plt.plot(q_g[0,0],q_g[1,0],'ko')
plt.plot(( V[1,shape(V)[1]-1],V[1,shape(V)[1]-2]),( V[2,shape(V)[1]-1],V[2,shape(V)[1]-2]),'b')
plt.figure(1).canvas.draw()
plt.pause(0.0001)
else:
self.ax.plot(q_g[0,0],q_g[1,0],'ko')
self.ax.plot(( V[1,shape(V)[1]-1],V[1,shape(V)[1]-2]),( V[2,shape(V)[1]-1],V[2,shape(V)[1]-2]),'b')
# path is not formed, try to append points onto the tree
if not path:
# connection_to_tree : connection to the tree is successful
if append_after_latest_node:
V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree = self.generateNewNode(V,V_theta,E,Other,regionPoly,stuck, append_after_latest_node)
else:
connection_to_tree = False
while not connection_to_tree:
V,V_theta,E,Other,stuck,append_after_latest_node, connection_to_tree = self.generateNewNode (V,V_theta,E,Other,regionPoly,stuck)
if self.finish_print:
print 'Here is the V matrix:', V, 'Here is the E matrix:',E
print >>sys.__stdout__, 'Here is the V matrix:\n', V, '\nHere is the E matrix:\n',E
#B: trim to a single path
single = 0
while single == 0:
trim = 0
for j in range(shape(V)[1]-3):
(row,col) = nonzero(E == j+1)
if len(col) == 1:
E = delete(E, col[0], 1)
trim = 1
if trim == 0:
single = 1;
####print with matlib
if self.plotting ==True :
if self.operate_system == 1:
plt.plot(V[1,:],V[2,:],'b')
for i in range(shape(E)[1]):
plt.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
plt.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)
plt.figure(1).canvas.draw()
plt.pause(0.0001)
else:
BoundPolyPoints = asarray(PolyUtils.pointList(regionPoly))
self.ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
self.ax.plot(V[1,:],V[2,:],'b')
for i in range(shape(E)[1]):
self.ax.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
self.ax.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)
# plot in simGUI
self.path = []
for i in range(shape(E)[1]):
self.path.append(tuple(map(int, self.executor.hsub.coordmap_lab2map([V[1,E[0,i]],V[2,E[0,i]]]))))
self.path.append(tuple(map(int, self.executor.hsub.coordmap_lab2map([V[1,E[1,i]],V[2,E[1,i]]]))))
self.executor.postEvent("RRT", self.path)
#return V, E, and the current node number on the tree
V = array(V)
return V, E, 0
def buildTree(p,theta,vert, R, system, regionPoly,nextRegionPoly,q_gBundle,\
mappedRegions,allRegions,max_angle_allowed, plotting, operate_system,ax, last=False):
"""
This function builds the RRT tree
pose: x,y position of the robot
theta: current orientation of the robot
R: radius of the robot
system: determine step_size and radius for the example
regionPoly: current region polygon
nextRegionPoly: next region polygon
q_gBundle: coordinates of q_goals that the robot can reach
mappedRegions: region polygons
allRegions: polygon that includes all the region
max_angle_allowed: the difference in angle between the two nodes allowed (between 0 to 2*pi)
plotting: 1 if plotting is enabled, 0- disabled
operate_system: Which operating system is used for execution. Ubuntu and Mac is 1, Windows is 2
ax: plot for windows
"""
max_angle = max_angle_allowed
"""
if operate_system == 2:
# start using anmination to plot Pioneer
self.fig = plt.figure()
self.ax = self.fig.add_subplot(111)
self.scope = _Scope(self.ax,self)
thread.start_new_thread(self.jplot,())
"""
## 1: Nao ; 2: STAGE ; 3: ODE
if system == 1: ## Nao
step_size = 0.2 #set the step_size for points be 1/5 of the norm from ORIGINAL = 0.4
elif system == 2:
step_size = 0.5
elif system == 3:
step_size = 15
if system == 1: ## Nao
timeStep = 5 #time step for calculation of x, y position
elif system == 2:
timeStep = 4 #time step for calculation of x, y position
elif system == 3:
timeStep = 10 #time step for calculation of x, y position #10
#############tune velocity OMEGA, TIME STEP
#fix velocity
## 1: Nao ; 2: STAGE ; 3: ODE
if system == 1: ## Nao
velocity = 0.05 #set the step_size for points be 1/5 of the norm from
elif system == 2:
velocity = 0.06
elif system == 3:
velocity = 2 # what is used in RRTControllerHelper.setVelocity #1.5
#############tune velocity OMEGA, TIME STEP
BoundPoly = regionPoly # Boundary polygon = current region polygon
radius = R
q_init = mat(p).T
V = vstack((0,q_init))
V_theta = array([theta])
original_figure = 1
#!!! CONTROL SPACE: generate a list of omega for random sampling
omegaLowerBound = -math.pi/20 # upper bound for the value of omega
omegaUpperBound = math.pi/20 # lower bound for the value of omega
omegaStepSize = 20
omega_range = linspace(omegaLowerBound,omegaUpperBound,omegaStepSize)
omega_range_abso = linspace(omegaLowerBound*4,omegaUpperBound*4,omegaStepSize*4) # range used when stuck > stuck_thres
edgeX = []
edgeY = []
# check faces of the current region for goal points
E = [[],[]]
Other = [[],[]]
path = 0 # if path formed then = 1
stuck = 0 # count for changing the range of sampling omega
stuck_thres = 300 # threshold for changing the range of sampling omega
if not plt.isinteractive():
plt.ion()
plt.hold(True)
while path == 0:
#step -1: try connection to q_goal (generate path to goal)
goalCheck = 0;
i = 0
# pushing possible q_goals into the current region (ensure path is covered by the current region polygon)
q_pass = [[],[],[]]
q_pass_dist = []
q_gBundle = mat(q_gBundle)
while i < q_gBundle.shape[1]: ###not sure about shape
q_g = q_gBundle[:,i]+(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*radius ##original 2*radius
trial = 1
if not BoundPoly.isInside(q_g[0],q_g[1]):
trial = 2
q_g = q_gBundle[:,i]-(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,i]-V[1:,(shape(V)[1]-1)])*radius ##original 2*radius
#forming polygon for path checking
cross_goal = cross(vstack((q_g-vstack((V[1,shape(V)[1]-1],V[2,shape(V)[1]-1])),0)).T,hstack((0,0,1)))
cross_goal = cross_goal.T
move_vector_goal = radius*cross_goal[0:2]/sqrt((cross_goal[0,0]**2 + cross_goal[1,0]**2))
upperEdgeG = hstack((vstack((V[1,shape(V)[1]-1],V[2,shape(V)[1]-1])),q_g)) + hstack((move_vector_goal,move_vector_goal))
lowerEdgeG = hstack((vstack((V[1,shape(V)[1]-1],V[2,shape(V)[1]-1])),q_g)) - hstack((move_vector_goal,move_vector_goal))
EdgePolyGoal = Polygon.Polygon((tuple(array(lowerEdgeG[:,0].T)[0]),tuple(array(upperEdgeG[:,0].T)[0]),tuple(array(upperEdgeG[:,1].T)[0]),tuple(array(lowerEdgeG[:,1].T)[0])))
dist = norm(q_g - V[1:,shape(V)[1]-1])
connect_goal = BoundPoly.covers(EdgePolyGoal) #check coverage of path from new point to goal
#check connection to goal
"""
if connect_goal:
print "connection is true"
path = 1
q_pass = hstack((q_pass,vstack((i,q_g))))
q_pass_dist = hstack((q_pass_dist,dist))
"""
# compare orientation difference
thetaPrev = V_theta[shape(V)[1]-1]
theta_orientation = abs(arctan((q_g[1,0]- V[2,shape(V)[1]-1])/(q_g[0,0]- V[1,shape(V)[1]-1])))
if thetaPrev < 0:
if q_g[1,0] > V[2,shape(V)[1]-1]:
if q_g[0,0] < V[1,shape(V)[1]-1]: # second quadrant
theta_orientation = -2*pi + theta_orientation
elif q_g[0,0] > V[1,shape(V)[1]-1]: # first quadrant
theta_orientation = -pi -theta_orientation
elif q_g[1,0] < V[2,shape(V)[1]-1]:
if q_g[0,0] < V[1,shape(V)[1]-1]: #third quadrant
theta_orientation = -pi + theta_orientation
elif q_g[0,0] > V[1,shape(V)[1]-1]: # foruth quadrant
theta_orientation = - theta_orientation
else:
if q_g[1,0] > V[2,shape(V)[1]-1]:
if q_g[0,0] < V[1,shape(V)[1]-1]: # second quadrant
theta_orientation = pi - theta_orientation
elif q_g[0,0] > V[1,shape(V)[1]-1]: # first quadrant
theta_orientation = theta_orientation
elif q_g[1,0] < V[2,shape(V)[1]-1]:
if q_g[0,0] < V[1,shape(V)[1]-1]: #third quadrant
theta_orientation = pi + theta_orientation
elif q_g[0,0] > V[1,shape(V)[1]-1]: # foruth quadrant
theta_orientation = 2*pi - theta_orientation
################################## PRINT PLT #################
if connect_goal :
if plotting == True:
if operate_system == 1:
plt.suptitle('Randomly-exploring rapid tree', fontsize=12)
BoundPolyPoints = asarray(PolyUtils.pointList(BoundPoly))
plt.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
plt.xlabel('x')
plt.ylabel('y')
if shape(V)[1] <= 2:
plt.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
else:
plt.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')
plt.figure(original_figure).canvas.draw()
else:
ax.suptitle('Randomly-exploring rapid tree', fontsize=12)
BoundPolyPoints = asarray(PolyUtils.pointList(BoundPoly))
ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
ax.xlabel('x')
ax.ylabel('y')
if shape(V)[1] <= 2:
ax.plot(( V[1,shape(V)[1]-1],q_g[0,0]),( V[2,shape(V)[1]-1],q_g[1,0]),'b')
else:
ax.plot(( V[1,E[0,shape(E)[1]-1]], V[1,shape(V)[1]-1],q_g[0,0]),( V[2,E[0,shape(E)[1]-1]], V[2,shape(V)[1]-1],q_g[1,0]),'b')
if connect_goal and abs(theta_orientation - thetaPrev) < max_angle:
#if connect_goal and abs(theta_orientation - thetaPrev) < pi/3:
print "connection is true.Path = 1"
path = 1
q_pass = hstack((q_pass,vstack((i,q_g))))
q_pass_dist = hstack((q_pass_dist,dist))
i = i + 1
# connection to goal has established
if path == 1:
if shape(q_pass_dist)[0] == 1:
cols = 0
else:
(cols,) = nonzero(q_pass_dist == min(q_pass_dist))
cols = asarray(cols)[0]
q_g = q_pass[1:,cols] ###Catherine
q_g = q_g-(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*3*radius #org 3
if not nextRegionPoly.isInside(q_g[0],q_g[1]):
q_g = q_g+(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])/norm(q_gBundle[:,q_pass[0,cols]]-V[1:,(shape(V)[1]-1)])*6*radius #org 3
if plotting == True:
if operate_system == 1:
plt.plot(q_g[0,0],q_g[1,0],'ko')
plt.figure(original_figure).canvas.draw()
else:
ax.plot(q_g[0,0],q_g[1,0],'ko')
numOfPoint = floor(norm(V[1:,shape(V)[1]-1]- q_g)/step_size)
if numOfPoint < 3:
numOfPoint = 3
x = linspace( V[1,shape(V)[1]-1], q_g[0,0], numOfPoint )
y = linspace( V[2,shape(V)[1]-1], q_g[1,0], numOfPoint )
for i in range(x.shape[0]):
if i != 0:
V = hstack((V,vstack((shape(V)[1],x[i],y[i]))))
E = hstack((E,vstack((shape(V)[1]-2,shape(V)[1]-1))))
# path is not formed, try to append points onto the tree
if path == 0:
success = 0 # whether adding a new point is successful
hit_count = 0 # count for regenerating new edge with the same q_rand
Icurrent = [] # to keep track of the index of the closest point to q_n
while success == 0 and hit_count <= 2:
if stuck > stuck_thres:
omega = random.choice(omega_range_abso)
else:
#!!!! CONTROL SPACE STEP 1 - generate random omega
omega = random.choice(omega_range)
#!!!! CONTROL SPACE STEP 2 - pick a random point on the tree
tree_index = random.choice(array(V[0])[0])
xPrev = V[1,tree_index]
yPrev = V[2,tree_index]
thetaPrev = V_theta[tree_index]
j = 1
#!!!! CONTROL SPACE STEP 3 - Check path of the robot
path_robot = PolyShapes.Circle(radius,(xPrev,yPrev))
while j <= timeStep:
xOrg = xPrev
yOrg = yPrev
xPrev = xPrev + velocity/omega*(sin(omega* 1 + thetaPrev)-sin(thetaPrev))
yPrev = yPrev - velocity/omega*(cos(omega* 1 + thetaPrev)-cos(thetaPrev))
thetaPrev = omega* 1 + thetaPrev
path_robot = path_robot + PolyShapes.Circle(radius,(xPrev,yPrev))
j = j + 1
path_all = PolyUtils.convexHull(path_robot)
in_bound = BoundPoly.covers(path_all)
# plotting
plotPoly(path_all,'r',1)
plotMap(original_figure,BoundPoly,allRegions)
stuck = stuck + 1
if in_bound:
stuck = stuck -5
x = []
y = []
for k in PolyUtils.pointList(path_all):
x = hstack((x,k[0]))
y = hstack((y,k[1]))
V = hstack((V,vstack((shape(V)[1],xPrev,yPrev))))
V_theta = hstack((V_theta,thetaPrev))
E = hstack((E,vstack((tree_index ,shape(V)[1]-1))))
Other = hstack((Other,vstack((velocity,omega))))
##################### E should add omega and velocity
success = 1
if finish_print == 1:
print 'Here is the V matrix:', V, 'Here is the E matrix:',E
print >>sys.__stdout__, 'Here is the V matrix:\n', V, '\nHere is the E matrix:\n',E
#B: trim to a single path
single = 0
while single == 0:
trim = 0
for j in range(shape(V)[1]-3):
(row,col) = nonzero(E == j+1)
if len(col) == 1:
E = delete(E, col[0], 1)
trim = 1
if trim == 0:
single = 1;
# generate V to be sent to SimGUI to plot
V_toPass = V[0:,0]
E_toPass = [[],[]]
for i in range(shape(E)[1]):
V_toPass = hstack((V_toPass,vstack((i,V[1,E[1,i-1]],V[2,E[1,i-1]]))))
E_toPass = hstack((E_toPass,vstack((i-1,i))))
####print with matlib
if plotting ==True:
if operate_system == 1:
BoundPolyPoints = asarray(PolyUtils.pointList(BoundPoly))
plt.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
plt.plot(V[1,:],V[2,:],'b')
for i in range(shape(E)[1]-1):
plt.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
plt.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)
plt.figure(original_figure).canvas.draw()
else:
BoundPolyPoints = asarray(PolyUtils.pointList(BoundPoly))
ax.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],'k')
ax.plot(V[1,:],V[2,:],'b')
for i in range(shape(E)[1]-1):
ax.text(V[1,E[0,i]],V[2,E[0,i]], V[0,E[0,i]], fontsize=12)
ax.text(V[1,E[1,i]],V[2,E[1,i]], V[0,E[1,i]], fontsize=12)
heading = E[0,0]
# parse string for RRT printing in GUI (in format: RRT:E[[1,2,3]]:V[[1,2,3]])
V = array(V)
V_toPass = array(V_toPass)
E_toPass = array(E_toPass)
return V, E, heading,0,V_toPass,E_toPass