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 plan():
# create an R^3 state space
space = ob.RealVectorStateSpace(3)
# set lower and upper bounds
bounds = ob.RealVectorBounds(3)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
start = ob.State(space)
start()[0] = -1.
start()[1] = -1.
start()[2] = -1.
goal = ob.State(space)
goal()[0] = 1.
goal()[1] = 1.
goal()[2] = 1.
ss.setStartAndGoalStates(start, goal, .05)
# set the planner
planner = RandomWalkPlanner(ss.getSpaceInformation())
ss.setPlanner(planner)
result = ss.solve(10.0)
if result:
if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION:
print("Solution is approximate")
# try to shorten the path
ss.simplifySolution()
# print the simplified path
print(ss.getSolutionPath())
def __init__(self, env):
self.space = ob.CompoundStateSpace()
self.setup = og.SimpleSetup(self.space)
bounds = ob.RealVectorBounds(1)
bounds.setLow(0)
bounds.setHigh(float(env.width) - 0.000000001)
self.m1 = mySpace1()
self.m1.setBounds(bounds)
bounds.setHigh(float(env.height) - 0.000000001)
self.m2 = mySpace1()
self.m2.setBounds(bounds)
self.space.addSubspace(self.m1, 1.0)
self.space.addSubspace(self.m2, 1.0)
isValidFn = ob.StateValidityCheckerFn(partial(isValid, env.grid))
self.setup.setStateValidityChecker(isValidFn)
state = ob.CompoundState(self.space)
state()[0][0] = env.start[0]
state()[1][0] = env.start[1]
self.start = ob.State(state)
gstate = ob.CompoundState(self.space)
gstate()[0][0] = env.goal[0]
gstate()[1][0] = env.goal[1]
self.goal = ob.State(gstate)
self.setup.setStartAndGoalStates(self.start, self.goal)
def plan():
# construct the state space we are planning in
space = ob.SE2StateSpace()
# set the bounds for R^3 portion of SE(3)
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, -40)
bounds.setHigh(0, 40)
bounds.setLow(1, 0)
bounds.setHigh(1, 90)
space.setBounds(bounds)
# define a simple setup class
ss = og.SimpleSetup(space)
# create a start state
start = ob.State(space)
start().setX(0)
start().setY(0)
start().setYaw(0)
# start().setZ(-9)
# start().rotation().setIdentity()
# create a goal state
goal = ob.State(space)
goal().setX(-25)
goal().setY(60)
goal().setYaw(0)
# goal().setZ(-9)
# goal().rotation().setIdentity()
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# Lets use PRM. It will have interesting PlannerData
planner = og.RRTstar(ss.getSpaceInformation())
ss.setPlanner(planner)
ss.setup()
# attempt to solve the problem
# To change time change value given to ss.solve
solved = ss.solve(1.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath())
# Extracting planner data from most recent solve attempt
pd = ob.PlannerData(ss.getSpaceInformation())
ss.getPlannerData(pd)
# Computing weights of all edges based on state space distance
pd.computeEdgeWeights()
path = get_path(ss)
draw_graph(path)
plt.show()
def 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 plan():
# create an Vector state space
space = ob.RealVectorStateSpace(SPACE_DIM)
# # set lower and upper bounds
bounds = ob.RealVectorBounds(SPACE_DIM)
for i in range(SPACE_DIM - 3):
bounds.setLow(i, min_bound)
bounds.setHigh(i, max_bound)
for i in range(SPACE_DIM - 3):
bounds.setLow(i + 3, -np.pi)
bounds.setHigh(i + 3, np.pi)
space.setBounds(bounds)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
# set planner
planner = og.RRTstar(ss.getSpaceInformation())
ss.setPlanner(planner)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
start = ob.State(space)
# start.random()
start_state = start.get()
for i in range(SPACE_DIM):
start_state[i] = start_s[i]
# print(start_state[i])
goal = ob.State(space)
# goal.random()
goal_state = goal.get()
for i in range(SPACE_DIM):
goal_state[i] = goal_s[i]
# print(goal_state[i])
ss.setStartAndGoalStates(start, goal)
# default parameters
solved = ss.solve(4.0)
if solved:
# パスの簡単化を実施
ss.simplifySolution()
# 結果を取得
path_result = ss.getSolutionPath()
print(path_result)
si = ss.getSpaceInformation()
pdata = ob.PlannerData(si)
ss.getPlannerData(pdata)
space = path_result.getSpaceInformation().getStateSpace()
plot_result(space, path_result, pdata)
def setBounds(self, high=None, low=None, dim=2):
if high is None:
high = [10.0, 10.0]
if low is None:
low = [-10.0, -10.0]
print "setting bounds to", high, low
bounds = ob.RealVectorBounds(dim)
(bounds.low[0], bounds.low[1]) = low
(bounds.high[0], bounds.high[1]) = high
self.state_space.setBounds(bounds)
def space_creation(self):
"""
Function of creating a configuration space
"""
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(const.LOW_BOUNDS)
bounds.setHigh(const.HIGH_BOUNDS)
space.setBounds(bounds)
return space
def space_creation(self):
"""
Function of creating a configuration space
"""
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(self.low_bounds)
bounds.setHigh(self.high_bounds)
space.setBounds(bounds)
return space
def __init__(self):
## add OMPL setting for different environments
self.env_name = rospy.get_param('env_name')
self.planner = rospy.get_param('planner_name')
if self.env_name == 's2d':
#data_loader = data_loader_2d
IsInCollision = plan_s2d.IsInCollision
# create an SE2 state space
space = ob.RealVectorStateSpace(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-20)
bounds.setHigh(20)
space.setBounds(bounds)
elif self.env_name == 'c2d':
#data_loader = data_loader_2d
IsInCollision = plan_c2d.IsInCollision
# create an SE2 state space
space = ob.RealVectorStateSpace(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-20)
bounds.setHigh(20)
space.setBounds(bounds)
elif self.env_name == 'r2d':
#data_loader = data_loader_r2d
IsInCollision = plan_r2d.IsInCollision
# create an SE2 state space
space = ob.SE2StateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(-20)
bounds.setHigh(20)
space.setBounds(bounds)
elif self.env_name == 'r3d':
#data_loader = data_loader_r3d
IsInCollision = plan_r3d.IsInCollision
# create an SE2 state space
space = ob.RealVectorStateSpace(3)
bounds = ob.RealVectorBounds(3)
bounds.setLow(-20)
bounds.setHigh(20)
space.setBounds(bounds)
self.IsInCollision = IsInCollision
self.space = space
def setSpace(self):
self.space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
self.bounds = ob.RealVectorBounds(2)
if not self.costMap:
self.bounds.setLow(-8)
self.bounds.setHigh(20)
else:
ox = self.costMap.getOriginX()
oy = self.costMap.getOriginY()
size_x = self.costMap.getSizeInMetersX()
size_y = self.costMap.getSizeInMetersY()
low = min(ox, oy)
high = max(ox + size_x, oy + size_y)
print("low", low)
print("high", high)
self.bounds.setLow(low)
self.bounds.setHigh(high)
self.space.setBounds(self.bounds)
# create a control space
self.cspace = oc.RealVectorControlSpace(self.space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -1.5)
cbounds.setHigh(0, 1.5)
cbounds.setLow(1, -3)
cbounds.setHigh(1, 3)
self.cspace.setBounds(cbounds)
# define a simple setup class
self.ss = oc.SimpleSetup(self.cspace)
validityChecker = ob.StateValidityCheckerFn(
partial(self.isStateValid, self.ss.getSpaceInformation()))
self.ss.setStateValidityChecker(validityChecker)
ode = oc.ODE(self.kinematicCarODE)
odeSolver = oc.ODEBasicSolver(self.ss.getSpaceInformation(), ode)
propagator = oc.ODESolver.getStatePropagator(odeSolver)
self.ss.setStatePropagator(propagator)
def setSpace(self):
# construct the state space we are planning in
self.space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
self.bounds = ob.RealVectorBounds(2)
if not self.costMap:
self.bounds.setLow(-8)
self.bounds.setHigh(20)
else:
ox = self.costMap.getOriginX()
oy = self.costMap.getOriginY()
size_x = self.costMap.getSizeInMetersX()
size_y = self.costMap.getSizeInMetersY()
low = min(ox, oy)
high = max(ox + size_x, oy + size_y)
print("low", low)
print("high", high)
#self.bounds.setLow(low)
#self.bounds.setHigh(high)
self.bounds.setLow(0, ox)
self.bounds.setHigh(0, ox + size_x)
self.bounds.setLow(1, oy)
self.bounds.setHigh(1, oy + size_y)
self.space.setBounds(self.bounds)
# create a control space
self.cspace = oc.RealVectorControlSpace(self.space, 2)
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, -1.5)
cbounds.setHigh(0, 1.5)
cbounds.setLow(1, -3)
cbounds.setHigh(1, 3)
self.cspace.setBounds(cbounds)
# define a simple setup class
self.ss = oc.SimpleSetup(self.cspace)
self.ss.setStateValidityChecker(
ob.StateValidityCheckerFn(
partial(self.isStateValid, self.ss.getSpaceInformation())))
self.ss.setStatePropagator(oc.StatePropagatorFn(self.propagate))
def plan(samplerIndex):
# construct the state space we are planning in
space = ob.RealVectorStateSpace(3)
# set the bounds
bounds = ob.RealVectorBounds(3)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# define a simple setup class
ss = og.SimpleSetup(space)
# set state validity checking for this space
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# create a start state
start = ob.State(space)
start[0] = 0
start[1] = 0
start[2] = 0
# create a goal state
goal = ob.State(space)
goal[0] = 0
goal[1] = 0
goal[2] = 1
# set the start and goal states;
ss.setStartAndGoalStates(start, goal)
# set sampler (optional; the default is uniform sampling)
si = ss.getSpaceInformation()
if samplerIndex == 1:
# use obstacle-based sampling
si.setValidStateSamplerAllocator(
ob.ValidStateSamplerAllocator(allocOBValidStateSampler))
elif samplerIndex == 2:
# use my sampler
si.setValidStateSamplerAllocator(
ob.ValidStateSamplerAllocator(allocMyValidStateSampler))
# create a planner for the defined space
planner = og.PRM(si)
ss.setPlanner(planner)
# attempt to solve the problem within ten seconds of planning time
solved = ss.solve(10.0)
if solved:
print("Found solution:")
# print the path to screen
print(ss.getSolutionPath())
else:
print("No solution found")
def __init__(self, state_space, ndof):
self.ndof = ndof
super(MyControlSpace, self).__init__(state_space, self.ndof)
joint_velocity_limit_bounds = ob.RealVectorBounds(self.ndof)
joint_velocities = [3.15, 3.15, 3.15, 3.2, 3.2, 3.2, 3.2]
for i in range(self.ndof):
joint_velocity_limit_bounds.setLow(i, -joint_velocities[i])
joint_velocity_limit_bounds.setHigh(i, joint_velocities[i])
self.setBounds(joint_velocity_limit_bounds)
def plan():
# construct the state space we are planning in
space = ob.SE3StateSpace()
# set the bounds for R^3 portion of SE(3)
bounds = ob.RealVectorBounds(3)
bounds.setLow(-10)
bounds.setHigh(10)
space.setBounds(bounds)
# define a simple setup class
ss = og.SimpleSetup(space)
# create a start state
start = ob.State(space)
start().setX(-9)
start().setY(-9)
start().setZ(-9)
start().rotation().setIdentity()
# create a goal state
goal = ob.State(space)
goal().setX(-9)
goal().setY(9)
goal().setZ(-9)
goal().rotation().setIdentity()
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# Lets use PRM. It will have interesting PlannerData
planner = og.PRM(ss.getSpaceInformation())
ss.setPlanner(planner)
ss.setup()
# attempt to solve the problem
solved = ss.solve(20.0)
if solved:
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath())
# Extracting planner data from most recent solve attempt
pd = ob.PlannerData(ss.getSpaceInformation())
ss.getPlannerData(pd)
# Computing weights of all edges based on state space distance
pd.computeEdgeWeights()
if graphtool:
useGraphTool(pd)
def __init__(self, point_robot_manager):
self.point_robot_manager = point_robot_manager
# create an SE2 state space
space = ob.SE2StateSpace()
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, -point_robot_manager.dimension_length)
bounds.setLow(1, -point_robot_manager.dimension_length)
bounds.setHigh(0, point_robot_manager.dimension_length)
bounds.setHigh(1, point_robot_manager.dimension_length)
space.setBounds(bounds)
self.space = space
def __init__(self, robot_env: DifferentialDriveEnv):
self.robot_env = robot_env
bounds = self.robot_env.get_bounds()
self.state_bounds = bounds['state_bounds']
self.control_bounds = bounds['control_bounds']
# add the state space
space = ob.RealVectorStateSpace(len(self.state_bounds))
bounds = ob.RealVectorBounds(len(self.state_bounds))
# add state bounds
for k, v in enumerate(self.state_bounds):
bounds.setLow(k, float(v[0]))
bounds.setHigh(k, float(v[1]))
space.setBounds(bounds)
self.space = space
# add the control space
cspace = oc.RealVectorControlSpace(space, len(self.control_bounds))
# set the bounds for the control space
cbounds = ob.RealVectorBounds(len(self.control_bounds))
for k, v in enumerate(self.control_bounds):
cbounds.setLow(k, float(v[0]))
cbounds.setHigh(k, float(v[1]))
cspace.setBounds(cbounds)
self.cspace = cspace
# define a simple setup class
self.ss = oc.SimpleSetup(cspace)
self.ss.setStateValidityChecker(ob.StateValidityCheckerFn(
partial(self.isStateValid, self.ss.getSpaceInformation())))
self.ss.setStatePropagator(oc.StatePropagatorFn(self.propagate))
# set the planner
si = self.ss.getSpaceInformation()
planner = oc.SST(si)
self.ss.setPlanner(planner)
si.setPropagationStepSize(.1)
self.start = None
self.goal = None
self.planning_time = 0.0
self.path = None
def rrt_star_traj(start_conf, goal_conf, env_params, env, robot,
planner_params, obs_params):
#RRTstar setup
space = ob.RealVectorStateSpace(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(env_params['x_lims'][0])
bounds.setHigh(env_params['x_lims'][1])
space.setBounds(bounds)
init_planner = RRTStar(space, bounds, env, robot, planner_params, 0.0)
init_path = init_planner.plan(start_conf, goal_conf, 4.0)
th_init = path_to_traj_avg_vel(init_path, planner_params['total_time_sec'],
planner_params['dof'])
return th_init
def initialize_space():
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)
return space
def plan():
# construct the state space we are planning in
ss = app.KinematicCarPlanning()
ss.setEnvironmentMesh('./mesh/UniqueSolutionMaze_env.dae')
ss.setRobotMesh('./mesh/car2_planar_robot.dae')
# space = ob.SE2StateSpace()
cspace = ss.getControlSpace()
# 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)
# create a start state
start = ob.State(ss.getSpaceInformation())
start().setX(3)
start().setY(-3)
start().setYaw(0.0)
# create a goal state
goal = ob.State(ss.getSpaceInformation())
goal().setX(45)
goal().setY(25)
goal().setYaw(0.0)
#si = ss.getSpaceInformation()
information = ss.getSpaceInformation()
information.setStatePropagator(oc.StatePropagatorFn(propagate))
# set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05)
# 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 configure(ss):
# create a start state
cspace = ss.getControlSpace()
# set the bounds for the control space
cbounds = ob.RealVectorBounds(2)
cbounds.setLow(0, 0.0)
cbounds.setHigh(0, 3.0)
cbounds.setLow(1, 0.0)
cbounds.setHigh(1, 3.0)
cspace.setBounds(cbounds)
si = ss.getSpaceInformation()
si.setStatePropagator(oc.StatePropagatorFn(propagate))
def __init__(self, ndof):
self.ndof = ndof
super(MyStateSpace, self).__init__(self.ndof)
lower_limits = [
-1.7016, -2.147, -3.0541, -0.05, -3.059, -1.5707, -3.509
]
upper_limits = [1.7016, 1.047, 3.0541, 2.618, 3.059, 2.094, 3.509]
joint_bounds = ob.RealVectorBounds(self.ndof)
for i in range(self.ndof):
joint_bounds.setLow(i, lower_limits[i])
joint_bounds.setHigh(i, upper_limits[i])
self.setBounds(joint_bounds)
self.setup()
def newplanner(self, si):
spacebounds = si.getStateSpace().getBounds()
bounds = ob.RealVectorBounds(2)
bounds.setLow(0, spacebounds.low[0]);
bounds.setLow(1, spacebounds.low[1]);
bounds.setHigh(0, spacebounds.high[0]);
bounds.setHigh(1, spacebounds.high[1]);
# Create a 10x10 grid decomposition for Syclop
decomp = SyclopDecomposition(10, bounds)
planner = oc.SyclopEST(si, decomp)
# Set syclop parameters conducive to a tiny workspace
planner.setNumFreeVolumeSamples(1000)
planner.setNumRegionExpansions(10)
planner.setNumTreeExpansions(5)
return planner
def __init__(self, space):
super(MyProjection, self).__init__(space)
## \brief Convenience parameter for defining which subspace our
# projection uses; in this case, the position space of the first object
self.subspaceIndex = 4 * 0 + 0
## \brief Bounds for the projection space (used by OMPL)
self.bounds_ = ob.RealVectorBounds(self.getDimension())
for i in range(self.getDimension()):
self.bounds_.low[i] = space.getSubspace(
self.subspaceIndex).getBounds().low[i]
self.bounds_.high[i] = space.getSubspace(
self.subspaceIndex).getBounds().high[i]
self.defaultCellSizes()
def plan():
N = 10
# create an R^2 state space
space = ob.RealVectorStateSpace(2)
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(0)
bounds.setHigh(N)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
start = ob.State(space)
start[0] = 0 # random.randint(0, int(N / 2))
start[1] = 0 # random.randint(0, int(N / 2))
goal = ob.State(space)
goal[0] = N # random.randint(int(N / 2), N)
goal[1] = N # random.randint(int(N / 2), N)
ss.setStartAndGoalStates(start, goal)
planner = Astar(ss.getSpaceInformation())
ss.setPlanner(planner)
result = ss.solve(.01)
if result:
if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION:
print("Solution is approximate")
matrix = ss.getSolutionPath().printAsMatrix()
print(matrix)
verts = []
for line in matrix.split("\n"):
x = []
for item in line.split():
x.append(float(item))
if len(x) is not 0:
verts.append(list(x))
# print(verts)
plt.axis([0, N, 0, N])
x = []
y = []
for i in range(0, len(verts)):
x.append(verts[i][0])
y.append(verts[i][1])
# plt.plot(verts[i][0], verts[i][1], 'r*-')
plt.plot(x, y, 'ro-')
plt.show()
def dynamicCarDemo():
setup = oa.DynamicCarPlanning()
# comment out next two lines if you want to ignore obstacles
setup.setRobotMesh('2D/car1_planar_robot.dae')
setup.setEnvironmentMesh('2D/BugTrap_planar_env.dae')
# plan for dynamic car in SE(2)
stateSpace = setup.getStateSpace()
# set the bounds for the R^2 part of SE(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-10)
bounds.setHigh(10)
stateSpace.getSubspace(0).setBounds(bounds)
# define start state
start = ob.State(stateSpace)
start[0] = 6.
start[1] = 12.
start[2] = start[3] = start[4] = 0.
# define goal state
goal = ob.State(stateSpace)
goal[0] = -39.
goal[1] = goal[2] = goal[3] = goal[4] = 0.
# set the start & goal states
setup.setStartAndGoalStates(start, goal, 3.)
# set the planner
planner = oc.RRT(setup.getSpaceInformation())
#setup.setPlanner(planner)
# try to solve the problem
print("\n\n***** Planning for a %s *****\n" % setup.getName())
print(setup)
if setup.solve(40):
# print the (approximate) solution path: print states along the path
# and controls required to get from one state to the next
path = setup.getSolutionPath()
#path.interpolate(); # uncomment if you want to plot the path
print(path.printAsMatrix())
if not setup.haveExactSolutionPath():
print("Solution is approximate. Distance to actual goal is %g" %
setup.getProblemDefinition().getSolutionDifference())
def kinematicCarDemo():
setup = oa.KinematicCarPlanning()
# comment out next two lines if you want to ignore obstacles
setup.setRobotMesh("2D/car2_planar_robot.dae")
setup.setEnvironmentMesh("2D/Maze_planar_env.dae")
SE2 = setup.getStateSpace()
bounds = ob.RealVectorBounds(2)
bounds.setLow(-55)
bounds.setHigh(55)
SE2.setBounds(bounds)
# define start state
start = ob.State(SE2)
start().setX(0.01)
start().setY(-0.15)
start().setYaw(0)
# define goal state
goal = ob.State(SE2)
goal().setX(45.01)
goal().setY(-0.15)
goal().setYaw(0.0)
# set the start & goal states
setup.setStartAndGoalStates(start, goal, 1.0)
# set the planner
planner = oc.RRT(setup.getSpaceInformation())
setup.setPlanner(planner)
# try to solve the problem
print("\n\n***** Planning for a %s *****\n" % setup.getName())
print(setup)
if setup.solve(10):
# print the (approximate) solution path: print states along the path
# and controls required to get from one state to the next
path = setup.getSolutionPath()
path.interpolate()
# path.interpolate(); # uncomment if you want to plot the path
print(path.printAsMatrix())
if not setup.haveExactSolutionPath():
print("Solution is approximate. Distance to actual goal is %g" %
setup.getProblemDefinition().getSolutionDifference())
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 plan():
# create an Vector state space
space = ob.RealVectorStateSpace(SPACE_DIM)
# print(space)
# # set lower and upper bounds
bounds = ob.RealVectorBounds(SPACE_DIM)
for i in range(SPACE_DIM):
bounds.setLow(i, -1)
bounds.setHigh(i, 1)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
# set planner
planner = og.RRTConnect(ss.getSpaceInformation())
ss.setPlanner(planner)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
start = ob.State(space)
# we can pick a random start state...
start.random()
start_state = start.get()
for i in range(SPACE_DIM):
# start_state[i] = 0.1
print(start_state[i])
goal = ob.State(space)
# we can pick a random goal state...
goal.random()
ss.setStartAndGoalStates(start, goal)
# default parameters
solved = ss.solve(1.0)
if solved:
# try to shorten the path
ss.simplifySolution()
# print the simplified path
print(ss.getSolutionPath())