Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also compare across forks.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also compare across forks.
...
  • 8 commits
  • 7 files changed
  • 0 commit comments
  • 1 contributor
View
244 src/examples/RRT_example/RRT_example.regions
@@ -0,0 +1,244 @@
+# This is a region definition file for the LTLMoP toolkit.
+# Format details are described at the beginning of each section below.
+# Note that all values are separated by *tabs*.
+
+Background: # Relative path of background image file
+None
+
+CalibrationPoints: # Vertices to use for map calibration: (vertex_region_name, vertex_index)
+
+Obstacles: # Names of regions to treat as obstacles
+r3
+
+Regions: # Stored as JSON string
+[
+ {
+ "name": "boundary",
+ "color": [
+ 0,
+ 255,
+ 0
+ ],
+ "position": [
+ 29,
+ 73
+ ],
+ "type": "rect",
+ "size": [
+ 694,
+ 511
+ ]
+ },
+ {
+ "name": "r6",
+ "color": [
+ 160,
+ 32,
+ 240
+ ],
+ "holeList": [],
+ "points": [
+ [
+ 122,
+ 2
+ ],
+ [
+ 136,
+ 85
+ ],
+ [
+ 76,
+ 133
+ ],
+ [
+ 0,
+ 103
+ ],
+ [
+ 5,
+ 0
+ ]
+ ],
+ "position": [
+ 106,
+ 281
+ ],
+ "type": "poly",
+ "size": [
+ 136,
+ 133
+ ]
+ },
+ {
+ "name": "r5",
+ "color": [
+ 61,
+ 28,
+ 81
+ ],
+ "holeList": [],
+ "points": [
+ [
+ 73,
+ 0
+ ],
+ [
+ 126,
+ 70
+ ],
+ [
+ 119,
+ 199
+ ],
+ [
+ 57,
+ 226
+ ],
+ [
+ 43,
+ 143
+ ],
+ [
+ 0,
+ 72
+ ]
+ ],
+ "position": [
+ 185,
+ 140
+ ],
+ "type": "poly",
+ "size": [
+ 126,
+ 226
+ ]
+ },
+ {
+ "name": "r4",
+ "color": [
+ 255,
+ 80,
+ 0
+ ],
+ "holeList": [],
+ "points": [
+ [
+ 0,
+ 0
+ ],
+ [
+ 154,
+ 0
+ ],
+ [
+ 81,
+ 72
+ ],
+ [
+ 124,
+ 143
+ ],
+ [
+ 7,
+ 141
+ ],
+ [
+ 0,
+ 3
+ ]
+ ],
+ "position": [
+ 104,
+ 140
+ ],
+ "type": "poly",
+ "size": [
+ 154,
+ 143
+ ]
+ },
+ {
+ "name": "r3",
+ "color": [
+ 0,
+ 0,
+ 255
+ ],
+ "holeList": [],
+ "points": [
+ [
+ 387,
+ 0
+ ],
+ [
+ 336,
+ 0
+ ],
+ [
+ 339,
+ 308
+ ],
+ [
+ 0,
+ 309
+ ],
+ [
+ 2,
+ 360
+ ],
+ [
+ 382,
+ 354
+ ]
+ ],
+ "position": [
+ 68,
+ 155
+ ],
+ "type": "poly",
+ "size": [
+ 387,
+ 360
+ ]
+ },
+ {
+ "name": "r2",
+ "color": [
+ 255,
+ 165,
+ 0
+ ],
+ "position": [
+ 505,
+ 164
+ ],
+ "type": "rect",
+ "size": [
+ 152,
+ 68
+ ]
+ },
+ {
+ "name": "r1",
+ "color": [
+ 0,
+ 255,
+ 0
+ ],
+ "position": [
+ 534,
+ 397
+ ],
+ "type": "rect",
+ "size": [
+ 101,
+ 96
+ ]
+ }
+]
+
+Transitions: # Region 1 Name, Region 2 Name, Bidirectional transition faces (face1_x1, face1_y1, face1_x2, face1_y2, face2_x1, ...)
+r6 r5 228 283 242 366
+r6 r4 111 281 228 283
+r5 r4 185 212 228 283 185 212 258 140
+
View
38 src/examples/RRT_example/RRT_example.spec
@@ -0,0 +1,38 @@
+# This is a specification definition file for the LTLMoP toolkit.
+# Format details are described at the beginning of each section below.
+
+
+======== SETTINGS ========
+
+Actions: # List of action propositions and their state (enabled = 1, disabled = 0)
+
+CompileOptions:
+convexify: False
+fastslow: False
+
+CurrentConfigName:
+RRT with ODE
+
+Customs: # List of custom propositions
+
+RegionFile: # Relative path of region description file
+RRT_example.regions
+
+Sensors: # List of sensor propositions and their state (enabled = 1, disabled = 0)
+
+
+======== SPECIFICATION ========
+
+RegionMapping: # Mapping between region names and their decomposed counterparts
+r4 = p4
+r5 = p3
+r6 = p2
+r1 = p7
+r2 = p6
+others = p1
+
+Spec: # Specification in structured English
+
+visit r5
+visit r4
+
View
53 src/examples/RRT_example/configs/RRT_with_ODE.config
@@ -0,0 +1,53 @@
+# This is a configuration definition file for the example "RRT_example".
+# Format details are described at the beginning of each section below.
+
+
+======== General Config ========
+
+Actuator_Proposition_Mapping: # Mapping between actuator propositions and actuator handler functions
+
+Initial_Truths: # Initially true propositions
+
+Main_Robot: # The name of the robot used for moving in this config
+Pioneer ODE
+
+Name: # Configuration name
+RRT with ODE
+
+Sensor_Proposition_Mapping: # Mapping between sensor propositions and sensor handler functions
+
+
+======== Robot1 Config ========
+
+ActuatorHandler: # Actuator handler file in robots/Type folder
+PioneerODEActuator()
+
+CalibrationMatrix: # 3x3 matrix for converting coordinates, stored as lab->map
+array([[ 3.3333, 0. , 0. ],
+ [ 0. , -3.3333, 0. ],
+ [ 0. , 0. , 1. ]])
+
+DriveHandler: # Input value for robot drive handler, refer to file inside the handlers/drive folder
+differentialDrive(d=0.65)
+
+InitHandler: # Input value for robot init handler, refer to the init file inside the handlers/robots/Type folder
+PioneerODEInit(init_region="r1")
+
+LocomotionCommandHandler: # Input value for robot locomotion command handler, refer to file inside the handlers/robots/Type folder
+PioneerODELocomotionCommand(speed=12.0,host="localhost",port=23456)
+
+MotionControlHandler: # Input value for robot motion control handler, refer to file inside the handlers/motionControl folder
+RRTController(robot_type=3)
+
+PoseHandler: # Input value for robot pose handler, refer to file inside the handlers/pose folder
+pioneerSimPose(host="localhost",port=23456)
+
+RobotName: # Robot Name
+Pioneer ODE
+
+SensorHandler: # Sensor handler file in robots/Type folder
+PioneerODESensor()
+
+Type: # Robot type
+PioneerODE
+
View
7 src/lib/execute.py
@@ -224,10 +224,11 @@ def main(argv):
init_region = None
for i, r in enumerate(proj.rfi.regions):
- pointArray = [proj.coordmap_map2lab(x) for x in r.getPoints()]
- vertices = mat(pointArray).T
+ #pointArray = [proj.coordmap_map2lab(x) for x in r.getPoints()]
+ #vertices = mat(pointArray).T
- if is_inside([pose[0], pose[1]], vertices):
+ #if is_inside([pose[0], pose[1]], vertices):
+ if r.objectContainsPoint(*proj.coordmap_lab2map(pose)):
init_region = i
break
View
184 src/lib/handlers/motionControl/RRTController.py
@@ -0,0 +1,184 @@
+#!/usr/bin/env python
+"""
+===================================================================
+RRTController.py - Rapidly-Exploring Random Trees Motion Controller
+===================================================================
+
+Uses the vector field algorithm developed by Stephen R. Lindemann to calculate a global velocity vector to take the robot from the current region to the next region, through a specified exit face.
+"""
+
+import _RRTControllerHelper
+from numpy import *
+from is_inside import *
+import Polygon
+import time, math
+import sys,os
+
+class motionControlHandler:
+ def __init__(self, proj, shared_data,robot_type):
+ """
+ Rapidly-Exploring Random Trees alogorithm motion planning controller
+
+ robot_type (int): Which robot is used for execution. Nao is 1, STAGE is 2,ODE is 3(default=3)
+ """
+
+ # Get references to handlers we'll need to communicate with
+ self.drive_handler = proj.h_instance['drive']
+ self.pose_handler = proj.h_instance['pose']
+
+ # Get information about regions
+ self.proj = proj
+ #self.proj.rfi = proj.rfi
+ self.coordmap_map2lab = proj.coordmap_map2lab
+ self.coordmap_lab2map = proj.coordmap_lab2map
+ self.last_warning = 0
+ self.previous_next_reg = None
+
+ # Store the Rapidly-Exploring Random Tress Built
+ self.RRT_V = None # array containing all the points on the RRT Tree
+ self.RRT_E = None # array specifying the connection of points on the Tree
+ self.RRT_V_toPass = None
+ self.RRT_E_toPass = None
+ self.point_to_go = None
+ self.heading = None
+ self.E_prev = None
+ self.Velocity = None
+ self.currentRegionPoly = None
+ self.nextRegionPoly = None
+ self.map = {}
+ self.all = Polygon.Polygon()
+
+ # Information about the robot
+ if robot_type not in [1,2,3]:
+ robot_type = 1
+ self.system = robot_type
+
+ ## 1: Nao ; 2: STAGE; 3: 0DE
+ if self.system == 1:
+ self.radius = 0.15*1.2
+ elif self.system == 2:
+ self.radius = 0.1
+ elif self.system == 3:
+ self.radius = 5
+
+ for region in self.proj.rfi.regions:
+ self.map[region.name] = self.createRegionPolygon(region)
+ for n in range(len(region.holeList)): # no of holes
+ self.map[region.name] -= self.createRegionPolygon(region,n)
+
+ for regionName,regionPoly in self.map.iteritems():
+ self.all += regionPoly
+
+ 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.
+ """
+
+ if current_reg == next_reg and not last:
+ # No need to move!
+ self.drive_handler.setVelocity(0, 0) # So let's stop
+ return False
+
+ # Find our current configuration
+ pose = self.pose_handler.getPose()
+
+ # Check if Vicon has cut out
+ # TODO: this should probably go in posehandler?
+ if math.isnan(pose[2]):
+ 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 tree will be used.
+ if not self.previous_next_reg == current_reg:
+ self.nextRegionPoly = self.map[self.proj.rfi.regions[next_reg].name]
+ self.currentRegionPoly = self.map[self.proj.rfi.regions[current_reg].name]
+
+ print "next Region is "+str(self.proj.rfi.regions[next_reg].name)
+ print "Current Region is "+str(self.proj.rfi.regions[current_reg].name)
+
+ pointArray = [x for x in self.proj.rfi.regions[current_reg].getPoints()]
+ pointArray = map(self.coordmap_map2lab, pointArray)
+ vertices = mat(pointArray).T
+
+ #set to zero velocity before tree is generated
+ self.drive_handler.setVelocity(0, 0)
+ if last:
+ transFace = None
+ else:
+ # Find a face to go through
+ # TODO: Account for non-determinacy?
+
+ transFace = None
+ # Find the index of this face
+ q_gBundle = [[],[]]
+
+ 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))))
+
+ 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)
+
+ # Run algorithm to build the Rapid-Exploring Random Trees
+ self.RRT_V = None
+ self.RRT_E = None
+ self.RRT_V,self.RRT_E,self.heading,self.E_prev,self.RRT_V_toPass,self.RRT_E_toPass = _RRTControllerHelper.buildTree([pose[0], pose[1]],pose[2], vertices, self.radius,self.system,self.currentRegionPoly,self.nextRegionPoly,q_gBundle,self.map,self.all)
+
+ # map the lab coordinates back to pixels
+ V_tosend = array(mat(self.RRT_V[1:,:])).T
+ V_tosend = map(self.coordmap_lab2map, V_tosend)
+ V_tosend = mat(V_tosend).T
+ s = 'RRT:E'+"["+str(list(self.RRT_E[0]))+","+str(list(self.RRT_E[1]))+"]"+':V'+"["+str(list(self.RRT_V[0]))+","+str(list(V_tosend[0]))+","+str(list(V_tosend[1]))+"]"+':T'+"["+str(list(q_gBundle[0]))+","+str(list(q_gBundle[1]))+"]"
+
+ print s
+
+ # Run algorithm to find a velocity vector (global frame) to take the robot to the next region
+ V = _RRTControllerHelper.setVelocity([pose[0], pose[1]], self.RRT_V,self.RRT_E,self.heading,self.E_prev,self.radius)
+ self.Velocity = V[0:2,0]
+ self.heading = V[2,0]
+ self.E_prev = V[3,0]
+ self.previous_next_reg = current_reg
+
+ # Pass this desired velocity on to the drive handler
+ self.drive_handler.setVelocity(V[0,0], V[1,0], pose[2])
+ RobotPoly = Polygon.Shapes.Circle(self.radius,(pose[0],pose[1]))
+ #step 4: check whether robot is inside the boundary
+
+ departed = not self.currentRegionPoly.covers(RobotPoly)
+ arrived = self.nextRegionPoly.covers(RobotPoly)
+
+
+ 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 createRegionPolygon(self,region,hole = None):
+ """
+ This function takes in the region points and make it a Polygon.
+ """
+ if hole == None:
+ pointArray = [x for x in region.getPoints()]
+ else:
+ pointArray = [x for x in region.getPoints(hole_id = hole)]
+ pointArray = map(self.coordmap_map2lab, pointArray)
+ regionPoints = [(pt[0],pt[1]) for pt in pointArray]
+ formedPolygon= Polygon.Polygon(regionPoints)
+ return formedPolygon
View
367 src/lib/handlers/motionControl/_RRTControllerHelper.py
@@ -0,0 +1,367 @@
+#!/usr/bin/env python
+"""
+ =============================================================
+ _RRTControllerHelper.py - Randomly Exploring Rapid Tree Controller
+ =============================================================
+
+"""
+
+from numpy import *
+from scipy.linalg import norm
+from numpy.matlib import zeros
+import is_inside
+import time, sys,os
+import scipy as Sci
+import scipy.linalg
+import Polygon, Polygon.IO
+import Polygon.Utils as PolyUtils
+import Polygon.Shapes as PolyShapes
+import matplotlib.pyplot as plt
+from math import sqrt, fabs , pi
+import random
+
+distance_from_next = 0 #set to 1 if wants to print distance from next point, set o otherwise
+system_print = 0 #set to 1 to print to terminal
+step4 = 0 #set to 1 to print check after step 4
+obstacle_print = 0 #set to 1 to print obstacle check
+check_E_print = 0 #set to 1 to print check edge appending
+step56 = 0 #set to 1 to print check for step 5 and 6
+finish_print = 0 #set to 1 to print finish E and V before trimming
+move_goal = 0 #set to 1 when move goal testing
+
+def setVelocity(p, V, E, heading,E_prev,radius, last=False):
+ """
+ This function calculates the velocity for the robot with RRT.
+ The inputs are (given in order):
+ p = the current x-y position of the robot
+ E = edges of the tree (2 x No. of nodes on the tree)
+ V = points of the tree (2 x No. of vertices)
+ heading = index of the previous heading point on the tree
+ E_prev = current index of E
+ last = True, if the current region is the last region
+ = False, if the current region is NOT the last region
+ """
+
+ pose = mat(p).T
+ #dis_cur = distance between current position and the next point
+ dis_cur = vstack((V[1,E[1,E_prev]],V[2,E[1,E_prev]]))- pose
+ if norm(dis_cur) < 1.5*radius: # go to next point
+ if not heading == shape(V)[1]-1:
+ E_prev = E_prev + 1
+ dis_cur = vstack((V[1,E[1,E_prev]],V[2,E[1,E_prev]]))- pose
+ heading = E[1,E_prev]
+ else:
+ dis_cur = vstack((V[1,E[1,E_prev]],V[2,E[1,E_prev]]))- vstack((V[1,E[0,E_prev]],V[2,E[0,E_prev]]))
+ Vel = zeros([4,1])
+
+
+ Vel[0:2,0] = dis_cur/norm(dis_cur)*0.5 #TUNE THE SPEED LATER
+ Vel[2,0] = heading
+ Vel[3,0] = E_prev
+
+ return Vel
+
+def buildTree(p,theta,vert, R, system, regionPoly,nextRegionPoly,q_gBundle,mappedRegions,allRegions, 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
+ """
+ ## 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 :
+ 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()
+
+ 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
+
+ plt.plot(q_g[0,0],q_g[1,0],'ko')
+ plt.figure(original_figure).canvas.draw()
+
+ 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
+
+ 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()
+
+ 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 plotMap(number,currentRegion,allRegions):
+ """
+ Plotting regions and obstacles with matplotlib.pyplot
+
+ number: figure number (see on top)
+ """
+
+ if not plt.isinteractive():
+ plt.ion()
+ plt.hold(True)
+
+ plotPoly(currentRegion,'k')
+ plt.figure(number).canvas.draw()
+
+def plotPoly(c,string,w = 1):
+ """
+ Plot polygons inside the boundary
+ c = polygon to be plotted with matlabplot
+ string = string that specify color
+ w = width of the line plotting
+ """
+ for i in range(len(c)):
+ toPlot = Polygon.Polygon(c.contour(i))
+ if bool(toPlot):
+ BoundPolyPoints = asarray(PolyUtils.pointList(toPlot))
+ plt.plot(BoundPolyPoints[:,0],BoundPolyPoints[:,1],string,linewidth=w)
+ plt.plot([BoundPolyPoints[-1,0],BoundPolyPoints[0,0]],[BoundPolyPoints[-1,1],BoundPolyPoints[0,1]],string,linewidth=w)
+
+
+
View
7 src/lib/simulator/ode/pioneer/PioneerSim.py
@@ -7,8 +7,9 @@
from numpy import *
import math, time, copy, sys
# needs to add the path of ltlmop_root to sys path
-sys.path.append('../../..')
-import lib.regions
+sys.path.append('lib')
+import regions
+
info = """DiffDriveSim
@@ -51,7 +52,7 @@ def __init__(self,standalone=1,obstaclefile=None,regionfile=None,region_calib=No
# If regionfile=0, render the ground as default solid green terrain.
# Otherwise, load the regions file specified and draw the region colors on the ground.
# Initialize the region file interface
- self.rfi = lib.regions.RegionFileInterface()
+ self.rfi = regions.RegionFileInterface()
# Load a region file if it has been specified on instantiation.
#regionfile = "examples/sandbox/sandbox.regions"

No commit comments for this range

Something went wrong with that request. Please try again.