def __init_traj(self, joint_target, algorithm): """ Creates an initial trajectory plan between start and end goal poses """ # Issue with the init and goal configuratons -> Planner thinks that the arms are in collision algo = "OMPL_" + algorithm planner = op.RaveCreatePlanner(self.env, algo) # Initializes a planner with algorithm simplifier = op.RaveCreatePlanner(self.env, 'OMPL_Simplifier') params = planner.PlannerParameters() # Creates an empty param to be filled params.SetRobotActiveJoints(self.robot) params.SetGoalConfig(joint_target) extraParams = ('<_nmaxiterations>{:d}</_nmaxiterations>'.format(20000)) params.SetExtraParameters(extraParams) with self.env: with self.get_robot(): print "Starting intial plan using {:s} algorithm".format(algorithm) traj = op.RaveCreateTrajectory(self.env, '') planner.InitPlan(self.get_robot(), params) result = planner.PlanPath(traj) IPython.embed() assert result == op.PlannerStatus.HasSolution # print 'Calling the OMPL_Simplifier for shortcutting.' # simplifier.InitPlan(self.get_robot(), op.Planner.PlannerParameters()) # result = simplifier.PlanPath(traj) # assert result == op.PlannerStatus.HasSolution trajectory = [traj.GetWaypoint(i).tolist() for i in range(traj.GetNumWaypoints())] return trajectory
def __init_traj(self, manip, joint_target, algorithm): """ Creates an initial trajectory plan between start and end goal poses """ # Issue with the init and goal configuratons -> Planner thinks that the arms are in collision Algo = "OMPL_" + algorithm planner = op.RaveCreatePlanner( self.env, Algo) # Initializes a planner with algorithm simplifier = op.RaveCreatePlanner(self.env, 'OMPL_Simplifier') # self.env.GetCollisionChecker().SetCollisionOptions(op.CollisionOptions.Contacts) with self.env: arm_indx = self.get_manip(name=manip).GetArmIndices() self.get_robot().SetActiveDOFs( arm_indx) # Plan for only the arm specified in manip self.get_robot().SetActiveDOFValues(self.right_arm_DOF) self.get_robot().SetActiveManipulator(self.get_manip(name=manip)) params = planner.PlannerParameters( ) # Creates an empty param to be filled params.SetRobotActiveJoints(self.robot) params.SetGoalConfig(joint_target[0]) extraParams = ('<_nmaxiterations>{:d}</_nmaxiterations>'.format(10000)) params.SetExtraParameters(extraParams) with self.env: with self.get_robot(): print "Starting intial plan using {:s} algorithm".format( algorithm) traj = op.RaveCreateTrajectory(self.env, '') planner.InitPlan(self.get_robot(), params) result = planner.PlanPath(traj) assert result == op.PlannerStatus.HasSolution print 'Calling the OMPL_Simplifier for shortcutting.' simplifier.InitPlan(self.get_robot(), op.Planner.PlannerParameters()) result = simplifier.PlanPath(traj) assert result == op.PlannerStatus.HasSolution trajectory = [ traj.GetWaypoint(i).tolist() for i in range(traj.GetNumWaypoints()) ] return trajectory
def PlanToConfigurations(self, robot, goal_configs, **kw_args): params = self.defaults._replace(**kw_args) # create family with type(self).AddedFamilyModule(self.env, robot) as family: setcache_path = self.get_setcache_path(family, params.roadmap, read_only=True) planner = openravepy.RaveCreatePlanner(self.env, 'FamilyPlanner') if planner is None: raise prpy.planning.base.UnsupportedPlanningError('Unable to create FamilyPlanner planner.') # serialize parameters orparams = openravepy.Planner.PlannerParameters() orparams.SetRobotActiveJoints(robot) goal_configs_stacked = [] for goal_config in goal_configs: goal_configs_stacked.extend(goal_config) orparams.SetGoalConfig(goal_configs_stacked) xml = self.xml_from_params(params) xml.append('<family_module>{}</family_module>'.format(family.SendCommand('GetInstanceId'))) if setcache_path != '': xml.append('<family_setcaches><setcache><filename>{}</filename></setcache></family_setcaches>'.format(setcache_path)) orparams.SetExtraParameters('\n'.join(xml)) planner.InitPlan(robot, orparams) # plan path traj = openravepy.RaveCreateTrajectory(self.env, '') status = planner.PlanPath(traj) if status == openravepy.PlannerStatus.HasSolution: return traj else: raise prpy.planning.base.PlanningError('LEMUR status: {}'.format(status))
def plan(self, qgoal, max_iters=40, max_ppiters=40): traj = orpy.RaveCreateTrajectory(self.env, '') params = orpy.Planner.PlannerParameters() # Configuration specification robot_name = self.robot.GetName() torso_idx = self.torso_joint.GetDOFIndex() spec = orpy.ConfigurationSpecification() spec.AddGroup('joint_values {0} {1}'.format(robot_name, torso_idx), 1, '') left_spec = self.left_manip.GetArmConfigurationSpecification() right_spec = self.right_manip.GetArmConfigurationSpecification() spec += left_spec + right_spec params.SetConfigurationSpecification(self.env, spec) params.SetGoalConfig(qgoal) params.SetMaxIterations(max_iters) params.SetPostProcessing( 'ParabolicSmoother', '<_nmaxiterations>{}</_nmaxiterations>'.format(max_ppiters)) planner = orpy.RaveCreatePlanner(self.env, 'BiRRT') success = planner.InitPlan(None, params) status = orpy.PlannerStatus.Failed if success: status = planner.PlanPath(traj) if status != orpy.PlannerStatus.HasSolution: traj = None return traj
def retime_trajectory(robot, traj, method): """ Retime an OpenRAVE trajectory using the specified method. Parameters ---------- robot: orpy.Robot The OpenRAVE robot traj: orpy.Trajectory The traj to be retimed. The time paremetrization will be *overwritten*. method: str Retiming method. Available options are: `LinearTrajectoryRetimer`, `ParabolicTrajectoryRetimer`, `CubicTrajectoryRetimer` Returns ------- status: orpy.PlannerStatus Flag indicating the status of the trajectory retiming. It can be: `Failed`, `HasSolution`, `Interrupted` or `InterruptedWithSolution`. """ env = robot.GetEnv() # Populate planner parameters params = orpy.Planner.PlannerParameters() params.SetRobotActiveJoints(robot) params.SetMaxIterations(20) params.SetPostProcessing('', '') # Generate the trajectory planner = orpy.RaveCreatePlanner(env, method) status = orpy.PlannerStatus.Failed success = planner.InitPlan(robot, params) if success: status = planner.PlanPath(traj) return status
def __init__(self,env,youbots,parallelplanning=False): self.env = env self.planner = orpy.RaveCreatePlanner(self.env,'birrt') self.youbots = youbots self.parallelplanning = parallelplanning #self.youbots = {} # TODO remove after debug #self.youbots['drc1'] = youbots['drc1'] # TODO remove #self.youbots['drc2'] = youbots['drc2'] # TODO remove if self.parallelplanning: self.awaypose = np.eye(4) self.awaypose[:2,3] = np.array([100.0, 100.0]) self.youbots_base_joints = {} for yname in self.youbots: print yname youbot_base_joints = env.ReadRobotURI('robots/kuka-youbot-baseasjoints.robot.xml') youbot_base_joints.SetName(yname+'_baseasjoints') self.env.Add(youbot_base_joints,True) youbot_base_joints.SetTransform(self.awaypose) youbot_base_joints.GetLink('base0').Enable(False) youbot_base_joints.GetLink('base1').Enable(False) youbot_base_joints.GetLink('base2').Enable(False) self.youbots_base_joints[yname] = youbot_base_joints else: self.planners = {} for y in youbots: self.planners[y] = orpy.interfaces.BaseManipulation(youbots[y],plannername='BiRRT')
def visualize_motions(self): planner = orpy.RaveCreatePlanner(self.env, 'birrt') params = orpy.Planner.PlannerParameters() params.SetRobotActiveJoints(self.robot) candidate_traj = [] for i in range(len(self.solutions)): print "Visualizing the ", i, " solution" self.get_current_joint_states() time.sleep(1) #raw_input("Press enter to bring robot home") self.robot.SetActiveDOFValues(self.initConfig) params.SetInitialConfig(self.initConfig) params.SetGoalConfig(self.solutions[i-1]) params.SetPostProcessing('ParabolicSmoother', '<_nmaxiterations>50</_nmaxiterations>') planner.InitPlan(self.robot, params) traj = orpy.RaveCreateTrajectory(self.env, '') planner.PlanPath(traj) ros_traj = ros_trajectory_from_openrave(self.robot.GetName(), traj) print "duration of the path is: ", ros_traj.points[-1].time_from_start candidate_traj.append(ros_traj) controller = self.robot.GetController() raw_input("Press enter to visualize the plan in openrave") controller.SetPath(traj) # sort all the feasible paths according to their durations candidate_traj.sort(key=sort_time) self.optimalPath = candidate_traj[0]
def run_planner(self, planner_name): with self.env: # Setup params = openravepy.Planner.PlannerParameters() params.SetRobotActiveJoints(self.robot) params.SetGoalConfig(self.GOAL_CONFIG) params.SetExtraParameters('<time_limit>30</time_limit>') cspec = self.robot.GetActiveConfigurationSpecification() traj = openravepy.RaveCreateTrajectory(self.env, '') for i in xrange(self.NUM_ATTEMPTS): # Act planner = openravepy.RaveCreatePlanner(self.env, 'OMPL_' + planner_name) planner.InitPlan(self.robot, params) result = planner.PlanPath(traj) if result == openravepy.PlannerStatus.HasSolution: break # Assert self.assertEqual(result, openravepy.PlannerStatus.HasSolution) self.assertGreaterEqual(traj.GetNumWaypoints(), 1) numpy.testing.assert_array_almost_equal(traj.GetWaypoint(0, cspec), self.START_CONFIG) numpy.testing.assert_array_almost_equal( traj.GetWaypoint(traj.GetNumWaypoints() - 1, cspec), self.GOAL_CONFIG)
def __init__(self, robot_name, _env, end_effector_name='gripper'): self.env = _env # initialize robot self.robot = self.env.GetRobot(robot_name) self.manipulator = self.robot.SetActiveManipulator(end_effector_name) self.robot.SetActiveDOFs(self.manipulator.GetArmIndices()) # initialize IK model self.ikmodel = orpy.databases.inversekinematics.InverseKinematicsModel( self.robot, iktype=orpy.IkParameterization.Type.Transform6D) if not self.ikmodel.load(): print "[INFO] generating IK model" self.ikmodel.autogenerate() # create path planner self.planner = orpy.RaveCreatePlanner( self.env, 'birrt') # Using bidirectional RRT self.params = orpy.Planner.PlannerParameters() self.params.SetRobotActiveJoints(self.robot) # # get robot controller # self.controller = self.robot.GetController() # create interface for manipulation task self.taskmanip = orpy.interfaces.TaskManipulation(self.robot)
def __init__(self): super(SBPLPlanner, self).__init__() try: self.planner = openravepy.RaveCreatePlanner(self.env, 'SBPL') except openravepy.openrave_exception: raise UnsupportedPlanningError('Unable to create SBPL module')
def calculate_all_paths(self, goal_config_list): result = [] path_list = [] ros_traj_list = [] sum_plan_time = 0 for goal_config in goal_config_list: planner = orpy.RaveCreatePlanner(self.env, 'birrt') params = orpy.Planner.PlannerParameters() params.SetRobotActiveJoints(self.robot) params.SetInitialConfig(self.init_config) params.SetGoalConfig(goal_config) #params.SetPostProcessing('ParabolicSmoother', '<_nmaxiterations>40</_nmaxiterations>') #params.SetPostProcessing('shortcut_linear', '<_nmaxiterations>40</_nmaxiterations>') planner.InitPlan(self.robot, params) traj = orpy.RaveCreateTrajectory(self.env, '') curr_start_time = time.time() planner.PlanPath(traj) curr_end_time = time.time() sum_plan_time = sum_plan_time + curr_end_time - curr_start_time ros_traj = test_irb1200_model.ros_trajectory_from_openrave( self.robot.GetName(), traj) print "duration of the path is: ", ros_traj.points[ -1].time_from_start path_list.append(traj) ros_traj_list.append(ros_traj) average_plan_time = sum_plan_time / len(goal_config_list) print "average motion planning time is: ", average_plan_time result.append(path_list) result.append(ros_traj_list) return result
def _Plan(self, robot, goals, maxiter=500, continue_planner=False, or_args=None, **kw_args): env = robot.GetEnv() planner = openravepy.RaveCreatePlanner(env, self.algorithm) if planner is None: raise UnsupportedPlanningError( 'Unable to create {:s} module.'.format(str(self))) # Get rid of default postprocessing extraParams = ('<_postprocessing planner="">' '<_nmaxiterations>0</_nmaxiterations>' '</_postprocessing>') # Maximum planner iterations extraParams += ( '<_nmaxiterations>{:d}</_nmaxiterations>'.format(maxiter)) if or_args is not None: for key, value in or_args.iteritems(): extraParams += '<{k:s}>{v:s}</{k:s}>'.format(k=str(key), v=str(value)) params = openravepy.Planner.PlannerParameters() params.SetRobotActiveJoints(robot) params.SetGoalConfig(goals) params.SetExtraParameters(extraParams) traj = openravepy.RaveCreateTrajectory(env, 'GenericTrajectory') try: env.Lock() with robot.CreateRobotStateSaver( openravepy.Robot.SaveParameters.LinkTransformation): # Plan. if (not continue_planner) or not self.setup: planner.InitPlan(robot, params) self.setup = True status = planner.PlanPath(traj, releasegil=True) from openravepy import PlannerStatus if status not in [ PlannerStatus.HasSolution, PlannerStatus.InterruptedWithSolution ]: raise PlanningError( 'Planner returned with status {:s}.'.format( str(status))) except Exception as e: raise PlanningError('Planning failed with error: {:s}'.format(e)) finally: env.Unlock() return traj
def wam7_test(self): env = openravepy.Environment() try: robot = env.ReadRobotXMLFile('robots/barrettwam.robot.xml') env.Add(robot) robot.SetActiveDOFs(robot.GetManipulator('arm').GetArmIndices()) robot.SetActiveDOFValues(q_start) planner = openravepy.RaveCreatePlanner(env, 'LEMUR') self.assertIsNotNone(planner) params = openravepy.Planner.PlannerParameters() params.SetRobotActiveJoints(robot) params.SetGoalConfig(q_goal) params.SetExtraParameters( '<roadmap_type>Halton</roadmap_type>' + '<roadmap_param>num=1000</roadmap_param>' + '<roadmap_param>radius=2.0</roadmap_param>') planner.InitPlan(robot, params) traj = openravepy.RaveCreateTrajectory(env, '') result = planner.PlanPath(traj) self.assertEqual(result, openravepy.PlannerStatus.HasSolution) self.assertEqual(6, traj.GetNumWaypoints()) for iwp in range(6): wp = traj.GetWaypoint(iwp) self.assertEqual(7, len(wp)) for a, b in zip(wp, traj_expected[iwp]): self.assertAlmostEqual(a, b, delta=1.0e-5) finally: env.Destroy()
def __init__(self): super(OMPLSimplifier, self).__init__() planner_name = 'OMPL_Simplifier' self.planner = openravepy.RaveCreatePlanner(self.env, planner_name) if self.planner is None: raise UnsupportedPlanningError( 'Unable to create OMPL_Simplifier planner.')
def __init__(self, algorithm='birrt'): super(OpenRAVEPlanner, self).__init__() self.setup = False self.algorithm = algorithm try: self.planner = openravepy.RaveCreatePlanner(self.env, algorithm) except openravepy.openrave_exception: raise UnsupportedPlanningError( 'Unable to create {:s} module.'.format(str(self)))
def __init__(self, **kw_args): super(LEMURPlanner, self).__init__() self.planner = openravepy.RaveCreatePlanner(self.env, 'LEMUR') if self.planner is None: raise prpy.planning.base.UnsupportedPlanningError( 'Unable to create LEMUR planner.') Params = type(self).Params self.defaults = Params(*[None for f in Params._fields])._replace(**kw_args)
def plan_to_joint_values(qgoal): # Create the planner planner = orpy.RaveCreatePlanner(env,'birrt') # Using bidirectional RRT params = orpy.Planner.PlannerParameters() params.SetRobotActiveJoints(robot) params.SetGoalConfig(qgoal) params.SetExtraParameters('<_postprocessing planner="ParabolicSmoother"><_nmaxiterations>40</_nmaxiterations></_postprocessing>') planner.InitPlan(robot, params) # Plan a trajectory traj = orpy.RaveCreateTrajectory(env, '') planner.PlanPath(traj) return traj
def __init__(self, algorithm='RRTConnect'): super(OMPLPlanner, self).__init__() self.setup = False self.algorithm = algorithm planner_name = 'OMPL_{:s}'.format(algorithm) self.planner = openravepy.RaveCreatePlanner(self.env, planner_name) if self.planner is None: raise UnsupportedPlanningError( 'Unable to create "{:s}" planner. Is or_ompl installed?'. format(planner_name))
def initialize(self, robot): # construct a new module/planner if necessary if self.module is None: self.module = openravepy.RaveCreateModule(self.env, 'Family') if self.module is None: raise prpy.planning.base.UnsupportedPlanningError( 'Unable to create Family module.') if self.planner is None: self.planner = openravepy.RaveCreatePlanner( self.env, 'FamilyPlanner') if self.planner is None: raise prpy.planning.base.UnsupportedPlanningError( 'Unable to create FamilyPlanner planner.')
def path_planning(self): planner = orpy.RaveCreatePlanner(self.env, 'birrt') # Using bidirectional RRT params = orpy.Planner.PlannerParameters() params.SetRobotActiveJoints(self.robot) params.SetGoalConfig(self.qgrasp) print "goal is: ", self.qgrasp params.SetPostProcessing('ParabolicSmoother', '<_nmaxiterations>40</_nmaxiterations>') planner.InitPlan(self.robot, params) # Plan a trajectory traj = orpy.RaveCreateTrajectory(self.env, '') planner.PlanPath(traj) # Execute the trajectory controller = self.robot.GetController() controller.SetPath(traj)
def motion_plan(self, target_config = []): planner = orpy.RaveCreatePlanner(self.env, 'birrt') params = orpy.Planner.PlannerParameters() params.SetRobotActiveJoints(self.robot) self.get_current_joint_states() params.SetInitialConfig(self.initConfig) self.robot.SetActiveDOFValues(self.initConfig) self.initConfig = [] if len(target_config)>0: print "going home" print target_config params.SetGoalConfig(target_config) else: choose = raw_input("Enter 1 to execute the shortest path, and 2 to customize path selection") if choose == "1": return self.optimalPath elif choose == "2": sol_num = raw_input("Please enter the solution number that you want to execute") sol_num = int(sol_num)-1 print "planning to goal" print self.solutions[sol_num] params.SetGoalConfig(self.solutions[sol_num]) else: print "Invalid input, exiting ..." return params.SetPostProcessing('ParabolicSmoother', '<_nmaxiterations>40</_nmaxiterations>') planner.InitPlan(self.robot, params) # Plan a trajectory traj = orpy.RaveCreateTrajectory(self.env, '') planner.PlanPath(traj) controller = self.robot.GetController() raw_input("Press enter to visualize the plan in openrave") controller.SetPath(traj) # print the sequence of joint angles times = np.arange(0, traj.GetDuration(), 0.01) qvect = np.zeros((len(times), self.robot.GetActiveDOF())) spec = traj.GetConfigurationSpecification() for i in range(len(times)): trajdata = traj.Sample(times[i]) qvect[i,:] = spec.ExtractJointValues(trajdata, self.robot, self.manipulator.GetArmIndices(), 0) print qvect ros_traj = ros_trajectory_from_openrave(self.robot.GetName(), traj) print "duration of the path is: ", ros_traj.points[-1].time_from_start return ros_traj
def ShortcutPath(self, robot, path, timeout=1., **kwargs): # The planner operates in-place, so we need to copy the input path. We # also need to copy the trajectory into the planning environment. planner_name = 'OMPL_Simplifier' env = robot.GetEnv() planner = openravepy.RaveCreatePlanner(env, planner_name) if planner is None: raise UnsupportedPlanningError( 'Unable to create {} planner.'.format(planner_name)) output_path = CopyTrajectory(path, env=env) extraParams = '<time_limit>{:f}</time_limit>'.format(timeout) params = openravepy.Planner.PlannerParameters() params.SetRobotActiveJoints(robot) params.SetExtraParameters(extraParams) # TODO: It would be nice to call planningutils.SmoothTrajectory here, # but we can't because it passes a NULL robot to InitPlan. This is an # issue that needs to be fixed in or_ompl. with robot.CreateRobotStateSaver( Robot.SaveParameters.LinkTransformation): planner.InitPlan(robot, params) status = planner.PlanPath(output_path, releasegil=True) if status not in [ PlannerStatus.HasSolution, PlannerStatus.InterruptedWithSolution ]: raise PlanningError( 'Simplifier returned with status {0:s}.'.format( str(status))) # Tag the trajectory as non-deterministic since the OMPL path # simplifier samples random candidate shortcuts. It does not, however, # change the endpoint, so we leave DETERMINISTIC_ENDPOINT unchanged. SetTrajectoryTags(output_path, { Tags.SMOOTH: True, Tags.DETERMINISTIC_TRAJECTORY: False, }, append=True) return output_path
def plan(self, method, args, kwargs): # Can't handle deferred planning yet. assert not kwargs['defer'] # According to prpy spec, the first positional argument is 'robot'. robot = args[0] # Call the wrapped planner to get the seed trajectory. planner_method = getattr(self._planners[0], method) traj = planner_method(*args, **kwargs) # Simplify redundant waypoints in the trajectory. from prpy.util import SimplifyTrajectory traj = SimplifyTrajectory(traj, robot) # Run path shortcutting on the RRT path. import openravepy params = openravepy.Planner.PlannerParameters() params.SetExtraParameters('<time_limit>0.25</time_limit>') simplifier = openravepy.RaveCreatePlanner(robot.GetEnv(), 'OMPL_Simplifier') simplifier.InitPlan(robot, params) with robot: result = simplifier.PlanPath(traj) assert result == openravepy.PlannerStatus.HasSolution # Call Trajopt to optimize the seed trajectory. # Try different distance penalties until out of collision. penalties = numpy.logspace(numpy.log(0.05), numpy.log(0.5), num=4) for distance_penalty in penalties: try: return self._trajopt.OptimizeTrajectory(robot, traj, **kwargs) except PlanningError: logger.warn( "Failed to optimize trajectory " "with distance penalty: {:f}".format(distance_penalty)) # If all of the optimizations ended in collision, # just return the original. logger.warn("Failed to optimize trajectory, " "returning unoptimized solution.") return traj
def planRrt(self, configStart, configEnd, mover): '''TODO''' print("Planning with RRT* ...") planner = openravepy.RaveCreatePlanner(mover.env, 'OMPL_RRTstar') # Setup the planning instance. params = openravepy.Planner.PlannerParameters() params.SetRobotActiveJoints(mover.robot) params.SetInitialConfig(configStart) params.SetGoalConfig(configEnd) # Set the timeout and planner-specific parameters. You can view a list of # supported parameters by calling: planner.SendCommand('GetParameters') params.SetExtraParameters('<range>' + str(self.maxCSpaceJump) + '</range>') params.SetExtraParameters('<time_limit>' + str(self.timeout) + '</time_limit>') isGoalValid = planner.InitPlan(mover.robot, params) if not isGoalValid: print("planRrt: End configuration is invalid.") return None # invoke the planner traj = openravepy.RaveCreateTrajectory(mover.env, '') result = planner.PlanPath(traj) if not result == openravepy.PlannerStatus.HasSolution: print("planRrt: No solution found.") return None traj_waypoints = [] for i in xrange(traj.GetNumWaypoints()): traj_waypoints.append(traj.GetWaypoint(i)[:7]) subsampled_traj = traj_waypoints subsampled_traj = self.subsampleTraj(traj_waypoints) return subsampled_traj
def plan_to_joint_configuration(robot, qgoal, planner='birrt', max_planner_iterations=20, max_postprocessing_iterations=40): env = robot.GetEnv() rave_planner = orpy.RaveCreatePlanner(env, planner) params = orpy.Planner.PlannerParameters() params.SetRobotActiveJoints(robot) params.SetGoalConfig(qgoal) params.SetMaxIterations(max_planner_iterations) params.SetPostProcessing( 'ParabolicSmoother', '<_nmaxiterations>{0}</_nmaxiterations>'.format( max_postprocessing_iterations)) success = rave_planner.InitPlan(robot, params) if not success: return None # Plan a trajectory traj = orpy.RaveCreateTrajectory(env, '') status = rave_planner.PlanPath(traj) if status != orpy.PlannerStatus.HasSolution: return None return traj
def test_Simplifier(self): with self.env: # Setup simplifier = openravepy.RaveCreatePlanner(self.env, 'OMPL_Simplifier') params = openravepy.Planner.PlannerParameters() cspec = self.robot.GetActiveConfigurationSpecification() traj = openravepy.RaveCreateTrajectory(self.env, '') traj.deserialize(self.TRAJECTORY_XML) # Act simplifier.InitPlan(self.robot, params) result = simplifier.PlanPath(traj) # Assert self.assertEqual(result, openravepy.PlannerStatus.HasSolution) self.assertGreaterEqual(traj.GetNumWaypoints(), 1) numpy.testing.assert_array_almost_equal(traj.GetWaypoint(0, cspec), self.START_CONFIG) numpy.testing.assert_array_almost_equal( traj.GetWaypoint(traj.GetNumWaypoints() - 1, cspec), self.GOAL_CONFIG)
def plan_transit_motion(robot, q_start, q_goal, pregrasp_start=True, pregrasp_goal=True): """ Plan a transit motion (non-grasping motion) for robot moving from q_start to q_goal. @type robot: openravepy.Robot @param robot: The robot to plan this motion for @type q_start: numpy.ndarray @param q_start: Start configuration of the robot @type q_goal: numpy.ndarray @param q_goal: Goal configuration of the robot @type pregrasp_start: bool @param pregrasp_start: Indicates if pregrasp motion at the beginning of the trajectory should be planned @type pregrasp_goal: bool @param pregrasp_goal: Indicates if pregrasp motion at the end of the trajectory should be planned @rtype: oepnravepy.Trajectory @return: An OpenRAVE trajectory for the robot moving from q_start to q_goal """ # MoveHandStraight parameters minsteps = 1 maxsteps = 10 steplength = 0.005 execute = False env = robot.GetEnv() manip = robot.GetActiveManipulator() basemanip = orpy.interfaces.BaseManipulation(robot) q_original = robot.GetActiveDOFValues() traj_pregrasp_start = None traj_pregrasp_goal = None with robot: # Plan pregrasp motion at the start configuration if pregrasp_start: robot.SetActiveDOFValues(q_start) eematrix = manip.GetTransform() direction = -eematrix[0:3, 2] try: traj_pregrasp_start = basemanip.MoveHandStraight( direction, minsteps=minsteps, maxsteps=maxsteps, steplength=steplength, starteematrix=eematrix, execute=execute, outputtrajobj=True) except: _log.info( "Caught an exception in MoveHandStraight (pregrasp_start)." ) return None if traj_pregrasp_start is None: _log.info("MoveHandStraight failed (pregrasp_start).") new_q_start = q_start pregrasp_start = False else: new_q_start = traj_pregrasp_start.GetWaypoint( traj_pregrasp_start.GetNumWaypoints() - 1, traj_pregrasp_start.GetConfigurationSpecification(). GetGroupFromName("joint_values")) robot.SetActiveDOFValues(new_q_start) if robot.CheckSelfCollision(): new_q_start = q_start pregrasp_start = False else: orpy.planningutils.RetimeActiveDOFTrajectory( traj_pregrasp_start, robot, hastimestamps=False, maxvelmult=0.9, maxaccelmult=0.81, plannername="parabolictrajectoryretimer") else: new_q_start = q_start # Plan pregrasp motion at the goal configuration if pregrasp_goal: robot.SetActiveDOFValues(q_goal) eematrix = manip.GetTransform() direction = -eematrix[0:3, 2] try: traj_pregrasp_goal = basemanip.MoveHandStraight( direction, minsteps=minsteps, maxsteps=maxsteps, steplength=steplength, starteematrix=eematrix, execute=execute, outputtrajobj=True) except: _log.info( "Caught an exception in MoveHandStraight (pregrasp_goal).") return None if traj_pregrasp_goal is None: _log.info("MoveHandStraight failed (pregrasp_goal).") new_q_goal = q_goal pregrasp_goal = False else: new_q_goal = traj_pregrasp_goal.GetWaypoint( traj_pregrasp_goal.GetNumWaypoints() - 1, traj_pregrasp_goal.GetConfigurationSpecification(). GetGroupFromName("joint_values")) robot.SetActiveDOFValues(new_q_goal) if robot.CheckSelfCollision(): new_q_goal = q_goal pregrasp_goal = False else: traj_pregrasp_goal_rev = orpy.planningutils.ReverseTrajectory( traj_pregrasp_goal) orpy.planningutils.RetimeActiveDOFTrajectory( traj_pregrasp_goal_rev, robot, hastimestamps=False, maxvelmult=0.9, maxaccelmult=0.81, plannername="parabolictrajectoryretimer") traj_pregrasp_goal = traj_pregrasp_goal_rev else: new_q_goal = q_goal _log.info("new_q_start = {0}".format(new_q_start.__repr__())) _log.info("new_q_goal = {0}".format(new_q_goal.__repr__())) # Plan a motion connecting new_q_start and new_q_goal try: # Using MoveActiveJoints sometimes has problems with jittering. As a fix, we plan using this birrt + parabolicsmoother planner = orpy.RaveCreatePlanner(env, 'birrt') params = orpy.Planner.PlannerParameters() params.SetRobotActiveJoints( robot ) # robot's active DOFs must be correctly set before calling this function params.SetInitialConfig(new_q_start) params.SetGoalConfig(new_q_goal) params.SetMaxIterations(10000) extraparams = '<_postprocessing planner="parabolicsmoother2"><_nmaxiterations>100</_nmaxiterations></_postprocessing>' params.SetExtraParameters(extraparams) planner.InitPlan(robot, params) traj_connect = orpy.RaveCreateTrajectory(env, '') res = planner.PlanPath(traj_connect) if not (res == orpy.PlannerStatus.HasSolution): _log.info("Planner failed.") return None except Exception as e: _log.info( "Caught an exception ({0}) in MoveActiveJoints.".format(e)) return None if traj_connect is None: _log.info("MoveActiveJoints failed.") return None trajs_list = [ traj for traj in [traj_pregrasp_start, traj_connect, traj_pregrasp_goal] if traj is not None ] final_traj = combine_openrave_trajectories(trajs_list) return final_traj
def plan_to_joint_configuration(robot, qgoal, pname='BiRRT', max_iters=20, max_ppiters=40, try_swap=False): """ Plan a trajectory to the given `qgoal` configuration. Parameters ---------- robot: orpy.Robot The OpenRAVE robot qgoal: array_like The goal configuration pname: str Name of the planning algorithm. Available options are: `BasicRRT`, `BiRRT` max_iters: float Maximum iterations for the planning stage max_ppiters: float Maximum iterations for the post-processing stage. It will use a parabolic smoother wich short-cuts the trajectory and then smooths it try_swap: bool If set, will compute the direct and reversed trajectory. The minimum duration trajectory is used. Returns ------- traj: orpy.Trajectory Planned trajectory. If plan fails, this function returns `None`. """ qstart = robot.GetActiveDOFValues() env = robot.GetEnv() planner = orpy.RaveCreatePlanner(env, pname) params = orpy.Planner.PlannerParameters() params.SetMaxIterations(max_iters) if max_ppiters > 0: params.SetPostProcessing('ParabolicSmoother', '<_nmaxiterations>{0}</_nmaxiterations>'.format(max_ppiters)) else: params.SetPostProcessing('', '') # Plan trajectory best_traj = None min_duration = float('inf') reversed_is_better = False count = 0 for qa, qb in itertools.permutations([qstart, qgoal], 2): count += 1 with robot: robot.SetActiveDOFValues(qa) params.SetGoalConfig(qb) params.SetRobotActiveJoints(robot) initsuccess = planner.InitPlan(robot, params) if initsuccess: traj = orpy.RaveCreateTrajectory(env, '') # Plan the trajectory status = planner.PlanPath(traj) if status == orpy.PlannerStatus.HasSolution: duration = traj.GetDuration() if duration < min_duration: min_duration = duration best_traj = orpy.RaveCreateTrajectory( env, traj.GetXMLId()) best_traj.Clone(traj, 0) if count == 2: reversed_is_better = True if not try_swap: break # Check if we need to reverse the trajectory if reversed_is_better: best_traj = orpy.planningutils.ReverseTrajectory(best_traj) return best_traj
def run(self, offset=0.01): """Run the demo. For each object, the robot first move to a pose that is directly on top of it. Afterward, it moves down. Next, it grabs the object to be transported. Finally, it moves to another configuration. Note: planning is done using rrt, not constrained rrt. The later is far too inefficient to be of any use. An even more critical issue with constrained rrt is that its path is too jerky, failing most smoothing attempts. Args: offset: (float, optional) Approach distance. """ fail = False # Start planning/control loop: # Every pick cycle follows the same procedure: # 1. APPROACH: visit a configuration that is directly on top of the object to pick # 2. REACH: move down to make contact w the object # 3. APPROACH: visit the same configuration as 1. # 4. TRANSPORT: visit the goal configuration. for obj_dict in self._scenario['objects']: t0 = self.get_time() # Basic setup manip_name = obj_dict["object_attach_to"] manip = self.get_robot().SetActiveManipulator(manip_name) Tstart = np.array(obj_dict['T_start']) # object's transform Tgoal = np.array(obj_dict['T_goal']) T_ee_start = np.dot( Tstart, self.get_object( obj_dict['name']).get_T_object_link()) self.verify_transform(manip, T_ee_start) T_ee_top = np.array(T_ee_start) try: T_ee_top[:3, 3] -= obj_dict['offset'] * T_ee_top[:3, 2] except BaseException: T_ee_top[:3, 3] -= offset * T_ee_top[:3, 2] q_nominal = np.r_[-0.3, 0.9, 0.9, 0, 0, 0] t1 = self.get_time() # 1. APPROACH logger.info("Plan path to APPROACH") traj0 = plan_to_manip_transform( self._robot, T_ee_top, q_nominal, max_ppiters=200, max_iters=100) t1a = self.get_time() if self.mode == "PRACTICE": self.check_continue() fail = not self.execute_trajectory(traj0) self.get_robot().WaitForController(0) # 2. REACH: Move a "short" trajectory to reach the object logger.info("Plan path to REACH") t2 = self.get_time() traj0b = plan_to_manip_transform( self._robot, T_ee_start, q_nominal, max_ppiters=200, max_iters=100) t2a = self.get_time() if self.mode == "PRACTICE": self.check_continue() fail = not self.execute_trajectory(traj0b) self.get_robot().WaitForController(0) with self._env: self._robot.Grab(self.get_env().GetKinBody(obj_dict['name'])) logger.info("Grabbing the object. Continue moving in 1 sec.") # time.sleep(1) # 3+4: APPROACH+TRANSPORT: Plan two trajectories, one # trajectory to reach the REACH position, another # trajectory to reach the GOAL position. Merge them, then # execute. t3 = self.get_time() logger.info("Plan path to GOAL") # 1st trajectory traj0c = plan_to_manip_transform( self._robot, T_ee_top, q_nominal, max_ppiters=1, max_iters=100) traj0c_waypoints, traj0c_ss = self.extract_waypoints(traj0c) T_ee_goal = np.dot( Tgoal, self.get_object( obj_dict['name']).get_T_object_link()) self.verify_transform(manip, T_ee_goal) q_nominal = np.r_[0.3, 0.9, 0.9, 0, 0, 0] # 2nd trajectory self._robot.SetActiveDOFValues(traj0c_waypoints[-1]) traj1_transport = plan_to_manip_transform( self._robot, T_ee_goal, q_nominal, max_ppiters=1, max_iters=100) self._robot.SetActiveDOFValues(traj0c_waypoints[0]) traj1_transport_waypoints, traj1_transport_ss = self.extract_waypoints( traj1_transport) # concatenate the two trajectories traj2_waypoints = np.vstack( (traj0c_waypoints, traj1_transport_waypoints[1:])) # retime traj2_ss = np.hstack( (traj0c_ss, traj0c_ss[-1] + traj1_transport_ss[1:])) traj2_ss[:] = traj2_ss / traj2_ss[-1] traj2_rave = orpy.RaveCreateTrajectory(self._env, "") spec = self._robot.GetActiveConfigurationSpecification() traj2_rave.Init(spec) for p in traj2_waypoints: traj2_rave.Insert(traj2_rave.GetNumWaypoints(), p) t3a = self.get_time() planner = orpy.RaveCreatePlanner(self._env, "ParabolicSmoother") params = orpy.Planner.PlannerParameters() params.SetRobotActiveJoints(self._robot) params.SetMaxIterations(100) params.SetPostProcessing('', '') success = planner.InitPlan(self._robot, params) status = planner.PlanPath(traj2_rave) if not success or not status: logger.fatal("[Plan Transport Path] Init status: {1:}, Plan status: {0:}. " "Use traj2_rave directly.".format(status, success)) t3b = self.get_time() # Retime the transport trajectory and execute it logger.debug( "Original traj nb waypoints: {:d}".format( traj1_transport.GetNumWaypoints())) logger.debug("Retime using toppra.") t4 = self.get_time() contact = self.get_object(obj_dict['name']).get_contact() contact_constraint = create_object_transporation_constraint( contact, self.get_object(obj_dict['name'])) contact_constraint.set_discretization_type(1) traj2_retimed = toppra.retime_active_joints_kinematics( traj2_rave, self._robot, additional_constraints=[contact_constraint], solver_wrapper=SOLVER, vmult=0.999, amult=0.999) if traj2_retimed is None: logger.error( "Transport trajectory retime fails! Try again without contact constraints.") traj2_retimed = toppra.retime_active_joints_kinematics( traj2_rave, self.get_robot(), additional_constraints=[]) t4a = self.get_time() if self.mode == "PRACTICE": self.check_continue() fail = not self.execute_trajectory(traj2_retimed) self.get_robot().WaitForController(0) # release the object logger.info("RELEASE object") self.check_continue() # release object now self._robot.Release(self.get_env().GetKinBody(obj_dict['name'])) t4b = self.get_time() logger.info("Time report" "\n - setup :{0:f} secs" "\n - APPROACH plan :{1:f} secs" "\n - APPROACH duration :{6:f} secs" "\n - REACH plan :{2:f} secs" "\n - REACH duration :{7:f} secs" "\n - MOVE plan :{3:f} secs" "\n - MOVE shortcut :{4:f} secs" "\n - MOVE retime :{5:f} secs" "\n - MOVE duration :{8:f} secs" "\n - TOTAL duration :{9:f} secs".format( t1 - t0, t1a - t1, t2a - t2, t3a - t3, t3b - t3a, t4a - t4, traj0.GetDuration(), traj0b.GetDuration(), traj2_retimed.GetDuration(), t4b - t0)) # remove objects from environment T_cur = self.get_env().GetKinBody(obj_dict['name']).GetTransform() T_cur[2, 3] -= 3e-3 # simulate 5mm drop self.get_env().GetKinBody(obj_dict['name']).SetTransform(T_cur) # Move the robot back to HOME trajHOME = plan_to_joint_configuration(self._robot, self._qHOME) fail = not self.execute_trajectory(trajHOME) return not fail
raw_input('enter to quit') exit() # save with box family.SendCommand('Let AllBox = $live') print('PrintCurrentFamily ...') s = family.SendCommand('PrintCurrentFamily') print(s) self_header = family.SendCommand('GetHeaderFromSet Self') print('self_header:') print(self_header) # create a planner planner = openravepy.RaveCreatePlanner(env, 'FamilyPlanner') # turn off the box, and plan box.Enable(False) # plan q_start -> q_goal (with no box!) robot.SetActiveDOFValues(q_start) params = openravepy.Planner.PlannerParameters() params.SetRobotActiveJoints(robot) params.SetExtraParameters(''' <roadmap_type>RGGDens</roadmap_type> <roadmap> <num_per_batch>10000</num_per_batch> <radius_first_batch>2.0</radius_first_batch> <seed>0</seed> </roadmap>