def solve_multiple(runs, problem):
"""
"""
result = {}
result['multiple'] = "true"
solutions = []
for i in range(0, runs):
OMPL_INFORM("Solving run number: {}".format(i))
solutions.append(solve(problem))
result['solutions'] = solutions
return result
def solve(problem):
"""
Given an instance of the Problem class, containing the problem configuration
data, solves the motion planning problem and returns either the solution
path or a failure message.
"""
# Sets up the robot type related information
ompl_setup = setup(problem)
# Load the planner
space_info = ompl_setup.getSpaceInformation()
if problem['planner'].startswith('ompl.control.Syclop'):
decomposition = ompl_setup.allocDecomposition()
planner = eval('%s(space_info, decomposition)' % problem['planner'])
ompl_setup.setPlanner(planner)
else:
planner = eval("%s(space_info)" % problem['planner'])
ompl_setup.setPlanner(planner)
# Set the optimization objective
objectives = {'length': 'PathLengthOptimizationObjective', \
'max_min_clearance': 'MaximizeMinClearanceObjective', \
'mechanical_work': 'MechanicalWorkOptimizationObjective'}
objective = objectives[problem['objective']]
obj = eval('ob.%s(space_info)' % objective)
cost = ob.Cost(float(problem['objective.threshold']))
obj.setCostThreshold(cost)
ompl_setup.setOptimizationObjective(obj)
# Set each parameter that was configured by the user
for param in problem['planner_params']:
planner.params().setParam(str(param), str(problem['planner_params'][param]))
# Solve the problem
solution = {}
solved = ompl_setup.solve(float(problem['solve_time']))
# Check for validity
if solved:
if problem['isGeometric']:
path = ompl_setup.getSolutionPath()
initialValid = path.check()
if initialValid:
# If initially valid, attempt to simplify
ompl_setup.simplifySolution()
# Get the simplified path
simple_path = ompl_setup.getSolutionPath()
simplifyValid = simple_path.check()
if simplifyValid:
path = simple_path
else:
OMPL_ERROR("Simplified path was invalid.")
else:
OMPL_ERROR("Invalid solution path.")
else:
path = ompl_setup.getSolutionPath().asGeometric()
OMPL_INFORM("Path simplification skipped due to non rigid body.")
# Interpolate path
ns = int(100.0 * float(path.length()) / \
float(ompl_setup.getStateSpace().getMaximumExtent()))
if problem["isGeometric"] and len(path.getStates()) < ns:
path.interpolate(ns)
if len(path.getStates()) != ns:
OMPL_WARN("Interpolation produced " + str(len(path.getStates())) + \
" states instead of " + str(ns) + " states.")
solution = format_solution(path, True)
else:
solution = format_solution(None, False)
solution['name'] = str(problem['name'])
solution['planner'] = ompl_setup.getPlanner().getName()
solution['status'] = str(solved)
# Store the planner data
pd = ob.PlannerData(ompl_setup.getSpaceInformation())
ompl_setup.getPlannerData(pd)
explored_states = []
for i in range(0, pd.numVertices()):
coords = []
state = pd.getVertex(i).getState()
coords.append(state.getX())
coords.append(state.getY())
if problem["is3D"]:
coords.append(state.getZ())
else:
coords.append(0)
explored_states.insert(i, coords)
solution["explored_states"] = explored_states
return solution
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