def createPathWithGivenPositionsXY(self, xys):
def createState(spaceInformation, xy):
state = spaceInformation.allocState()
state.setXY(xy[0], xy[1])
return state
# Create dummy path and change private variables directly
state = sbot.BoatState(globalWaypoint=latlon(0.2, 0.2),
position=latlon(0, 0),
globalWindDirectionDegrees=90,
globalWindSpeedKmph=10,
AISData=AISMsg(),
headingDegrees=0,
speedKmph=0)
path = createPath(state, runtimeSeconds=1, numRuns=2)
spaceInformation = path.getSpaceInformation()
# Create acutal path
actualPath = og.PathGeometric(spaceInformation)
for xy in xys:
actualPath.append(createState(spaceInformation, xy))
path.getOMPLPath()._solutionPath = actualPath
path._latlons = path._getLatlonsFromOMPLPath(path._omplPath)
return path
def steerTo(start, end, obc, IsInCollision, step_sz=DEFAULT_STEP):
# test if there is a collision free path from start to end, with step size
# given by step_sz, and with generic collision check function
# here we assume start and end are tensors
# return 0 if in coliision; 1 otherwise
start_t = time.time()
start_ompl = ob.State(setup.getSpaceInformation())
start_ompl().setX(start[0].item())
start_ompl().setY(start[1].item())
start_ompl().setZ(start[2].item())
angle = np.array(
[start[6].item(), start[3].item(), start[4].item(), start[5].item()])
angle = QtoAxisAngle(angle)
start_ompl().rotation().setAxisAngle(angle[0], angle[1], angle[2],
angle[3])
end_ompl = ob.State(setup.getSpaceInformation())
end_ompl().setX(end[0].item())
end_ompl().setY(end[1].item())
end_ompl().setZ(end[2].item())
angle = np.array(
[end[6].item(), end[3].item(), end[4].item(), end[5].item()])
angle = QtoAxisAngle(angle)
end_ompl().rotation().setAxisAngle(angle[0], angle[1], angle[2], angle[3])
path_ompl = og.PathGeometric(si)
path_ompl.append(start_ompl())
path_ompl.append(end_ompl())
return path_ompl.check()
def findBestSolution(validSolutions, invalidSolutions):
# If no valid solutions found, use the best invalid one. Do not perform any path simplifying on invalid paths.
if len(validSolutions) == 0:
# Set the average distance between waypoints. Must be done before cost calculation and comparison
for solution in invalidSolutions:
setAverageDistanceBetweenWaypoints(solution.getSolutionPath())
bestSolution = min(invalidSolutions,
key=lambda x: x.getSolutionPath().cost(
x.getOptimizationObjective()).value())
bestSolutionPath = bestSolution.getSolutionPath()
minCost = bestSolutionPath.cost(
bestSolution.getOptimizationObjective()).value()
else:
# Set the average distance between waypoints. Must be done before cost calculation and comparison
for solution in validSolutions:
setAverageDistanceBetweenWaypoints(solution.getSolutionPath())
# Find path with minimum cost. Can be either simplified or unsimplified path.
# Need to recheck that simplified paths are valid before using
minCost = sys.maxsize
bestSolution = None
bestSolutionPath = None
for solution in validSolutions:
# Check unsimplified path
unsimplifiedPath = og.PathGeometric(solution.getSolutionPath())
unsimplifiedCost = unsimplifiedPath.cost(
solution.getOptimizationObjective()).value()
if unsimplifiedCost < minCost:
bestSolution = solution
bestSolutionPath = unsimplifiedPath
minCost = unsimplifiedCost
return bestSolution, bestSolutionPath, minCost
def solve(self, ptc):
pdef = self.getProblemDefinition()
goal = pdef.getGoal()
si = self.getSpaceInformation()
pi = self.getPlannerInputStates()
st = pi.nextStart()
while st:
self.states_.append(st)
st = pi.nextStart()
solution = None
approxsol = 0
approxdif = 1e6
while not ptc():
rstate = si.allocState()
# pick a random state in the state space
self.sampler_.sampleUniform(rstate)
# check motion
if si.checkMotion(self.states_[-1], rstate):
self.states_.append(rstate)
sat = goal.isSatisfied(rstate)
dist = goal.distanceGoal(rstate)
if sat:
approxdif = dist
solution = len(self.states_)
break
if dist < approxdif:
approxdif = dist
approxsol = len(self.states_)
solved = False
approximate = False
if not solution:
solution = approxsol
approximate = True
if solution:
path = og.PathGeometric(si)
for s in self.states_[:solution]:
path.append(s)
pdef.addSolutionPath(path)
solved = True
return ob.PlannerStatus(solved, approximate)
def solve(self, ptc):
pdef = self.getProblemDefinition()
si = self.getSpaceInformation()
pi = self.getPlannerInputStates()
goal = pdef.getGoal()
st = pi.nextStart()
while st:
self.states_.append(st)
st = pi.nextStart()
start_state = pdef.getStartState(0)
goal_state = goal.getState()
self.tree.append(Node(None, start_state))
solution = None
approxsol = 0
approxdif = 1e6
while not ptc():
if self.expand(self.tree, goal_state) == GrowState.Reached:
current = self.tree[-1]
path = []
while current is not None:
path.append(current.position)
current = current.parent
for i in range(1, len(path)):
self.states_.append(path[len(path) - i - 1])
solution = len(self.states_)
break
solved = False
approximate = False
if not solution:
solution = approxsol
approximate = True
if solution:
path = og.PathGeometric(si)
for s in self.states_[:solution]:
path.append(s)
pdef.addSolutionPath(path)
solved = True
return ob.PlannerStatus(solved, approximate)
def solve(self, ptc):
pdef = self.getProblemDefinition()
goal = pdef.getGoal()
si = self.getSpaceInformation()
pi = self.getPlannerInputStates()
st = pi.nextStart()
while st:
self.states_.append(st)
st = pi.nextStart()
solution = None
approxsol = 0
approxdif = 1e6
start_state = pdef.getStartState(0)
goal_state = goal.getState()
start_node = Node(None, start_state)
start_node.g = start_state.h = start_node.f = 0
end_node = Node(None, goal_state)
end_node.g = end_node.h = end_node.f = 0
open_list = []
closed_list = []
heapq.heapify(open_list)
adjacent_squares = ((1, 0, 0), (1, 1, 45), (0, 1, 90), (-1, 1, 135),
(-1, 0, 0), (-1, -1, -135), (0, -1, -90), (1, -1,
-45))
heapq.heappush(open_list, start_node)
while len(open_list) > 0 and not ptc():
current_node = heapq.heappop(open_list)
if current_node == end_node: # if we hit the goal
current = current_node
path = []
while current is not None:
path.append(current.position)
current = current.parent
for i in range(1, len(path)):
self.states_.append(path[len(path) - i - 1])
solution = len(self.states_)
break
closed_list.append(current_node)
children = []
for new_position in adjacent_squares:
node_position = si.allocState()
current_node_x = current_node.position.getX()
current_node_y = current_node.position.getY()
node_position.setXY(current_node_x + new_position[0],
current_node_y + new_position[1])
node_position.setYaw(new_position[2] * math.pi / 180)
if not si.checkMotion(current_node.position, node_position):
continue
if not si.satisfiesBounds(node_position):
continue
new_node = Node(current_node, node_position)
children.append(new_node)
for child in children:
if child in closed_list:
continue
if child.position.getYaw() % (math.pi / 2) == 0:
child.g = current_node.g + 1
else:
child.g = current_node.g + math.sqrt(2)
child.h = goal.distanceGoal(child.position)
child.f = child.g + child.h
if len([i for i in open_list if child == i and child.g >= i.g
]) > 0:
continue
heapq.heappush(open_list, child)
solved = False
approximate = False
if not solution:
solution = approxsol
approximate = True
if solution:
path = og.PathGeometric(si)
for s in self.states_[:solution]:
path.append(s)
pdef.addSolutionPath(path)
solved = True
return ob.PlannerStatus(solved, approximate)
def construct_path(si, states):
path = og.PathGeometric(si)
for s in states:
path.append(s)
return path
def solve(self, ptc):
pdef = self.getProblemDefinition()
si = self.getSpaceInformation()
pi = self.getPlannerInputStates()
goal = pdef.getGoal()
st = pi.nextStart()
while st:
self.states_.append(st)
st = pi.nextStart()
start_state = pdef.getStartState(0)
goal_state = goal.getState()
self.treeA.append(Node(None, start_state))
self.treeB.append(Node(None, goal_state))
solution = None
approxsol = 0
approxdif = 1e6
random_state = si.allocState()
while not ptc():
# new random node
self.sampler_.sampleUniform(random_state)
if self.expand(self.treeA, random_state) != GrowState.Trapped:
if self.connect(self.treeB,
self.treeA[-1].position) == GrowState.Reached:
if self.treeA[0].position == start_state:
start_tree = self.treeA
end_tree = self.treeB
print('A to start')
else:
start_tree = self.treeB
end_tree = self.treeA
print('B to start')
path_from_mid_to_start = []
path_from_mid_to_end = []
current = start_tree[-1]
while current.position != start_state:
path_from_mid_to_start.append(current.position)
current = current.parent
current = end_tree[-1]
while current is not None:
path_from_mid_to_end.append(current.position)
current = current.parent
path = path_from_mid_to_start[::-1] + path_from_mid_to_end
for i in range(0, len(path)):
self.states_.append(path[i])
solution = len(self.states_)
#if self.treeB[0].position == start_state:
# print('B to start')
# path_from_mid_to_start = []
# path_from_mid_to_end = []
# current = self.treeB[-1]
# while current.position != start_state:
# path_from_mid_to_start.append(current.position)
# current = current.parent
# current = self.treeA[-1]
# while current is not None:
# path_from_mid_to_end.append(current.position)
# current = current.parent
# path = path_from_mid_to_start[::-1] + path_from_mid_to_end
# for i in range(0, len(path)):
# self.states_.append(path[i])
# solution = len(self.states_)
break
self.treeA, self.treeB = self.treeB, self.treeA
# print('treeA:', self.treeA)
# print('treeB:', self.treeB)
solved = False
approximate = False
if not solution:
solution = approxsol
approximate = True
if solution:
path = og.PathGeometric(si)
for s in self.states_[:solution]:
path.append(s)
pdef.addSolutionPath(path)
solved = True
return ob.PlannerStatus(solved, approximate)
def isValid(self, state):
return bool(self.clearance(state) > 0.0)
def clearance(self, state):
x = state[0]
y = state[1]
return np.sqrt(x * x + y * y) - 1
space = ob.RealVectorStateSpace(2)
bounds = ob.RealVectorBounds(2)
bounds.setLow(-2.5)
bounds.setHigh(2.5)
space.setBounds(bounds)
ss = og.SimpleSetup(space)
si = ss.getSpaceInformation()
ss.setStateValidityChecker(ValidityChecker(ss.getSpaceInformation()))
state1 = space.allocState()
si.allocStateSampler().sampleUniform(state1)
state2 = space.allocState()
si.allocStateSampler().sampleUniform(state2)
path = og.PathGeometric(si)
path.append(state1)
path.append(state2)
path.check()
def neural_plan_trajectory(self,
start_point,
goal_point,
planner_number,
joint_names,
group_name,
planning_time,
planner_config_name,
plan_type='pfs'):
"""
Given a start and goal point, plan by Neural Network.
Args:
start_point (list of float): A starting joint configuration.
goal_point (list of float): A goal joint configuration.
planner_number (int): The index of the planner to be used as
returned by acquire_planner().
joint_names (list of str): The name of the joints corresponding to
start_point and goal_point.
group_name (str): The name of the group to which the joint names
correspond.
planning_time (float): Maximum allowed time for planning, in seconds.
planner_config_name (str): Type of planner to use.
Return:
list of list of float: A sequence of points representing the joint
configurations at each point on the path.
"""
"""
# TODO: add hybrid planning for Baxter environment
"""
# obtain obstacle information through rostopic
rospy.loginfo(
"%s Plan Trajectory Wrapper: waiting for obstacle message..." %
(rospy.get_name()))
obc = rospy.wait_for_message('obstacles/obc', Float64Array2D)
obc = torch.FloatTensor([obc_i.values for obc_i in obc.points])
obs = rospy.wait_for_message('obstacles/obs', Float64Array)
obs = obs.values
obs = torch.FloatTensor(obs)
rospy.loginfo(
"%s Plan Trajectory Wrapper: obstacle message received." %
(rospy.get_name()))
rospy.loginfo(
'%s Plan Trajectory Wrapper: using neural network for planning...'
% (rospy.get_name()))
step_sz = DEFAULT_STEP
MAX_NEURAL_REPLAN = 11
IsInCollision = self.IsInCollision
path = [torch.FloatTensor(start_point), torch.FloatTensor(goal_point)]
mpNet = self.neural_planners[0]
time_flag = False
fp = 0
plan_time = time.time()
def isStateValid(state):
return not IsInCollision(state, obc)
si = ob.SpaceInformation(self.space)
si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
si.setup()
for t in xrange(MAX_NEURAL_REPLAN):
# adaptive step size on replanning attempts
if (t == 2):
step_sz = 1.2
elif (t == 3):
step_sz = 0.5
elif (t > 3):
step_sz = 0.1
if time_flag:
path, time_norm = plan_general.neural_replan(mpNet, path, obc, obs, IsInCollision, \
self.normalize_func, self.unnormalize_func, t==0, step_sz=step_sz, \
time_flag=time_flag, device=self.device)
else:
path = plan_general.neural_replan(mpNet, path, obc, obs, IsInCollision, \
self.normalize_func, self.unnormalize_func, t==0, step_sz=step_sz, \
time_flag=time_flag, device=self.device)
time_norm = 0
# print lvc time
print('Neural Planner: path length: %d' % (len(path)))
lvc_start = time.time()
path = plan_general.lvc(path, obc, IsInCollision, step_sz=step_sz)
print('Neural Planner: lvc time: %f' % (time.time() - lvc_start))
feasible_check_time = time.time()
# check feasibility using OMPL
path_ompl = og.PathGeometric(si)
for i in range(len(path)):
state = ob.State(self.space)
for j in range(len(path[i])):
state[j] = path[i][j].item()
sref = state() # a reference to the state
path_ompl.append(sref)
#path.check()
if path_ompl.check():
#if plan_general.feasibility_check(path, obc, IsInCollision, step_sz=0.01):
fp = 1
rospy.loginfo('%s Neural Planner: plan is feasible.' %
(rospy.get_name()))
break
if self.finished:
# if other planner has finished, stop
break
if time.time() - plan_time >= planning_time:
# we can't allow the planner to go too long
break
if fp:
# only for successful paths
plan_time = time.time() - plan_time
plan_time -= time_norm
print('test time: %f' % (plan_time))
## TODO: make sure path is indeed list
return plan_time, path
else:
return np.inf, None
