def __init__(self, space, robot, projection_choice, N, cellSizes = None):
# Inheritted from base.ProjectionEvaluator
super(MyProjection, self).__init__(space)
self.robot = robot
self.manip = self.robot.GetManipulator('Flange')
self.ndof = N
if PROJECTION_CHOICE == 1:
# tool tip (3) + norm of velocities (1)
self.dim = 4
elif PROJECTION_CHOICE == 2:
# projection 2 : (ndof) + norm of velocities (1)
self.dim = self.ndof + 1
elif PROJECTION_CHOICE == 3:
# random projection
self.dim = RP_DIM
self.projectionMatrix = CreateRandomOrthonormalMatrix(2*self.ndof, self.dim)
# print "projection matrix:"
# print self.projectionMatrix
else:
raise ValueError
if cellSizes is None:
self.cellSizes_ = util.vectorDouble()
self.cellSizes_.extend(CELLSIZE*np.ones(self.dim))
else:
self.cellSizes_ = util.vectorDouble()
self.cellSizes_.extend(cellSizes*np.ones(self.dim))
self.setCellSizes(self.cellSizes_)
def newplanner(self, si):
planner = oc.KPIECE1(si)
cdim = ou.vectorDouble()
cdim.extend([1, 1])
ope = MyProjectionEvaluator(si.getStateSpace(), cdim)
planner.setProjectionEvaluator(ope)
return planner
def newplanner(self, si):
planner = oc.KPIECE1(si)
cdim = ou.vectorDouble()
cdim.extend([1, 1])
ope = MyProjectionEvaluator(si.getStateSpace(), cdim)
planner.setProjectionEvaluator(ope)
return planner
def newplanner(self, si):
planner = og.ProjEST(si)
planner.setRange(10.0)
projection = ou.vectorUint()
projection.extend([0, 1])
cdim = ou.vectorDouble()
cdim.extend([1, 1])
proj = ob.RealVectorOrthogonalProjectionEvaluator(si.getStateSpace(), cdim, projection)
planner.setProjectionEvaluator(proj)
return planner
def newplanner(self, si):
planner = og.ProjEST(si)
planner.setRange(10.0)
projection = ou.vectorUint()
projection.extend([0, 1])
cdim = ou.vectorDouble()
cdim.extend([1, 1])
proj = ob.RealVectorOrthogonalProjectionEvaluator(si.getStateSpace(), cdim, projection)
planner.setProjectionEvaluator(proj)
return planner
def __init__(self, space, robot, cellSizes = None):
# Inheritted from base.ProjectionEvaluator
super(MyProjection, self).__init__(space)
self.robot = robot
self.manip = self.robot.GetManipulator('Flange')
self.ndof = self.robot.GetActiveDOF()
if cellSizes is None:
self.cellSizes = util.vectorDouble()
self.cellSizes.extend(0.1*np.ones(3))
else:
self.cellSizes = cellSizes
self.setCellSizes(self.cellSizes)
def list2vec(l):
ret = ou.vectorDouble()
for e in l:
ret.append(e)
return ret
def list2vec(l):
ret = ou.vectorDouble()
for e in l:
ret.append(e)
return ret
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)
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)
