def allocateObjective(si, objectiveType):
if objectiveType.lower() == "pathlength":
return getPathLengthObjective(si)
else:
ou.OMPL_ERROR(
"Optimization-objective is not implemented in allocation function."
)
def allocatePlanner(si, plannerType):
if plannerType.lower() == "bfmtstar":
return og.BFMT(si)
elif plannerType.lower() == "bitstar":
return og.BITstar(si)
elif plannerType.lower() == "fmtstar":
return og.FMT(si)
elif plannerType.lower() == "informedrrtstar":
return og.InformedRRTstar(si)
elif plannerType.lower() == "prmstar":
return og.PRMstar(si)
elif plannerType.lower() == "rrtstar":
return og.RRTstar(si)
elif plannerType.lower() == "sorrtstar":
return og.SORRTstar(si)
elif plannerType.lower() == "rrtxstatic":
return og.RRTXstatic(si)
elif plannerType.lower() == "rrtsharp":
return og.RRTsharp(si)
# multi-query
elif plannerType.lower() == "lazyprmstar":
return og.LazyPRMstar(si)
# single-query
elif plannerType.lower() == "rrtconnect":
return og.RRTConnect(si)
elif plannerType.lower() == "lbtrrt":
return og.LBTRRT(si)
elif plannerType.lower() == "lazylbtrrt":
return og.LazyLBTRRT(si)
else:
ou.OMPL_ERROR(
"Planner-type is not implemented in allocation function.")
def allocateObjective(si, objectiveType):
if objectiveType.lower() == "pathclearance":
return getClearanceObjective(si)
elif objectiveType.lower() == "pathlength":
return getPathLengthObjective(si)
elif objectiveType.lower() == "thresholdpathlength":
return getThresholdPathLengthObj(si)
elif objectiveType.lower() == "weightedlengthandclearancecombo":
return getBalancedObjective1(si)
else:
ou.OMPL_ERROR("Optimization-objective is not implemented in allocation function.")
def allocate_objective(si, objectiveType):
if objectiveType.lower() == "pathclearance":
return get_clearance_objective(si)
elif objectiveType.lower() == "pathlength":
return get_path_length_objective(si)
elif objectiveType.lower() == "thresholdpathlength":
return get_threshold_path_length_objective(si)
else:
ou.OMPL_ERROR(
"Optimization-objective is not implemented in allocation function."
)
def allocate_objective(si, objectiveType, windDirectionDegrees=None, headingDegrees=None):
if objectiveType.lower() == "pathclearance":
return get_clearance_objective(si)
elif objectiveType.lower() == "pathlength":
return get_path_length_objective(si)
elif objectiveType.lower() == "thresholdpathlength":
return get_threshold_path_length_objective(si)
elif objectiveType.lower() == "sailing":
if windDirectionDegrees is None or headingDegrees is None:
raise ValueError("allocate_objective requires windDirectionDegrees and headingDegrees"
"parameters for sailing")
return getSailingObjective(si, windDirectionDegrees, headingDegrees)
else:
ou.OMPL_ERROR("Optimization-objective is not implemented in allocation function.")
def allocate_planner(si, planner_type, decomp):
if planner_type.lower() == "est":
return og.EST(si)
elif planner_type.lower() == "kpiece":
return og.KPIECE1(si)
elif planner_type.lower() == "pdst":
return og.PDST(si)
elif planner_type.lower() == "rrt":
return og.RRT(si)
elif planner_type.lower() == "syclopest":
return og.SyclopEST(si, decomp)
elif planner_type.lower() == "sycloprrt":
return og.SyclopRRT(si, decomp)
else:
ou.OMPL_ERROR("Planner-type is not implemented in allocation function.")
def allocatePlanner(si, plannerType):
if plannerType.lower() == "bfmtstar":
return og.BFMT(si)
elif plannerType.lower() == "bitstar":
return og.BITstar(si)
elif plannerType.lower() == "fmtstar":
return og.FMT(si)
elif plannerType.lower() == "informedrrtstar":
return og.InformedRRTstar(si)
elif plannerType.lower() == "prmstar":
return og.PRMstar(si)
elif plannerType.lower() == "rrtstar":
return og.RRTstar(si)
elif plannerType.lower() == "sorrtstar":
return og.SORRTstar(si)
else:
ou.OMPL_ERROR("Planner-type is not implemented in allocation function.")
def choose_planner(self, si, planner_type):
if planner_type.lower() == "bfmtstar":
return og.BFMT(si)
elif planner_type.lower() == "bitstar":
return og.BITstar(si)
elif planner_type.lower() == "fmtstar":
return og.FMT(si)
elif planner_type.lower() == "informedrrtstar":
return og.InformedRRTstar(si)
elif planner_type.lower() == "prmstar":
return og.PRMstar(si)
elif planner_type.lower() == "rrtconnect":
return og.RRTConnect(si)
elif planner_type.lower() == "rrtsharp":
return og.RRTsharp(si)
elif planner_type.lower() == "rrtstar":
return og.RRTstar(si)
elif planner_type.lower() == "sorrtstar":
return og.SORRTstar(si)
else:
ou.OMPL_ERROR(
"Planner-type is not implemented in allocation function.")
parser.add_argument('-pdf', '--pdfile', default=None, \
help='(Optional) Specify an output path for the planner data.')
parser.add_argument('-pd', '--plotData', type=bool, default=False, help=\
'(Optional) Specify if plot the planner data and path.')
parser.add_argument('-i', '--info', type=int, default=0, choices=[0, 1, 2], \
help='(Optional) Set the OMPL log level. 0 for WARN, 1 for INFO, 2 for DEBUG.' \
' Defaults to WARN.')
# Parse the arguments
args = parser.parse_args()
# Check that time is positive
if args.runtime <= 0:
raise argparse.ArgumentTypeError(
"argument -t/--runtime: invalid choice: %r (choose a positive number greater than 0)" \
% (args.runtime,))
# Set the log level
if args.info == 0:
ou.setLogLevel(ou.LOG_WARN)
elif args.info == 1:
ou.setLogLevel(ou.LOG_INFO)
elif args.info == 2:
ou.setLogLevel(ou.LOG_DEBUG)
else:
ou.OMPL_ERROR("Invalid log-level integer.")
# Solve the planning problem
plan(args.runtime, args.planner, args.objective, args.plotData, args.file,
args.pdfile)
