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 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 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 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
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 _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