def __call__(self, instance, robot, *args, **kw_args):
env = robot.GetEnv()
defer = kw_args.get('defer')
with Clone(env, clone_env=instance.env,
lock=True, unlock=False) as cloned_env:
cloned_robot = cloned_env.Cloned(robot)
def call_planner():
try:
planner_traj = self.func(instance, cloned_robot,
*args, **kw_args)
# Tag the trajectory with the planner and planning method
# used to generate it. We don't overwrite these tags if
# they already exist.
tags = GetTrajectoryTags(planner_traj)
tags.setdefault(Tags.PLANNER, instance.__class__.__name__)
tags.setdefault(Tags.METHOD, self.func.__name__)
SetTrajectoryTags(planner_traj, tags, append=False)
return CopyTrajectory(planner_traj, env=env)
finally:
cloned_env.Unlock()
if defer is True:
from trollius.executor import get_default_executor
from trollius.futures import wrap_future
executor = kw_args.get('executor') or get_default_executor()
return wrap_future(executor.submit(call_planner))
else:
return call_planner()
def __call__(self, instance, robot, *args, **kw_args):
env = robot.GetEnv()
defer = kw_args.get('defer')
# Store the original joint values and indices.
joint_indices = [robot.GetActiveDOFIndices(), None]
joint_values = [robot.GetActiveDOFValues(), None]
with Clone(env, clone_env=instance.env,
lock=True, unlock=False) as cloned_env:
cloned_robot = cloned_env.Cloned(robot)
# Store the cloned joint values and indices.
joint_indices[1] = cloned_robot.GetActiveDOFIndices()
joint_values[1] = cloned_robot.GetActiveDOFValues()
# Check for mismatches in the cloning and hackily reset them.
# (This is due to a possible bug in OpenRAVE environment cloning where in
# certain situations, the Active DOF ordering and values do not match the
# parent environment. It seems to be exacerbated by multirotation joints,
# but the exact cause and repeatability is unclear at this point.)
if not numpy.array_equal(joint_indices[0], joint_indices[1]):
logger.warning("Cloned Active DOF index mismatch: %s != %s",
str(joint_indices[0]),
str(joint_indices[1]))
cloned_robot.SetActiveDOFs(joint_indices[0])
if not numpy.allclose(joint_values[0], joint_values[1]):
logger.warning("Cloned Active DOF value mismatch: %s != %s",
str(joint_values[0]),
str(joint_values[1]))
cloned_robot.SetActiveDOFValues(joint_values[0])
def call_planner():
try:
planner_traj = self.func(instance, cloned_robot,
*args, **kw_args)
# Tag the trajectory with the planner and planning method
# used to generate it. We don't overwrite these tags if
# they already exist.
tags = GetTrajectoryTags(planner_traj)
tags.setdefault(Tags.PLANNER, instance.__class__.__name__)
tags.setdefault(Tags.METHOD, self.func.__name__)
SetTrajectoryTags(planner_traj, tags, append=False)
return CopyTrajectory(planner_traj, env=env)
finally:
cloned_env.Unlock()
if defer is True:
from trollius.executor import get_default_executor
from trollius.futures import wrap_future
executor = kw_args.get('executor') or get_default_executor()
return wrap_future(executor.submit(call_planner))
else:
return call_planner()
def plan(self, method, args, kw_args):
all_planners = self._planners
planners = []
results = [None] * len(self._planners)
# Find only planners that support the required planning method.
for index, planner in enumerate(all_planners):
if not planner.has_planning_method(method):
results[index] = PlanningError(
"{:s} does not implement method {:s}."
.format(planner, method)
)
continue
else:
planners.append((index, planner))
# Helper function to call a planner and store its result or error.
def call_planner(index, planner):
try:
planning_method = getattr(planner, method)
kw_args['defer'] = False
results[index] = planning_method(*args, **kw_args)
except MetaPlanningError as e:
results[index] = e
except PlanningError as e:
logger.warning("Planning with {:s} failed: {:s}"
.format(planner, e))
results[index] = e
# Call every planners in parallel using a concurrent executor and
# return the first non-error result in the ordering when available.
from trollius.executor import get_default_executor
from trollius.futures import wrap_future
from trollius.tasks import as_completed
executor = kw_args.get('executor') or get_default_executor()
futures = [wrap_future(executor.submit(call_planner, planner))
for planner in planners]
# Each time a planner completes, check if we have a valid result
# (a planner found a solution and all higher-ranked planners had
# already failed).
for _ in as_completed(futures):
for result in results:
if result is None:
break
elif isinstance(result, openravepy.Trajectory):
return result
elif not isinstance(result, PlanningError):
logger.warning(
"Planner {:s} returned {} of type {}; "
"expected a trajectory or PlanningError."
.format(str(planner), result, type(result))
)
raise MetaPlanningError("All planners failed.",
dict(zip(all_planners, results)))
def meta_wrapper(*args, **kw_args):
defer = kw_args.get('defer')
if defer is True:
from trollius.executor import get_default_executor
from trollius.futures import wrap_future
executor = kw_args.get('executor') or get_default_executor()
return wrap_future(executor.submit(self.plan, method_name,
args, kw_args))
else:
return self.plan(method_name, args, kw_args)
def ExecutePath(self, path, defer=False, executor=None, **kwargs):
""" Post-process and execute an un-timed path.
This method calls PostProcessPath, then passes the result to
ExecuteTrajectory. Any extra **kwargs are forwarded to both of these
methods. This function returns the timed trajectory that was executed
on the robot.
@param path OpenRAVE trajectory representing an un-timed path
@param defer execute asynchronously and return a future
@param executor if defer = True, which executor to use
@param **kwargs forwarded to PostProcessPath and ExecuteTrajectory
@return timed trajectory executed on the robot
"""
from ..util import Timer
def do_execute():
logger.info('Post-processing path with %d waypoints.',
path.GetNumWaypoints())
with Timer() as timer:
traj = self.PostProcessPath(path, defer=False, **kwargs)
SetTrajectoryTags(traj, {Tags.POSTPROCESS_TIME: timer.get_duration()}, append=True)
logger.info('Post-processing took %.3f seconds and produced a path'
' with %d waypoints and a duration of %.3f seconds.',
timer.get_duration(),
traj.GetNumWaypoints(),
traj.GetDuration()
)
with Timer() as timer:
exec_traj = self.ExecuteTrajectory(traj, defer=False, **kwargs)
SetTrajectoryTags(exec_traj, {Tags.EXECUTION_TIME: timer.get_duration()}, append=True)
return exec_traj
if defer is True:
from trollius.executor import get_default_executor
from trollius.futures import wrap_future
if executor is None:
executor = get_default_executor()
return wrap_future(executor.submit(do_execute))
elif defer is False:
return do_execute()
else:
raise ValueError('Received unexpected value "{:s}" for defer.'.format(str(defer)))
def PostProcessPath(self, path, defer=False, executor=None,
constrained=None, smooth=None, default_timelimit=0.5,
shortcut_options=None, smoothing_options=None,
retiming_options=None, affine_retiming_options=None,
**kwargs):
""" Post-process a geometric path to prepare it for execution.
This method post-processes a geometric path by (optionally) optimizing
it and timing it. Four different post-processing pipelines are used:
1. For base trajectories (i..e affine DOFs), we time the trajectory
using self.affine_retimer. This function does not currently support
trajectories that contain both regular and affine DOFs.
2. For constrained trajectories, we do not modify the geometric path
and retime the path to be time-optimal via self.retimer. This
trajectory must stop at every waypoint. The only exception is for...
3. For smooth trajectories, we attempt to fit a time-optimal smooth
curve through the waypoints (not implemented). If this curve is
not collision free, then we fall back on...
4. By default, we run a smoother that jointly times and smooths the
path via self.smoother. This algorithm can change the geometric path
to optimize runtime.
The behavior in (2) and (3) can be forced by passing constrained=True
or smooth=True. By default, the case is inferred by the tag(s) attached
to the trajectory: (1) is triggered by the CONSTRAINED tag and (2) is
tiggered by the SMOOTH tag.
Options an be passed to each post-processing routine using the
affine_retiming_options, shortcut_options, smoothing_options, and
retiming_options **kwargs dictionaries. If no "timelimit" is specified
in any of these dictionaries, it defaults to default_timelimit seconds.
@param path un-timed OpenRAVE trajectory
@param defer return immediately with a future trajectory
@param executor executor to use when defer = True
@param constrained the path is constrained; do not change it
@param smooth the path is smooth; attempt to execute it directly
@param default_timelimit timelimit for all operations, if not set
@param shortcut_options kwargs to self.simplifier
@param smoothing_options kwargs to self.smoother
@param retiming_options kwargs to self.retimer
@param affine_retiming_options kwargs to self.affine_retimer
@return trajectory ready for execution
"""
from ..planning.base import Tags
from ..util import GetTrajectoryTags, CopyTrajectory
from openravepy import DOFAffine
# Default parameters.
if shortcut_options is None:
shortcut_options = dict()
if smoothing_options is None:
smoothing_options = dict()
if retiming_options is None:
retiming_options = dict()
if affine_retiming_options is None:
affine_retimer_options = dict()
shortcut_options.setdefault('timelimit', default_timelimit)
smoothing_options.setdefault('timelimit', default_timelimit)
retiming_options.setdefault('timelimit', default_timelimit)
affine_retimer_options.setdefault('timelimit', default_timelimit)
# Read default parameters from the trajectory's tags.
tags = GetTrajectoryTags(path)
if constrained is None:
constrained = tags.get(Tags.CONSTRAINED, False)
logger.debug('Detected "%s" tag on trajectory: Setting'
' constrained = True.', Tags.CONSTRAINED)
if smooth is None:
smooth = tags.get(Tags.SMOOTH, False)
logger.debug('Detected "%s" tag on trajectory: Setting smooth'
' = True', Tags.SMOOTH)
def do_postprocess():
with Clone(self.GetEnv()) as cloned_env:
cloned_robot = cloned_env.Cloned(self)
# Planners only operate on the active DOFs. We'll set any DOFs
# in the trajectory as active.
env = path.GetEnv()
cspec = path.GetConfigurationSpecification()
used_bodies = cspec.ExtractUsedBodies(env)
if self not in used_bodies:
raise ValueError(
'Robot "{:s}" is not in the trajectory.'.format(
self.GetName()))
# Extract active DOFs from teh trajectory and set them as active.
dof_indices, _ = cspec.ExtractUsedIndices(self)
if util.HasAffineDOFs(cspec):
affine_dofs = (DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis)
# Bug in OpenRAVE ExtractUsedIndices function makes
# dof_indices = affine_dofs. Temporary workaround for that bug.
dof_indices = []
logger.warning(
'Trajectory contains affine DOFs. Any regular DOFs'
' will be ignored.'
)
else:
affine_dofs = 0
cloned_robot.SetActiveDOFs(dof_indices, affine_dofs)
logger.debug(
'Setting robot "%s" DOFs %s (affine? %d) as active for'
' post-processing.',
cloned_robot.GetName(), list(dof_indices), affine_dofs
)
if len(dof_indices) and affine_dofs:
raise ValueError(
'Trajectory contains both affine and regular DOFs.')
# Special case for timing affine-only trajectories.
elif affine_dofs:
traj = self.affine_retimer.RetimeTrajectory(
cloned_robot, path, defer=False, **affine_retimer_options)
else:
# Directly compute a timing of smooth trajectories.
if smooth:
logger.warning(
'Post-processing smooth paths is not supported.'
' Using the default post-processing logic; this may'
' significantly change the geometric path.'
)
# The trajectory is constrained. Retime it without changing the
# geometric path.
if constrained:
logger.debug('Retiming a constrained path. The output'
' trajectory will stop at every waypoint.')
traj = self.retimer.RetimeTrajectory(
cloned_robot, path, defer=False, **retiming_options)
# The trajectory is not constrained, so we can shortcut it
# before execution.
else:
if self.simplifier is not None:
logger.debug('Shortcutting an unconstrained path.')
shortcut_path = self.simplifier.ShortcutPath(
cloned_robot, path, defer=False, **shortcut_options)
else:
logger.debug('Skipping shortcutting; no simplifier'
' available.')
shortcut_path = path
logger.debug('Smoothing an unconstrained path.')
traj = self.smoother.RetimeTrajectory(
cloned_robot, shortcut_path, defer=False,
**smoothing_options)
# Copy the trajectory into the output environment.
output_traj = CopyTrajectory(traj, env=self.GetEnv())
# Copy meta-data from the path to the output trajectory.
output_traj.SetDescription(path.GetDescription())
return output_traj
if defer is True:
from trollius.executor import get_default_executor
from trollius.futures import wrap_future
if executor is None:
executor = get_default_executor()
return wrap_future(executor.submit(do_postprocess))
elif defer is False:
return do_postprocess()
else:
raise ValueError('Received unexpected value "{:s}" for defer.'.format(defer))
