def setSpace_3d(self):
# construct the state space we are planning in
self.space = ob.SE3StateSpace()
# set the bounds for the R^2 part of SE(2)
self.bounds = ob.RealVectorBounds(3)
if not self.costMap:
self.bounds.setLow(0)
self.bounds.setHigh(20)
else:
ox = self.costMap.getOriginX()
oy = self.costMap.getOriginY()
oz = self.costMap.getOriginZ()
size_x = self.costMap.getSizeInMetersX()
size_y = self.costMap.getSizeInMetersY()
size_z = self.costMap.getSizeInMetersZ()
print('o', ox, oy, oz)
print('size', size_x, size_y, size_z)
low = min(ox, oy, oz)
high = max(ox + size_x, oy + size_y, oz + size_z)
print("low", low)
print("high", high)
self.bounds.setLow(0, ox)
self.bounds.setLow(1, oy)
self.bounds.setLow(2, oz)
self.bounds.setHigh(0, ox + size_x)
self.bounds.setHigh(1, oy + size_y)
self.bounds.setHigh(2, oz + size_z)
self.space.setBounds(self.bounds)
# define a simple setup class
self.ss = og.SimpleSetup(self.space)
self.ss.setStateValidityChecker(
ob.StateValidityCheckerFn(self.isStateValid))
def plan():
# create an 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 plan():
# create an SE2 state space
space = ob.SE2StateSpace()
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(-1)
bounds.setHigh(1)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
start = ob.State(space)
# we can pick a random start state...
start.random()
# ... or set specific values
start().setX(.5)
goal = ob.State(space)
# we can pick a random goal state...
goal.random()
# ... or set specific values
goal().setX(-.5)
ss.setStartAndGoalStates(start, goal)
# this will automatically choose a default planner with
# default parameters
solved = ss.solve(1.0)
if solved:
# try to shorten the path
ss.simplifySolution()
# print the simplified path
print ss.getSolutionPath()
def __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 __init__(self):
img = Image.open('/home/alphonsus/ompl_code/test_codes/rrt_2d/rrt.png')
self.env = np.array(img, dtype='uint8')
self.maxWidth = self.env.shape[0] - 1
self.maxHeight = self.env.shape[1] - 1
# bounds = ob.RealVectorBounds(2)
# bounds.setLow(0,0); bounds.setHigh(0,self.env.shape[0])
# bounds.setLow(1,0); bounds.setHigh(1,self.env.shape[1])
# self.space = ob.SE2StateSpace()
self.space = ob.RealVectorStateSpace()
self.space.addDimension(0.0, self.env.shape[0])
self.space.addDimension(0.0, self.env.shape[1])
# self.space.setBounds(bounds)
self.ss_ = og.SimpleSetup(self.space)
self.ss_.setStateValidityChecker(
ob.StateValidityCheckerFn(self.isStateValid))
self.space.setup()
self.ss_.getSpaceInformation().setStateValidityCheckingResolution(
1.0 / self.space.getMaximumExtent())
self.ss_.setPlanner(og.RRTConnect(self.ss_.getSpaceInformation()))
self.ss_.setup()
def setSpace(self):
# construct the state space we are planning in
self.space = ob.SE2StateSpace()
# set the bounds for the R^2 part of SE(2)
self.bounds = ob.RealVectorBounds(2)
if not self.costMap:
self.bounds.setLow(-8)
self.bounds.setHigh(20)
else:
ox = self.costMap.getOriginX()
oy = self.costMap.getOriginY()
size_x = self.costMap.getSizeInMetersX()
size_y = self.costMap.getSizeInMetersY()
low = min(ox, oy)
high = max(ox + size_x, oy + size_y)
print("low", low)
print("high", high)
self.bounds.setLow(low)
self.bounds.setHigh(high)
self.space.setBounds(self.bounds)
# define a simple setup class
self.ss = og.SimpleSetup(self.space)
self.ss.setStateValidityChecker(
ob.StateValidityCheckerFn(self.isStateValid))
def plan():
# 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 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 plan(runTime, plannerType, objectiveType, plotData, fname, pdfname):
space = ob.RealVectorStateSpace(2)
space.setBounds(0.0, 1.0)
ss = og.SimpleSetup(space)
start = ob.State(space)
start[0] = 0.0
start[1] = 0.0
goal = ob.State(space)
goal[0] = 1.0
goal[1] = 1.0
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
ss.setStartAndGoalStates(start, goal)
si = ss.getSpaceInformation()
# Create the optimization objective specified by our command-line argument.
# This helper function is simply a switch statement.
ss.setOptimizationObjective(allocateObjective(si, objectiveType))
ss.setPlanner(allocatePlanner(si, plannerType))
ss.setup()
# attempt to solve the problem
solved = ss.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( \
ss.getSolutionPlannerName(), \
ss.getSolutionPath().length(), \
ss.getSolutionPath().cost(ss.getOptimizationObjective()).value()))
# print the path to screen
print("Found solution:\n%s" % ss.getSolutionPath())
# Extracting planner data from most recent solve attempt
pd = ob.PlannerData(si)
ss.getPlannerData(pd)
# Computing weights of all edges based on state space distance
pd.computeEdgeWeights()
if graphtool and plotData:
useGraphTool(pd)
# 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(ss.getSolutionPath().printAsMatrix())
if pdfname:
pds = ob.PlannerDataStorage()
pds.store(pd, pdfname)
else:
print("No solution found.")
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 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 main():
directory_formatter = './data/latent_space/{}_num_joints/{}_beta'
file_formatter = 'right_{}_{}.csv'
rospy.init_node("LatentSpaceSamplesGenerator")
space = initialize_space()
ss = og.SimpleSetup(space)
si = ss.getSpaceInformation()
state_validity_checker = MoveitOMPLStateValidityChecker(si)
# TODO: Implement a state validity checker
ss.setStateValidityChecker(state_validity_checker)
d_input = 7
h_dim1 = 256
h_dim2 = 100
d_output = 7
beta = 0.1
generated_batch_size = 1000
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
directory = directory_formatter.format(str(d_input), str(beta))
filename = file_formatter.format(str(d_input), str(generated_batch_size))
headers = ['latent' + name for name in JOINT_NAMES][0:d_input]
headers.append("collision_free")
if not os.path.exists(directory):
os.makedirs(directory)
print("Writing to file: %s" % filename)
# load the generator model;
model = VAE(d_input, h_dim1, h_dim2, d_output)
model.load_state_dict(torch.load("./data/model/model.pth"))
model.eval()
with torch.no_grad():
normals = torch.randn(generated_batch_size, d_output).to(device)
samples = model.decode(normals).double().cpu().numpy()
collisionFlags = np.zeros((normals.shape[0], 1), dtype=int)
for i in range(samples.shape[0]):
sample = samples[i, :]
state = ob.State(space)
for j in range(sample.shape[0]):
state[j] = sample[j]
isValid = state_validity_checker.isValid(
state) # checking collision status in c-space;
collisionFlags[i, 0] = isValid
data = np.append(samples, collisionFlags, axis=1)
writeToFile(os.path.join(directory, filename), data, headers=headers)
def benchmark(goal_config='fixed'):
# for publishing trajectory;
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', DisplayTrajectory, queue_size=0)
space = initialize_space()
ss = og.SimpleSetup(space)
state_validity_checker = TimedMoveitOMPLStateValidityChecker(
ss.getSpaceInformation())
ss.setStateValidityChecker(state_validity_checker)
states = [None, None] # start and goal states
if goal_config == 'fixed':
states[0] = nominal_pos(space) # fixed position
states[1] = neutral_position(space)
elif goal_config == 'random':
validity = np.zeros(2)
for i in range(len(states)):
states[i] = generate_random_state(space, 'right')
validity[i] = state_validity_checker.isValid(states[i])
indx = np.where(validity == 0)[0]
attempts = 0
if indx.size != 0:
for ind in np.nditer(indx):
while not state_validity_checker.isValid(
states[int(ind)]) and attempts < ATTEMPT_LIMIT:
print("State is not valid, regenerating a new state %s" %
states[int(ind)])
states[int(ind)] = generate_random_state(space, 'right')
attempts += 1
if attempts >= ATTEMPT_LIMIT:
print("Cannot find a valid goal after %s attempts. Exiting" %
attempts)
exit(0)
state_validity_checker.resetTimer()
t0, s0 = plan(0, states[0], states[1], ss, space,
display_trajectory_publisher)
c0, counts0 = state_validity_checker.getTime()
state_validity_checker.resetTimer()
t1, s1 = plan(1, states[0], states[1], ss, space,
display_trajectory_publisher)
c1, counts1 = state_validity_checker.getTime()
return (t0, t1, s0, s1, c0, c1, counts0, counts1)
def plan(self, start, goal, plan_time=20.0, simplify=True):
# create a simple setup object
ss = og.SimpleSetup(self.space)
ss.setStateValidityChecker(
ob.StateValidityCheckerFn(self.is_state_valid))
ss.setStartAndGoalStates(self._get_state(start), self._get_state(goal))
# this will automatically choose a default planner with
# default parameters
solved = ss.solve(plan_time)
if solved:
# try to shorten the path
if simplify:
ss.simplifySolution()
path = ss.getSolutionPath()
return [(s.getX(), s.getY()) for s in path.getStates()]
return None
def __init__(self, ppm_file):
self.ppm_ = ou.PPM()
self.ppm_.loadFile(ppm_file)
space = ob.RealVectorStateSpace()
space.addDimension(0.0, self.ppm_.getWidth())
space.addDimension(0.0, self.ppm_.getHeight())
self.maxWidth_ = self.ppm_.getWidth() - 1
self.maxHeight_ = self.ppm_.getHeight() - 1
self.ss_ = og.SimpleSetup(space)
# set state validity checking for this space
self.ss_.setStateValidityChecker(ob.StateValidityCheckerFn(
partial(Plane2DEnvironment.isStateValid, self)))
space.setup()
self.ss_.getSpaceInformation().setStateValidityCheckingResolution( \
1.0 / space.getMaximumExtent())
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 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())
def plan(space, planner, runTime, start, goal):
ss = og.SimpleSetup(space)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
ss.setStartAndGoalStates(start, goal)
if planner == 'RRT':
ss.setPlanner(og.RRT(ss.getSpaceInformation()))
elif planner == 'Astar':
ss.setPlanner(Astar.Astar(ss.getSpaceInformation()))
else:
print('Bad planner')
solved = ss.solve(runTime)
if solved:
path = ss.getSolutionPath()
path.interpolate(1000)
return path.printAsMatrix()
# return ss.getSolutionPath().printAsMatrix()
else:
print("No solution found.")
return None
def __init__(self, spaceType, space, constraint, options):
self.spaceType = spaceType
self.space = space
self.constraint = constraint
self.constraint.setTolerance(options.tolerance)
self.constraint.setMaxIterations(options.tries)
self.options = options
self.bench = None
self.request = None
self.pp = None
if spaceType == "PJ":
ou.OMPL_INFORM("Using Projection-Based State Space!")
self.css = ob.ProjectedStateSpace(space, constraint)
self.csi = ob.ConstrainedSpaceInformation(self.css)
elif spaceType == "AT":
ou.OMPL_INFORM("Using Atlas-Based State Space!")
self.css = ob.AtlasStateSpace(space, constraint)
self.csi = ob.ConstrainedSpaceInformation(self.css)
elif spaceType == "TB":
ou.OMPL_INFORM("Using Tangent Bundle-Based State Space!")
self.css = ob.TangentBundleStateSpace(space, constraint)
self.csi = ob.TangentBundleSpaceInformation(self.css)
self.css.setup()
self.css.setDelta(options.delta)
self.css.setLambda(options.lambda_)
if not spaceType == "PJ":
self.css.setExploration(options.exploration)
self.css.setEpsilon(options.epsilon)
self.css.setRho(options.rho)
self.css.setAlpha(options.alpha)
self.css.setMaxChartsPerExtension(options.charts)
if options.bias:
self.css.setBiasFunction(
lambda c, atlas=self.css: atlas.getChartCount(
) - c.getNeighborCount() + 1.)
if spaceType == "AT":
self.css.setSeparated(not options.no_separate)
self.css.setup()
self.ss = og.SimpleSetup(self.csi)
def plan(runTime, plannerType, fname, space, start, goal):
ss = og.SimpleSetup(space)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
# Set the start and goal states
ss.setStartAndGoalStates(start, goal)
# Set the problem instance for our planner to solve
ss.setPlanner(allocatePlanner(ss.getSpaceInformation(), plannerType))
# attempt to solve the planning problem in the given runtime
solved = ss.solve(runTime)
if solved:
# If a filename was specified, output the path as a matrix to
# that file for visualization
# ss.simplifySolution()
if fname:
with open(fname, 'w') as outFile:
outFile.write(ss.getSolutionPath().printAsMatrix())
return ss.getSolutionPath().printAsMatrix()
else:
print("No solution found.")
def plan_rrt(self, name, init, goal):
init = self.createState(*tuple(init))
goal = self.createState(*tuple(goal))
self.name_target = name
planner = og.RRTstar(self.si)
ss = og.SimpleSetup(self.si)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(self.isStateValid))
ss.setPlanner(planner)
ss.setStartAndGoalStates(init, goal)
solved = ss.solve(1.0)
if solved:
ss.simplifySolution()
path = ss.getSolutionPath()
path_res = []
for state in path.getStates():
path_res.append((state.getX(),state.getY(),state.getYaw()))
return path_res
else:
return None
def plan():
# create an ReedsShepp State space
space = ob.ReedsSheppStateSpace(5)
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(0)
bounds.setHigh(100)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
start = ob.State(space)
start[0] = 10.
start[1] = 90.
goal = ob.State(space)
goal[0] = 90.
goal[1] = 10.
ss.setStartAndGoalStates(start, goal, .05)
# set the planner
planner = RRT_Connect(ss.getSpaceInformation())
ss.setPlanner(planner)
result = ss.solve(100.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
path = ss.getSolutionPath()
path.interpolate(100)
#print(path.printAsMatrix())
path = path.printAsMatrix()
plt.plot(start[0], start[1], 'g*')
plt.plot(goal[0], goal[1], 'y*')
Plan.plot_path(path, 'b-', 0, 100)
plt.show()
def plan():
# create an R^3 state space
space = ob.RealVectorStateSpace(2)
# set lower and upper bounds
bounds = ob.RealVectorBounds(2)
bounds.setLow(-2.5)
bounds.setHigh(2.5)
space.setBounds(bounds)
# create a simple setup object
ss = og.SimpleSetup(space)
ss.setStateValidityChecker(ValidityChecker(ss.getSpaceInformation()))
start = ob.State(space)
start()[0] = 0.0
start()[1] = -2.0
goal = ob.State(space)
goal()[0] = 0.0
goal()[1] = 2.0
ss.setStartAndGoalStates(start, goal, .05)
# set the planner
planner = DPMPPlanner(ss.getSpaceInformation(), 15, 50)
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()
path = ss.getSolutionPath()
print(path)
plot_solution(path)
def test_augment_end_effector_pos():
rospy.init_node('End_effector_pos_test')
# for publishing trajectory;
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', DisplayTrajectory, queue_size=0)
rospy.sleep(0.5)
space = initialize_space()
ss = og.SimpleSetup(space)
state_validity_checker = MoveitOMPLStateValidityChecker(
ss.getSpaceInformation())
ss.setStateValidityChecker(state_validity_checker)
start = nominal_pos(space)
file = "/home/nikhildas/ros_ws/baxter_interfaces/baxter_moveit_config/data/sampled_data/self_and_environment_collision/right_7_test.csv"
publisher = WaypointPublisher()
label = get_collision_label_name("one_box_environment")
with open(file, mode='rb') as input_file:
csv_reader = csv.DictReader(input_file, quoting=csv.QUOTE_NONNUMERIC)
for waypoint in csv_reader:
if waypoint[label] == 1:
point = {}
for key in get_joint_names('right'):
point[key] = waypoint[key]
goal = construct_robot_state(space, point)
plan(0, start, goal, ss, space, display_trajectory_publisher)
position = Point()
position.x = waypoint['x']
position.y = waypoint['y']
position.z = waypoint['z']
# print("The position of the data point is: %s" % position)
publisher.publish_waypoints([position],
scale=[0.05, 0.05, 0.05])
time.sleep(5.0)
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 generate_self_collision_dataset(start, num_joints, num_points):
"""
Generate the base self-collision dataset labels.
:return: The file that corresponds to the dataset.
"""
total_number_points = 0
# import environment
empty_environment()
# initialize validity check
space = initialize_space()
ss = og.SimpleSetup(space)
state_validity_checker = MoveitOMPLStateValidityChecker(
ss.getSpaceInformation())
# header for the dataset
limb_name = 'right'
header_written = False
header = get_joint_names(limb_name) + [SELF_COLLISION_KEY]
directory = DIRECTORY % "empty_environment"
if not os.path.exists(directory):
os.makedirs(directory)
file_name = os.path.join(
directory, get_file_name(limb_name, num_joints, num_points, start))
# statistics for tracking generation
batch_time = []
for i in range(0, int(math.ceil(float(num_points) / BATCH_SIZE))):
print("Elasped time: %s" % (time.time() - start))
print("Start generating data for batch: %s" % (i + 1))
batch_start = i * BATCH_SIZE
batch_end = batch_start + BATCH_SIZE
size = BATCH_SIZE
if batch_end >= num_points:
# We've come to the last batch
size = num_points - batch_start
batch_time_start = time.time()
samples = generate_configs(size, state_validity_checker, limb_name)
with open(file_name, mode='a') as csv_file:
# Quote header such that it is easier for reader to parse data
writer = csv.DictWriter(csv_file,
fieldnames=header,
quoting=csv.QUOTE_NONNUMERIC)
if not header_written:
writer.writeheader()
header_written = True
writer.writerows(samples)
total_number_points += size
# Save time to generate batch for later statistics
batch_time_end = time.time()
batch_time.append(batch_time_end - batch_time_start)
# output statistics;
end = time.time()
total_time = end - start
print("Finished generating self-collision dataset. " +
"Total time elapsed: %s; Total number of points: %s" %
(total_time, total_number_points))
print(
"Average time to produce a batch of size %s: %s" %
(BATCH_SIZE, reduce(lambda x, y: x + y, batch_time) / len(batch_time)))
return file_name
for i in range(total_poses - 2):
new_mat = []
for j in range(4):
mat = np.dot(RotationMatrix(poseTh[i]), np.transpose(np.matrix([def_points[j][0], \
def_points[j][1], 1]))) + np.transpose(np.matrix([poseX[i], poseY[i], 0]))
new_mat.append(mat.tolist())
test_x = []
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])
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
if __name__ == "__main__":
# create an SE2 state space
space = ob.RealVectorStateSpace(2)
# 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(
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
