def __init__(self, shape, calc_time, resolution, distance):
ou.setLogLevel(ou.LOG_WARN)
self._calc_time = calc_time
self._space = ob.RealVectorStateSpace(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(0.0)
bounds.setHigh(0, shape[0] - 1)
bounds.setHigh(1, shape[1] - 1)
self._space.setBounds(bounds)
self._si = ob.SpaceInformation(self._space)
self._validity_checker = OmplPlannerImpl.ValidityChecker(self._si)
self._si.setStateValidityChecker(self._validity_checker)
self._si.setStateValidityCheckingResolution(resolution)
self._si.setup()
self._start = ob.State(self._space)
self._goal = ob.State(self._space)
self._pdef = ob.ProblemDefinition(self._si)
objective = ob.MultiOptimizationObjective(self._si)
objective.addObjective(OmplPlannerImpl.MapCostObjective(self._si), 0.5)
# ## setting length objective
length_objective = ob.PathLengthOptimizationObjective(self._si)
objective.addObjective(length_objective, 0.1)
# objective.setCostThreshold(1000.0)
self._pdef.setOptimizationObjective(objective)
self._planner = og.RRTstar(self._si)
self._planner.setRange(distance)
self._planner.setup()
def setup_ompl(self):
print "set space"
self.space = ob.RealVectorStateSpace(dim=self.space_dimension)
bounds = ob.RealVectorBounds(self.space_dimension)
print "set bounds"
for i in xrange(self.space_dimension):
bounds.setLow(i, self.joint_constraints[0])
bounds.setHigh(i, self.joint_constraints[1])
print "set bounds 2"
self.space.setBounds(bounds)
print "set si"
self.si = ob.SpaceInformation(self.space)
print "set motion validator"
self.motion_validator = MotionValidator(self.si)
self.motion_validator.set_link_dimensions(self.link_dimensions)
self.motion_validator.set_workspace_dimension(self.workspace_dimension)
self.motion_validator.set_max_distance(self.max_velocity, self.delta_t)
print "set state validiy checker"
self.si.setStateValidityChecker(
ob.StateValidityCheckerFn(self.motion_validator.isValid))
print "set motion validator"
self.si.setMotionValidator(self.motion_validator)
print "si.setup"
self.si.setup()
self.problem_definition = ob.ProblemDefinition(self.si)
def __init__(self, lower_bound, upper_bound):
"""Initialize the object."""
# Create an R2 state space
self.space = ob.RealVectorStateSpace(2)
# Set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(lower_bound)
bounds.setHigh(upper_bound)
self.space.setBounds(bounds)
# Create a new space information
self.si = ob.SpaceInformation(self.space)
# Set the state validity checker
self.si.setStateValidityChecker(
ob.StateValidityCheckerFn(self.is_state_valid))
self.si.setup()
# Set the problem definition
self.pdef = ob.ProblemDefinition(self.si)
# Setup an optimizing planner with RRT*
self.optimizingPlanner = og.RRTstar(self.si)
# Empty obstacles list
self.obstacles = []
def test_optimal(calc_time=0.01):
space = ob.RealVectorStateSpace(2)
space.setBounds(0, WIDTH)
si = ob.SpaceInformation(space)
si.setStateValidityChecker(ValidityChecker(si))
si.setup()
start = ob.State(space)
goal = ob.State(space)
start[0] = 0.0
start[1] = 0.0
goal[0] = WIDTH - 1
goal[1] = WIDTH - 1
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
objective = ob.MultiOptimizationObjective(si)
objective.addObjective(MapCostObjective(si), 0.9)
objective.addObjective(ob.PathLengthOptimizationObjective(si), 0.1)
pdef.setOptimizationObjective(objective)
planner = og.RRTstar(si)
planner.setProblemDefinition(pdef)
planner.setup()
status = planner.solve(calc_time)
if pdef.hasExactSolution():
states = pdef.getSolutionPath().getStates()
cost = pdef.getSolutionPath().cost(objective).value()
states_np = np.array([(state[0], state[1]) for state in states])
print(states)
print(cost)
print(states_np)
def planBITstar(q_st, q_g, res):
# create an SE2 state space
space = ob.RealVectorStateSpace(3)
# set lower and upper bounds
bounds = ob.RealVectorBounds(3)
bounds.setLow(0)
bounds.setHigh(res)
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 start state
start = ob.State(space)
start[0] = q_st[0]
start[1] = q_st[1]
# create goal state
goal = ob.State(space)
goal[0] = q_g[0]
goal[1] = q_g[1]
# 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.BITstar(si)
# set the problem we are trying to solve for the planner
planner.setProblemDefinition(pdef)
# perform setup steps for the planner
planner.setup()
# 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")
path_arr = []
print(path.getStateCount())
for i in range(path.getStateCount()):
path_arr.append([])
path_arr[i].append(path.getState(i)[0])
path_arr[i].append(path.getState(i)[1])
return path_arr
else:
print("No solution found")
return []
def _get_path(
self,
is_state_valid: Callable[[np.ndarray], bool],
start_js: np.ndarray,
robot_targ: RobotTarget,
use_sim: MpSim,
mp_space: MpSpace,
timeout: int,
):
"""
Does the low-level path planning with OMPL.
"""
if not self._should_render:
ou.setLogLevel(ou.LOG_ERROR)
dim = mp_space.get_state_dim()
space = ob.RealVectorStateSpace(dim)
bounds = ob.RealVectorBounds(dim)
lims = mp_space.get_state_lims()
for i, lim in enumerate(lims):
bounds.setLow(i, lim[0])
bounds.setHigh(i, lim[1])
space.setBounds(bounds)
si = ob.SpaceInformation(space)
si.setStateValidityChecker(ob.StateValidityCheckerFn(is_state_valid))
si.setup()
pdef = ob.ProblemDefinition(si)
mp_space.set_problem(pdef, space, si, start_js, robot_targ)
planner = mp_space.get_planner(si)
planner.setProblemDefinition(pdef)
planner.setup()
if mp_space.get_range() is not None:
planner.setRange(mp_space.get_range())
solved = planner.solve(timeout)
if not solved:
self._log("Could not find plan")
return None
objective = pdef.getOptimizationObjective()
if objective is not None:
cost = (pdef.getSolutionPath().cost(
pdef.getOptimizationObjective()).value())
else:
cost = np.inf
self._log("Got a path of length %.2f and cost %.2f" %
(pdef.getSolutionPath().length(), cost))
path = pdef.getSolutionPath()
joint_plan = mp_space.convert_sol(path)
self._is_approx_sol = pdef.hasApproximateSolution()
return joint_plan
def space_info_creation(self, space):
"""
Function of creating space information that includes valid and invalid states
"""
space_info = ob.SpaceInformation(space)
space_info.setStateValidityChecker(
ob.StateValidityCheckerFn(self.isStateValid))
space_info.setup()
return space_info
def space_info_creation(self, space, obstacle_list):
"""
Function of creating space information that includes valid and invalid states
"""
space_info = ob.SpaceInformation(space)
isValidFn = ob.StateValidityCheckerFn(partial(self.isStateValid, obstacle_list))
space_info.setStateValidityChecker(isValidFn)
#space_info.setStateValidityChecker(ob.StateValidityCheckerFn(self.isStateValid))
space_info.setup()
return space_info
def plan(runTime, plannerType, objectiveType, fname, bound, start_pt, goal_pt):
print "Run Time: ", runTime
# 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.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,1].
space.setBounds(bound[0], bound[1])
# 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] = start_pt[0]
start[1] = start_pt[1]
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = goal_pt[0]
goal[1] = goal_pt[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 our command line argument.
# This helper function is simply a switch statement.
optimizingPlanner = allocatePlanner(si, plannerType)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
# attempt to solve the planning problem in the given runtime
solved = optimizingPlanner.solve(runTime)
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(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())
return True, pdef.getSolutionPath().printAsMatrix()
else:
print("No solution found.")
return False, "No solution found"
def __init__(self, ppm_file):
self.ppm_ = ou.PPM()
self.ppm_.loadFile(ppm_file)
self.space = ob.RealVectorStateSpace()
self.space.addDimension(0.0, self.ppm_.getWidth())
self.space.addDimension(0.0, self.ppm_.getHeight())
self.maxWidth_ = self.ppm_.getWidth() - 1
self.maxHeight_ = self.ppm_.getHeight() - 1
self.si = ob.SpaceInformation(self.space)
# set state validity checking for this space
self.si.setStateValidityChecker(ob.StateValidityCheckerFn(
partial(Plane2DEnvironment.isStateValid, self)))
self.si.setup()
self.si.setStateValidityCheckingResolution(1.0 / self.space.getMaximumExtent())
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 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 test_rigid(calc_time=0.01):
space = ob.RealVectorStateSpace(2)
space.setBounds(0, WIDTH)
si = ob.SpaceInformation(space)
si.setStateValidityChecker(ValidityChecker(si))
si.setup()
start = ob.State(space)
goal = ob.State(space)
start[0] = 0.0
start[1] = 0.0
goal[0] = WIDTH - 1
goal[1] = WIDTH - 1
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
planner = og.RRTstar(si)
planner.setProblemDefinition(pdef)
planner.setup()
status = planner.solve(calc_time)
if pdef.hasExactSolution():
cost = pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()
print(cost)
def get_invalid_sections_for_paths(self, orig_paths, group_name):
"""
Returns the invalid sections for a set of paths.
Args:
orig_paths (list of paths): The paths to collision check,
represnted by a list of individual joint configurations.
group_name (str): The joint group for which the paths were created.
Return:
list of list of pairs of indicies, where each index in a pair is the
start and end of an invalid section.
"""
# obtain obstacle information through rostopic
rospy.loginfo(
"Invalid Section Wrapper: waiting for obstacle message...")
obc = rospy.wait_for_message('obstacles/obc', Float64Array2D)
# obs = rospy.wait_for_message('obstacles/obs', Float64Array2D)
obc = [obc_i.values for obc_i in obc.points]
obc = np.array(obc)
rospy.loginfo("Invalid Section Wrapper: obstacle message received.")
# transform from orig_paths
# for each path, check the invalid sections
"""
general idea:
invalid section: 1. start and end should be not in collision, but intermediate nodes are all in collision.
2. start and end not in collision, no intermediate nodes, but not line collision free
we assume that for all paths, the start and end are not in collision (valid planning problem, since projected)
psuedo code:
tracking = False
while True:
if not tracking:
if is last node, then break
line search (including end point) to next node:
fail ->
update start -> current
tracking = True
success ->
move to next node
else:
collision free on current point:
fail ->
move to next node
success ->
end -> current
save invalid section (start - end)
tracking -> False
"""
inv_sec_paths = []
# set up OMPL env for line searching
IsInCollision = self.IsInCollision
def isStateValid(state):
return not IsInCollision(state, obc)
for orig_path in orig_paths:
si = ob.SpaceInformation(self.space)
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
si.setup()
# use motionValidator
motionVal = ob.DiscreteMotionValidator(si)
path = orig_path
states = []
for i in range(len(path)):
state = ob.State(self.space)
for j in range(len(path[i])):
state[j] = path[i][j]
states.append(state)
inv_sec_path = []
start_i = -1
end_i = -1
invalid_tracking = False
point_i = 0
while point_i < len(orig_path):
if not invalid_tracking:
if point_i == len(orig_path) - 1:
break
# line search to the next node
ind = motionVal.checkMotion(states[point_i](),
states[point_i + 1]())
# also consider the endpoint
ind = ind and isStateValid(states[point_i + 1])
if ind == False:
start_i = point_i
invalid_tracking = True
# move to next point
point_i += 1
else:
# collision check on current point
ind = isStateValid(path[point_i])
if ind == True:
end_i = point_i
inv_sec_path.append([start_i, end_i])
invalid_tracking = False
else:
point_i += 1
inv_sec_paths.append(inv_sec_path)
return inv_sec_paths
def plan(self, 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.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,1].
space.setBounds(0.0, 1.0)
# 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] = self.start_x
start[1] = self.start_y
# Set our robot's goal state to be the top-right corner of the
# environment, or (1,1).
goal = ob.State(space)
goal[0] = self.goal_x
goal[1] = self.goal_y
print("goal")
print self.goal_x
print self.goal_y
# 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 our command line argument.
# This helper function is simply a switch statement.
optimizingPlanner = allocatePlanner(si, plannerType)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
# attempt to solve the planning problem in the given runtime
solved = optimizingPlanner.solve(runTime)
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(optimizingPlanner.getName(), pdef.getSolutionPath().length(), pdef.getSolutionPath().cost(pdef.getOptimizationObjective()).value()))
str = pdef.getSolutionPath().printAsMatrix()
#print(type(str))
#
x = str.split()
self.path = [float(x) for x in x if x]
#print(self.path)
#talker()
else:
print("No solution found.")
return self.path
def plan(fname=None):
# 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.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,1].
space.setBounds(0.0, 1.0)
# 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.0
start[1] = 0.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.0
goal[1] = 1.0
# Create a problem instance
pdef = ob.ProblemDefinition(si)
# Set the start and goal states
pdef.setStartAndGoalStates(start, goal)
# Since we want to find an optimal plan, we need to define what
# is optimal with an OptimizationObjective structure. Un-comment
# exactly one of the following 6 lines to see some examples of
# optimization objectives.
pdef.setOptimizationObjective(getPathLengthObjective(si))
# pdef.setOptimizationObjective(getThresholdPathLengthObj(si))
# pdef.setOptimizationObjective(getClearanceObjective(si))
# pdef.setOptimizationObjective(getBalancedObjective1(si))
# pdef.setOptimizationObjective(getBalancedObjective2(si))
# pdef.setOptimizationObjective(getPathLengthObjWithCostToGo(si))
# Construct our optimal planner using the RRTstar algorithm.
optimizingPlanner = og.RRTstar(si)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
# attempt to solve the planning problem within one second of
# planning time
solved = optimizingPlanner.solve(10.0)
if solved:
# Output the length of the path found
print("Found solution of path length %g" %
pdef.getSolutionPath().length())
# 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())
else:
print("No solution found.")
def plan(self, 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.
self.space = ob.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,1].
self.space.setBounds(0, 400.0)
# Construct a space information instance for this state space
self.si = ob.SpaceInformation(self.space)
# Set the object used to check which states in the space are valid
validityChecker = ValidityChecker(self.si, self.mapIm)
self.si.setStateValidityChecker(validityChecker)
self.si.setup()
self.start = ob.State(self.space)
self.start[0] = self.startPose[0]
self.start[1] = self.startPose[1]
self.goal = ob.State(self.space)
self.goal[0] = self.goalPose[0]
self.goal[1] = self.goalPose[1]
# Create a problem instance
self.pdef = ob.ProblemDefinition(self.si)
# Set the start and goal states
self.pdef.setStartAndGoalStates(self.start, self.goal)
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
self.pdef.setOptimizationObjective(
self.allocateObjective(self.si, objectiveType))
# Construct the optimal planner specified by our command line argument.
# This helper function is simply a switch statement.
self.optimizingPlanner = self.allocatePlanner(self.si, plannerType)
# Set the problem instance for our planner to solve
self.optimizingPlanner.setProblemDefinition(self.pdef)
self.optimizingPlanner.setup()
# attempt to solve the planning problem in the given runtime
solved = self.optimizingPlanner.solve(runTime)
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(
self.optimizingPlanner.getName(),
self.pdef.getSolutionPath().length(),
self.pdef.getSolutionPath().cost(
self.pdef.getOptimizationObjective()).value()))
print self.pdef.getSolutionPath()
# 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())
else:
print("No solution found.")
def plan(mapfile,start_node, goal_node,runTime, plannerType, objectiveType, d, fname):
boundary, blocks = load_map(mapfile)
boundary[:,:3] = boundary[:,:3] +0.0005
boundary[:,3:] = boundary[:,3:] -0.0005
alpha = 1.05
blocks = blocks*alpha
# Construct the robot state space in which we're planning. We're
space = ob.RealVectorStateSpace(3)
sbounds = ob.RealVectorBounds(3)
sbounds.low[0] =float(boundary[0,0])
sbounds.high[0] =float(boundary[0,3])
sbounds.low[1] = float(boundary[0,1])
sbounds.high[1] = float(boundary[0,4])
sbounds.low[2] = float(boundary[0,2])
sbounds.high[2] = float(boundary[0,5])
space.setBounds(sbounds)
# 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, boundary, blocks)
si.setStateValidityChecker(validityChecker)
mv = MyMotionValidator(si, boundary, blocks)
si.setMotionValidator(mv)
si.setup()
# Set our robot's starting state
start = ob.State(space)
start[0] = start_node[0]
start[1] = start_node[1]
start[2] = start_node[2]
# Set our robot's goal state
goal = ob.State(space)
goal[0] = goal_node[0]
goal[1] = goal_node[1]
goal[2] = goal_node[2]
# 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 our command line argument.
# This helper function is simply a switch statement.
optimizingPlanner = allocatePlanner(si,d, plannerType)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
solved = "Approximate solution"
# attempt to solve the planning problem in the given runtime
t1 = tic()
while(str(solved) == 'Approximate solution'):
solved = optimizingPlanner.solve(runTime)
# print(pdef.getSolutionPath().printAsMatrix())
t2 = toc(t1)
p = pdef.getSolutionPath()
ps = og.PathSimplifier(si)
ps.simplifyMax(p)
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( \
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
env = mapfile.split(".")[1].split("/")[-1]
fname = fname + "{}_{}_{}_{}.txt".format(env, d,np.round(pdef.getSolutionPath().length(),2),np.round(t2,2))
if fname:
with open(fname, 'w') as outFile:
outFile.write(pdef.getSolutionPath().printAsMatrix())
path = np.genfromtxt(fname)
print("The found path : ")
print(path)
pathlength = np.sum(np.sqrt(np.sum(np.diff(path,axis=0)**2,axis=1)))
print("pathlength : {}".format(pathlength))
fig, ax, hb, hs, hg = draw_map(boundary, blocks, start_node, goal_node)
ax.plot(path[:,0],path[:,1],path[:,2],'r-')
plt.show()
def plan_trajectory(self,
start_point,
goal_point,
planner_number,
joint_names,
group_name,
planning_time,
planner_config_name,
plan_type='pfs'):
"""
Use OMPL library for planning. Obtain obstacle information from rostopic for
collision checking
"""
# obtain obstacle information through rostopic
rospy.loginfo(
"%s Plan Trajectory Wrapper: waiting for obstacle message..." %
(rospy.get_name()))
obc = rospy.wait_for_message('obstacles/obc', Float64Array2D)
# obs = rospy.wait_for_message('obstacles/obs', Float64Array2D)
obc = [obc_i.values for obc_i in obc.points]
obc = np.array(obc)
rospy.loginfo(
"%s Plan Trajectory Wrapper: obstacle message received." %
(rospy.get_name()))
# depending on plan type, obtain path_length from published topic or not
if plan_type == 'pfs':
# obtain path length through rostopic
rospy.loginfo(
"%s Plan Trajectory Wrapper: waiting for planning path length message..."
% (rospy.get_name()))
path_length = rospy.wait_for_message(
'planning/path_length_threshold', Float64)
path_length = path_length.data
rospy.loginfo(
"%s Plan Trajectory Wrapper: planning path length received." %
(rospy.get_name()))
elif plan_type == 'rr':
path_length = np.inf # set a very large path length because we only want feasible paths
# reshape
# plan
IsInCollision = self.IsInCollision
rospy.loginfo("%s Plan Trajectory Wrapper: start planning..." %
(rospy.get_name()))
# create a simple setup object
start = ob.State(self.space)
# we can pick a random start state...
# ... or set specific values
for k in xrange(len(start_point)):
start[k] = start_point[k]
goal = ob.State(self.space)
for k in xrange(len(goal_point)):
goal[k] = goal_point[k]
def isStateValid(state):
return not IsInCollision(state, obc)
si = ob.SpaceInformation(self.space)
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
si.setup()
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
pdef.setOptimizationObjective(getPathLengthObjective(si, path_length))
ss = allocatePlanner(si, self.planner_name)
ss.setProblemDefinition(pdef)
ss.setup()
plan_time = time.time()
solved = ss.solve(planning_time)
plan_time = time.time() - plan_time
if solved:
rospy.loginfo(
"%s Plan Trajectory Wrapper: OMPL Planner solved successfully."
% (rospy.get_name()))
# obtain planned path
ompl_path = pdef.getSolutionPath().getStates()
solutions = np.zeros((len(ompl_path), 2))
for k in xrange(len(ompl_path)):
solutions[k][0] = float(ompl_path[k][0])
solutions[k][1] = float(ompl_path[k][1])
return plan_time, solutions.tolist()
else:
return np.inf, None
def plan(grid):
#agent and goal are represented by a point(x,y) and radius
global x
global time
x = grid
agent: Agent = grid.agent
goal: Goal = grid.goal
# Construct the robot state space in which we're planning. R2
space = ob.RealVectorStateSpace(2)
# Set the bounds of space to be inside Map
bounds = ob.RealVectorBounds(2)
# projection
pj = ProjectionEvaluator(space)
print('pj=', pj)
pj.setCellSizes(list2vec([1.0, 1.0]))
space.registerDefaultProjection(pj)
# Construct the robot state space in which we're planning.
bounds.setLow(0, 0) #set min x to _ 0
bounds.setHigh(0, grid.size.width) #set max x to _ x width
bounds.setLow(1, 0) #set min y to _ 0
bounds.setHigh(1, grid.size.height) #set max y to _ y height
space.setBounds(bounds)
print("bounds=", bounds.getVolume())
# 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
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# Set robot's starting state to agent's position (x,y) -> e.g. (0,0)
start = ob.State(space)
start[0] = float(agent.position.x)
start[1] = float(agent.position.y)
print(start[0], start[1])
# Set robot's goal state (x,y) -> e.g. (1.0,0.0)
goal = ob.State(space)
goal[0] = float(grid.goal.position.x)
goal[1] = float(grid.goal.position.y)
# 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))
# ******create a planner for the defined space
planner = og.RRTXstatic(si)
# set the problem we are trying to solve for the planner
planner.setProblemDefinition(pdef)
print(planner)
#print('checking projection',planner.getProjectionEvaluator())
print("__________________________________________________________")
# perform setup steps for the planner
planner.setup()
# print the settings for this space
print("space settings\n")
print(si.settings())
print("****************************************************************")
print("problem settings\n")
# print the problem settings
print(pdef)
# attempt to solve the problem within ten second of planning time
solved = planner.solve(time)
# For troubleshooting
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)
#return trace for _find_path_internal method
print(
"$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$"
)
if path is None:
return None
x = pdef.getSolutionPath().printAsMatrix()
lst = x.split()
lst = [int(round(float(x), 0)) for x in lst]
print(x)
print(lst)
trace = []
for i in range(0, len(lst), 2):
trace.append(Point(lst[i], lst[i + 1]))
print(trace)
return trace
else:
print("No solution found")
return None
def OMPL_plan(self, runTime, plannerType, objectiveType, mult):
# Construct the robot state space in which we're planning. We're
# planning in [0,1]x[0,1], a subset of R^2.
if self.dimension == '2D':
space = ob.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,1].
bounds = ob.RealVectorBounds(2)
x_bound = []
y_bound = []
for idx in range(len(self.region)):
x_bound.append(self.region[idx][0])
y_bound.append(self.region[idx][1])
bounds.setLow(0, min(x_bound))
bounds.setHigh(0, max(x_bound))
bounds.setLow(1, min(y_bound))
bounds.setHigh(1, max(y_bound))
self.x_bound = (min(x_bound), max(x_bound))
self.y_bound = (min(y_bound), max(y_bound))
#test = bounds.getDifference()
#test = bounds.check()
space.setBounds(bounds)
# Set our robot's starting state to be the bottom-left corner of
# the environment, or (0,0).
start = ob.State(space)
start[0] = self.start[0]
start[1] = self.start[1]
#start[2] = 0.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] = self.goal[0]
goal[1] = self.goal[1]
#goal[2] = 1.0
else:
pass
# 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 = self.ValidityChecker(si)
validityChecker.setInteractive()
validityChecker.obstacle(self.start, self.goal, self.region,
self.obstacle, self.dimension)
si.setStateValidityChecker(validityChecker)
si.setup()
# 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(self.allocateObjective(
si, objectiveType))
# Construct the optimal planner specified by our command line argument.
# This helper function is simply a switch statement.
optimizingPlanner = self.allocatePlanner(si, plannerType)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setRewireFactor(1.1)
optimizingPlanner.setup()
# attempt to solve the planning problem in the given runtime
solved = optimizingPlanner.solve(runTime)
self.PlannerStates.append(
(validityChecker.getInteractive(), plannerType))
if solved:
# Output the length of the path found
try:
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()))
return self.decodeSolutionPath(
pdef.getSolutionPath().printAsMatrix(), plannerType,
pdef.getSolutionPath().cost(
pdef.getOptimizationObjective()).value(), mult)
except:
print("No solution found.")
pass
else:
print("No solution found.")
def shortcut_path(self, original_path, group_name):
"""
Shortcuts a path, where the path is for a given group name.
Args:
original_path (list of list of float): The path, represented by
a list of individual joint configurations.
group_name (str): The group for which the path was created.
Return:
list of list of float: The shortcutted version of the path.
"""
# obtain obstacle information through rostopic
rospy.loginfo("Shortcut Path Wrapper: waiting for obstacle message...")
obc = rospy.wait_for_message('obstacles/obc', Float64Array2D)
# obs = rospy.wait_for_message('obstacles/obs', Float64Array2D)
obc = [obc_i.values for obc_i in obc.points]
obc = np.array(obc)
#original_path = np.array(original_path)
#print(original_path)
#rospy.loginfo("Shortcut Path Wrapper: obstacle message received.")
#path = plan_general.lvc(original_path, obc, self.IsInCollision, step_sz=rospy.get_param("step_size"))
#path = np.array(path).tolist()
# try using OMPL method for shortcutting
IsInCollision = self.IsInCollision
def isStateValid(state):
return not IsInCollision(state, obc)
si = ob.SpaceInformation(self.space)
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
si.setup()
# use motionValidator
motionVal = ob.DiscreteMotionValidator(si)
path = original_path
states = []
for i in range(len(path)):
state = ob.State(self.space)
for j in range(len(path[i])):
state[j] = path[i][j]
states.append(state)
state_idx = list(range(len(states)))
def lvc(path, state_idx):
# state idx: map from path idx -> state idx
for i in range(0, len(path) - 1):
for j in range(len(path) - 1, i + 1, -1):
ind = 0
ind = motionVal.checkMotion(states[state_idx[i]](),
states[state_idx[j]]())
if ind == True:
pc = []
new_state_idx = []
for k in range(0, i + 1):
pc.append(path[k])
new_state_idx.append(state_idx[k])
for k in range(j, len(path)):
pc.append(path[k])
new_state_idx.append(state_idx[k])
return lvc(pc, new_state_idx)
return path
path = lvc(original_path, state_idx)
return path
"""
pathSimplifier = og.PathSimplifier(si)
rospy.loginfo("Shortcut Path Wrapper: obstacle message received.")
path = original_path
path_ompl = og.PathGeometric(si)
for i in range(len(path)):
state = ob.State(self.space)
for j in range(len(path[i])):
state[j] = path[i][j]
sref = state() # a reference to the state
path_ompl.append(sref)
# simplify by LVC
path_ompl_states = path_ompl.getStates()
solutions = np.zeros((len(path_ompl_states), len(path[0])))
pathSimplifier.collapseCloseVertices(path_ompl)
for i in xrange(path_ompl.getStateCount()):
for j in xrange(len(path[0])):
solutions[i][j] = float(path_ompl.getState(i)[j])
"""
return solutions
space = ob.RealVectorStateSpace(4)
space = myStateSpace()
# space.distance = myDistance
# set lower and upper bounds
bounds = ob.RealVectorBounds(4)
bounds.setLow(0, 0)
bounds.setHigh(0, 1)
bounds.setLow(1, 0)
bounds.setHigh(1, 1)
bounds.setLow(2, -1)
bounds.setHigh(2, 1)
bounds.setLow(3, -1)
bounds.setHigh(3, 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
obs_manager = obs2fcl(obs, dimW)
mychecker = ValidityChecker(si, obs_manager, space)
si.setStateValidityChecker(mychecker)
si.setStateValidityCheckingResolution(0.03)
si.getStateSpace().setStateSamplerAllocator(ob.StateSamplerAllocator(allocMyStateSampler))
si.setup()
# 起点终点
start = ob.State(space)
start.random()
start[0] = float(startend[0])
start[1] = float(startend[1])
start[2] = float(0)
start[3] = float(0)
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 __init__(
self,
fwd_model: BaseDynamicsFunction,
filter_model: BaseFilterFunction,
classifier_model: BaseConstraintChecker,
planner_params: Dict,
action_params: Dict,
scenario: ExperimentScenario,
verbose: int,
):
super().__init__(scenario=scenario,
fwd_model=fwd_model,
filter_model=filter_model)
self.verbose = verbose
self.fwd_model = fwd_model
self.classifier_model = classifier_model
self.params = planner_params
self.action_params = action_params
# TODO: consider making full env params h/w come from elsewhere. res should match the model though.
self.si = ob.SpaceInformation(ob.StateSpace())
self.classifier_rng = np.random.RandomState(0)
self.state_sampler_rng = np.random.RandomState(0)
self.goal_sampler_rng = np.random.RandomState(0)
self.control_sampler_rng = np.random.RandomState(0)
self.scenario = scenario
self.scenario_ompl = get_ompl_scenario(self.scenario)
self.state_space = self.scenario_ompl.make_ompl_state_space(
planner_params=self.params,
state_sampler_rng=self.state_sampler_rng,
plot=self.verbose >= 2)
# self.state_space.sanityChecks()
self.control_space = self.scenario_ompl.make_ompl_control_space(
self.state_space,
self.control_sampler_rng,
action_params=self.action_params)
self.ss = oc.SimpleSetup(self.control_space)
self.si = self.ss.getSpaceInformation()
self.ss.setStatePropagator(oc.AdvancedStatePropagatorFn(
self.propagate))
self.ss.setMotionsValidityChecker(
oc.MotionsValidityCheckerFn(self.motions_valid))
self.ss.setStateValidityChecker(
ob.StateValidityCheckerFn(self.is_valid))
self.cleanup_before_plan(0)
# just for debugging
self.cc = CollisionCheckerClassifier(
[pathlib.Path("cl_trials/cc_baseline/cc")], self.scenario, 0.0)
self.cc_but_accept_count = 0
self.rrt = oc.RRT(self.si)
self.rrt.setIntermediateStates(
True
) # this is necessary, because we use this to generate datasets
self.ss.setPlanner(self.rrt)
self.si.setMinMaxControlDuration(1, 50)
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()
si_vec.append(si_R2)
si_vec.append(si_SE2)
# Define Planning Problem
start_SE2 = ob.State(SE2)
def plan(self, controlled_object, x1, y1, v1, a1):
a1 = (a1 + 2 * np.pi) % (2 * np.pi)
car = deepcopy(controlled_object)
space = ob.RealVectorStateSpace(3)
bounds = ob.RealVectorBounds(3)
bounds.setLow(0, 0)
bounds.setLow(1, 0)
bounds.setLow(2, 0)
bounds.setHigh(0, 1000)
bounds.setHigh(1, 1000)
bounds.setHigh(2, 2 * np.pi)
space.setBounds(bounds)
si = ob.SpaceInformation(space)
validityChecker = ValidityChecker(si, self.state, car)
si.setStateValidityChecker(validityChecker)
#si.setMotionValidator(MotionValidityChecker(si, state, agent_num))
si.setup()
start = ob.State(space)
start[0] = car.x
start[1] = car.y
start[2] = car.angle
goal = ob.State(space)
goal[0] = x1
goal[1] = y1
goal[2] = a1
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
pdef.setOptimizationObjective(ob.PathLengthOptimizationObjective(si))
def objStateCost(state):
return ob.Cost(0)
def objMotionCost(state1, state2):
x, y, a = state1[0], state1[1], state1[2]
xt, yt, at = state2[0], state2[1], state2[2]
yd, xd = yt - y, xt - x
ax = np.arctan(yd / (xd + 0.001))
ax = ax % (np.pi)
if yd < 0:
ax = ax + np.pi
ad = min(np.abs(at - ax), np.abs((ax - at)))
return 10 * ad
pathObj = ob.OptimizationObjective(si)
pathObj.stateCost = objStateCost
pathObj.motionCost = objMotionCost
pathObj.setCostThreshold(1)
#pdef.setOptimizationObjective(pathObj)
optimizingPlanner = og.RRTstar(si)
optimizingPlanner.setRange(self.inter_point_d)
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
solved = optimizingPlanner.solve(self.planning_time)
sol = pdef.getSolutionPath()
if not sol:
return []
sol = sol.printAsMatrix()
s = [[float(j) for j in i.split(" ")[:-1]]
for i in sol.splitlines()][:-1]
v0 = car.vel
num_points = len(s)
for i in range(num_points):
[i].append(v0 * (1 - float(i) / float(num_points)) + v1 *
(float(i) / float(num_points)))
return s
pcp.set_alpha(0.7)
pcc.set_linestyle('dashed')
pcp.set_linestyle('dashed')
pcc.set_edgecolor('#73cdc9')
pcp.set_edgecolor('#73cdc9')
pcc.set_linewidth(0.8)
pcp.set_linewidth(0.8)
pcc.set_joinstyle('round')
pcp.set_joinstyle('round')
else:
pcc.set_color('gray')
pcp.set_color('gray')
ax.add_collection(pcc)
ax.add_collection(pcp)
if args.plannerdata:
pd = ob.PlannerData(ob.SpaceInformation(ob.RealVectorStateSpace(2)))
pds = ob.PlannerDataStorage()
pds.load(args.plannerdata, pd)
pd.computeEdgeWeights()
# Extract the graphml representation of the planner data
graphml = pd.printGraphML()
f = open("graph.graphml", 'w')
f.write(graphml)
f.close()
# Load the graphml data using graph-tool
graph = gt.load_graph("graph.graphml", fmt="xml")
os.remove("graph.graphml")
edgeweights = graph.edge_properties["weight"]
def plan(runTime, plannerType, objectiveType, fname):
# Construct the robot state space in which we're planning. We're
# planning in [0,500]x[0,500], a subset of R^2.
space = ob.RealVectorStateSpace(2)
# Set the bounds of space to be in [0,500].
space.setBounds(0.0, 500.0)
# 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 random
start = ob.State(space)
start[0] = random.randint(0,500)
start[1] = random.randint(0,500)
while (sqrt((start[0]-center[0])*(start[0]-center[0]) + (start[1]-center[0])*(start[1]-center[0])) - radius < 0):
start[0] = random.randint(0,500)
start[1] = random.randint(0,500)
# Set our robot's goal state to be random
goal = ob.State(space)
goal[0] = random.randint(0,500)
goal[1] = random.randint(0,500)
while (sqrt((goal[0]-center[0])*(goal[0]-center[0]) + (goal[1]-center[0])*(goal[1]-center[0])) - radius < 0):
goal[0] = random.randint(0,500)
goal[1] = random.randint(0,500)
# 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 our command line argument.
# This helper function is simply a switch statement.
optimizingPlanner = allocatePlanner(si, plannerType)
# Set the problem instance for our planner to solve
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
# attempt to solve the planning problem in the given runtime
solved = optimizingPlanner.solve(runTime)
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( \
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())
else:
print("No solution found.")
def plan(self, start_tup, goal_tup, turning_radius=10, planning_time=30.0):
def isStateValid(state):
y = int(state.getY())
x = int(state.getX())
if y < 0 or x < 0 or y >= self.map_data.shape[
0] or x >= self.map_data.shape[1]:
return False
return bool(self.map_data[y, x] == 0)
space = ob.DubinsStateSpace(turningRadius=turning_radius)
# Set State Space bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, 0)
bounds.setHigh(0, self.map_data.shape[1])
bounds.setLow(1, 0)
bounds.setHigh(1, self.map_data.shape[0])
space.setBounds(bounds)
si = ob.SpaceInformation(space)
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
si.setStateValidityCheckingResolution(
self.params["validity_resolution"]
) # Set based on thinness of walls in map
si.setValidStateSamplerAllocator(
ob.ValidStateSamplerAllocator(
ob.ObstacleBasedValidStateSampler(si)))
si.setup()
# Set Start and Goal states
start = ob.State(space)
start().setX(start_tup[0])
start().setY(start_tup[1])
start().setYaw(start_tup[2])
goal = ob.State(space)
goal().setX(goal_tup[0])
goal().setY(goal_tup[1])
goal().setYaw(goal_tup[2])
# Set Problem Definition
pdef = ob.ProblemDefinition(si)
pdef.setStartAndGoalStates(start, goal)
optimObj = ob.PathLengthOptimizationObjective(si)
pdef.setOptimizationObjective(optimObj)
# Set up planner
optimizingPlanner = og.BITstar(si)
optimizingPlanner.setProblemDefinition(pdef)
optimizingPlanner.setup()
solved = optimizingPlanner.solve(planning_time)
def solution_path_to_tup(solution_path):
result = []
states = solution_path.getStates()
for state in states:
x = state.getX()
y = state.getY()
yaw = state.getYaw()
result.append((x, y, yaw))
return result
solutionPath = None
if solved:
solutionPath = pdef.getSolutionPath()
solutionPath.interpolate(self.params['interpolation_density'])
solutionPath = solution_path_to_tup(solutionPath)
return bool(solved), solutionPath
