def setSpace(self):
# construct the state space we are planning in
self.space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
self.bounds = ob.RealVectorBounds(2)
if not self.costMap:
self.bounds.setLow(-8)
self.bounds.setHigh(20)
else:
ox = self.costMap.getOriginX()
oy = self.costMap.getOriginY()
size_x = self.costMap.getSizeInMetersX()
size_y = self.costMap.getSizeInMetersY()
low = min(ox, oy)
high = max(ox + size_x, oy + size_y)
print("low", low)
print("high", high)
self.bounds.setLow(low)
self.bounds.setHigh(high)
self.space.setBounds(self.bounds)
# define a simple setup class
self.ss = og.SimpleSetup(self.space)
self.ss.setStateValidityChecker(
ob.StateValidityCheckerFn(self.isStateValid))
def plan():
# create an SE2 state space
space = ob.SE2StateSpace()
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
start = ob.State(space)
# we can pick a random start state...
start.random()
# ... or set specific values
start().setX(.5)
goal = ob.State(space)
# we can pick a random goal state...
goal.random()
# ... or set specific values
goal().setX(-.5)
ss.setStartAndGoalStates(start, goal)
# this will automatically choose a default planner with
# default parameters
solved = ss.solve(1.0)
if solved:
# try to shorten the path
ss.simplifySolution()
# print the simplified path
print ss.getSolutionPath()
def plan():
# construct the state space we are planning in
space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# create a control space
cspace = oc.RealVectorControlSpace(space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(-.3)
cbounds.setHigh(.3)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
ss.setStateValidityChecker(ob.StateValidityCheckerFn( \
partial(isStateValid, ss.getSpaceInformation())))
ss.setStatePropagator(oc.StatePropagatorFn(propagate))
# create a start state
start = ob.State(space)
start().setX(-0.5)
start().setY(0.0)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(0.0)
goal().setY(0.5)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# (optionally) set planner
si = ss.getSpaceInformation()
#planner = oc.RRT(si)
#planner = oc.EST(si)
#planner = oc.KPIECE1(si) # this is the default
# SyclopEST and SyclopRRT require a decomposition to guide the search
decomp = MyDecomposition(32, bounds)
planner = oc.SyclopEST(si, decomp)
#planner = oc.SyclopRRT(si, decomp)
ss.setPlanner(planner)
# (optionally) set propagation step size
si.setPropagationStepSize(.1)
# attempt to solve the problem
solved = ss.solve(20.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
def plan():
# construct the state space we are planning in
space = ob.SE2StateSpace()
# set the bounds for R^3 portion of SE(3)
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, -40)
bounds.setHigh(0, 40)
bounds.setLow(1, 0)
bounds.setHigh(1, 90)
space.setBounds(bounds)
# define a simple setup class
ss = og.SimpleSetup(space)
# create a start state
start = ob.State(space)
start().setX(0)
start().setY(0)
start().setYaw(0)
# start().setZ(-9)
# start().rotation().setIdentity()
# create a goal state
goal = ob.State(space)
goal().setX(-25)
goal().setY(60)
goal().setYaw(0)
# goal().setZ(-9)
# goal().rotation().setIdentity()
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# Lets use PRM. It will have interesting PlannerData
planner = og.RRTstar(ss.getSpaceInformation())
ss.setPlanner(planner)
ss.setup()
# attempt to solve the problem
# To change time change value given to ss.solve
solved = ss.solve(1.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath())
# Extracting planner data from most recent solve attempt
pd = ob.PlannerData(ss.getSpaceInformation())
ss.getPlannerData(pd)
# Computing weights of all edges based on state space distance
pd.computeEdgeWeights()
path = get_path(ss)
draw_graph(path)
plt.show()
def space_creation(self):
"""
Function of creating a configuration space
"""
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(const.LOW_BOUNDS)
bounds.setHigh(const.HIGH_BOUNDS)
space.setBounds(bounds)
return space
def space_creation(self):
"""
Function of creating a configuration space
"""
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(self.low_bounds)
bounds.setHigh(self.high_bounds)
space.setBounds(bounds)
return space
def plan():
# construct the state space we are planning in
# The state space of the car is SE(2) (x and y position with one angle for orientation).
space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# create a control space
# ?? for this simple car model consists of the velocity and steering angle, both real valued.
cspace = oc.RealVectorControlSpace(space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(-.3)
cbounds.setHigh(.3)
cspace.setBounds(cbounds)
# define a simple setup class
global ss
ss = oc.SimpleSetup(cspace)
validityChecker = ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation()))
ss.setStateValidityChecker(validityChecker)
ode = oc.ODE(kinematicCarODE)
#odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode)
odeSolver = oc.ODEErrorSolver(ss.getSpaceInformation(), ode)
propagator = oc.ODESolver.getStatePropagator(odeSolver)
ss.setStatePropagator(propagator)
# create a start state
start = ob.State(space)
start().setX(-0.5)
start().setY(0.0)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(0.0)
goal().setY(0.5)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# attempt to solve the problem
solved = ss.solve(120.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
def __init__(self, point_robot_manager):
self.point_robot_manager = point_robot_manager
# create an SE2 state space
space = ob.SE2StateSpace()
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, -point_robot_manager.dimension_length)
bounds.setLow(1, -point_robot_manager.dimension_length)
bounds.setHigh(0, point_robot_manager.dimension_length)
bounds.setHigh(1, point_robot_manager.dimension_length)
space.setBounds(bounds)
self.space = space
def plan():
# construct the state space we are planning in
space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# create a control space
cspace = oc.RealVectorControlSpace(space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(-.3)
cbounds.setHigh(.3)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
validityChecker = ob.StateValidityCheckerFn(
partial(isStateValid, ss.getSpaceInformation()))
ss.setStateValidityChecker(validityChecker)
ode = oc.ODE(kinematicCarODE)
odeSolver = oc.ODEBasicSolver(ss.getSpaceInformation(), ode)
propagator = oc.ODESolver.getStatePropagator(odeSolver)
ss.setStatePropagator(propagator)
# create a start state
start = ob.State(space)
start().setX(-0.5)
start().setY(0.0)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(0.0)
goal().setY(0.5)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# attempt to solve the problem
solved = ss.solve(120.0)
if solved:
# print the path to screen
print("Found solution:\n%s" %
ss.getSolutionPath().asGeometric().printAsMatrix())
def __init__(self, setup=None, contact_resolution=0.02, pushing_dists=[0.02,0.04,0.06,0.08,0.10], debug=False, use_multiproc=False):
self.name2cells = {}
self.space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
self.space.setBounds(bounds)
self.si = ob.SpaceInformation(self.space)
self.name_actor = 'finger'
self.type_actor = 'finger'
self.color_actor = [1,1,0,1]
self.dims_actor = [0.03, 0.03]
self.name2names = {}
self.name2probs = {}
self.sim_cols = {}
self.sim_stbs = {}
self.sim_objs = {}
self.stbs_objs = {}
self.trans_objs = {}
self.actions_objs = {}
self.n_actions_objs = {}
self.uvec_contact2com = {}
self.com_objs = {}
if setup is None:
setup = {'table_type','table_round'}
self.setup = setup
self.setup['table_radius'] = SimBullet.get_shapes()[self.setup['table_type']]['radius']
self.debug = debug
if self.debug:
self.sim_debug = SimBullet(gui=self.debug)
self.addDefaultObjects(self.sim_debug)
self.resolution_contact = contact_resolution
self.pushing_dists = sorted(pushing_dists)
self.use_multiproc = use_multiproc
if self.use_multiproc:
self.pool = Pool(int(multiprocessing.cpu_count()*0.8+0.5))
def __init__(self):
## add OMPL setting for different environments
self.env_name = rospy.get_param('env_name')
self.planner = rospy.get_param('planner_name')
if self.env_name == 's2d':
#data_loader = data_loader_2d
IsInCollision = plan_s2d.IsInCollision
# create an SE2 state space
space = ob.RealVectorStateSpace(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-20)
bounds.setHigh(20)
space.setBounds(bounds)
elif self.env_name == 'c2d':
#data_loader = data_loader_2d
IsInCollision = plan_c2d.IsInCollision
# create an SE2 state space
space = ob.RealVectorStateSpace(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-20)
bounds.setHigh(20)
space.setBounds(bounds)
elif self.env_name == 'r2d':
#data_loader = data_loader_r2d
IsInCollision = plan_r2d.IsInCollision
# create an SE2 state space
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(-20)
bounds.setHigh(20)
space.setBounds(bounds)
elif self.env_name == 'r3d':
#data_loader = data_loader_r3d
IsInCollision = plan_r3d.IsInCollision
# create an SE2 state space
space = ob.RealVectorStateSpace(3)
bounds = ob.RealVectorBounds(3)
bounds.setLow(-20)
bounds.setHigh(20)
space.setBounds(bounds)
self.IsInCollision = IsInCollision
self.space = space
def planTheHardWay():
# create an SE2 state space
space = ob.SE2StateSpace()
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# construct an instance of space information from this state space
si = ob.SpaceInformation(space)
# set state validity checking for this space
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# create a random start state
start = ob.State(space)
start.random()
# create a random goal state
goal = ob.State(space)
goal.random()
# create a problem instance
pdef = ob.ProblemDefinition(si)
# set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# create a planner for the defined space
planner = og.RRTConnect(si)
# set the problem we are trying to solve for the planner
planner.setProblemDefinition(pdef)
# perform setup steps for the planner
planner.setup()
# print the settings for this space
print(si.settings())
# print the problem settings
print(pdef)
# attempt to solve the problem within one second of planning time
solved = planner.solve(1.0)
if solved:
# get the goal representation from the problem definition (not the same as the goal state)
# and inquire about the found path
path = pdef.getSolutionPath()
print("Found solution:\n%s" % path)
else:
print("No solution found")
def setSpace(self):
# construct the state space we are planning in
self.space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
self.bounds = ob.RealVectorBounds(2)
if not self.costMap:
self.bounds.setLow(-8)
self.bounds.setHigh(20)
else:
ox = self.costMap.getOriginX()
oy = self.costMap.getOriginY()
size_x = self.costMap.getSizeInMetersX()
size_y = self.costMap.getSizeInMetersY()
low = min(ox, oy)
high = max(ox + size_x, oy + size_y)
print("low", low)
print("high", high)
#self.bounds.setLow(low)
#self.bounds.setHigh(high)
self.bounds.setLow(0, ox)
self.bounds.setHigh(0, ox + size_x)
self.bounds.setLow(1, oy)
self.bounds.setHigh(1, oy + size_y)
self.space.setBounds(self.bounds)
# create a control space
self.cspace = oc.RealVectorControlSpace(self.space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -1.5)
cbounds.setHigh(0, 1.5)
cbounds.setLow(1, -3)
cbounds.setHigh(1, 3)
self.cspace.setBounds(cbounds)
# define a simple setup class
self.ss = oc.SimpleSetup(self.cspace)
self.ss.setStateValidityChecker(
ob.StateValidityCheckerFn(
partial(self.isStateValid, self.ss.getSpaceInformation())))
self.ss.setStatePropagator(oc.StatePropagatorFn(self.propagate))
def setSpace(self):
self.space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
self.bounds = ob.RealVectorBounds(2)
if not self.costMap:
self.bounds.setLow(-8)
self.bounds.setHigh(20)
else:
ox = self.costMap.getOriginX()
oy = self.costMap.getOriginY()
size_x = self.costMap.getSizeInMetersX()
size_y = self.costMap.getSizeInMetersY()
low = min(ox, oy)
high = max(ox + size_x, oy + size_y)
print("low", low)
print("high", high)
self.bounds.setLow(low)
self.bounds.setHigh(high)
self.space.setBounds(self.bounds)
# create a control space
self.cspace = oc.RealVectorControlSpace(self.space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -1.5)
cbounds.setHigh(0, 1.5)
cbounds.setLow(1, -3)
cbounds.setHigh(1, 3)
self.cspace.setBounds(cbounds)
# define a simple setup class
self.ss = oc.SimpleSetup(self.cspace)
validityChecker = ob.StateValidityCheckerFn(
partial(self.isStateValid, self.ss.getSpaceInformation()))
self.ss.setStateValidityChecker(validityChecker)
ode = oc.ODE(self.kinematicCarODE)
odeSolver = oc.ODEBasicSolver(self.ss.getSpaceInformation(), ode)
propagator = oc.ODESolver.getStatePropagator(odeSolver)
self.ss.setStatePropagator(propagator)
def plan(run_time,
planner_type,
wind_direction_degrees,
dimensions,
start_pos,
goal_pos,
obstacles,
heading_degrees,
state_sampler='',
objective_type='sailing'):
# Construct the robot state space in which we're planning
space = ob.SE2StateSpace()
# Create bounds on the position
bounds = ob.RealVectorBounds(2)
x_min, y_min, x_max, y_max = dimensions
bounds.setLow(0, x_min)
bounds.setLow(1, y_min)
bounds.setHigh(0, x_max)
bounds.setHigh(1, y_max)
space.setBounds(bounds)
# Use custom state sampler
if len(state_sampler) > 0:
if 'grid' in state_sampler.lower():
space.setStateSamplerAllocator(
ob.StateSamplerAllocator(GridStateSampler))
else:
print("WARNING: Unknown state_sampler = {}".format(state_sampler))
# Define a simple setup class
ss = og.SimpleSetup(space)
# Construct a space information instance for this state space
si = ss.getSpaceInformation()
# Set resolution of state validity checking, which is fraction of space's extent (Default is 0.01)
desiredResolution = VALIDITY_CHECKING_RESOLUTION_KM / ss.getStateSpace(
).getMaximumExtent()
si.setStateValidityCheckingResolution(desiredResolution)
# Set the objects used to check which states in the space are valid
validity_checker = ph.ValidityChecker(si, obstacles)
ss.setStateValidityChecker(validity_checker)
# Set our robot's starting state
start = ob.State(space)
start[0] = start_pos[0]
start[1] = start_pos[1]
# Set our robot's goal state
goal = ob.State(space)
goal[0] = goal_pos[0]
goal[1] = goal_pos[1]
# Set the start and goal states
ss.setStartAndGoalStates(start, goal)
# Create the optimization objective (helper function is simply a switch statement)
objective = ph.allocate_objective(si, objective_type,
wind_direction_degrees, heading_degrees)
ss.setOptimizationObjective(objective)
# Construct the optimal planner (helper function is simply a switch statement)
optimizing_planner = allocatePlanner(si, planner_type)
ss.setPlanner(optimizing_planner)
# Attempt to solve the planning problem in the given runtime
ss.solve(run_time)
# Return the SimpleSetup object, which contains the solutionPath and spaceInformation
# Must return ss, or else the object will be removed from memory
return ss
boundsize = [0.0, 20.0]
start_pt = [1.0, 19.0]
goal_pt = [19.0, 1.0]
def isStateValid(state):
# print dir(state)
# state.getX state.getY state.getYaw
# state.setX state.setXY state.setY state.setYaw
validity = True
return validity
#### Initiating Space
space = ob.SE2StateSpace()
#### Initiating Bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(boundsize[0])
bounds.setHigh(boundsize[1])
#### Setting Bounds to Space
space.setBounds(bounds)
#### Create Setup Object
setup = og.SimpleSetup(space)
setup.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
#### Initiating Start and Goal
start = ob.State(space)
start().setX(start_pt[0])
def __init__(self,
map_,
pose,
target,
max_time=120,
allow_moving_backward=True,
omega_max=0.5,
vmax=0.08,
objective='duration',
planner=oc.SST,
threshold=0.9,
tolerance=0.3,
control_duration=1,
k=1.0,
min_y=None,
max_y=None):
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(0)
bounds.setHigh(map_.size)
if min_y is not None:
bounds.setLow(min_y)
if max_y is not None:
bounds.setHigh(max_y)
space.setBounds(bounds)
cspace = oc.RealVectorControlSpace(space, 2)
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(-omega_max)
cbounds.setHigh(omega_max)
cspace.setBounds(cbounds)
ss = oc.SimpleSetup(cspace)
si = ss.getSpaceInformation()
ss.setStateValidityChecker(
ob.StateValidityCheckerFn(partial(valid, si)))
self.propagation_data = {'t': 0, 'nt': 0}
ss.setStatePropagator(
oc.StatePropagatorFn(
partial(propagate, self.propagation_data, map_, threshold,
allow_moving_backward)))
start = ob.State(space)
state_from_tuple(start(), pose)
goal = ob.State(space)
state_from_tuple(goal(), target)
ss.setStartAndGoalStates(start, goal, tolerance)
p = planner(si)
ss.setPlanner(p)
si.setPropagationStepSize(control_duration)
si.setMinMaxControlDuration(control_duration, control_duration)
if objective == 'duration':
do = DurationObjective(si)
elif objective == 'balanced':
do = getBalancedObjective1(si, k=k)
else:
do = ob.PathLengthOptimizationObjective(si)
ss.setOptimizationObjective(do)
ss.setup()
self.ss = ss
self.map = map_
self.s = pose
self.t = target
self.objective = objective
self.planner_name = 'SST'
self.planner_options = {}
self.threshold = threshold
self.tolerance = tolerance
self.comp_duration = 0
self.max_time = max_time
self.allow_moving_backward = allow_moving_backward
pos_cnstr = sqrt(x * x + y * y)
return pos_cnstr > 0.2
def isStateValid_SE2(state):
return boxConstraint(state.getX(),
state.getY()) and state.getYaw() < pi / 2.0
def isStateValid_R2(state):
return boxConstraint(state[0], state[1])
if __name__ == "__main__":
# Setup SE2
SE2 = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(0)
bounds.setHigh(1)
SE2.setBounds(bounds)
si_SE2 = ob.SpaceInformation(SE2)
si_SE2.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid_SE2))
# Setup Quotient-Space R2
R2 = ob.RealVectorStateSpace(2)
R2.setBounds(0, 1)
si_R2 = ob.SpaceInformation(R2)
si_R2.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid_R2))
# Create vector of spaceinformationptr
si_vec = og.vectorSpaceInformation()
def plan(self,goalPoints,current_region,next_region,samplerIndex):
"""
goal points: array that contains the coordinates of all the possible goal states
current_reg: name of the current region (p1 etc)
next_reg : name of the next region (p1 etc)
"""
# construct the state space we are planning in
if self.Space_Dimension == 2:
space = ob.SE2StateSpace()
else:
space = ob.SE3StateSpace()
# set the bounds
bounds = ob.RealVectorBounds(self.Space_Dimension)
BoundaryMaxMin = self.all.boundingBox() #0-3:xmin, xmax, ymin and ymax
bounds.setLow(0,BoundaryMaxMin[0]) # 0 stands for x axis
bounds.setHigh(0,BoundaryMaxMin[1])
bounds.setLow(1,BoundaryMaxMin[2]) # 1 stands for y axis
bounds.setHigh(1,BoundaryMaxMin[3])
if self.Space_Dimension == 3:
bounds.setLow(2,0) # 2 stands for z axis
bounds.setHigh(2,self.maxHeight)
space.setBounds(bounds)
if self.system_print == True:
print "The bounding box of the boundary is: " + str(self.all.boundingBox() )
print "The volume of the bounding box is: " + str(bounds.getVolume())
if self.Geometric_Control == 'C':
# create a control space
cspace = oc.RealVectorControlSpace(space, self.Space_Dimension)
# set the bounds for the control space
# cbounds[0] for velocity
# cbounds[1] for angular velocity
cbounds = ob.RealVectorBounds(self.Space_Dimension)
cbounds.setLow(0,0)
cbounds.setHigh(0,max((BoundaryMaxMin[1]-BoundaryMaxMin[0]),(BoundaryMaxMin[3]-BoundaryMaxMin[2]))/100)
cbounds.setLow(1,-pi/5)
cbounds.setHigh(1,pi/5)
cspace.setBounds(cbounds)
if self.system_print == True:
print cspace.settings()
if self.Geometric_Control == 'G':
# define a simple setup class
ss = og.SimpleSetup(space)
else:
# define a simple setup class
ss = oc.SimpleSetup(cspace)
# set state validity checking for this space
ss.setStatePropagator(oc.StatePropagatorFn(self.propagate))
ss.setStateValidityChecker(ob.StateValidityCheckerFn(self.isStateValid))
# create a start state
start = ob.State(space)
pose = self.pose_handler.getPose() #x,y,w,(z if using ROS quadrotor)
start().setX(pose[0])
start().setY(pose[1])
if self.Space_Dimension == 2:
while pose[2] > pi or pose[2] < -pi:
if pose[2]> pi:
pose[2] = pose[2] - pi
else:
pose[2] = pose[2] + pi
start().setYaw(pose[2]) #
else:
start().setZ(pose[3])
start().rotation().setIdentity()
if self.system_print is True and self.Space_Dimension == 3:
print "start:" + str(start().getX())+","+str(start().getY()) +"," + str(start().getZ())
print goalPoints
# create goal states
goalStates = ob.GoalStates(ss.getSpaceInformation())
for i in range(shape(goalPoints)[1]):
goal = ob.State(space)
goal().setX(goalPoints[0,i])
goal().setY(goalPoints[1,i])
if self.Space_Dimension == 3:
if self.system_print is True:
print current_region,next_region
print self.map['height'][current_region],self.map['height'][next_region]
z_goalPoint = min(self.map['height'][current_region]/2,self.map['height'][next_region]/2)
goal().setZ(z_goalPoint)
goal().rotation().setIdentity()
else:
goal().setYaw(0.0)
if self.plotting == True:
if self.Space_Dimension == 3:
self.ax.plot([goalPoints[0,i]],[goalPoints[1,i]],[z_goalPoint],'ro')
else:
self.ax.plot([goalPoints[0,i]],[goalPoints[1,i]],'ro')
self.setPlotLimitXYZ()
self.ax.get_figure().canvas.draw()
goalStates.addState(goal)
if self.system_print is True:
print goalStates
# set the start and goal states;
ss.setGoal(goalStates)
ss.setStartState(start)
# set sampler (optional; the default is uniform sampling)
si = ss.getSpaceInformation()
# set planner
planner_prep = self.planner_dictionary[self.Geometric_Control][self.planner]
planner = planner_prep(si)
ss.setPlanner(planner)
if self.Geometric_Control == 'G':
if not self.planner == 'PRM':
planner.setRange(self.radius*2)
if self.system_print is True:
print "planner.getRange():" + str(planner.getRange())
#if not self.planner == 'RRTConnect':
# planner.setGoalBias(0.5)
else:
# (optionally) set propagation step size
si.setPropagationStepSize(1) #actually is the duration in propagate
si.setMinMaxControlDuration(3,3) # is the no of steps taken with the same velocity and omega
if self.system_print is True:
print "radius: " +str(self.radius)
print "si.getPropagationStepSize():" + str(si.getPropagationStepSize())
planner.setGoalBias(0.5)
ss.setup()
# attempt to solve the problem within ten seconds of planning time
solved = ss.solve(1000.0) #10
if (solved):
print("Found solution:")
# print the path to screen
print >>sys.__stdout__,(ss.getSolutionPath())
else:
print("No solution found")
if self.plotting == True :
self.ax.set_title('Map with Geo/Control: '+str(self.Geometric_Control) + ",Planner:" +str(self.planner), fontsize=12)
self.ax.set_xlabel('x')
self.ax.set_ylabel('y')
for i in range(ss.getSolutionPath().getStateCount()-1):
if self.Space_Dimension == 3:
self.ax.plot(((ss.getSolutionPath().getState(i)).getX(),(ss.getSolutionPath().getState(i+1)).getX()),((ss.getSolutionPath().getState(i)).getY(),(ss.getSolutionPath().getState(i+1)).getY()),((ss.getSolutionPath().getState(i)).getZ(),(ss.getSolutionPath().getState(i+1)).getZ()),'b')
ro=Polygon.Shapes.Circle (self.radius,((ss.getSolutionPath().getState(i)).getX(),(ss.getSolutionPath().getState(i)).getY()))
self.plotPoly(ro,'r')
else:
self.ax.plot(((ss.getSolutionPath().getState(i)).getX(),(ss.getSolutionPath().getState(i+1)).getX()),((ss.getSolutionPath().getState(i)).getY(),(ss.getSolutionPath().getState(i+1)).getY()),0,'b')
self.setPlotLimitXYZ()
#self.ax.get_figure().canvas.draw()
return ss
def plan(run_time, planner_type, objective_type, wind_direction, dimensions, obstacles):
# Construct the robot state space in which we're planning. We're
# planning in [0,1]x[0,1], a subset of R^2.
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, dimensions[0])
bounds.setLow(1, dimensions[1])
bounds.setHigh(0, dimensions[2])
bounds.setHigh(1, dimensions[3])
# Set the bounds of space to be in [0,1].
space.setBounds(bounds)
# define a simple setup class
ss = og.SimpleSetup(space)
# Construct a space information instance for this state space
si = ss.getSpaceInformation()
# Set resolution of state validity checking. This is fraction of space's extent.
# si.setStateValidityCheckingResolution(0.001)
# Set the object used to check which states in the space are valid
validity_checker = ValidityChecker(si, obstacles)
ss.setStateValidityChecker(validity_checker)
# Set our robot's starting state to be the bottom-left corner of
# the environment, or (0,0).
start = ob.State(space)
start[0] = dimensions[0]
start[1] = dimensions[1]
start[2] = math.pi / 4
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = dimensions[2]
goal[1] = dimensions[3]
goal[2] = math.pi / 4
# Set the start and goal states
ss.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
objective = allocate_objective(si, objective_type)
lengthObj = ob.PathLengthOptimizationObjective(si)
clearObj = ClearanceObjective(si)
minTurnObj = MinTurningObjective(si)
windObj = WindObjective(si)
tackingObj = TackingObjective(si)
ss.setOptimizationObjective(objective)
print("ss.getProblemDefinition().hasOptimizationObjective( ){}".format(ss.getProblemDefinition().hasOptimizationObjective()))
print("ss.getProblemDefinition().hasOptimizedSolution() {}".format(ss.getProblemDefinition().hasOptimizedSolution()))
# Create a decomposition of the state space
decomp = MyDecomposition(256, bounds)
# Construct the optimal planner specified by our command line argument.
# This helper function is simply a switch statement.
# optimizing_planner = allocate_planner(si, planner_type, decomp)
optimizing_planner = allocatePlanner(si, planner_type)
ss.setPlanner(optimizing_planner)
# attempt to solve the planning problem in the given runtime
solved = ss.solve(run_time)
if solved:
# Output the length of the path found
print('{0} found solution of path length {1:.4f} with an optimization '
'objective value of {2:.4f}'.format(ss.getPlanner().getName(),
ss.getSolutionPath().length(),
0.1))
solution_path = ss.getSolutionPath()
states = solution_path.getStates()
prevState = states[0]
for state in states[1:]:
print(space.validSegmentCount(prevState, state))
prevState = state
print("ss.getSolutionPath().printAsMatrix() = {}".format(ss.getSolutionPath().printAsMatrix()))
print("ss.haveSolutionPath() = {}".format(ss.haveSolutionPath()))
print("ss.haveExactSolutionPath() = {}".format(ss.haveExactSolutionPath()))
print("***")
print("ss.getSolutionPath().length() = {}".format(ss.getSolutionPath().length()))
print("ss.getSolutionPath().check() = {}".format(ss.getSolutionPath().check()))
print("ss.getSolutionPath().clearance() = {}".format(ss.getSolutionPath().clearance()))
print("ss.getSolutionPath().cost(objective).value() = {}".format(ss.getSolutionPath().cost(objective).value()))
print("ss.getSolutionPath().cost(lengthObj).value() = {}".format(ss.getSolutionPath().cost(lengthObj).value()))
print("ss.getSolutionPath().cost(clearObj).value() = {}".format(ss.getSolutionPath().cost(clearObj).value()))
print("ss.getSolutionPath().cost(minTurnObj).value() = {}".format(ss.getSolutionPath().cost(minTurnObj).value()))
print("ss.getSolutionPath().cost(windObj ).value() = {}".format(ss.getSolutionPath().cost(windObj).value()))
print("ss.getSolutionPath().cost(tackingObj ).value() = {}".format(ss.getSolutionPath().cost(tackingObj).value()))
print("ss.getProblemDefinition().hasOptimizedSolution() {}".format(ss.getProblemDefinition().hasOptimizedSolution()))
plot_path(ss.getSolutionPath(), dimensions, obstacles)
print("***")
# print("Simplifying path")
# ss.simplifySolution()
# solution_path = ss.getSolutionPath()
# states = solution_path.getStates()
# prevState = states[0]
# for state in states[1:]:
# print(space.validSegmentCount(prevState, state))
# prevState = state
# print("ss.getSolutionPath().printAsMatrix() = {}".format(ss.getSolutionPath().printAsMatrix()))
# print("ss.haveSolutionPath() = {}".format(ss.haveSolutionPath()))
# print("ss.haveExactSolutionPath() = {}".format(ss.haveExactSolutionPath()))
# print("***")
# print("ss.getSolutionPath().length() = {}".format(ss.getSolutionPath().length()))
# print("ss.getSolutionPath().check() = {}".format(ss.getSolutionPath().check()))
# print("ss.getSolutionPath().clearance() = {}".format(ss.getSolutionPath().clearance()))
# print("ss.getSolutionPath().cost(objective).value() = {}".format(ss.getSolutionPath().cost(objective).value()))
# print("ss.getSolutionPath().cost(lengthObj).value() = {}".format(ss.getSolutionPath().cost(lengthObj).value()))
# print("ss.getSolutionPath().cost(clearObj).value() = {}".format(ss.getSolutionPath().cost(clearObj).value()))
# print("ss.getSolutionPath().cost(minTurnObj).value() = {}".format(ss.getSolutionPath().cost(minTurnObj).value()))
# plot_path(ss.getSolutionPath(), dimensions, obstacles)
else:
print("No solution found.")
def __init__(self, node_type, num_planners=1):
"""
Constructor for OMPLPlanTrajectoryWrapper.
In orignal PathTools, it is using multi-threading with multiple ROS services/machines.
Hence it required locks to record if each service/machine is idle or busy.
However, in this version we don't need to worry about that, since the planning function
does not utilize any ROS services. It is only calling the OMPL library code (if there
is no service involved in OMPL). And all shared variables won't be in danger
in the trajectory_planning function. Hence it is safe to call many trajectory planning
jobs, without the need of using locks.
However, for API safety and future extension (we may use neural network in this setting)
we'll still use the locks.
But it can be removed from this library.
Args:
node_type (string): The type of planner that this is being used by,
generally "pfs" or "rr".
num_planners (int): The number of planner nodes that are being used.
"""
PathTools.PlanTrajectoryWrapper.__init__(self, node_type, num_planners)
## add OMPL setting for different environments
self.env_name = rospy.get_param('env_name')
self.planner_name = rospy.get_param('planner_name')
if self.env_name == 's2d':
#data_loader = data_loader_2d
IsInCollision = plan_s2d.IsInCollision
# create an SE2 state space
space = ob.RealVectorStateSpace(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-20)
bounds.setHigh(20)
space.setBounds(bounds)
elif self.env_name == 'c2d':
#data_loader = data_loader_2d
IsInCollision = plan_c2d.IsInCollision
# create an SE2 state space
space = ob.RealVectorStateSpace(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-20)
bounds.setHigh(20)
space.setBounds(bounds)
elif self.env_name == 'r2d':
#data_loader = data_loader_r2d
IsInCollision = plan_r2d.IsInCollision
# create an SE2 state space
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(-20)
bounds.setHigh(20)
space.setBounds(bounds)
elif self.env_name == 'r3d':
#data_loader = data_loader_r3d
IsInCollision = plan_r3d.IsInCollision
# create an SE2 state space
space = ob.RealVectorStateSpace(3)
bounds = ob.RealVectorBounds(3)
bounds.setLow(-20)
bounds.setHigh(20)
space.setBounds(bounds)
self.IsInCollision = IsInCollision
self.space = space
def plan(runTime, plannerType, objectiveType, fname):
# Construct the robot state space in which we're planning. We're
# planning in [0,1]x[0,1], a subset of R^2.
space = ob.SE2StateSpace()
# Set the bounds of space to be in [0,1]
bounds = ob.RealVectorBounds(2)
bounds.setLow(0)
bounds.setHigh(1)
space.setBounds(bounds)
# Construct a space information instance for this state space
si = ob.SpaceInformation(space)
# Set the object used to check which states in the space are valid
validityChecker = ValidityChecker(si)
si.setStateValidityChecker(validityChecker)
si.setup()
# Set our robot's starting state to be the bottom-left corner of
# the environment, or (0,0).
start = ob.State(space)
start[0] = 0
start[1] = 0
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = 1
goal[1] = 1
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
pdef.setOptimizationObjective(allocateObjective(si, objectiveType))
# Construct the optimal planner specified by us.
# This helper function is simply a switch statement.
optimizingPlanner = allocatePlanner(si, plannerType)
# Set the problem we are trying to solve to the planner
optimizingPlanner.setProblemDefinition(pdef)
#perform setup steps for the planner
optimizingPlanner.setup()
#print the settings for this space
#print(si.settings())
#print the problem settings
#print(pdef)
# attempt to solve the planning problem in the given runtime
solved = optimizingPlanner.solve(runTime)
if solved:
#print solution path
path = pdef.getSolutionPath()
print("Found Solution:\n%s" % path)
# Output the length of the path found
print('{0} found solution of path length {1:.4f} with an optimization ' \
'objective value of {2:.4f}'.format( \
optimizingPlanner.getName(), \
pdef.getSolutionPath().length(), \
pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()))
# If a filename was specified, output the path as a matrix to
# that file for visualization
if fname:
with open(fname, 'w') as outFile:
outFile.write(pdef.getSolutionPath().printAsMatrix())
print("saved final path as 'mat.txt' for matplotlib")
# Extracting planner data from most recent solve attempt
pd = ob.PlannerData(pdef.getSpaceInformation())
optimizingPlanner.getPlannerData(pd)
# Computing weights of all edges based on state space distance
pd.computeEdgeWeights()
#dataStorage=ob.PlannerDataStorage()
#dataStorage.store(pd,"myPlannerData")
graphml = pd.printGraphML()
f = open("graph.graphml", 'w')
f.write(graphml)
f.close()
print("saved")
else:
print("No solution found.")
def setup(problem):
OMPL_INFORM("Robot type is: %s" % str(problem["robot.type"]))
ompl_setup = eval("oa.%s()" % problem["robot.type"])
problem["is3D"] = isinstance(ompl_setup.getGeometricComponentStateSpace(), ob.SE3StateSpace)
if str(ompl_setup.getAppType()) == "GEOMETRIC":
problem["isGeometric"] = True
else:
problem["isGeometric"] = False
ompl_setup.setEnvironmentMesh(str(problem['env_loc']))
ompl_setup.setRobotMesh(str(problem['robot_loc']))
if problem["is3D"]:
# Set the dimensions of the bounding box
bounds = ob.RealVectorBounds(3)
bounds.low[0] = float(problem['volume.min.x'])
bounds.low[1] = float(problem['volume.min.y'])
bounds.low[2] = float(problem['volume.min.z'])
bounds.high[0] = float(problem['volume.max.x'])
bounds.high[1] = float(problem['volume.max.y'])
bounds.high[2] = float(problem['volume.max.z'])
ompl_setup.getGeometricComponentStateSpace().setBounds(bounds)
space = ob.SE3StateSpace()
# Set the start state
start = ob.State(space)
start().setXYZ(float(problem['start.x']), float(problem['start.y']), \
float(problem['start.z']))
# Set the start rotation
start().rotation().x = float(problem['start.q.x'])
start().rotation().y = float(problem['start.q.y'])
start().rotation().z = float(problem['start.q.z'])
start().rotation().w = float(problem['start.q.w'])
# Set the goal state
goal = ob.State(space)
goal().setXYZ(float(problem['goal.x']), float(problem['goal.y']), float(problem['goal.z']))
# Set the goal rotation
goal().rotation().x = float(problem['goal.q.x'])
goal().rotation().y = float(problem['goal.q.y'])
goal().rotation().z = float(problem['goal.q.z'])
goal().rotation().w = float(problem['goal.q.w'])
start = ompl_setup.getFullStateFromGeometricComponent(start)
goal = ompl_setup.getFullStateFromGeometricComponent(goal)
ompl_setup.setStartAndGoalStates(start, goal)
else:
# Set the dimensions of the bounding box
bounds = ob.RealVectorBounds(2)
bounds.low[0] = float(problem['volume.min.x'])
bounds.low[1] = float(problem['volume.min.y'])
bounds.high[0] = float(problem['volume.max.x'])
bounds.high[1] = float(problem['volume.max.y'])
ompl_setup.getGeometricComponentStateSpace().setBounds(bounds)
space = ob.SE2StateSpace()
start = ob.State(space)
start().setX(float(problem['start.x']))
start().setY(float(problem['start.y']))
start().setYaw(float(problem['start.yaw']))
goal = ob.State(space)
goal().setX(float(problem['goal.x']))
goal().setY(float(problem['goal.y']))
goal().setYaw(float(problem['goal.yaw']))
start = ompl_setup.getFullStateFromGeometricComponent(start)
goal = ompl_setup.getFullStateFromGeometricComponent(goal)
ompl_setup.setStartAndGoalStates(start, goal)
return ompl_setup
def plan(run_time, planner_type, objective_type, wind_direction, dimensions,
obstacles):
# Construct the robot state space in which we're planning. We're
# planning in [0,1]x[0,1], a subset of R^2.
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, dimensions[0])
bounds.setLow(1, dimensions[1])
bounds.setHigh(0, dimensions[2])
bounds.setHigh(1, dimensions[3])
# Set the bounds of space to be in [0,1].
space.setBounds(bounds)
# create a control space
cspace = oc.RealVectorControlSpace(space, 1)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(1)
cbounds.setLow(-math.pi / 3)
cbounds.setHigh(math.pi / 3)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
propagator = get_state_propagator(wind_direction)
ss.setStatePropagator(oc.StatePropagatorFn(propagator))
# Construct a space information instance for this state space
si = ss.getSpaceInformation()
si.setPropagationStepSize(1)
# Set the object used to check which states in the space are valid
validity_checker = ValidityChecker(si, obstacles)
ss.setStateValidityChecker(validity_checker)
# Set our robot's starting state to be the bottom-left corner of
# the environment, or (0,0).
start = ob.State(space)
start[0] = dimensions[0]
start[1] = dimensions[1]
start[2] = math.pi / 4
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = dimensions[2]
goal[1] = dimensions[3]
goal[2] = math.pi / 4
# Set the start and goal states
ss.setStartAndGoalStates(start, goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
objective = allocate_objective(si, objective_type)
ss.setOptimizationObjective(objective)
# Create a decomposition of the state space
decomp = MyDecomposition(256, bounds)
# Construct the optimal planner specified by our command line argument.
# This helper function is simply a switch statement.
optimizing_planner = allocate_planner(si, planner_type, decomp)
ss.setPlanner(optimizing_planner)
# attempt to solve the planning problem in the given runtime
solved = ss.solve(run_time)
if solved:
# Output the length of the path found
print('{0} found solution of path length {1:.4f} with an optimization '
'objective value of {2:.4f}'.format(
ss.getPlanner().getName(),
ss.getSolutionPath().length(), 0.1))
print(ss.getSolutionPath().printAsMatrix())
print(ss.haveExactSolutionPath())
plot_path(ss.getSolutionPath(), dimensions, obstacles)
else:
print("No solution found.")
