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(-55)
bounds.setHigh(55)
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(0, -3.0)
cbounds.setHigh(0, 3.0)
cbounds.setLow(1, 0.0)
cbounds.setHigh(1, 3.0)
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.0)
start().setY(0.0)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(24.0)
goal().setY(6.0)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# (optionally) set planner
si = ss.getSpaceInformation()
# planner = oc.PDST(si)
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(40.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
with open('path.txt', 'w') as outFile:
outFile.write(ss.getSolutionPath().printAsMatrix())
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)
goal_SE2 = ob.State(SE2)
start_SE2().setXY(0, 0)
start_SE2().setYaw(0)
goal_SE2().setXY(1, 1)
goal_SE2().setYaw(0)
pdef = ob.ProblemDefinition(si_SE2)
pdef.setStartAndGoalStates(start_SE2, goal_SE2)
# Setup Planner using vector of spaceinformationptr
planner = og.QRRT(si_vec)
# Planner can be used as any other OMPL algorithm
planner.setProblemDefinition(pdef)
planner.setup()
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
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(boundLow)
bounds.setHigh(boundHigh)
space.setBounds(bounds)
axes = plt.gca()
axes.set_xlim([boundLow, boundHigh])
axes.set_ylim([boundLow, boundHigh])
# create a simple setup object
ss = og.SimpleSetup(space)
c = Constraint()
ss.setStateValidityChecker(ob.StateValidityCheckerFn(c.isStateValid))
start = ob.State(space)
start()[0] = 0
start()[1] = 0
goal = ob.State(space)
goal()[0] = 2
goal()[1] = 1.5
ss.setStartAndGoalStates(start, goal)
si = ss.getSpaceInformation()
si.setValidStateSamplerAllocator(
ob.ValidStateSamplerAllocator(allocMyValidStateSampler))
planner = og.RRTstar(si)
planner = og.PRM(si)
def to_ob_state(vec: np.ndarray, space: "ob.StateSpace", dim: int):
ob_vec = ob.State(space)
for i in range(dim):
ob_vec[i] = vec[i]
return ob_vec
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 set_start_and_goal(self, start: np.ndarray, goal: np.ndarray):
self.start = ob.State(self.space)
self.goal = ob.State(self.space)
self.array2obRealVector(start, self.start)
self.array2obRealVector(goal, self.goal)
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.")
def set_start_state(self, start_state):
self.start_state = ob.State(self.si.getStateSpace())
for i in xrange(self.si.getStateSpace().getDimension()):
self.start_state[i] = start_state[i]
def plan(p0, pk, fname, time):
# construct the state space we are planning in
space = ob.DubinsStateSpace(1 / Car.max_curvature)
se2_bounds = ob.RealVectorBounds(2)
se2_bounds.setLow(0, 0.)
se2_bounds.setHigh(0, 25.6)
se2_bounds.setLow(1, 0.)
se2_bounds.setHigh(1, 25.6)
se2_bounds.setLow(2, -pi)
se2_bounds.setHigh(2, pi)
space.setBounds(se2_bounds)
# define a simple setup class
ss = og.SimpleSetup(space)
env = Environment(fname)
validator = lambda x: isStateValid(x, env)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(validator))
#ss.setStateValidityChecker()
# create a start state
start = ob.State(space)
start[0] = p0[0]
start[1] = p0[1]
start[2] = p0[2]
# create a goal state
goal = ob.State(space)
goal[0] = pk[0]
goal[1] = pk[1]
goal[2] = pk[2]
print(goal)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.2)
#ss.setStartAndGoalStates(start, goal, 0.4)
si = ss.getSpaceInformation()
si.setStateValidityCheckingResolution(1. / 128)
#planner = og.RRTstar(si)
#planner = og.SST(si)
#planner = og.BFMT(si)
#planner = og.BITstar(si)
#planner = og.InformedRRTstar(si)
planner = og.ABITstar(si)
#planner = og.AITstar(si)
#planner = og.BKPIECE1(si)
#planner = og.TRRT(si)
#planner = og.RRTConnect(si)
ss.setPlanner(planner)
# attempt to solve the problem
solved = ss.solve(time)
#solved = ss.solve(5.5)
#solved = ss.solve(10.0)
valid = int(ss.haveExactSolutionPath())
total_dtheta = -1.
length = -1.
total_k = -1.
if valid == 1:
path = ss.getSolutionPath()
length = path.length()
path.interpolate(6 * 128)
path = np.array([[f[0][0], f[0][1], f[1].value] for f in path.getStates()])
theta = path[:, 2]
dtheta = np.abs(np.diff(theta))
x = path[:, 0]
y = path[:, 1]
print(x[0], x[-1])
print(y[0], y[-1])
print(theta[0], theta[-1])
dist = np.sqrt(np.diff(x)**2 + np.diff(y)**2)
dist = np.sum(dist)
print(dist)
#dist = np.abs(np.diff(dist))
k = dtheta / dist
total_k = np.mean(k)
#total_dtheta = np.sum(dtheta)
#print()
#print()
#print("XD")
#print()
#print()
#plt.imshow(env.map)
#cp = calculate_car_crucial_points(path[:, 0], path[:, 1], path[:, 2])
#for i in range(5):
# x, y = transform_to_img(cp[:, i, 0], cp[:, i, 1], env)
# plt.plot(y, x)
#x, y = transform_to_img(np.array(p0[0]), np.array(p0[1]), env)
#plt.plot(y, x, 'gx')
#x, y = transform_to_img(np.array(pk[0]), np.array(pk[1]), env)
plt.plot(y, x, 'rx')
plt.show()
time_limit = time
def plan(self, planning_query: PlanningQuery):
self.cleanup_before_plan(planning_query.seed)
self.environment = planning_query.environment
self.goal_region = self.scenario_ompl.make_goal_region(
self.si,
rng=self.goal_sampler_rng,
params=self.params,
goal=planning_query.goal,
plot=self.verbose >= 2)
# create start and goal states
start_state = planning_query.start
start_state['stdev'] = np.array([0.0])
start_state['num_diverged'] = np.array([0.0])
self.start_state = start_state
ompl_start_scoped = ob.State(self.state_space)
self.scenario_ompl.numpy_to_ompl_state(start_state,
ompl_start_scoped())
# visualization
self.scenario.reset_planning_viz()
self.scenario.plot_environment_rviz(planning_query.environment)
self.scenario.plot_start_state(start_state)
self.scenario.plot_goal_rviz(planning_query.goal,
self.params['goal_params']['threshold'])
self.ss.clear()
self.ss.setStartState(ompl_start_scoped)
self.ss.setGoal(self.goal_region)
self.ptc = TimeoutOrNotProgressing(self,
self.params['termination_criteria'],
self.verbose)
# START TIMING
t0 = time.time()
# acutally run the planner
ob_planner_status = self.ss.solve(self.ptc)
# END TIMING
planning_time = time.time() - t0
# handle results and cleanup
planner_status = interpret_planner_status(ob_planner_status, self.ptc)
if planner_status == MyPlannerStatus.Solved:
ompl_path = self.ss.getSolutionPath()
actions, planned_path = self.convert_path(ompl_path)
planner_data = ob.PlannerData(self.si)
self.rrt.getPlannerData(planner_data)
tree = planner_data_to_json(planner_data, self.scenario_ompl)
elif planner_status == MyPlannerStatus.Timeout:
# Use the approximate solution, since it's usually pretty darn close, and sometimes
# our goals are impossible to reach so this is important to have
try:
ompl_path = self.ss.getSolutionPath()
actions, planned_path = self.convert_path(ompl_path)
except RuntimeError:
rospy.logerr(
"Timeout before any edges were added. Considering this as Not Progressing."
)
planner_status = MyPlannerStatus.NotProgressing
actions = []
tree = {}
planned_path = [start_state]
else: # if no exception was raised
planner_data = ob.PlannerData(self.si)
self.rrt.getPlannerData(planner_data)
tree = planner_data_to_json(planner_data, self.scenario_ompl)
elif planner_status == MyPlannerStatus.Failure:
rospy.logerr(f"Failed at starting state: {start_state}")
tree = {}
actions = []
planned_path = [start_state]
elif planner_status == MyPlannerStatus.NotProgressing:
tree = {}
actions = []
planned_path = [start_state]
print()
return PlanningResult(status=planner_status,
path=planned_path,
actions=actions,
time=planning_time,
tree=tree)
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 setGoal(self, x=0, y=0, yaw=0, threshold=0.1):
self.goal = ob.State(self.state_space)
self.goal().setXY(x, y)
self.goal().setYaw(yaw)
self.car.setGoalState(self.goal, threshold)
def setStart(self, x=0, y=0, yaw=0):
self.start = ob.State(self.state_space)
self.start().setXY(x, y)
self.start().setYaw(yaw)
self.car.setStartState(self.start)
test_y = []
for k in range(len(new_mat)):
test_x.append(new_mat[k][0:2][0][0])
test_y.append(new_mat[k][0:2][1][0])
newPoseX.append(test_x)
newPoseY.append(test_y)
SE2 = ob.SE2StateSpace()
setup = og.SimpleSetup(SE2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-bound_limit)
bounds.setHigh(bound_limit)
SE2.setBounds(bounds)
# define start state
newRobotId = ID[-2]
start = ob.State(SE2)
start().setX(poseX[-2])
start().setY(poseY[-2])
start().setYaw(poseTh[-2])
# define goal state
goal = ob.State(SE2)
goal().setX(poseX[-1])
goal().setY(poseY[-1])
goal().setYaw(poseTh[-1])
print poseX, poseX, poseTh
setup.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# set the start & goal states
setup.setStartAndGoalStates(start, goal, .1)
# set the planner
def set_goal_state(self, goal_state):
self.goal_state = ob.State(self.si.getStateSpace())
for i in xrange(self.si.getStateSpace().getDimension()):
self.goal_state[i] = goal_state[i]
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
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 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 execute(self, env, time, pathLength, show = False):
result = True
sSpace = MyStateSpace()
sbounds = ob.RealVectorBounds(4)
# dimension 0 (x) spans between [0, width)
# dimension 1 (y) spans between [0, height)
# since sampling is continuous and we round down, we allow values until
# just under the max limit
# the resolution is 1.0 since we check cells only
sbounds.low = ou.vectorDouble()
sbounds.low.extend([0.0, 0.0, -MAX_VELOCITY, -MAX_VELOCITY])
sbounds.high = ou.vectorDouble()
sbounds.high.extend([float(env.width) - 0.000000001,
float(env.height) - 0.000000001,
MAX_VELOCITY, MAX_VELOCITY])
sSpace.setBounds(sbounds)
cSpace = oc.RealVectorControlSpace(sSpace, 2)
cbounds = ob.RealVectorBounds(2)
cbounds.low[0] = -MAX_VELOCITY
cbounds.high[0] = MAX_VELOCITY
cbounds.low[1] = -MAX_VELOCITY
cbounds.high[1] = MAX_VELOCITY
cSpace.setBounds(cbounds)
ss = oc.SimpleSetup(cSpace)
isValidFn = ob.StateValidityCheckerFn(partial(isValid, env.grid))
ss.setStateValidityChecker(isValidFn)
propagator = MyStatePropagator(ss.getSpaceInformation())
ss.setStatePropagator(propagator)
planner = self.newplanner(ss.getSpaceInformation())
ss.setPlanner(planner)
# the initial state
start = ob.State(sSpace)
start()[0] = env.start[0]
start()[1] = env.start[1]
start()[2] = 0.0
start()[3] = 0.0
goal = ob.State(sSpace)
goal()[0] = env.goal[0]
goal()[1] = env.goal[1]
goal()[2] = 0.0
goal()[3] = 0.0
ss.setStartAndGoalStates(start, goal, 0.05)
startTime = clock()
if ss.solve(SOLUTION_TIME):
elapsed = clock() - startTime
time = time + elapsed
if show:
print('Found solution in %f seconds!' % elapsed)
path = ss.getSolutionPath()
path.interpolate()
if not path.check():
return (False, time, pathLength)
pathLength = pathLength + path.length()
if show:
print(env, '\n')
temp = copy.deepcopy(env)
for i in range(len(path.states)):
x = int(path.states[i][0])
y = int(path.states[i][1])
if temp.grid[x][y] in [0,2]:
temp.grid[x][y] = 2
else:
temp.grid[x][y] = 3
print(temp, '\n')
else:
result = False
return (result, time, pathLength)
robot_move = RobotMove()
iksolver = IKsolver(limb)
iksolver.reset()
iksolver.update_human()
planner = MyRRT(ndof, iksolver, 0.1)
# get start joint state
joint_angles = iksolver._right_limb_interface.joint_angles()
joint_names = iksolver._right_limb_interface.joint_names()
start_vector = np.empty(ndof)
for idx, name in enumerate(joint_names):
start_vector[idx] = joint_angles[name]
start = ob.State(planner.state_space)
goal = ob.State(planner.state_space)
# start_vector = np.array([0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0])
# goal_vector = np.array([-0.3, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0])
for i in range(ndof):
start[i] = start_vector[i]
# goal[i] = goal_vector[i]
# ============== plan ===============
result_path = planner.solve(start, goal, 200)
path = []
joints = np.empty(ndof)
for i in range(result_path.getStateCount()):
for j in range(ndof):
joints[j] = result_path.getStates()[i][j]
def plan():
# construct the state space we are planning in
ripl = app.SE2RigidBodyPlanning()
ripl.setEnvironmentMesh('./mesh/UniqueSolutionMaze_env.dae')
ripl.setRobotMesh('./mesh/car2_planar_robot.dae')
# space = ob.SE2StateSpace()
space = ripl.getStateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-55)
bounds.setHigh(55)
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(0, -3.0)
cbounds.setHigh(0, 3.0)
cbounds.setLow(1, 3.0)
cbounds.setHigh(1, 3.0)
cspace.setBounds(cbounds)
# define a simple setup class
ss = oc.SimpleSetup(cspace)
# ss.setStateValidityChecker(ob.StateValidityCheckerFn( \
# partial(isStateValid, ss.getSpaceInformation())))
sp_info = ss.getSpaceInformation()
extractor = ripl.getGeometricStateExtractor()
enabled = ripl.isSelfCollisionEnabled()
checker = ripl.allocStateValidityChecker(sp_info, extractor, enabled)
ss.setStateValidityChecker(checker)
ss.setStatePropagator(oc.StatePropagatorFn(propagate))
# create a start state
start = ob.State(space)
start().setX(3)
start().setY(-3)
start().setYaw(0.0)
# create a goal state
goal = ob.State(space)
goal().setX(45)
goal().setY(25)
goal().setYaw(0.0)
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# (optionally) set planner
si = ss.getSpaceInformation()
# planner = oc.PDST(si)
# 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())
with open('path.txt', 'w') as outFile:
outFile.write(ss.getSolutionPath().printAsMatrix())
def planBITstar(
q_st,
q_g,
res,
):
# create an SE2 state space
space = ob.RealVectorStateSpace(2)
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
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 a random start state
start = ob.State(space)
start[0] = q_st[0]
start[1] = q_st[1]
# create a random 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()
# 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")
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 __init__(self,
map,
pose,
target,
max_time,
objective='duration',
planner=og.RRTstar,
threshold=0.9,
tolerance=0.3,
planner_options={'range': 0.45}):
space = ob.RealVectorStateSpace(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(0)
bounds.setHigh(map.size)
space.setBounds(bounds)
s = ob.State(space)
t = ob.State(space)
self.theta = pose[-1]
for arg, state in zip([pose[:2], target], [s, t]):
for i, x in enumerate(arg):
state[i] = x
ss = og.SimpleSetup(space)
ss.setStartAndGoalStates(s, t, tolerance)
si = ss.getSpaceInformation()
ss.setStateValidityChecker(
ob.StateValidityCheckerFn(partial(valid, si)))
self.motion_val = EstimatedMotionValidator(si,
map,
threshold,
theta=self.theta)
si.setMotionValidator(self.motion_val)
if objective == 'duration':
self.do = DurationObjective(si, map, self.theta)
elif objective == 'survival':
self.do = SurvivalObjective(si, map, self.theta)
elif objective == 'balanced':
self.do = getBalancedObjective1(si, map, self.theta)
else: # the objective is the euclidean path length
self.do = ob.PathLengthOptimizationObjective(si)
ss.setOptimizationObjective(self.do)
# RRTstar BITstar BFMT RRTsharp
p = planner(si)
if 'range' in planner_options:
try:
p.setRange(planner_options.get('range'))
except AttributeError:
pass
ss.setPlanner(p)
ss.setup()
self.ss = ss
self.max_time = max_time
self.map = map
self.s = pose
self.t = list(target) + [pose[-1]]
self.planner_options = planner_options
if planner == og.RRTstar:
self.planner_name = 'RRT*'
else:
self.planner_name = ''
self.threshold = threshold
self.tolerance = tolerance
self.comp_duration = 0
self.objective = objective
def _get_state(self, coordinates):
s = ob.State(self.space)
s.get().setX(coordinates[0])
s.get().setY(coordinates[1])
return s
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
from ompl import base as ob
from ompl import app as oa
except ImportError:
sys.path.insert(0, join(ompl_app_root, 'ompl/py-bindings'))
from ompl import base as ob
from ompl import app as oa
# plan in SE(3)
setup = oa.SE3RigidBodyPlanning()
# load the robot and the environment
setup.setRobotMesh(join(ompl_resources_dir, 'cubicles_robot.dae'))
setup.setEnvironmentMesh(join(ompl_resources_dir, 'cubicles_env.dae'))
# define start state
start = ob.State(setup.getSpaceInformation())
start().setX(-4.96)
start().setY(-40.62)
start().setZ(70.57)
start().rotation().setIdentity()
goal = ob.State(setup.getSpaceInformation())
goal().setX(200.49)
goal().setY(-40.62)
goal().setZ(70.57)
goal().rotation().setIdentity()
# set the start & goal states
setup.setStartAndGoalStates(start, goal)
# setting collision checking resolution to 1% of the space extent
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 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.")
sample = sample.astype(float)
for i in range(7):
state[i] = sample[0, i]
# return an obstacle-based sampler
def allocSelfCollisionFreeStateSampler(si):
# we can perform any additional setup / configuration of a sampler here,
# but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.
return CollisionFreeStateSampler(si)
if __name__=="__main__":
dof = 7
space = ob.RealVectorStateSpace(dof)
# set the bounds
bounds = ob.RealVectorBounds(dof)
for key, value in JOINT_LIMITS.items():
i = JOINT_INDEX[key]
bounds.setLow(i, value[0])
bounds.setHigh(i, value[1])
print("Setting bound for the %sth joint: %s, bound: %s" % (i, key, value))
space.setBounds(bounds)
# ss = og.SimpleSetup(space)
#
# si = ss.getSpaceInformation()
sampler = allocSelfCollisionFreeStateSampler(space)
state = ob.State(space)
sampler.sampleUniform(state)
print("The state is: %s" % state)
