def get_rrt_traj(env, robot, active_dof, init_dof, end_dof):
# assert body in env.GetRobot()
active_dofs = robot.GetActiveDOFIndices()
robot.SetActiveDOFs(active_dof)
robot.SetActiveDOFValues(init_dof)
params = Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig(end_dof) # set goal to all ones
# # forces parabolic planning with 40 iterations
# import ipdb; ipdb.set_trace()
params.SetExtraParameters("""<_postprocessing planner="parabolicsmoother">
<_nmaxiterations>20</_nmaxiterations>
</_postprocessing>""")
planner = RaveCreatePlanner(env, 'birrt')
planner.InitPlan(robot, params)
traj = RaveCreateTrajectory(env, '')
result = planner.PlanPath(traj)
if result == False:
robot.SetActiveDOFs(active_dofs)
return None
traj_list = []
for i in range(traj.GetNumWaypoints()):
# get the waypoint values, this holds velocites, time stamps, etc
data = traj.GetWaypoint(i)
# extract the robot joint values only
dofvalues = traj.GetConfigurationSpecification().ExtractJointValues(
data, robot, robot.GetActiveDOFIndices())
# raveLogInfo('waypint %d is %s'%(i,np.round(dofvalues, 3)))
traj_list.append(np.round(dofvalues, 3))
robot.SetActiveDOFs(active_dofs)
return np.array(traj_list)
def get_ompl_rrtconnect_traj(env, robot, active_dof, init_dof, end_dof):
# assert body in env.GetRobot()
dof_inds = robot.GetActiveDOFIndices()
robot.SetActiveDOFs(active_dof)
robot.SetActiveDOFValues(init_dof)
params = Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig(end_dof) # set goal to all ones
# forces parabolic planning with 40 iterations
planner = RaveCreatePlanner(env, 'OMPL_RRTConnect')
planner.InitPlan(robot, params)
traj = RaveCreateTrajectory(env, '')
planner.PlanPath(traj)
traj_list = []
for i in range(traj.GetNumWaypoints()):
# get the waypoint values, this holds velocites, time stamps, etc
data = traj.GetWaypoint(i)
# extract the robot joint values only
dofvalues = traj.GetConfigurationSpecification().ExtractJointValues(
data, robot, robot.GetActiveDOFIndices())
# raveLogInfo('waypint %d is %s'%(i,np.round(dofvalues, 3)))
traj_list.append(np.round(dofvalues, 3))
robot.SetActiveDOFs(dof_inds)
return traj_list
def PlanPath(self, planner_name):
params = Planner.PlannerParameters()
params.SetRobotActiveJoints(self.robot_)
init = self.env_.RobotGetInitialPosition()
goal = self.env_.RobotGetGoalPosition()
params.SetInitialConfig(init)
params.SetGoalConfig(goal)
planner = RaveCreatePlanner(self.env_.env, planner_name)
if planner is None:
print "###############################################################"
print "PLANNER", planner_name, "not implemented"
print "###############################################################"
sys.exit(0)
#######################################################################
planner.InitPlan(self.robot_, params)
rave_traj = RaveCreateTrajectory(self.env_.env, '')
t1 = time.time()
result = planner.PlanPath(rave_traj)
t2 = time.time()
if result != PlannerStatus.HasSolution:
print "Could not find geometrical path"
print "Planner:", planner_name
print "Status :", result
return None
print "Planner", planner_name, " success | time:", t2 - t1
return rave_traj
class MacSmoother(BasePlanner):
def __init__(self):
super(MacSmoother, self).__init__()
self.blender = RaveCreatePlanner(self.env, 'PrSplineMacBlender')
if self.blender is None:
raise UnsupportedPlanningError(
'Unable to create PrSplineMacBlender planner. Is or_pr_spline'
' installed and in your OPENRAVE_PLUGIN path?')
self.retimer = RaveCreatePlanner(self.env, 'PrSplineMacTimer')
if self.retimer is None:
raise UnsupportedPlanningError(
'Unable to create PrSplineMacRetimer planner. Is or_pr_spline'
' installed and in your OPENRAVE_PLUGIN path?')
@ClonedPlanningMethod
def RetimeTrajectory(self, robot, path):
# Copy the input trajectory into the planning environment. This is
# necessary for two reasons: (1) the input trajectory may be in another
# environment and/or (2) the retimer modifies the trajectory in-place.
output_traj = CopyTrajectory(path, env=self.env)
# Remove co-linear waypoints.
if not IsTimedTrajectory(output_traj):
output_traj = SimplifyTrajectory(output_traj, robot)
# Blend the piecewise-linear input trajectory. The blender outputs a
# collision-free path, consisting of piecewise-linear segments and
# quintic blends through waypoints.
try:
params = Planner.PlannerParameters()
self.blender.InitPlan(robot, params)
status = self.blender.PlanPath(output_traj)
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError('Blending trajectory failed.')
except openrave_exception as e:
raise PlanningError('Blending trajectory failed: ' + str(e))
# Find the time-optimal trajectory that follows the blended path
# subject to joint velocity and acceleration limits.
try:
params = Planner.PlannerParameters()
self.retimer.InitPlan(robot, params)
status = self.retimer.PlanPath(output_traj)
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError('Timing trajectory failed.')
except openrave_exception as e:
raise PlanningError('Timing trajectory failed: ' + str(e))
SetTrajectoryTags(output_traj, {Tags.SMOOTH: True}, append=True)
return output_traj
def __init__(self, algorithm, default_options=None):
from .base import UnsupportedPlanningError
from openravepy import RaveCreatePlanner
super(OpenRAVERetimer, self).__init__()
self.algorithm = algorithm
self.default_options = default_options or dict()
self.planner = RaveCreatePlanner(self.env, algorithm)
if self.planner is None:
raise UnsupportedPlanningError(
'Unable to create "{:s}" planner.'.format(algorithm))
def planing(env, robot, params, traj, planner):
t0 = time.time()
planner = RaveCreatePlanner(env, planner)
planner.InitPlan(robot, params)
planner.PlanPath(traj)
traj_list = []
for i in range(traj.GetNumWaypoints()):
dofvalues = traj.GetConfigurationSpecification().ExtractJointValues(
traj.GetWaypoint(i), robot, robot.GetActiveDOFIndices())
traj_list.append(np.round(dofvalues, 3))
t1 = time.time()
total = t1 - t0
print "{} Proforms: {}s".format(planner, total)
return traj_list
def __init__(self):
super(MacSmoother, self).__init__()
self.blender = RaveCreatePlanner(self.env, 'PrSplineMacBlender')
if self.blender is None:
raise UnsupportedPlanningError(
'Unable to create PrSplineMacBlender planner. Is or_pr_spline'
' installed and in your OPENRAVE_PLUGIN path?')
self.retimer = RaveCreatePlanner(self.env, 'PrSplineMacTimer')
if self.retimer is None:
raise UnsupportedPlanningError(
'Unable to create PrSplineMacRetimer planner. Is or_pr_spline'
' installed and in your OPENRAVE_PLUGIN path?')
def _Plan(self,
robot,
goal=None,
tsrchains=None,
timelimit=None,
ompl_args=None,
formatted_extra_params=None,
timeout=None,
**kw_args):
extraParams = ''
# Handle the 'timelimit' parameter.
if timeout is not None:
warnings.warn('"timeout" has been replaced by "timelimit".',
DeprecationWarning)
timelimit = timeout
if timelimit is None:
timelimit = self.default_timelimit
if timelimit <= 0.:
raise ValueError('"timelimit" must be positive.')
extraParams += '<time_limit>{:f}</time_limit>'.format(timelimit)
env = robot.GetEnv()
planner_name = 'OMPL_{:s}'.format(self.algorithm)
planner = RaveCreatePlanner(env, planner_name)
if planner is None:
raise UnsupportedPlanningError(
'Unable to create "{:s}" planner. Is or_ompl installed?'.
format(planner_name))
# Handle other parameters that get passed directly to OMPL.
if ompl_args is None:
ompl_args = dict()
merged_ompl_args = deepcopy(self.default_ompl_args)
merged_ompl_args.update(ompl_args)
for key, value in merged_ompl_args.iteritems():
extraParams += '<{k:s}>{v:s}</{k:s}>'.format(k=str(key),
v=str(value))
# Serialize TSRs into the space-delimited format used by CBiRRT.
if tsrchains is not None:
for chain in tsrchains:
if chain.sample_start:
raise UnsupportedPlanningError(
'OMPL does not support start TSRs.')
# Goal TSRs are parsed using the space delimited CBiRRT format.
if chain.sample_goal:
extraParams += '<{k:s}>{v:s}</{k:s}>'.format(
k='tsr_chain', v=SerializeTSRChain(chain))
# Constraint TSRs are parsed by pr_constraint_or_ompl, which
# uses a JSON format. This differs from the space delimited
# format used by CBiRRT.
if chain.constrain:
if self.supports_constraints:
extraParams += '<{k:s}>{v:s}</{k:s}>'.format(
k='tsr_chain_constraint', v=chain.to_json())
else:
raise UnsupportedPlanningError(
'The "{:s}" OMPL planner does not support TSR'
' constraints.'.format(self.algorithm))
# Enable baked collision checking. This is handled natively.
if self._is_baked and merged_ompl_args.get('do_baked', True):
extraParams += '<do_baked>1</do_baked>'
# Serialize formatted values last, in case of any overrides.
if formatted_extra_params is not None:
extraParams += formatted_extra_params
# Setup the planning query.
params = openravepy.Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
if goal is not None:
params.SetGoalConfig(goal)
params.SetExtraParameters(extraParams)
planner.InitPlan(robot, params)
# Bypass the context manager since or_ompl does its own baking.
env = robot.GetEnv()
robot_checker = self.robot_checker_factory(robot)
options = robot_checker.collision_options
with CollisionOptionsStateSaver(env.GetCollisionChecker(), options), \
robot.CreateRobotStateSaver(Robot.SaveParameters.LinkTransformation):
traj = RaveCreateTrajectory(env, 'GenericTrajectory')
status = planner.PlanPath(traj, releasegil=True)
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError('Planner returned with status {:s}.'.format(
str(status)),
deterministic=False)
# Tag the trajectory as non-determistic since most OMPL planners are
# randomized. Additionally tag the goal as non-deterministic if OMPL
# chose from a set of more than one goal configuration.
SetTrajectoryTags(traj, {
Tags.DETERMINISTIC_TRAJECTORY: False,
Tags.DETERMINISTIC_ENDPOINT: tsrchains is None,
},
append=True)
return traj
class OpenRAVERetimer(BasePlanner):
def __init__(self, algorithm, default_options=None):
from .base import UnsupportedPlanningError
from openravepy import RaveCreatePlanner
super(OpenRAVERetimer, self).__init__()
self.algorithm = algorithm
self.default_options = default_options or dict()
self.planner = RaveCreatePlanner(self.env, algorithm)
if self.planner is None:
raise UnsupportedPlanningError(
'Unable to create "{:s}" planner.'.format(algorithm))
def __str__(self):
return self.algorithm
@ClonedPlanningMethod
def RetimeTrajectory(self, robot, path, options=None, **kw_args):
from openravepy import CollisionOptions, CollisionOptionsStateSaver
from copy import deepcopy
# Validate the input path.
cspec = path.GetConfigurationSpecification()
joint_values_group = cspec.GetGroupFromName('joint_values')
if joint_values_group is None:
raise ValueError('Trajectory is missing the "joint_values" group.')
elif HasAffineDOFs(cspec):
raise UnsupportedPlanningError(
'OpenRAVERetimer does not support affine DOFs.')
elif joint_values_group.interpolation != 'linear':
logger.warning(
'Path has interpolation of type "%s"; only "linear"'
' interpolation is supported.',
joint_values_group.interpolation)
# Set parameters.
all_options = deepcopy(self.default_options)
if options is not None:
all_options.update(options)
params = Planner.PlannerParameters()
params.SetConfigurationSpecification(
self.env, cspec.GetTimeDerivativeSpecification(0))
params_str = CreatePlannerParametersString(all_options, params)
# Copy the input trajectory into the planning environment. This is
# necessary for two reasons: (1) the input trajectory may be in another
# environment and/or (2) the retimer modifies the trajectory in-place.
output_traj = CopyTrajectory(path, env=self.env)
# Remove co-linear waypoints. Some of the default OpenRAVE retimers do
# not perform this check internally (e.g. ParabolicTrajectoryRetimer).
if not IsTimedTrajectory(output_traj):
output_traj = SimplifyTrajectory(output_traj, robot)
# Only collision check the active DOFs.
dof_indices, _ = cspec.ExtractUsedIndices(robot)
robot.SetActiveDOFs(dof_indices)
# Compute the timing. This happens in-place.
self.planner.InitPlan(None, params_str)
with CollisionOptionsStateSaver(self.env.GetCollisionChecker(),
CollisionOptions.ActiveDOFs):
status = self.planner.PlanPath(output_traj, releasegil=True)
if status not in [
PlannerStatus.HasSolution,
PlannerStatus.InterruptedWithSolution
]:
raise PlanningError('Retimer returned with status {:s}.'.format(
str(status)))
return output_traj